ROS與移動(dòng)底盤(pán)通信教程

本實(shí)驗(yàn)是實(shí)現(xiàn)機(jī)器人自主導(dǎo)航的重要步驟，對(duì)于輪式機(jī)器人，可以通過(guò)在底盤(pán)加裝輪式里程計(jì)的方式來(lái)獲得機(jī)器人的速度數(shù)據(jù)，這些數(shù)據(jù)可以用來(lái)輔助機(jī)器人實(shí)現(xiàn)自主定位，同時(shí)機(jī)器人還需要將控制指令發(fā)送給移動(dòng)底盤(pán)，實(shí)現(xiàn)自主控制，本實(shí)驗(yàn)就將實(shí)現(xiàn)ROS與移動(dòng)底盤(pán)的通信。

實(shí)驗(yàn)環(huán)境：

· 軟件環(huán)境：Ubuntu18.04 + ROS melodic、Windows + Keil 5、VSCode

· 硬件環(huán)境：Jetson Nano(以下稱(chēng)為ROS端）、小車(chē)(以下稱(chēng)為STM32端)

01 實(shí)驗(yàn)內(nèi)容

ROS與STM32的通信流程如圖所示

主要包含兩個(gè)方面：

· 小車(chē)?yán)锍逃?jì)數(shù)據(jù)的上傳與接收

· 控制指令的下發(fā)與接收

1.1 原始消息內(nèi)容

在ROS中，里程計(jì)數(shù)據(jù)主要包括機(jī)器人的位姿(位置和姿態(tài))，以及機(jī)器人的速度(線速度和角速度)。對(duì)于本實(shí)驗(yàn)所用到的麥輪地面機(jī)器人，只需要知道機(jī)器人的x軸與y軸線速度、x軸與y軸位置、z軸角速度、偏航角即可。

由于對(duì)速度積分可以得到位置，對(duì)角速度積分可以得到角度，所以STM32端上傳的里程計(jì)數(shù)據(jù)只需要包括機(jī)器人的x軸與y軸線速度、z軸角速度，ROS端在接收到這些數(shù)據(jù)后，進(jìn)行積分即可得到位置和角度。

另外，在本實(shí)驗(yàn)用到的STM32端集成了一個(gè)ICM20602姿態(tài)傳感器，其中內(nèi)置了姿態(tài)解算算法，可以獲得準(zhǔn)確的機(jī)器人姿態(tài)數(shù)據(jù)，因此本實(shí)驗(yàn)使用STM32端上傳的偏航角來(lái)代替對(duì)角速度積分得到的航向角。

所以STM32上傳的里程計(jì)數(shù)據(jù)包括機(jī)器人的x軸線速度、y軸線速度、z軸角速度、偏航角。

與里程計(jì)數(shù)據(jù)類(lèi)似，對(duì)于麥輪地面機(jī)器人，控制指令只需要包括機(jī)器人的x軸速度、y軸速度、z軸角速度即可，機(jī)器人坐標(biāo)系如圖所示：


1.2 轉(zhuǎn)換為字節(jié)數(shù)組

知道了消息的原始數(shù)據(jù)，還需要將它轉(zhuǎn)換成傳輸效率更高的字節(jié)數(shù)組，如圖：



在C/C++中，有很多種將原始數(shù)據(jù)轉(zhuǎn)換為字節(jié)數(shù)組的方法，其中一種常用的方法是使用聯(lián)合體(union)。

聯(lián)合體的所有成員占用同一段內(nèi)存，修改一個(gè)成員會(huì)影響其余成員，如果要實(shí)現(xiàn)一個(gè)float數(shù)據(jù)與字節(jié)數(shù)組的互相轉(zhuǎn)換，我們可以定義如下的聯(lián)合體：

typedefunion{
float data;
uint8_t data8[4];
}data_u;

?

這個(gè)聯(lián)合體中有兩個(gè)成員，一個(gè)是32位的float數(shù)據(jù)data，另一個(gè)同樣是占據(jù)了32位字長(zhǎng)的字節(jié)數(shù)組data8，根據(jù)聯(lián)合體的性質(zhì)，這兩個(gè)成員所在的內(nèi)存位置是一樣的，也就是說(shuō)，改變其中任何一個(gè)成員的值，另一個(gè)也會(huì)被改變。

利用這個(gè)性質(zhì)，我們就可以實(shí)現(xiàn)float與字節(jié)數(shù)據(jù)的互相轉(zhuǎn)換。

1.3 添加幀頭和校驗(yàn)碼

本實(shí)驗(yàn)選擇常用的0xAA 0x55作為幀頭，同時(shí)對(duì)原始數(shù)據(jù)轉(zhuǎn)換得到的字節(jié)數(shù)組進(jìn)行求和，將結(jié)果保存在1字節(jié)數(shù)據(jù)中，作為校驗(yàn)碼。

