[嵌入式]在STM32中实现ROS节点——Rosserial的用法

内容介绍

本文介绍如何将stm32控制板作为一个单独的ROS节点接入整个机器人ROS系统。

前言

在一个完整的机器人硬件系统中，由于众多传感器接口和实时性的需求，不可避免的需要加入嵌入式控制器，现在的机器人大多使用了分布式ROS系统，这套系统主要基于linux运行，而以stm32为例的大多数嵌入式控制器不支持linux。于是，当工控机想要与stm32进行数据交换时，只能脱离ROS体系采用自定义通讯协议。
那么，能不能在STM32中使用ROS架构呢，答案是肯定的，通过rosserial模块，我们可以把一部分ROS的接口定义移植到STM32中编译，虽然不能实现完整的ROS移植，但是可以把整个STM32控制器作为一个单独的ROS节点加入到ROS系统中，实现消息定义与收发、服务创建等功能。本文接下来将介绍该实现方法。

生成要移植到stm32的自定义消息和服务

在ros工作空间catkin_ws中，我们可以创建自己的包，并在msg和srv目录中加入自己定义的消息和服务，这些消息和服务是STM32与工控机通讯时需要用到的，这样接下来生成roslibs时，就会自动包含这些自定义消息和服务。

生成针对stm32的移植库包roslibs

1. 下载rosserial_stm32包：进入自己的ROS工作空间src目录，如~/catkin_ws/src，输入

git clone https://github.com/yoneken/rosserial_stm32.git

2. 编译rosserial_stm32：在~/catkin_ws/目录下输入catkin_make编译新加入的包，之后记得输入source ~/.bashrc使得编译内容生效

cd ~/catkin_ws
catkin_make
source ~/.bashrc

3. 生成待移植的头文件：
   在工作目录下新建一个文件夹，例如stm32_roslib，然后进入stm32_roslib新建一个Inc文件夹，首字母I要大写。在stm32_roslib目录（不是Inc目录）下执行命令：

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的项目中混合编译的内容。

注意，这里有两点需要说明：

• 根据ros版本的不同，在执行make_libraries.py时可能报错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下包含的目录很多，但是不一定所有的消息和服务我们在stm32都要用到，可以视情况删除一部分，减少stm32项目体量。

在Mdk中实现C和C++代码混合编译

将上一步生成的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配置

在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的内容完成移植。

修改stm32 ROS通讯接口驱动

打开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功能。