本文介绍如何将stm32控制板作为一个单独的ROS节点接入整个机器人ROS系统。
在一个完整的机器人硬件系统中，由于众多传感器接口和实时性的需求，不可避免的需要加入嵌入式控制器，现在的机器人大多使用了分布式ROS系统，这套系统主要基于linux运行，而以stm32为例的大多数嵌入式控制器不支持linux。于是，当工控机想要与stm32进行数据交换时，只能脱离ROS体系采用自定义通讯协议。
那么，能不能在STM32中使用ROS架构呢，答案是肯定的，通过rosserial模块，我们可以把一部分ROS的接口定义移植到STM32中编译，虽然不能实现完整的ROS移植，但是可以把整个STM32控制器作为一个单独的ROS节点加入到ROS系统中，实现消息定义与收发、服务创建等功能。本文接下来将介绍该实现方法。
在ros工作空间catkin_ws中，我们可以创建自己的包，并在msg和srv目录中加入自己定义的消息和服务，这些消息和服务是STM32与工控机通讯时需要用到的，这样接下来生成roslibs时，就会自动包含这些自定义消息和服务。
git clone https://github.com/yoneken/rosserial_stm32.git
catkin_make编译新加入的包，之后记得输入source ~/.bashrc使得编译内容生效cd ~/catkin_ws
catkin_make
source ~/.bashrc
cd ~
mkdir -p stm32_roslib/Inc
cd stm32_roslib
rosrun rosserial_stm32 make_libraries.py ./
这将在stm32_roslib/Inc目录下生成一堆文件夹，包含ROS自带的消息和服务，以及catkin_ws下所有包内自定义的消息和服务，均以C++头文件的方式定义，此外，还生成了ros.h、STM32Hardware.h、duration.cpp、time.cpp四个文件。这就是我们需要移植到stm32的项目中混合编译的内容。
注意，这里有两点需要说明：
SyntaxError: Missing parentheses in call to 'print'. Did you mean print(__usage__)?，这是由于脚本在python3执行，但是python3的语法下print需要加（），我们需要修改rosserial_stm32包下面的make_libraries.py文件cd ~/catkin_ws/src/rosserial_stm32/src/rosserial_stm32
vim make_libraries.py
第74行和第81行有两个print，把后面的内容加上括号即可。
# need correct inputs
if (len(sys.argv) < 2):
print (__usage__)
exit()
# get output path
path = sys.argv[1]
if path[-1] == "/":
path = path[0:-1]
print ("\nExporting to %s" % path)
将上一步生成的Inc文件夹整个拷贝出来，进入windows系统，将Inc文件夹复制到stm32的Mdk项目下面，例如，我们放置在stm32_ros_lib/Inc下。接下来，打开mdk，进入stm32的项目，由于我们生成的Inc下所有的头文件都是C++编写的，所以我们要开启Mdk的c++编译。
一定要注意，这里网上很多文章说应该添加–cpp修改mdk的编译配置，据我测试这么做限制很大，因为这样会让mdk对项目所有文件均采用c++编译器编译，如果你的项目添加了第三方模组，如freertos、虚拟串口VCP等，这些c文件都会编译报错。
兼容性最好的方法应该是采用mdk的c/c++混合编译模式，因为默认情况下，mdk会通过文件扩展名来选择对应的编译器，.c文件会用c编译器，.cpp文件会采用c++编译器，所以我们应该利用cpp文件，将所有与ROS有关的内容都写到单独的cpp文件(如rosserial_lib.cpp)里，然后在头文件rosserial_lib.h中将cpp的函数声明用extern C包装一下，其它c文件中即可使用#include rosserial_lib.h来包含cpp文件的内容了，至于cpp中调用其它c文件内容，本来就是向下兼容的，所以无需烦恼。
具体来说，我们综合freertos的任务架构以及ROS节点的while循环写法，将ROS移植相关内容都放在同一个freertos任务中。
首先是rosserial_lib.h文件：
#ifndef ROSSERIAL__H_
#define ROSSERIAL__H_
#ifdef __cplusplus
extern "C" {
#endif
void RosserialSetup(void);
void RosserialLoop(void);
#ifdef __cplusplus
}
#endif
#endif
这里我们定义了两个函数，分别是节点初始化函数RosserialSetup(void)和节点循环函数RosserialLoop(void)
其次是freertos的任务，我们创建了一个ControlTask的任务，在进入无限循环前先执行节点初始化函数RosserialSetup(void)，然后再无限循环中执行RosserialLoop()，注意到这里是通过#include "rosserial_lib.h"，来调用C++函数RosserialSetup(void)和RosserialLoop(void)，由于在头文件中加入了extern "C"，这种调用编译器是不会报错的。
#include "rosserial_lib.h"
void ControlTask(void *argument) {
osDelay(500);
RosserialSetup();
osDelay(500);
for(;;) {
RosserialLoop();
}
}
接下来，我们介绍和分析rosserial_lib.cpp里面两个函数的具体实现。如下代码所示，这是一段stm32中ROS节点的具体实现示例，ros.h是ros功能的核心头文件，std_msgs/String.h是ros自带的标准消息头文件，ts_vision_ctrl/final_data.h是用户自定义消息头文件，这些都包含在之前生成的Inc文件夹内。
基本写法和标准ROS节点类似，只是语法有些微区别，同样是定义nodehandle类、消息收发类型、接收消息回调函数，在 RosserialSetup(void)函数中进行节点初始化，注册需要收发的消息和服务，在RosserialLoop(void)函数中进行节点消息的更新发送，编写相关控制代码，最后执行nh.spinOnce()来响应各种回调函数（这里不能用spin()，否则任务循环会被阻塞）。注意由于RosserialLoop()函数本身在freertos的任务无限循环中执行，所以RosserialLoop()函数内部不再需要while循环。
#include "rosserial_lib.h"
#include "cmsis_os.h"
#include <ros.h>
#include <std_msgs/String.h>
#include <ts_vision_ctrl/final_data.h>
void command_callback(const std_msgs::String &rxbuff);
ros::NodeHandle nh;
std_msgs::String stm32_to_pc_word;
ts_vision_ctrl::final_data my_data;
ros::Subscriber<std_msgs::String> cmd_sub("pc_to_stm32", command_callback);
ros::Publisher publisher("stm32_to_pc", &stm32_to_pc_word);
ros::Publisher my_pub("stm32_my_data", &my_data);
void command_callback(const std_msgs::String &rxbuff) {
stm32_to_pc_word = rxbuff;
publisher.publish(&stm32_to_pc_word);
}
void RosserialSetup(void) {
nh.initNode();
nh.advertise(publisher);
nh.advertise(my_pub);
nh.subscribe(cmd_sub);
my_data.heading = 3.5;
my_data.x = 1.23;
my_data.y = 2.56;
my_data.header.frame_id = "position";
my_data.header.seq = 0;
}
void RosserialLoop(void) {
static int i = 0;
my_data.header.seq = i;
i++;
my_pub.publish(&my_data);
nh.spinOnce();
osDelay(20);
}
在Mdk宏定义中加入,__USE_C99_MATH，这样可以避免roslib编译错误
在include paths中加入…/stm32_ros_lib/Inc目录
在左边project项目名称右键，选择Manage Project Items，在Groups新建一个组别，如RosLibs，添加早先用make_libraries.py生成Inc文件夹下的四个文件：ros.h、STM32Hardware.h、duration.cpp、time.cpp。其实两个头文件加不加都可以，我们只需要修改STM32Hardware.h的内容完成移植。
打开STM32Hardware.h文件，STM32Hardware.h中的类STM32Hardware会在node_handle.h中调用
需要在这个类中实现read()和write()的公共方法。这里就和stm32的硬件接口相关了，由于我们用的是rosserial，所以这里将会调用stm32的串口驱动与工控机进行通讯。
class STM32Hardware {
protected:
public:
STM32Hardware() {}
void init() {}
int read() {
if (Uart_Available()) {
return Uart_Read();
} else {
return -1;
}
}
void flush(void) {}
void write(uint8_t *data, int length) { Uart_Write(data, length); }
unsigned long time() { return HAL_GetTick(); }
protected:
};
我们需要实现三个函数，分别是int read()、 void write(uint8_t *data, int length)以及unsigned long time()。其中read函数返回值是一个整型，其实是每次读取并返回一个字节，write函数每次则是发送长度为length的字节数组。为此，最好为串口配置一个接收环形缓冲区，每次串口接收到数据，就写入环形缓冲区。read()函数首先调用Uart_Available()函数，判断环形缓冲区是否有数据，如果有，就通过Uart_Read()函数从环形缓冲区读取一个字节并返回这个字节。
下面的代码是最简单的环形缓冲区实现，仅供参考，实际应用时，应考虑多任务对全局变量操作时可能产生的竞争冒险现象，添加信号量等同步机制。至于串口数据接收写入环形缓冲区，是在串口中断里面实现的。
int Uart_Available(void) {
return ((uint32_t)(UART_RX_DATA_SIZE + uart_rxBufPtrIn - uart_rxBufPtrOut)) %
UART_RX_DATA_SIZE;
}
//从接收缓冲区中读取
int Uart_Read(void) {
// if the head isn't ahead of the tail, we don't have any characters
if (uart_rxBufPtrIn == uart_rxBufPtrOut) {
return -1;
} else {
unsigned char ch = uart_rxBuffer[uart_rxBufPtrOut];
uart_rxBufPtrOut = (uint16_t)(uart_rxBufPtrOut + 1) % UART_RX_DATA_SIZE;
return ch;
}
}
//通过usb_vcp向外发送
void Uart_Write(uint8_t *Buf, uint16_t Len) {
while (UART2_Transmit(Buf, Len) != HAL_OK) {
osDelay(1);
}
}
最后是time()函数，需要提供一个毫秒计数的系统时间，一般我们的freertos系统节拍都是1ms，因此直接返回HAL_GetTick()即可 ，这个函数返回的是32位的毫秒计数，超时时间很长，不用担心溢出问题。
至此，stm32全部的ROS移植就完成了，将stm32项目编译后下载，然后将其对应的串口接入工控机，在工控机中执行
rosrun rosserial_python serial_node.py _port:=/dev/ttyUSB0 _baud:=115200
注意上述ttyUSB0 要改成实际识别的串口名，波特率也要和stm32中设置的波特率匹配。如果一切正常，可以看到输出信息
[INFO] [1661936312.766461]: ROS Serial Python Node
[INFO] [1661936312.784234]: Connecting to /dev/ttyUSB0 at 115200 baud
[INFO] [1661936314.895818]: Requesting topics...
[INFO] [1661936314.915546]: Note: publish buffer size is 1024 bytes
[INFO] [1661936315.080871]: Setup publisher on odom [nav_msgs/Odometry]
[INFO] [1661936315.179145]: Setup publisher on imu [sensor_msgs/Imu]
[INFO] [1661936316.061922]: Setup publisher on /tf [tf/tfMessage]
[INFO] [1661936316.073155]: Note: subscribe buffer size is 1024 bytes
[INFO] [1661936316.077387]: Setup subscriber on cmd_vel [geometry_msgs/Twist]
...
...
此时，整个stm32嵌入式系统已经作为一个ROS节点运行在ROS系统内了，之后新打开一个终端，执行rostopic list，可以看到相关的收发消息
gm@controlboard:~$ rostopic list
/cmd_vel
/diagnostics
/imu
/odom
/rosout
/rosout_agg
/tf
以上内容是基于stm32的标准串口实现，但是实际使用时，由于ROS数据传输量较大，标准串口的带宽不够用，因此大部分情况下，我们都使用STM32的usb虚拟串口VCP来取代标准串口，VCP带宽就完全能够满足ROS的常用需求了，下一篇文章，我们将详细介绍如何结合虚拟串口来实现Rosserial功能。