ROS与移动底盘通信-USB迷|专注于互联网分享

ROS与移动底盘通信

作者丨白鸟无言

来源丨

编辑丨一点人工一点智能

点击进入—>3D视觉工坊学习交流群

本实验是实现机器人自主导航的重要步骤，对于轮式机器人，可以通过在底盘加装轮式里程计的方式来获得机器人的速度数据，这些数据可以用来辅助机器人实现自主定位，同时机器人还需要将控制指令发送给移动底盘，实现自主控制，本实验就将实现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::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返回的偏航角转换为四元数，程序如下：

/* 对速度进行积分得到位移 */
// 获取当前时间
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;

‍

以上就获取了完整的机器人里程计数据，接下来需要将里程计数据发布到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即可。

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<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, termiospub = 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.0while not rospy.is_shutdown():    tty.setraw(sys.stdin.fileno())  ch = sys.stdin.read( 1 )  
if ch == "w":cmd.linear.x = 0.2cmd.linear.y = 0cmd.angular.z = 0
elif ch == "s":cmd.linear.x = -0.2cmd.linear.y = 0cmd.angular.z = 0
elif ch == "a":cmd.linear.x = 0cmd.linear.y = 0cmd.angular.z = 0.5
elif ch == "d":cmd.linear.x = 0cmd.linear.y = 0cmd.angular.z = -0.5
elif ch == "q":
break
else:cmd.linear.x = 0cmd.linear.y = 0cmd.angular.z = 0rospy.loginfo(str( cmd.linear.x) + " " + str(cmd.linear.y) + " " + str(cmd.angular.z) + "\r\n")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;  
}

本文仅做学术分享，如有侵权，请联系删文。

点击进入—>3D视觉工坊学习交流群

干货下载与学习

后台回复：巴塞罗那自治大学课件，即可下载国外大学沉淀数年3D Vison精品课件

后台回复：计算机视觉书籍，即可下载3D视觉领域经典书籍pdf

后台回复：3D视觉课程，即可学习3D视觉领域精品课程

3D视觉工坊精品课程官网：3dcver.com

1.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
2.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
3.国内首个面向工业级实战的点云处理课程
4.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
5.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
6.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
7.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

8.从零搭建一套结构光3D重建系统[理论+源码+实践]

9.单目深度估计方法：算法梳理与代码实现

10.自动驾驶中的深度学习模型部署实战

11.相机模型与标定(单目+双目+鱼眼）

12.重磅！四旋翼飞行器：算法与实战

13.ROS2从入门到精通：理论与实战

14.国内首个3D缺陷检测教程：理论、源码与实战

15.基于Open3D的点云处理入门与实战教程

16.透彻理解视觉ORB-SLAM3：理论基础+代码解析+算法改进

17.机械臂抓取从入门到实战

重磅！粉丝学习交流群已成立

交流群主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、ORB-SLAM系列源码交流、深度估计、TOF、求职交流等方向。

扫描以下二维码，添加小助理微信（dddvisiona）,一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿，微信号：dddvisiona

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、源码分享、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答等进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，6000+星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看，3天内无条件退款

高质量教程资料、答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~