準(zhǔn)備工作：

1. 在ROS端安裝serial功能包

sudoapt-getinstallros-melodic-serial

2. 在ROS端創(chuàng)建一個(gè)功能包，命名為xrobot，添加依賴(lài)項(xiàng)roscpp rospy tf serial

02 里程計(jì)數(shù)據(jù)的上傳與接收

2.1 通信協(xié)議

里程計(jì)數(shù)據(jù)格式(19字節(jié))



2.2 STM32端

/**
 * @brief 發(fā)送里程計(jì)數(shù)據(jù)
 */
void DataTrans_Odom(void)
{
uint8_t _cnt = 0;
  data_u _temp; // 聲明一個(gè)聯(lián)合體實(shí)例，使用它將待發(fā)送數(shù)據(jù)轉(zhuǎn)換為字節(jié)數(shù)組
uint8_t data_to_send[100] = {0}; // 待發(fā)送的字節(jié)數(shù)組


  data_to_send[_cnt++]=0xAA;
  data_to_send[_cnt++]=0x55;


uint8_t _start = _cnt;


float datas[] = {kinematics.odom.vel.linear_x, 
                     kinematics.odom.vel.linear_y, 
                     kinematics.odom.vel.angular_z, 
                     kinematics.odom.pose.theta
                    };


for(int i = 0; i < sizeof(datas) / sizeof(float); i++)
  {
// 將要發(fā)送的數(shù)據(jù)賦值給聯(lián)合體的float成員
// 相應(yīng)的就能更改字節(jié)數(shù)組成員的值
    _temp.data = datas[i];
    data_to_send[_cnt++]=_temp.data8[0];
    data_to_send[_cnt++]=_temp.data8[1];
    data_to_send[_cnt++]=_temp.data8[2];
    data_to_send[_cnt++]=_temp.data8[3]; // 最高位
  }


uint8_t checkout = 0;
for(int i = _start; i < _cnt; i++)
  {
    checkout += data_to_send[i];
  }
  data_to_send[_cnt++] = checkout;
// 串口發(fā)送
  SendData(data_to_send, _cnt); 
}

2.3 ROS端

采用狀態(tài)機(jī)的方式來(lái)接收STM32端上傳的里程計(jì)數(shù)據(jù)，每讀取一字節(jié)數(shù)據(jù)，則在狀態(tài)機(jī)中處理一次，部分程序如下：

uint8_tbuffer=0;
ser.read(&buffer, 1); // ser是串口類(lèi)的一個(gè)實(shí)例，該語(yǔ)句表示從串口中讀取一個(gè)字節(jié)
if(state == 0 && buffer == 0xAA)
{
    state++;
}
else if(state == 1 && buffer == 0x55)
{
    state++;
}
else if(state == 2)
{
    data_receive[data_cnt++]=buffer;
if(data_cnt == 17)
    {
/* 進(jìn)行數(shù)據(jù)校驗(yàn) */
uint8_t checkout = 0;
for(int k = 0; k < data_cnt - 1; k++)
        {
            checkout += data_receive[k];
        }
if(checkout == data_receive[data_cnt - 1]) // 串口接收到的最后一個(gè)字節(jié)是校驗(yàn)碼 
        {
/* 校驗(yàn)通過(guò)，進(jìn)行解碼 */
float vx, vy, vth, th; // x軸線速度，y軸線速度，z軸角速度，偏航角
float* datas_ptr[] = {&vx, &vy, &vth, &th};
            data_u temp;
for(int i = 0; i < sizeof(datas_ptr) / sizeof(float*); i++)
            {
                temp.data8[0] = data_receive[4 * i + 0];
                temp.data8[1] = data_receive[4 * i + 1];
                temp.data8[2] = data_receive[4 * i + 2];
                temp.data8[3] = data_receive[4 * i + 3];              
                *(datas_ptr[i]) = temp.data;
            }
            th *= D2R; // 轉(zhuǎn)換為弧度
        }
        data_cnt = 0;
        state = 0;
    }
}
else state = 0;

? ROS端在運(yùn)行時(shí)可能會(huì)提示串口打開(kāi)失敗，有兩種原因，一是串口號(hào)不對(duì)，使用dmesg | grep ttyS*列出檢測(cè)到的串口號(hào)，逐個(gè)測(cè)試；

二是沒(méi)有操作權(quán)限，使用sudo chmod 666 /dev/ttyACM0即可解決，也可以使用sudo usermod -aG dialout 用戶(hù)名來(lái)獲得永久權(quán)限，用戶(hù)名可使用whoami查看。

