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序列槽通訊進階-多個話題釋出micro_ros通訊進階——多個話題釋出

micro_ros通訊進階——多個話題釋出

0. 寫在前面

0.1 MCU的定位

0.2 micro_ros的定位

0.3 特别注意

1. 多個話題釋出

1.1 完整代碼

1.2 代碼解析

2. 上位機配置

3. 其他

繼續閱讀

2021-08-13c++——類之操作符重載

swmm與lisflood-fp源碼如何一起編譯 CMake指令

Windows下VS開發環境環境安裝工程項目設定關于Debug和Release的提示

一文看懂字元串的加減乘除

伺服器配置——Apache

C++ 第十五周報告1--《冒泡法排序》

ubuntu 16.04 源碼安裝httpd和php

Ubuntu16.04安裝Apache+MySQL+PHP1. 安裝Apache2. 安裝MySQL3. 安裝PHP4. 安裝phpMyAdmin

Ubuntu14.04 LTS下安裝mongodb

C++實作簡單順序表

禁止ubuntu系統彈出報錯界面

C經典書籍筆記——C陷阱與缺陷②(文法陷阱之優先級)一、錯誤案列二、優先級規律

線性表之順序表的實作

C++判斷素數、求最大公約數代碼判斷一個數是否為素數求兩個數的最大公約數

SequoiaDB巨杉資料庫C++驅動概述

JBoss,Geronimo和Glassfish初窺