天天看点
返回

micro_ros串口通讯进阶-多个话题发布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。
micro_ros串口通讯进阶-多个话题发布micro_ros通讯进阶——多个话题发布

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串口的方式连接。
micro_ros串口通讯进阶-多个话题发布micro_ros通讯进阶——多个话题发布
  • 新建终端，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
    micro_ros串口通讯进阶-多个话题发布micro_ros通讯进阶——多个话题发布
    • 可以看到多我们自定义的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的输出：
    micro_ros串口通讯进阶-多个话题发布micro_ros通讯进阶——多个话题发布

3. 其他

  • publisher示例代码的话题发布频率默认为1Hz，可以进行调整，10Hz没问题。但是想要100Hz往上就不行了
  • 需要更高的话题发布频率需要使用rclc_publisher_init_best_effort代替rclc_publisher_init_default
micro_ros笔记 ubuntu c++ MCU
上一篇: Micro python 和circuit python 的区别
下一篇: go-micro学习日记(四)--熔断

继续阅读