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

    micro_ros串口通讯进阶-多个话题发布_第1张图片

    • 可以看到多我们自定义的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串口通讯进阶-多个话题发布_第2张图片

3. 其他

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

你可能感兴趣的:(micro_ros笔记,ubuntu,c++,mcu)