ROS与移动底盘通信教程

本实验是实现机器人自主导航的重要步骤，对于轮式机器人，可以通过在底盘加装轮式里程计的方式来获得机器人的速度数据，这些数据可以用来辅助机器人实现自主定位，同时机器人还需要将控制指令发送给移动底盘，实现自主控制，本实验就将实现ROS与移动底盘的通信。

实验环境：

· 软件环境：Ubuntu18.04 + ROS melodic、Windows + Keil 5、VSCode

· 硬件环境：Jetson Nano(以下称为ROS端）、小车(以下称为STM32端)

01  实验内容

ROS与STM32的通信流程如图所示

主要包含两个方面：

· 小车里程计数据的上传与接收

· 控制指令的下发与接收

1.1 原始消息内容

在ROS中，里程计数据主要包括机器人的位姿(位置和姿态)，以及机器人的速度(线速度和角速度)。对于本实验所用到的麦轮地面机器人，只需要知道机器人的x轴与y轴线速度、x轴与y轴位置、z轴角速度、偏航角即可。

由于对速度积分可以得到位置，对角速度积分可以得到角度，所以STM32端上传的里程计数据只需要包括机器人的x轴与y轴线速度、z轴角速度，ROS端在接收到这些数据后，进行积分即可得到位置和角度。

另外，在本实验用到的STM32端集成了一个ICM20602姿态传感器，其中内置了姿态解算算法，可以获得准确的机器人姿态数据，因此本实验使用STM32端上传的偏航角来代替对角速度积分得到的航向角。

所以STM32上传的里程计数据包括机器人的x轴线速度、y轴线速度、z轴角速度、偏航角。

与里程计数据类似，对于麦轮地面机器人，控制指令只需要包括机器人的x轴速度、y轴速度、z轴角速度即可，机器人坐标系如图所示：


1.2 转换为字节数组

知道了消息的原始数据，还需要将它转换成传输效率更高的字节数组，如图：



在C/C++中，有很多种将原始数据转换为字节数组的方法，其中一种常用的方法是使用联合体(union)。

联合体的所有成员占用同一段内存，修改一个成员会影响其余成员，如果要实现一个float数据与字节数组的互相转换，我们可以定义如下的联合体：

typedef union{
float data;
uint8_t data8[4];
}data_u;

‍

这个联合体中有两个成员，一个是32位的float数据data，另一个同样是占据了32位字长的字节数组data8，根据联合体的性质，这两个成员所在的内存位置是一样的，也就是说，改变其中任何一个成员的值，另一个也会被改变。

利用这个性质，我们就可以实现float与字节数据的互相转换。

  1.3 添加帧头和校验码

本实验选择常用的0xAA 0x55作为帧头，同时对原始数据转换得到的字节数组进行求和，将结果保存在1字节数据中，作为校验码。

准备工作：

  1. 在ROS端安装serial功能包
 

sudo apt-get install ros-melodic-serial

  2. 在ROS端创建一个功能包，命名为xrobot，添加依赖项roscpp rospy tf serial
 
02  里程计数据的上传与接收

  2.1 通信协议

  里程计数据格式(19字节)

 

2.2 STM32端

/**
 * @brief 发送里程计数据
 */
void DataTrans_Odom(void)
{
uint8_t _cnt = 0;
  data_u _temp; // 声明一个联合体实例，使用它将待发送数据转换为字节数组
uint8_t data_to_send[100] = {0}; // 待发送的字节数组


  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++)
  {
// 将要发送的数据赋值给联合体的float成员
// 相应的就能更改字节数组成员的值
    _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;
// 串口发送
  SendData(data_to_send, _cnt); 
}

2.3 ROS端

  采用状态机的方式来接收STM32端上传的里程计数据，每读取一字节数据，则在状态机中处理一次，部分程序如下：

uint8_t buffer = 0;
ser.read(&buffer, 1); // ser是串口类的一个实例，该语句表示从串口中读取一个字节
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)
    {
/* 进行数据校验 */
uint8_t checkout = 0;
for(int k = 0; k < data_cnt - 1; k++)
        {
            checkout += data_receive[k];
        }
if(checkout == data_receive[data_cnt - 1]) // 串口接收到的最后一个字节是校验码 
        {
/* 校验通过，进行解码 */
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; // 转换为弧度
        }
        data_cnt = 0;
        state = 0;
    }
}
else state = 0;