2.4 里程計(jì)數(shù)據(jù)可視化

以上步驟僅僅得到了機(jī)器人的x軸線速度、y軸線速度、z軸角速度、偏航角，還需要進(jìn)一步處理來(lái)獲得完整的里程計(jì)數(shù)據(jù)。

STM32端返回的x軸線速度、y軸線速度是相對(duì)于自身的機(jī)體坐標(biāo)系的速度，而機(jī)器人的位置信息是相對(duì)于世界坐標(biāo)系的位置，所以在對(duì)速度進(jìn)行積分前，要先將機(jī)體坐標(biāo)系下的x軸線速度、y軸線速度轉(zhuǎn)換到世界坐標(biāo)系，如圖：


這個(gè)坐標(biāo)變換可以通過(guò)一個(gè)簡(jiǎn)單的旋轉(zhuǎn)矩陣來(lái)實(shí)現(xiàn)



其中θ就是機(jī)器人的偏航角。相應(yīng)的程序如下：

/*對(duì)速度進(jìn)行積分得到位移*/
// 獲取當(dāng)前時(shí)間
current_time = ros::now();
// 獲取積分間隔
double dt = (current_time - last_time).toSec();
last_time = current_time;
// 將機(jī)體系速度轉(zhuǎn)換到里程計(jì)坐標(biāo)系
double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
// 速度積分
x += delta_x;
y += delta_y;

? 在機(jī)器人中，一般使用四元數(shù)/旋轉(zhuǎn)矩陣的形式來(lái)表示機(jī)器人的姿態(tài)，而不是歐拉角形式。所以需要將STM32返回的偏航角轉(zhuǎn)換為四元數(shù)，程序如下：

/*對(duì)速度進(jìn)行積分得到位移*/
// 獲取當(dāng)前時(shí)間
current_time = ros::now();
// 獲取積分間隔
double dt = (current_time - last_time).toSec();
last_time = current_time;
// 將機(jī)體系速度轉(zhuǎn)換到里程計(jì)坐標(biāo)系
double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
// 速度積分
x += delta_x;
y += delta_y;

? 以上就獲取了完整的機(jī)器人里程計(jì)數(shù)據(jù)，接下來(lái)需要將里程計(jì)數(shù)據(jù)發(fā)布到ROS中。

nav_msgs::Odometryodom;
geometry_msgs::TransformStamped odom_trans;


odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";


odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
// 發(fā)布坐標(biāo)變換
odom_broadcaster.sendTransform(odom_trans);


odom.header.stamp = current_time;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";


// 設(shè)置機(jī)器人的位置和姿態(tài)
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;


// 設(shè)置機(jī)器人的速度
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = vy;
odom.twist.twist.angular.z = vth;


// 發(fā)布里程計(jì)消息
odom_pub.publish(odom);

? 運(yùn)行后，打開(kāi)PC上的Ubuntu，配置ip從而實(shí)現(xiàn)遠(yuǎn)程連接嵌入式處理器上的ROS系統(tǒng)。

配置完成后，重新打開(kāi)一個(gè)終端，輸入：rviz，打開(kāi)ROS的可視化工具，按照下圖操作即可


可視化結(jié)果如下：



最后將該rviz配置保存至文件，點(diǎn)擊File→Save Config As，將配置保存為xxxx.rviz。下次打開(kāi)時(shí)，在命令行運(yùn)行：rosrun rviz rviz -d xxxx.rviz即可。

03 控制指令的下發(fā)與接收

3.1 通信協(xié)議

控制指令格式(15字節(jié))


3.2 ROS端

在ROS端，首先需要接收從其他節(jié)點(diǎn)的控制消息，在ROS中常常使用geometry_msgs::Twist來(lái)傳遞控制指令，該消息格式包括兩個(gè)三維向量，如下，分別是三軸線速度和三軸角速度：

geometry_msgs/Vector3linear
geometry_msgs/Vector3 angular

