ROS与STM32通信

ROS与STM32是用串口进行通信的，主要有两种方式，一是将STM32作为一个节点，二是制作一个上位机节点,负责与STM32进行串口通信．第一种方式需要专门的硬件，所以我选择第二种方式

本文将实现在ROS上制作上位机节点接收来自STM32发送的IMU数据，并将接收到的数据作为话题进行发布．由于发送的数据均为float型数据，而串口只支持发送字节型的数据，所以实现两者通信的关键就是float与字节数据的相互转换．目前我知道两种将float拆分成字节数组的方式

1. 将float数据强制转换成int整型，再将int整型拆分成字节数组
2. 使用联合体将float型数据拆分成字节数组

第一种方式会导致部分精度的损失，所以本文将使用第二种方式，具体做法如下：
首先定义一个联合体

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

这个联合体中有两个成员，一个是32位的float数据data，另一个同样是占据了32位字长的字节数组data8，根据联合体的性质，这两个成员所在的内存位置是一样的，也就是说，改变其中任何一个成员的值，另一个也会被改变．利用这个性质，我们就可以实现float与字节数据的互换．

运行环境

• 软件环境：Ubuntu18.04 / ROS melodic / VScode / Keil 5
• 硬件环境：普通PC / STM32F401 Nucleo

STM32端

使用联合体的方式将float型数据拆分成字节数组，数据的传输协议如下，此外还可以在后面加上纠错码

帧头１	帧头２	Roll	Pitch	Yaw
0xAA	0xBB	4Bytes(低字节在前)	4Bytes(低字节在前)	4Bytes(低字节在前)

部分串口通信程序如下

// 联合体
typedef union
{
	float data;
	uint8_t data8[4];
} data_u;

void DataTrans(void)
{
	uint8_t _cnt = 0;
	data_u _temp; // 声明一个联合体实例，使用它将待发送数据转换为字节数组
	uint8_t data_to_send[100]; // 待发送的字节数组
	
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0xBB;
		
	// 将要发送的数据赋值给联合体的float成员
	// 相应的就能更改字节数组成员的值
	_temp.data = imu_data.yaw;
	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]; // 最高位
	
	_temp.data = imu_data.pit;
	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]; // 最高位
	
	_temp.data = imu_data.rol;
	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]; // 最高位
	
    // 串口发送
	HAL_UART_Transmit(&hlpuart1, data_to_send, _cnt, 0xFFFF); 
}

ROS端

ROS端主要实现串口数据的接收，并作为消息进行发布

首先ROS中需要安装串口开发的package，使用以下命令安装，API说明见于http://wjwwood.io/serial/doc/1.1.0/classserial_1_1_serial.html

sudo apt-get install ros-melodic-serial

然后安装Linux的串口调试助手cutecom，然后打开这个调试助手，查看STM32所连接的端口

sudo apt-get install cutecom
sudo cutecom

新建一个package，命名为serial_node，相关的ROS环境配置见 使用VScode搭建ROS开发环境

由于要发布接收到的imu数据，所以我们需要新建一个msg文件，命名为my_msg.msg，操作方式见ROS 自定义消息类型方法，文件内容如下

float32 roll
float32 pitch
float32 yaw

接下来在serial_node/src下新建一个serial_node.cpp文件，内容如下

#include <iostream>
#include <string>
#include <sstream>
using namespace std;

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "serial/serial.h"

#include "serial_node/my_msg.h"

#define sBUFFER_SIZE 1024
#define rBUFFER_SIZE 1024
unsigned char s_buffer[sBUFFER_SIZE];
unsigned char r_buffer[rBUFFER_SIZE];

serial::Serial ser;
typedef union
{
	float data;
	unsigned char data8[4];
} data_u;
data_u pitch;
data_u roll;
data_u yaw;

int main(int argc, char** argv)
{
	ros::init(argc, argv, "serial_node");
	ros::NodeHandle nh;

	// 打开串口
	try
	{
		ser.setPort("/dev/ttyACM0");　// 这个端口号就是之前用cutecom看到的端口名称
		ser.setBaudrate(115200);
		serial::Timeout to = serial::Timeout::simpleTimeout(1000);
		ser.setTimeout(to);
		ser.open();
	}
	catch(serial::IOException &e)
	{
		ROS_INFO_STREAM("Failed to open port ");
		return -1;
	}
	ROS_INFO_STREAM("Succeed to open port" );

	int cnt = 0;
	ros::Rate loop_rate(500);
	ros::Publisher imu_pub = nh.advertise<serial_node::my_msg>("imu", 1000);
	while(ros::ok())
	{
		serial_node::my_msg msg;
		if(ser.available())
		{
			// 读取串口数据
			size_t bytes_read = ser.read(r_buffer, ser.available());
			// 使用状态机处理读取到的数据，通信协议与STM32端一致
			int state = 0;
			unsigned char buffer[12] = {0};
			for(int i = 0; i < bytes_read && i < rBUFFER_SIZE; i++)
			{
				if(state == 0 && r_buffer[i] == 0xAA)
				{
					state++;
				}
				else if(state == 1 && r_buffer[i] == 0xBB)
				{
					state++;
				}
				else if(state >= 2 && state < 14)
				{
					buffer[state-2]=r_buffer[i];
					state ++;
					if(state == 14)
					{
						for(int k = 0; k < 4; k++)
						{
							roll.data8[k] = buffer[k];
							pitch.data8[k] = buffer[4 + k];
							yaw.data8[k] = buffer[8 + k];
						}						
						ROS_INFO("%f, %f, %f, %d", roll.data, pitch.data, yaw.data, cnt);
						state = 0;
					}
				}
				else state = 0;
			}
		}
		// 发布接收到的imu数据
		msg.roll = roll.data;
		msg.pitch = pitch.data;
		msg.yaw = yaw.data;
		imu_pub.publish(msg);
		// ros::spinOnce();
		loop_rate.sleep();
		cnt++;
	}
	// 关闭串口
	ser.close();
	return 0;
}

测试

运行编译生成的节点

source ./devel/setup.bash
rosrun serial_node serial_node

运行时如果提示串口打开失败，有两种原因，一是串口号不对，使用dmesg | grep ttyS*列出检测到的串口号，逐个测试；二是没有操作权限，使用sudo chmod 666 /dev/ttyACM0即可解决，也可以使用sudo usermod -aG dialout 用户名来获得永久权限，用户名可使用whoami查看

结果如下：

然后在该目录下新开一个终端，执行

rostopic list

可以看到/imu话题

再依次执行下面的命令，就可以看到该节点发布的imu信息

source ./devel/setup.bash
rostopic echo /imu

第一句命令必须执行，不然会报错：

ERROR: Cannot load message class for [serial_node/my_msg]. Are your messages built?

参考

ROS进阶——串口通讯