ROS与STM32通信

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

定义一个联合体（数据存放在同一地址）

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

STM32端主要代码

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

//typedef union
//{
//    float yaw;
//    float pit;
//    float rol;
//}imu_data;
struct imu_data {
    float yaw;
    float pit;
    float rol;
};

void DataTrans(struct imu_data imu_data)
{
	uint8_t _cnt = 0;
	data_u _temp; // 声明一个联合体实例，使用它将待发送数据转换为字节数组
    uint8_t data_to_send[14]; // 待发送的字节数组
	
	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;
    _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(&huart2, data_to_send, _cnt, 0xFFFF); 
    HAL_Delay(500);
}

struct imu_data imu_data;
  /* USER CODE END 2 */
      imu_data.pit=1;
      imu_data.rol=2;
      imu_data.yaw=3;
  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */
      
    DataTrans(imu_data);
    imu_data.pit++;
      imu_data.rol++;
      imu_data.yaw++;
    /* USER CODE BEGIN 3 */
  }

ROS端主要为实现串口数据接收，并发布

新建窗口创建工作空间，编译，将工作空间加到环境变量

mkdir - p test_serial_ws/src
cd test_serial_ws
catkin_make

......

source devel/setup.bash


cd src
catkin_create_pkg serial_node roscpp serial

新建窗口创建工作空间，编译，将工作空间加到环境变量

cd serial_node
cd  src 
touch serial_node.cpp

由于要发布接收到的imu数据，所以我们需要新建一个msg文件，命名为my_msg.msg

float32 roll
float32 pitch
float32 yaw

文件下

#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;
}

修改CMakeList文件

add_executable(serial_node src/serial_demo.cpp)

add（这个还有一个我忘记了）
target_link_libraries(serial_node  ${catkin_LIBRARIES})

新建窗口编译

cd test_serial_ws
catkin_make

烧录进stm32

sudo chmod 666 /dev/ttyUSB0
rosrun rosrun serial_node serial_node

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

完成，关于msg的配置可以看bilibili的赵虚左ROS的56集有教程