? 我們?cè)诳刂浦噶畹南⒒卣{(diào)函數(shù)中，將控制指令下發(fā)給STM32，部分程序如下，其中使用了C++的lambda表達(dá)式來(lái)替換回調(diào)函數(shù)

ros::Subscribersub=nh.subscribe("/cmd_vel",10,[&](constgeometry_msgs::ConstPtr&cmd_vel){
uint8_t _cnt = 0;
    data_u _temp; // 聲明一個(gè)聯(lián)合體實(shí)例，使用它將待發(fā)送數(shù)據(jù)轉(zhuǎn)換為字節(jié)數(shù)組
uint8_t data_to_send[100]; // 待發(fā)送的字節(jié)數(shù)組    
    data_to_send[_cnt++]=0xAA;
    data_to_send[_cnt++]=0x55;
uint8_t _start = _cnt;
float datas[] = {(float)cmd_vel->linear.x,
                     (float)cmd_vel->linear.y,
                     (float)cmd_vel->angular.z};    
for(int i = 0; i < sizeof(datas) / sizeof(float); i++){
// 將要發(fā)送的數(shù)據(jù)賦值給聯(lián)合體的float成員
// 相應(yīng)的就能更改字節(jié)數(shù)組成員的值
        _temp.data = datas[i];
        data_to_send[_cnt++]=_temp.data8[0];
        data_to_send[_cnt++]=_temp.data8[1];
        data_to_send[_cnt++]=_temp.data8[2];
        data_to_send[_cnt++]=_temp.data8[3]; // 最高位
    }
// 添加校驗(yàn)碼
char checkout = 0;
for(int i = _start; i < _cnt; i++)
        checkout += data_to_send[i];
    data_to_send[_cnt++] = checkout;
// 串口發(fā)送
    ser.write(data_to_send, _cnt);
});

? 最后，創(chuàng)建一個(gè)控制指令發(fā)布節(jié)點(diǎn)，用來(lái)發(fā)布cmd_vel話題，在xrobot功能包下新建一個(gè)scripts文件夾，添加remote_ctrl.py，內(nèi)容如下：

#!/usr/bin/envpython
# #-*- coding: UTF-8 -*- 


import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import String
import os, sys
import  tty, termios


pub = rospy.Publisher("cmd_vel", Twist)
rospy.init_node("remote_ctrl")
rate = rospy.Rate(rospy.get_param("-hz", 20))


cmd = Twist()
cmd.linear.x = 0.0
cmd.linear.y = 0.0
cmd.angular.z = 0.0


while not rospy.is_shutdown():    
    tty.setraw(sys.stdin.fileno())  
    ch = sys.stdin.read( 1 )  
if ch == "w":
        cmd.linear.x = 0.2
        cmd.linear.y = 0
        cmd.angular.z = 0
elif ch == "s":
        cmd.linear.x = -0.2
        cmd.linear.y = 0
        cmd.angular.z = 0
elif ch == "a":
        cmd.linear.x = 0
        cmd.linear.y = 0
        cmd.angular.z = 0.5
elif ch == "d":
        cmd.linear.x = 0
        cmd.linear.y = 0
        cmd.angular.z = -0.5
elif ch == "q":
break
else:
        cmd.linear.x = 0
        cmd.linear.y = 0
        cmd.angular.z = 0
    rospy.loginfo(str( cmd.linear.x) + " " + str(cmd.linear.y) + " " + str(cmd.angular.z) + "
")
    pub.publish(cmd)
    rate.sleep()

? 運(yùn)行robot_node和remote_ctrl節(jié)點(diǎn)，可以得到如下的節(jié)點(diǎn)圖：


3.3 STM32端

部分程序如下：

/**
 * @brief 從串口讀取單個(gè)字節(jié)
 * @param  data             讀取的字節(jié)數(shù)據(jù)
 */
void GetOneByte(uint8_t data)
{
  static u8 state = 0;
  static u8 cnt = 0;
if(state == 0 && data == 0xAA)
  {
    state++;
  }
else if(state == 1 && data == 0x55)
  {
    state++;
  }
else if(state == 2)
  {
    data_receive[cnt++] = data;
if(cnt >= 13)
    {
// 校驗(yàn)
      u8 checkout = 0;
for(int i = 0; i < cnt - 1; i++)
      {
        checkout += data_receive[i];
      }
if(checkout == data_receive[cnt - 1])
      {
// 校驗(yàn)通過(guò)，進(jìn)行解碼
        DataDecoder(data_receive);
      }
      state = 0;
      cnt = 0;
    }
  }
else state = 0;
}


/**
 * @brief 數(shù)據(jù)解碼
 * @param  data             待解碼數(shù)組
 */
void DataDecoder(u8 *data)
{
    data_u temp;
// x軸線速度
    temp.data8[0] = data[0];
    temp.data8[1] = data[1];
    temp.data8[2] = data[2];
    temp.data8[3] = data[3];
    kinematics.exp_vel.linear_x = temp.data;
// y軸線速度
    temp.data8[0] = data[4];
    temp.data8[1] = data[5];
    temp.data8[2] = data[6];
    temp.data8[3] = data[7];
    kinematics.exp_vel.linear_y = temp.data;
// z軸角速度
    temp.data8[0] = data[8];
    temp.data8[1] = data[9];
    temp.data8[2] = data[10];
    temp.data8[3] = data[11];
    kinematics.exp_vel.angular_z = temp.data;  
}

審核編輯：湯梓紅