當前位置：首頁 > 编程资源 > 编程问答 >内容正文

编程问答

[转]基于ROS平台的移动机器人-4-通过ROS利用键盘控制小车移动

發(fā)布時間：2023/12/15 编程问答 28 豆豆

生活随笔收集整理的這篇文章主要介紹了 [转]基于ROS平台的移动机器人-4-通过ROS利用键盘控制小车移动小編覺得挺不錯的,現(xiàn)在分享給大家,幫大家做個參考.

原文出處： https://blog.csdn.net/Forrest_Z/article/details/55002484

準備工作

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 ，右鍵>屬性>權限>勾選允許作為程序執(zhí)行文件。
最后一步：

（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 COMPONENTSroscpprospystd_msgsmessage_generationserialtfnav_msgs )

第二處修改

catkin_package( # INCLUDE_DIRS include # LIBRARIES base_controllerCATKIN_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="siat@todo.todo">siat</maintainer> <license>TODO</license>  <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>  <export>  </export> </package>

控制原理

1.當我們按下鍵盤時，teleop_twist_keyboard 包會發(fā)布 /cmd_vel 主題發(fā)布速度

2.我們在 base_controller 節(jié)點訂閱這個話題，接收速度數(shù)據(jù)，在轉換成與底盤通信的格式，然后寫入串口

3.我們在 base_controller 節(jié)點讀取底盤向串口發(fā)來的里程計數(shù)據(jù)，然后進行處理再將里程計發(fā)布出去，同時更新tf

4.當小車底盤接收到串口發(fā)來的速度后，就控制電機運轉，從而實現(xiàn)鍵盤控制小車的移動

運行

1.啟動鍵盤節(jié)點

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

2.啟動小車底盤控制節(jié)點

rosrun base_controller base_controller

注意事項

1.我們在啟動小車底盤控制節(jié)點時，有可以啟動不了，大多數(shù)是因為串口的端口號不對，在 base_controller.cpp 文件里，我用的是”/dev/ttyUSB0”串口端口號

2.我們在啟動啟動小車底盤控制節(jié)點前，應該查看一下我們底盤的串口號是否正確，查看指令如下：

ls -l /dev |grep ttyUSB

如果運行后顯示的端口號和我們程序中的一樣，那就沒問題，如果不一樣，我們將程序的代碼改動一下便可。

————————————————————————————————————————————————————————————————

base_controller.cpp 完整代碼：

/****************************************************************** 基于串口通信的ROS小車基礎控制器，功能如下： 1.實現(xiàn)ros控制數(shù)據(jù)通過固定的格式和串口通信，從而達到控制小車的移動 2.訂閱了/cmd_vel主題，只要向該主題發(fā)布消息，就能實現(xiàn)對控制小車的移動 3.發(fā)布里程計主題/odm串口通信說明： 1.寫入串口（1）內(nèi)容：左右輪速度，單位為mm/s （2）格式：１０字節(jié),[右輪速度４字節(jié)][左輪速度４字節(jié)][結束符"\r\n"２字節(jié)] 2.讀取串口（1）內(nèi)容：小車x,y坐標，方向角，線速度，角速度，單位依次為：mm,mm,rad,mm/s,rad/s （2）格式：２１字節(jié)，[Ｘ坐標４字節(jié)][Ｙ坐標４字節(jié)][方向角４字節(jié)][線速度４字節(jié)][角速度４字節(jié)][結束符"\n"１字節(jié)] *******************************************************************/ #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 ; //轉速轉換比例，執(zhí)行速度調(diào)整比例 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}; //要發(fā)給串口的數(shù)據(jù) string rec_buffer; //串口數(shù)據(jù)接收變量//發(fā)送給下位機的左右輪速度，里程計的坐標和方向 union floatData //union的作用為實現(xiàn)char數(shù)組和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主題回調(diào)函數(shù) {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/slinear_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 ;//存入數(shù)據(jù)到要發(fā)布的左右輪速度消息left_speed_data.d*=ratio; //放大１０００倍，mm/sright_speed_data.d*=ratio;//放大１０００倍，mm/sfor(int i=0;i<4;i++) //將左右輪速度存入數(shù)組中發(fā)送給串口{speed_data[i]=right_speed_data.data[i];speed_data[i+4]=left_speed_data.data[i];}//在寫入串口的左右輪速度數(shù)據(jù)后加入”/r/n“speed_data[8]=data_terminal0;speed_data[9]=data_terminal1;//寫入數(shù)據(jù)到串口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");//初始化串口節(jié)點ros::NodeHandle n; //定義節(jié)點進程句柄ros::Subscriber sub = n.subscribe("cmd_vel", 20, callback); //訂閱/cmd_vel主題ros::Publisher odom_pub= n.advertise<nav_msgs::Odometry>("odom", 20); //定義要發(fā)布/odom主題static tf::TransformBroadcaster odom_broadcaster;//定義tf對象geometry_msgs::TransformStamped odom_trans;//創(chuàng)建一個tf發(fā)布需要使用的TransformStamped類型消息nav_msgs::Odometry odom;//定義里程計對象geometry_msgs::Quaternion odom_quat; //四元數(shù)變量//定義covariance矩陣，作用為解決文職和速度的不同測量的不確定性float covariance[36] = {0.01, 0, 0, 0, 0, 0, // covariance on gps_x0, 0.01, 0, 0, 0, 0, // covariance on gps_y0, 0, 99999, 0, 0, 0, // covariance on gps_z0, 0, 0, 99999, 0, 0, // large covariance on rot x0, 0, 0, 0, 99999, 0, // large covariance on rot y0, 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"); //獲取串口發(fā)送來的數(shù)據(jù)const char *receive_data=rec_buffer.data(); //保存串口發(fā)送來的數(shù)據(jù)if(rec_buffer.length()==21) //串口接收的數(shù)據(jù)長度正確就處理并發(fā)布里程計數(shù)據(jù)消息{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; //mposition_y.d/=1000; //mvel_linear.d/=1000; //m/s//里程計的偏航角需要轉換成四元數(shù)才能發(fā)布odom_quat = tf::createQuaternionMsgFromYaw(oriention.d);//將偏航角轉換成四元數(shù)//載入坐標（tf）變換時間戳odom_trans.header.stamp = ros::Time::now();//發(fā)布坐標變換的父子坐標系odom_trans.header.frame_id = "odom"; odom_trans.child_frame_id = "base_footprint"; //tf位置數(shù)據(jù)：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; //發(fā)布tf坐標變化odom_broadcaster.sendTransform(odom_trans);//載入里程計時間戳odom.header.stamp = ros::Time::now(); //里程計的父子坐標系odom.header.frame_id = "odom";odom.child_frame_id = "base_footprint"; //里程計位置數(shù)據(jù)：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; //發(fā)布里程計odom_pub.publish(odom);ros::spinOnce();//周期執(zhí)行l(wèi)oop_rate.sleep();//周期休眠}//程序周期性調(diào)用//ros::spinOnce(); //callback函數(shù)必須處理所有問題時，才可以用到}return 0; }

轉載于:https://www.cnblogs.com/qiuheng/p/9261876.html

總結

以上是生活随笔為你收集整理的[转]基于ROS平台的移动机器人-4-通过ROS利用键盘控制小车移动的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。