micro_ros串口通讯进阶-多个话题发布

micro_ros通讯进阶——多个话题发布

参考：https://github.com/micro-ROS/micro_ros_arduino

[micro_ros配置]https://zhuanlan.zhihu.com/p/540924221

[发布twist]https://zhuanlan.zhihu.com/p/542563252

使用硬件：

• M5 stack Atom Lite(esp32 core)+旭日x3派(ros2 foxy)

软件环境:

• micro_ros_arduino(foxy分支)

• ubuntu 20.04

• TogetherROS(兼容ros2 foxy)

---

0. 写在前面

0.1 MCU的定位

• MCU适用于任务简单，实时性要求高的场景，但其算力往往不高；

• 上位机运行ROS or Linux，算力强，但实时性不如MCU。

• 因此复杂任务下，常见的情形是：

  • MCU作为下位机负责实时数据采集和运动控制等简单任务
    • 如传感器的数据采集、电机的PID控制等
  • 上位机运行ROS2进行复杂任务
    • 如SLAM建图，神经网络推理等

0.2 micro_ros的定位

• micro_ros是上位机ROS与下位机MCU的一个连接桥梁，本质上是一个与ros无缝兼容的通讯机制。

• 其最大的特征是将MCU视作ROS中的一个Node，进而实现话题的发布，订阅，服务或action等特性。

0.3 特别注意

• 并非所有的MCU都支持多个publisher，这主要取决于MCU的RAM，可以参考：https://github.com/micro-ROS/micro_ros_arduino/tree/humble/extras/library_generation。

• 这里面.meta文件定义了publisher的最大数量等等；

• 例如针对RAM非常低的MCU，限制最大publisher数量为2，最大Node为1等等；

• 后续可按需进行rebuild。

1. 多个话题发布

• 创建1个Node，创建3个Publishers
• 每个publisher的发布频率可自定义
• 分别发布Int32，IMU，twist的数据类型

1.1 完整代码

#include 

#include 
#include 
#include 
#include 
#include 

#include                  //msg1类型对应的头文件,int32
#include                 //msg2类型对应的头文件,imu
#include             //msg3类型对应的头文件,twist

rcl_publisher_t publisher1;	                    //第1个publisher
rcl_publisher_t publisher2;                     //第2个publisher
rcl_publisher_t publisher3;                     //第3个publisher

std_msgs__msg__Int32 msg1;                      //msg1：int32类型
sensor_msgs__msg__Imu msg2;                     //msg2：imu类型
geometry_msgs__msg__Twist msg3;                 //msg3：twist类型

rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;

// define 3 timer
rcl_timer_t timer1;
rcl_timer_t timer2;
rcl_timer_t timer3;

#define LED_PIN 27
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

void error_loop() {
  while (1) {
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

//timer1 callback
void timer1_callback(rcl_timer_t * timer, int64_t last_call_time)
{
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    RCSOFTCHECK(rcl_publish(&publisher1, &msg1, NULL));
    msg1.data++;
  }
}

//timer2 callback
void timer2_callback(rcl_timer_t * timer, int64_t last_call_time)
{
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
      /*在这里添加IMU的采集代码*/
      /*我用数据自加or自减的方式模拟*/
      /*ros2 interface show sensor_msgs/msg/Imu 查看IMU数据类型的详情*/
    msg2.linear_acceleration.x += 0.1;
    msg2.linear_acceleration.y += 0.1;
    msg2.linear_acceleration.z = 9.81f;
    msg2.angular_velocity.x += 0.01;
    msg2.angular_velocity.y += 0.01;
    msg2.angular_velocity.z += 0.01;
    msg2.header.stamp.sec += 1;
    msg2.header.stamp.nanosec += 1000;
    msg2.orientation_covariance[0] = -1;
    RCSOFTCHECK(rcl_publish(&publisher2, &msg2, NULL));
  }
}

//timer3 callback
void timer3_callback(rcl_timer_t * timer, int64_t last_call_time)
{
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    RCSOFTCHECK(rcl_publish(&publisher3, &msg3, NULL));
    static int cnt = 0;
    msg3.linear.x = 0.2;                            //const linear.x
    msg3.angular.z = 1.0 - 0.001 * cnt;             //variable angular.z
    cnt++;
  }
}