‍ ROS端在运行时可能会提示串口打开失败，有两种原因，一是串口号不对，使用dmesg | grep ttyS*列出检测到的串口号，逐个测试；

  二是没有操作权限，使用sudo chmod 666 /dev/ttyACM0即可解决，也可以使用sudo usermod -aG dialout 用户名来获得永久权限，用户名可使用whoami查看。

  2.4 里程计数据可视化

  以上步骤仅仅得到了机器人的x轴线速度、y轴线速度、z轴角速度、偏航角，还需要进一步处理来获得完整的里程计数据。

  STM32端返回的x轴线速度、y轴线速度是相对于自身的机体坐标系的速度，而机器人的位置信息是相对于世界坐标系的位置，所以在对速度进行积分前，要先将机体坐标系下的x轴线速度、y轴线速度转换到世界坐标系，如图：


这个坐标变换可以通过一个简单的旋转矩阵来实现



其中θ就是机器人的偏航角。相应的程序如下：

/* 对速度进行积分得到位移 */
// 获取当前时间
current_time = ros::now();
// 获取积分间隔
double dt = (current_time - last_time).toSec();
last_time = current_time;
// 将机体系速度转换到里程计坐标系
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;

  ‍ 在机器人中，一般使用四元数/旋转矩阵的形式来表示机器人的姿态，而不是欧拉角形式。所以需要将STM32返回的偏航角转换为四元数，程序如下：

/* 对速度进行积分得到位移 */
// 获取当前时间
current_time = ros::now();
// 获取积分间隔
double dt = (current_time - last_time).toSec();
last_time = current_time;
// 将机体系速度转换到里程计坐标系
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;

  ‍ 以上就获取了完整的机器人里程计数据，接下来需要将里程计数据发布到ROS中。

nav_msgs::Odometry odom;
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;
// 发布坐标变换
odom_broadcaster.sendTransform(odom_trans);


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


// 设置机器人的位置和姿态
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;


// 设置机器人的速度
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = vy;
odom.twist.twist.angular.z = vth;


// 发布里程计消息
odom_pub.publish(odom);

  ‍ 运行后，打开PC上的Ubuntu，配置ip从而实现远程连接嵌入式处理器上的ROS系统。

  配置完成后，重新打开一个终端，输入：rviz，打开ROS的可视化工具，按照下图操作即可


可视化结果如下：



最后将该rviz配置保存至文件，点击File→Save Config As，将配置保存为xxxx.rviz。下次打开时，在命令行运行：rosrun rviz rviz -d xxxx.rviz即可。

  03  控制指令的下发与接收

  3.1 通信协议

  控制指令格式(15字节)


3.2 ROS端

  在ROS端，首先需要接收从其他节点的控制消息，在ROS中常常使用geometry_msgs::Twist来传递控制指令，该消息格式包括两个三维向量，如下，分别是三轴线速度和三轴角速度：

geometry_msgs/Vector3 linear
geometry_msgs/Vector3 angular

  ‍ 我们在控制指令的消息回调函数中，将控制指令下发给STM32，部分程序如下，其中使用了C++的lambda表达式来替换回调函数

ros::Subscriber sub = nh.subscribe("/cmd_vel", 10, [&](const geometry_msgs::ConstPtr& cmd_vel){
uint8_t _cnt = 0;
    data_u _temp; // 声明一个联合体实例，使用它将待发送数据转换为字节数组
uint8_t data_to_send[100]; // 待发送的字节数组    
    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++){
// 将要发送的数据赋值给联合体的float成员
// 相应的就能更改字节数组成员的值
        _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]; // 最高位
    }
// 添加校验码
char checkout = 0;
for(int i = _start; i < _cnt; i++)
        checkout += data_to_send[i];
    data_to_send[_cnt++] = checkout;
// 串口发送
    ser.write(data_to_send, _cnt);
});

  ‍ 最后，创建一个控制指令发布节点，用来发布cmd_vel话题，在xrobot功能包下新建一个scripts文件夹，添加remote_ctrl.py，内容如下：

#!/usr/bin/env python
# #-*- 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()

  ‍ 运行robot_node和remote_ctrl节点，可以得到如下的节点图：


3.3 STM32端

  部分程序如下：

/**
 * @brief 从串口读取单个字节
 * @param  data             读取的字节数据
 */
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)
    {
// 校验
      u8 checkout = 0;
for(int i = 0; i < cnt - 1; i++)
      {
        checkout += data_receive[i];
      }
if(checkout == data_receive[cnt - 1])
      {
// 校验通过，进行解码
        DataDecoder(data_receive);
      }
      state = 0;
      cnt = 0;
    }
  }
else state = 0;
}


/**
 * @brief 数据解码
 * @param  data             待解码数组
 */
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;  
}

 

　　审核编辑：汤梓红