机器人自主导航 | 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字节数据中，作为校验码。

准备工作

1. 在ROS端安装serial功能包

sudo apt-get install ros-melodic-serial

2. 在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轴线速度转换到世界坐标系，如图：

这个坐标变换可以通过一个简单的旋转矩阵来实现
$${\left[\begin{array}{c}{v}_x\\ v_y\end{array}\right]}_{world}=\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]\cdot {\left[\begin{array}{c}{v}_x\\ v_y\end{array}\right]}_{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;	
}