void setup() {
  set_microros_transports();
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);
  delay(2000);
  allocator = rcl_get_default_allocator();

  //create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // create node
  RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_node", "", &support));

  // create publisher1
  RCCHECK(rclc_publisher_init_default(
            &publisher1,
            &node,
            ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
            "micro_ros_arduino_node_publisher1"));

  // create publisher2
  RCCHECK(rclc_publisher_init_default(
            &publisher2,
            &node,
            ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu),
            "micro_ros_arduino_node_publisher2"));

  // create publisher3
  RCCHECK(rclc_publisher_init_default(
            &publisher3,
            &node,
            ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
            "turtle1/cmd_vel"));

  // create timer1,
  const unsigned int timer1_timeout = 100;  //发布频率10Hz
  RCCHECK(rclc_timer_init_default(
            &timer1,
            &support,
            RCL_MS_TO_NS(timer1_timeout),
            timer1_callback));

  // create timer2,
  const unsigned int timer2_timeout = 1000; //发布频率1Hz
  RCCHECK(rclc_timer_init_default(
            &timer2,
            &support,
            RCL_MS_TO_NS(timer2_timeout),
            timer2_callback));

  // create timer3,
  const unsigned int timer3_timeout = 500;  //发布频率2Hz
  RCCHECK(rclc_timer_init_default(
            &timer3,
            &support,
            RCL_MS_TO_NS(timer3_timeout),
            timer3_callback));

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 3, &allocator)); 
   /*3个timer，故第三个参数为3*/
    
  RCCHECK(rclc_executor_add_timer(&executor, &timer1));   //添加timer1
  RCCHECK(rclc_executor_add_timer(&executor, &timer2));   //添加timer2
  RCCHECK(rclc_executor_add_timer(&executor, &timer3));   //添加timer3

  //   msg1初始化
  msg1.data = 0;

  //   msg2初始化
  msg2.header.frame_id.data = "IMUXX";
  msg2.header.frame_id.size = 5;

  //   msg3初始化
  msg3.linear.x = 0;
  msg3.linear.y = 0;
  msg3.linear.z = 0;
  msg3.angular.x = 0;
  msg3.angular.y = 0;
  msg3.angular.z = 0;
}

void loop() {
//  delay(100);
  RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}

1.2 代码解析

• 头文件、数据类型等的定义

  #include                  //msg1类型对应的头文件,int32
  #include                 //msg2类型对应的头文件,imu
  #include             //msg3类型对应的头文件,twist
  
  rcl_publisher_t publisher1;	                    //第1个publisher
  rcl_publisher_t publisher2;                     //第2个publisher
  rcl_publisher_t publisher3;                     //第3个publisher
  
  std_msgs__msg__Int32 msg1;                      //msg1：int32类型
  sensor_msgs__msg__Imu msg2;                     //msg2：imu类型
  geometry_msgs__msg__Twist msg3;                 //msg3：twist类型
  
  // define 3 timer
  rcl_timer_t timer1;
  rcl_timer_t timer2;
  rcl_timer_t timer3;
  

• 依次创建3个publishers

  • 注意每个publisher要发布的数据类型
  • 为每个publisher创建一个topic name
    • micro_ros_arduino_node_publisher1
    • micro_ros_arduino_node_publisher2
    • turtle1/cmd_vel

  // create publisher1
    RCCHECK(rclc_publisher_init_default(
              &publisher1,
              &node,
              ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
              "micro_ros_arduino_node_publisher1"));
  
    // create publisher2
    RCCHECK(rclc_publisher_init_default(
              &publisher2,
              &node,
              ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu),
              "micro_ros_arduino_node_publisher2"));
  
    // create publisher3
    RCCHECK(rclc_publisher_init_default(
              &publisher3,
              &node,
              ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
              "turtle1/cmd_vel"));
  

• 创建3个定时器

  • 分别定义定时器时间，进而调整发布频率
  • 注意：发布频率尽量调低
  • 要求高频率发送，需要使用rclc_publisher_best_effort代替rclc_publisher_init_default

  // create timer1,
    const unsigned int timer1_timeout = 100;  //发布频率10Hz
    RCCHECK(rclc_timer_init_default(
              &timer1,
              &support,
              RCL_MS_TO_NS(timer1_timeout),
              timer1_callback));
  
    // create timer2,
    const unsigned int timer2_timeout = 1000; //发布频率1Hz
    RCCHECK(rclc_timer_init_default(
              &timer2,
              &support,
              RCL_MS_TO_NS(timer2_timeout),
              timer2_callback));
  
    // create timer3,
    const unsigned int timer3_timeout = 500;  //发布频率2Hz
    RCCHECK(rclc_timer_init_default(
              &timer3,
              &support,
              RCL_MS_TO_NS(timer3_timeout),
              timer3_callback));
  

