基於ROS平臺的移動機器人-4-通過ROS利用鍵盤控制小車移動
準備工作
1.下載串口通信的ROS包
(1)cd ~/catkin_ws/src
(2)git clone https://github.com/Forrest-Z/serial.git
2.下載鍵盤控制的ROS包
(1)cd ~/catkin_ws/src
(2)git clone https://github.com/Forrest-Z/teleop_twist_keyboard.git
進入下載好的ROS包的文件夾,選中 keyboard_teleop_zbot.py ,右鍵>屬性>權限>勾選 允許作爲程序執行文件。
最後一步:
(1)cd ~/catkin_ws
(2)catkin_make
這樣子我們的鍵盤控制包就能使用了。
3.新建 base_controller ROS 包
(1)cd ~/catkin_ws/src
(2)catkin_create_pkg base_controller roscpp
在新建好的ROS包文件夾下新建一個“src”的文件夾,然後進入該文件夾,新建一個base_controller.cpp文件,將本博文最後提供的代碼粘貼進去,然後修改一下 CMakeLists.txt :
第一處修改
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
message_generation
serial
tf
nav_msgs
)
第二處修改
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES base_controller
CATKIN_DEPENDS roscpp rospy std_msgs
# DEPENDS system_lib
)
第三處修改
include_directories(
${catkin_INCLUDE_DIRS}
${serial_INCLUDE_DIRS}
)
第四處修改
add_executable(base_controller src/base_controller.cpp)
target_link_libraries(base_controller ${catkin_LIBRARIES})
然後修改一下 package.xml :
<?xml version="1.0"?>
<package>
<name>base_controller</name>
<version>0.0.0</version>
<description>The base_controller package</description>
email="[email protected]">siat</maintainer>
<license>TODO</license>
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>tf</build_depend>
<build_depend>nav_msgs</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>tf</run_depend>
<run_depend>nav_msgs</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
控制原理
1.當我們按下鍵盤時,teleop_twist_keyboard 包會發布 /cmd_vel 主題發佈速度
2.我們在 base_controller 節點訂閱這個話題,接收速度數據,在轉換成與底盤通信的格式,然後寫入串口
3.我們在 base_controller 節點讀取底盤向串口發來的里程計數據,然後進行處理再將里程計發佈出去,同時更新tf
4.當小車底盤接收到串口發來的速度後,就控制電機運轉,從而實現鍵盤控制小車的移動
運行
1.啓動鍵盤節點
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
2.啓動小車底盤控制節點
rosrun base_controller base_controller
注意事項
1.我們在啓動小車底盤控制節點時,有可以啓動不了,大多數是因爲串口的端口號不對,在 base_controller.cpp 文件裏,我用的是”/dev/ttyUSB0”串口端口號
2.我們在啓動啓動小車底盤控制節點前,應該查看一下我們底盤的串口號是否正確,查看指令如下:
ls -l /dev |grep ttyUSB
如果運行後顯示的端口號和我們程序中的一樣,那就沒問題,如果不一樣,我們將程序的代碼改動一下便可。
————————————————————————————————————————————————————————————————
base_controller.cpp 完整代碼:
/******************************************************************
基於串口通信的ROS小車基礎控制器,功能如下:
1.實現ros控制數據通過固定的格式和串口通信,從而達到控制小車的移動
2.訂閱了/cmd_vel主題,只要向該主題發佈消息,就能實現對控制小車的移動
3.發佈里程計主題/odm
串口通信說明:
1.寫入串口
(1)內容:左右輪速度,單位爲mm/s
(2)格式:10字節,[右輪速度4字節][左輪速度4字節][結束符"\r\n"2字節]
2.讀取串口
(1)內容:小車x,y座標,方向角,線速度,角速度,單位依次爲:mm,mm,rad,mm/s,rad/s
(2)格式:21字節,[X座標4字節][Y座標4字節][方向角4字節][線速度4字節][角速度4字節][結束符"\n"1字節]
*******************************************************************/
#include "ros/ros.h" //ros需要的頭文件
#include <geometry_msgs/Twist.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
//以下爲串口通訊需要的頭文件
#include <string>
#include <iostream>
#include <cstdio>
#include <unistd.h>
#include <math.h>
#include "serial/serial.h"
/****************************************************************************/
using std::string;
using std::exception;
using std::cout;
using std::cerr;
using std::endl;
using std::vector;
/*****************************************************************************/
float ratio = 1000.0f ; //轉速轉換比例,執行速度調整比例
float D = 0.2680859f ; //兩輪間距,單位是m
float linear_temp=0,angular_temp=0;//暫存的線速度和角速度
/****************************************************/
unsigned char data_terminal0=0x0d; //“/r"字符
unsigned char data_terminal1=0x0a; //“/n"字符
unsigned char speed_data[10]={0}; //要發給串口的數據
string rec_buffer; //串口數據接收變量
//發送給下位機的左右輪速度,里程計的座標和方向
union floatData //union的作用爲實現char數組和float之間的轉換
{
float d;
unsigned char data[4];
}right_speed_data,left_speed_data,position_x,position_y,oriention,vel_linear,vel_angular;
/************************************************************/
void callback(const geometry_msgs::Twist & cmd_input)//訂閱/cmd_vel主題回調函數
{
string port("/dev/ttyUSB0"); //小車串口號
unsigned long baud = 115200; //小車串口波特率
serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000)); //配置串口
angular_temp = cmd_input.angular.z ;//獲取/cmd_vel的角速度,rad/s
linear_temp = cmd_input.linear.x ;//獲取/cmd_vel的線速度.m/s
//將轉換好的小車速度分量爲左右輪速度
left_speed_data.d = linear_temp - 0.5f*angular_temp*D ;
right_speed_data.d = linear_temp + 0.5f*angular_temp*D ;
//存入數據到要發佈的左右輪速度消息
left_speed_data.d*=ratio; //放大1000倍,mm/s
right_speed_data.d*=ratio;//放大1000倍,mm/s
for(int i=0;i<4;i++) //將左右輪速度存入數組中發送給串口
{
speed_data[i]=right_speed_data.data[i];
speed_data[i+4]=left_speed_data.data[i];
}
//在寫入串口的左右輪速度數據後加入”/r/n“
speed_data[8]=data_terminal0;
speed_data[9]=data_terminal1;
//寫入數據到串口
my_serial.write(speed_data,10);
}
int main(int argc, char **argv)
{
string port("/dev/ttyUSB0");//小車串口號
unsigned long baud = 115200;//小車串口波特率
serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000));//配置串口
ros::init(argc, argv, "base_controller");//初始化串口節點
ros::NodeHandle n; //定義節點進程句柄
ros::Subscriber sub = n.subscribe("cmd_vel", 20, callback); //訂閱/cmd_vel主題
ros::Publisher odom_pub= n.advertise<nav_msgs::Odometry>("odom", 20); //定義要發佈/odom主題
static tf::TransformBroadcaster odom_broadcaster;//定義tf對象
geometry_msgs::TransformStamped odom_trans;//創建一個tf發佈需要使用的TransformStamped類型消息
nav_msgs::Odometry odom;//定義里程計對象
geometry_msgs::Quaternion odom_quat; //四元數變量
//定義covariance矩陣,作用爲解決文職和速度的不同測量的不確定性
float covariance[36] = {0.01, 0, 0, 0, 0, 0, // covariance on gps_x
0, 0.01, 0, 0, 0, 0, // covariance on gps_y
0, 0, 99999, 0, 0, 0, // covariance on gps_z
0, 0, 0, 99999, 0, 0, // large covariance on rot x
0, 0, 0, 0, 99999, 0, // large covariance on rot y
0, 0, 0, 0, 0, 0.01}; // large covariance on rot z
//載入covariance矩陣
for(int i = 0; i < 36; i++)
{
odom.pose.covariance[i] = covariance[i];;
}
ros::Rate loop_rate(10);//設置週期休眠時間
while(ros::ok())
{
rec_buffer =my_serial.readline(25,"\n"); //獲取串口發送來的數據
const char *receive_data=rec_buffer.data(); //保存串口發送來的數據
if(rec_buffer.length()==21) //串口接收的數據長度正確就處理併發布里程計數據消息
{
for(int i=0;i<4;i++)//提取X,Y座標,方向,線速度,角速度
{
position_x.data[i]=receive_data[i];
position_y.data[i]=receive_data[i+4];
oriention.data[i]=receive_data[i+8];
vel_linear.data[i]=receive_data[i+12];
vel_angular.data[i]=receive_data[i+16];
}
//將X,Y座標,線速度縮小1000倍
position_x.d/=1000; //m
position_y.d/=1000; //m
vel_linear.d/=1000; //m/s
//里程計的偏航角需要轉換成四元數才能發佈
odom_quat = tf::createQuaternionMsgFromYaw(oriention.d);//將偏航角轉換成四元數
//載入座標(tf)變換時間戳
odom_trans.header.stamp = ros::Time::now();
//發佈座標變換的父子座標系
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_footprint";
//tf位置數據:x,y,z,方向
odom_trans.transform.translation.x = position_x.d;
odom_trans.transform.translation.y = position_y.d;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
//發佈tf座標變化
odom_broadcaster.sendTransform(odom_trans);
//載入里程計時間戳
odom.header.stamp = ros::Time::now();
//里程計的父子座標系
odom.header.frame_id = "odom";
odom.child_frame_id = "base_footprint";
//里程計位置數據:x,y,z,方向
odom.pose.pose.position.x = position_x.d;
odom.pose.pose.position.y = position_y.d;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
//載入線速度和角速度
odom.twist.twist.linear.x = vel_linear.d;
//odom.twist.twist.linear.y = odom_vy;
odom.twist.twist.angular.z = vel_angular.d;
//發佈里程計
odom_pub.publish(odom);
ros::spinOnce();//週期執行
loop_rate.sleep();//週期休眠
}
//程序週期性調用
//ros::spinOnce(); //callback函數必須處理所有問題時,纔可以用到
}
return 0;
}