机器人自主导航 | ROS与移动底盘通信
本实验是实现机器人自主导航的重要步骤，对于轮式机器人，可以通过在底盘加装轮式里程计的方式来获得机器人的速度数据，这些数据可以用来辅助机器人实现自主定位，同时机器人还需要将控制指令发送给移动底盘，实现自主控制，本实验就将实现ROS与移动底盘的通信。
实验环境
- 软件环境：Ubuntu18.04 + ROS melodic、Windows + Keil 5、VSCode
- 硬件环境：Jetson Nano(以下称为ROS端）、小车(以下称为STM32端)
实验内容
ROS与STM32的通信流程如图所示
主要包含两个方面：
- 小车里程计数据的上传与接收
- 控制指令的下发与接收
原始消息内容
在ROS中，里程计数据主要包括机器人的位姿(位置和姿态)，以及机器人的速度(线速度和角速度)。对于本实验所用到的麦轮地面机器人，只需要知道机器人的x轴与y轴线速度、x轴与y轴位置、z轴角速度、偏航角即可。由于对速度积分可以得到位置，对角速度积分可以得到角度，所以STM32端上传的里程计数据只需要包括机器人的x轴与y轴线速度、z轴角速度，ROS端在接收到这些数据后，进行积分即可得到位置和角度。另外，在本实验用到的STM32端集成了一个ICM20602姿态传感器，其中内置了姿态解算算法，可以获得准确的机器人姿态数据，因此本实验使用STM32端上传的偏航角来代替对角速度积分得到的航向角。
所以STM32上传的里程计数据包括机器人的x轴线速度、y轴线速度、z轴角速度、偏航角。
与里程计数据类似，对于麦轮地面机器人，控制指令只需要包括机器人的x轴速度、y轴速度、z轴角速度即可，机器人坐标系如图所示：
转换为字节数组
知道了消息的原始数据，还需要将它转换成传输效率更高的字节数组，如图：
在C/C++中，有很多种将原始数据转换为字节数组的方法，其中一种常用的方法是使用联合体(union)。联合体的所有成员占用同一段内存，修改一个成员会影响其余成员，如果要实现一个float数据与字节数组的互相转换，我们可以定义如下的联合体：
typedef union{
float data;
uint8_t data8[4];
}data_u;
这个联合体中有两个成员，一个是32位的float数据data，另一个同样是占据了32位字长的字节数组data8，根据联合体的性质，这两个成员所在的内存位置是一样的，也就是说，改变其中任何一个成员的值，另一个也会被改变。利用这个性质，我们就可以实现float与字节数据的互相转换。
添加帧头和校验码
本实验选择常用的0xAA 0x55作为帧头，同时对原始数据转换得到的字节数组进行求和，将结果保存在1字节数据中，作为校验码。
准备工作
- 在ROS端安装serial功能包
sudo apt-get install ros-melodic-serial
- 在ROS端创建一个功能包，命名为
xrobot，添加依赖项roscpp rospy tf serial
里程计数据的上传与接收
通信协议
里程计数据格式(19字节)
| 帧头 | 帧头 | X轴线速度(float, m/s) | Y轴线速度(float, m/s) | Z轴角速度(float, rad/s) | 偏航角(float, deg) | 校验码 |
|---|---|---|---|---|---|---|
| 0xAA | 0x55 | 4Bytes | 4Bytes | 4Bytes | 4Bytes | 1Byte |
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);
}
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查看。
里程计数据可视化
以上步骤仅仅得到了机器人的x轴线速度、y轴线速度、z轴角速度、偏航角，还需要进一步处理来获得完整的里程计数据。
STM32端返回的x轴线速度、y轴线速度是相对于自身的机体坐标系的速度，而机器人的位置信息是相对于世界坐标系的位置，所以在对速度进行积分前，要先将机体坐标系下的x轴线速度、y轴线速度转换到世界坐标系，如图：
这个坐标变换可以通过一个简单的旋转矩阵来实现
[ v x v y ] w o r l d = [ c o s θ − s i n θ s i n θ c o s θ ] ⋅ [ v x v y ] b o d y \begin{bmatrix}v_x\\v_y\end{bmatrix}_{world}=\begin{bmatrix}cos\theta&-sin\theta\\sin\theta&cos\theta\end{bmatrix}\cdot \begin{bmatrix}v_x\\v_y\end{bmatrix}_{body} [vxvy]world=[cosθsinθ−sinθcosθ]⋅[vxvy]body
其中 θ \theta θ就是机器人的偏航角。相应的程序如下：
/* 对速度进行积分得到位移 */
// 获取当前时间
current_time = ros::Time::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返回的偏航角转换为四元数，程序如下：
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
以上就获取了完整的机器人里程计数据，接下来需要将里程计数据发布到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系统，参照：ROS多机通信。
配置完成后，重新打开一个终端，输入：rviz，打开ROS的可视化工具，按照下图操作即可
可视化结果如下：
最后将该rviz配置保存至文件，点击File→Save Config As，将配置保存为xxxx.rviz。下次打开时，在命令行运行：rosrun rviz rviz -d xxxx.rviz即可。
控制指令的下发与接收
本节内容与上节基本一致
通信协议
控制指令格式(15字节)
| 帧头１ | 帧头２ | X轴线速度(float, m/s) | Y轴线速度(float, m/s) | Z轴角速度(float, m/s) | 校验码 |
|---|---|---|---|---|---|
| 0xAA | 0x55 | 4Bytes | 4Bytes | 4Bytes | 1Byte |
ROS端
在ROS端，首先需要接收从其他节点的控制消息，在ROS中常常使用geometry_msgs::Twist来传递控制指令，该消息格式包括两个三维向量，如下，分别是三轴线速度和三轴角速度：
geometry_msgs/Vector3 linear
geometry_msgs/Vector3 angular
我们在控制指令的消息回调函数中，将控制指令下发给STM32，部分程序如下，其中使用了C++的lambda表达式来替换回调函数
ros::Subscriber sub = nh.subscribe<geometry_msgs::Twist>("/cmd_vel", 10, [&](const geometry_msgs::Twist::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) + "\r\n")
pub.publish(cmd)
rate.sleep()
运行robot_node和remote_ctrl节点，可以得到如下的节点图：
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;
}