• 创建3个定时器回调任务

  • 分别编写3个回调任务处理代码
  • msg1为int32数据类型，每次调用进行自加
  • msg2为IMU数据类型，处理同上
  • msg3为twist数据类型，处理同上

  //timer1 callback
  void timer1_callback(rcl_timer_t * timer, int64_t last_call_time)
  {
    RCLC_UNUSED(last_call_time);
    if (timer != NULL) {
      RCSOFTCHECK(rcl_publish(&publisher1, &msg1, NULL));
      msg1.data++;
    }
  }
  
  //timer2 callback
  void timer2_callback(rcl_timer_t * timer, int64_t last_call_time)
  {
    RCLC_UNUSED(last_call_time);
    if (timer != NULL) {
        /*在这里添加IMU的采集代码*/
        /*我用数据自加or自减的方式模拟*/
        /*ros2 interface show sensor_msgs/msg/Imu 查看IMU数据类型的详情*/
      msg2.linear_acceleration.x += 0.1;
      msg2.linear_acceleration.y += 0.1;
      msg2.linear_acceleration.z = 9.81f;
      msg2.angular_velocity.x += 0.01;
      msg2.angular_velocity.y += 0.01;
      msg2.angular_velocity.z += 0.01;
      msg2.header.stamp.sec += 1;
      msg2.header.stamp.nanosec += 1000;
      msg2.orientation_covariance[0] = -1;
      RCSOFTCHECK(rcl_publish(&publisher2, &msg2, NULL));
    }
  }
  
  //timer3 callback
  void timer3_callback(rcl_timer_t * timer, int64_t last_call_time)
  {
    RCLC_UNUSED(last_call_time);
    if (timer != NULL) {
      RCSOFTCHECK(rcl_publish(&publisher3, &msg3, NULL));
      static int cnt = 0;
      msg3.linear.x = 0.2;                            //const linear.x
      msg3.angular.z = 1.0 - 0.001 * cnt;             //variable angular.z
      cnt++;
    }
  }
  

• 3个msg数据初始化

    //   msg1初始化
    msg1.data = 0;
  
    //   msg2初始化
    msg2.header.frame_id.data = "IMUXX";
    msg2.header.frame_id.size = 5;
  
    //   msg3初始化
    msg3.linear.x = 0;
    msg3.linear.y = 0;
    msg3.linear.z = 0;
    msg3.angular.x = 0;
    msg3.angular.y = 0;
    msg3.angular.z = 0;
  

• 添加timer执行，并修改执行参数

  • rclc_executor_init参数调整可参考链接
  • 依次添加3个timer

    // create executor
    RCCHECK(rclc_executor_init(&executor, &support.context, 3, &allocator)); 
     /*3个timer，故第三个参数为3*/
      
    RCCHECK(rclc_executor_add_timer(&executor, &timer1));   //添加timer1
    RCCHECK(rclc_executor_add_timer(&executor, &timer2));   //添加timer2
    RCCHECK(rclc_executor_add_timer(&executor, &timer3));   //添加timer3
  

• 若发布频率大于10Hz，注释loop中delay

  void loop() {
  //  delay(100);
    RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
  }
  

• 最后，编译上传代码

2. 上位机配置

• 将下位机代码烧录后，将下位机通过串口连接上位机，这里我使用usb串口的方式连接。

• 新建终端，source一下ros2，再source一下micro_ros。

source /opt/tros/setup.bash   #或者 source /opt/ros/foxy/setup.bash
cd /microros_ws/              #进入micro_ros的工作空间
source install/setup.bash     #source一下，也可以将这些命令添加到 /.bashrc

• 首先提升串口读写权限(确保自己的串口是ttyUSB0，因硬件而异)

  sudo chmod -R 777 /dev/ttyUSB0
  

• 开启micro_agent

  ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyUSB0
  

• 按下下位机的复位键(特别注意，需要按下复位键)

• 重新开启1个终端，确保都进行了source，查看 topic list

  

  • 可以看到多我们自定义的3个topic

• 重新开启多个终端，确保都进行了source，依次查看每个topic的输出：

  ros2 topic echo /micro_ros_arduino_node_publisher1
  ros2 topic echo /micro_ros_arduino_node_publisher2
  ros2 topic echo /turtle1/cmd_vel 
  

• 可以看到每个Topic的输出：

  

3. 其他

• publisher示例代码的话题发布频率默认为1Hz，可以进行调整，10Hz没问题。但是想要100Hz往上就不行了
• 需要更高的话题发布频率需要使用rclc_publisher_init_best_effort代替rclc_publisher_init_default