[轉]基於ROS平臺的移動機器人-4-經過ROS利用鍵盤控制小車移動

原文出處： 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

• 1
• 2
• 3

2.下載鍵盤控制的ROS包

(1)cd ~/catkin_ws/src
(2)git clone https://github.com/Forrest-Z/teleop_twist_keyboard.git

• 1
• 2
• 3

進入下載好的ROS包的文件夾，選中 keyboard_teleop_zbot.py ，右鍵>屬性>權限>勾選 容許做爲程序執行文件。
最後一步：

（1）cd ~/catkin_ws
 (2)catkin_make

• 1
• 2
• 3

這樣子咱們的鍵盤控制包就能使用了。

3.新建 base_controller ROS 包

（1）cd ~/catkin_ws/src
（2）catkin_create_pkg base_controller roscpp

• 1
• 2
• 3

在新建好的ROS包文件夾下新建一個「src」的文件夾，而後進入該文件夾，新建一個base_controller.cpp文件，將本博文最後提供的代碼粘貼進去，而後修改一下 CMakeLists.txt ：

第一處修改

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
  serial
  tf
  nav_msgs
)

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10

第二處修改

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES base_controller
  CATKIN_DEPENDS roscpp rospy std_msgs
#  DEPENDS system_lib
)

• 1
• 2
• 3
• 4
• 5
• 6
• 7

第三處修改

include_directories(
  ${catkin_INCLUDE_DIRS}
  ${serial_INCLUDE_DIRS}
)

• 1
• 2
• 3
• 4
• 5

第四處修改

add_executable(base_controller src/base_controller.cpp)
target_link_libraries(base_controller ${catkin_LIBRARIES})

• 1
• 2
• 3

而後修改一下 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> <!-- <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
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34

控制原理

1.當咱們按下鍵盤時，teleop_twist_keyboard 包會發布 /cmd_vel 主題發佈速度

2.咱們在 base_controller 節點訂閱這個話題，接收速度數據，在轉換成與底盤通訊的格式，而後寫入串口

3.咱們在 base_controller 節點讀取底盤向串口發來的里程計數據，而後進行處理再將里程計發佈出去，同時更新tf

4.當小車底盤接收到串口發來的速度後，就控制電機運轉，從而實現鍵盤控制小車的移動

運行

1.啓動鍵盤節點

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

• 1
• 2

2.啓動小車底盤控制節點

rosrun base_controller base_controller

• 1
• 2

注意事項

1.咱們在啓動小車底盤控制節點時，有能夠啓動不了，大多數是由於串口的端口號不對，在 base_controller.cpp 文件裏，我用的是」/dev/ttyUSB0」串口端口號

2.咱們在啓動啓動小車底盤控制節點前，應該查看一下咱們底盤的串口號是否正確，查看指令以下：

ls -l /dev |grep ttyUSB

• 1
• 2

若是運行後顯示的端口號和咱們程序中的同樣，那就沒問題，若是不同，咱們將程序的代碼改動一下即可。

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

base_controller.cpp 完整代碼：

/******************************************************************
基於串口通訊的ROS小車基礎控制器，功能以下：
1.實現ros控制數據經過固定的格式和串口通訊，從而達到控制小車的移動
2.訂閱了/cmd_vel主題，只要向該主題發佈消息，就能實現對控制小車的移動
3.發佈里程計主題/odm

串口通訊說明：
1.寫入串口
（1）內容：左右輪速度，單位爲mm/s
（2）格式：１０字節,[右輪速度４字節][左輪速度４字節][結束符"\r\n"２字節]
2.讀取串口
（1）內容：小車x,y座標，方向角，線速度，角速度，單位依次爲：mm,mm,rad,mm/s,rad/s
（2）格式：２１字節，[Ｘ座標４字節][Ｙ座標４字節][方向角４字節][線速度４字節][角速度４字節][結束符"\n"１字節]
*******************************************************************/
#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;   //放大１０００倍，mm/s
    right_speed_data.d*=ratio;//放大１０００倍，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;
}