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

Published: August 21, 2022 3 minute read, with 775 words. Post views:

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 <micro_ros_arduino.h>

#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

#include <std_msgs/msg/int32.h>                 //msg1类型对应的头文件,int32
#include <sensor_msgs/msg/imu.h>                //msg2类型对应的头文件,imu
#include <geometry_msgs/msg/twist.h>            //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 <std_msgs/msg/int32.h>                 //msg1类型对应的头文件,int32
#include <sensor_msgs/msg/imu.h>                //msg2类型对应的头文件,imu
#include <geometry_msgs/msg/twist.h>            //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串口的方式连接。

微信图片_20220717085646

新建终端，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

Tags: esp32, micro_ros, ros2

Categories: 机器人技术

Share on: Twitter Facebook LinkedIn

Tianci/张天赐

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

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

0. 写在前面

0.1 MCU的定位

0.2 micro_ros的定位

0.3 特别注意

1. 多个话题发布

1.1 完整代码

1.2 代码解析

2. 上位机配置

3. 其他

Comments

Recent posts

ESP32 LVGL配置教程

micro_ros下位机自动连接micro_ros_agent

micro_ros自定义话题发布频率

micro_ros笔记——best_effort订阅者程序编写