micro-Ros2

ロボットの制御をやっているとリアルタイム性が良いArduinoマイコンなどで制御の部分を行って，Linux PC(ホストPC)などで計算負荷の大きい計画の部分を行いたい場合が出てくると思います．このときにmicro-Ros2を使うとマイコン側でRos2ノードを立ててホストPCで立ち上げられたRos2ノードとtopicによる通信を行うことができます．
今回はPlatformIOを用いてマイコン側のプログラムを書いて，micro-Ros2を利用した通信を行います．また，Custom messageの使い方についても試しみようと思います．

環境

Ubuntu 22.04
Ros2 humble
VsCodeのPlatformIO
Teensy 3.6

準備

micro-Ros2のインストール

Ros2 humbleがインストールされているPCで以下を実行していただければインストールが終了すると思います．

# Source the ROS 2 installation
source /opt/ros/$ROS_DISTRO/setup.bash

# Create a workspace and download the micro-ROS tools
mkdir microros_ws
cd microros_ws
git clone -b $ROS_DISTRO https://github.com/micro-ROS/micro_ros_setup.git src/micro_ros_setup

# Update dependencies using rosdep
sudo apt update && rosdep update --rosdistro $ROS_DISTRO
rosdep install --from-paths src --ignore-src -y --rosdistro $ROS_DISTRO

# Install pip
sudo apt-get install python3-pip

# Build micro-ROS tools and source them
colcon build
source install/local_setup.bash

# Create firmware step
ros2 run micro_ros_setup create_firmware_ws.sh host

# Build step
ros2 run micro_ros_setup build_firmware.sh
source install/local_setup.bash

# Download micro-ROS-Agent packages
ros2 run micro_ros_setup create_agent_ws.sh

# Build step
ros2 run micro_ros_setup build_agent.sh
source install/local_setup.bash

# Use RMW Micro XRCE-DDS implementation
export RMW_IMPLEMENTATION=rmw_microxrcedds

マイコン側のプログラム作成

VsCodeのPlatformIOを使ってプログラムを作成します．ここではPlatformIOの使い方については触れないの他の記事を参考にしてください．

プログラムはmicro-ros-platformioにあるサンプルをとりあえず使います．

PlatformIOで書くマイコン側のプログラム

main.cpp

#include <Arduino.h>
#include <micro_ros_platformio.h>

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

#include <std_msgs/msg/int32.h>

#if !defined(MICRO_ROS_TRANSPORT_ARDUINO_SERIAL)
#error This example is only avaliable for Arduino framework with serial transport.
#endif

rcl_publisher_t publisher;
std_msgs__msg__Int32 msg;

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

#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)){}}

// Error handle loop
void error_loop() {
  while(1) {
    delay(100);
  }
}

void timer_callback(rcl_timer_t * timer, int64_t last_call_time) {
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    msg.data++;
  }
}

void setup() {
  // Configure serial transport
  Serial.begin(115200);
  set_microros_serial_transports(Serial);
  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_platformio_node", "", &support));

  // create publisher
  RCCHECK(rclc_publisher_init_default(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "micro_ros_platformio_node_publisher"));

  // create timer,
  const unsigned int timer_timeout = 1000;
  RCCHECK(rclc_timer_init_default(
    &timer,
    &support,
    RCL_MS_TO_NS(timer_timeout),
    timer_callback));

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_timer(&executor, &timer));

  msg.data = 0;
}

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

次にmicro-Ros2を利用するためにplatformio.iniを書き換えます．重要なのは最後のlib_depsの部分でここでmicro-Ros2のライブラリを取ってくるようにしています．なおデフォルトでマイコンとの通信方法がserial, Ros2のバージョンがhumbleに設定されています．他の通信方法やRos2バージョンを利用したい場合は，ここでサポートされている通信方法やRos2バージョンを確認してから変更してください．

[env:teensy36]
platform = teensy
board = teensy36
framework = arduino
lib_deps =
    https://github.com/micro-ROS/micro_ros_platformio

実行

PlatformIOで書いたプログラムをマイコン (今回はteensy3.6)に書き込みます．そして，ホストPC側で以下を実行してmicro-ros-agentを立ち上げます．これは，マイコンとの通信を管理してくれるものです．

cd <micro-Ros2をインストールした場所>/micorros_ws
source install/setup.bash
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0

ここまでできたらros2 node listを使って/micro_ros_platformio_nodeが立ち上がっていることを確認してください．出てこない場合は，もう一度マイコンにプログラムを書き込んでみてください．どうやら，マイコン側でノードを作成したときに一度だけノードやトピックの情報をホストPC側に送信しているようなので，予めmicro-ros-agentが立ち上がっていないとだめなようです．

ノードが立ち上がっていることを確認したらトピックの中身を確認してみます．すると以下のように1ずつ値が大きくなるようにデータが送られているのが確認できると思います．

ros2 topic echo /micro_ros_platformio_node_publisher 
data: 1
---
data: 2
---
data: 3
---

Custom messageの使い方

micro-Ros2にはRos2の標準メッセージが実装されていますが，通信量を削減する目的などでCustom messageを使いたいことがあります．今回はgeometry_msgs::msg::Pointをfloat32型に変更したものを作ったので，こちらを使ってみます．

PlatformIO側の準備

まずは以下のようにしてプロジェクトのディレクトリにextra_packagesディレクトリを作成し，そこにcustom messageが定義されているのパッケージを入れてあげます．

cd <PlatformIOのプロジェクトディレクトリ>
mkdir extra_packages
cd extra_packages
git clone https://github.com/hosogaya/float_msgs.git

そして，コードを以下のように書き換えてgeometry_float_msgs::msg::Point型を送信するようにします．ほとんどはもとのサンプルコードと一緒です．

main.cpp

#include <Arduino.h>
#include <micro_ros_platformio.h>

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

#include <std_msgs/msg/int32.h>
// include geometry_float_msg::msg::Point
#include <geometry_float_msgs/msg/point.h> 

#if !defined(MICRO_ROS_TRANSPORT_ARDUINO_SERIAL)
#error This example is only avaliable for Arduino framework with serial transport.
#endif

rcl_publisher_t publisher;
// create message instance
geometry_float_msgs__msg__Point msg;

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

#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)){}}

// Error handle loop
void error_loop() {
  while(1) {
    delay(100);
  }
}

void timer_callback(rcl_timer_t * timer, int64_t last_call_time) {
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    // increment
    msg.x++;
    msg.y++;
    msg.z++;
  }
}

void setup() {
  // Configure serial transport
  Serial.begin(115200);
  set_microros_serial_transports(Serial);
  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_platformio_node", "", &support));

  // create publisher for geoemtry_flaot_msgs::msg::Point
  RCCHECK(rclc_publisher_init_default(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_float_msgs, msg, Point),
    "micro_ros_platformio_node_publisher"));

  // create timer,
  const unsigned int timer_timeout = 1000;
  RCCHECK(rclc_timer_init_default(
    &timer,
    &support,
    RCL_MS_TO_NS(timer_timeout),
    timer_callback));

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_timer(&executor, &timer));

  // initialize message
  msg.x = 0;
  msg.y = 0;
  msg.z = 0;
}

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

ホストPC側の準備

custom messageを使うためにはmicro-ros-agent側にcutom messageの情報を与える必要があります．なのでmicro-Ros2をインストールしたディレクトリにもcustom messageのパッケージを入れてあげます．

cd <micro-Ros2をインストールした場所>/micorros_ws/src
git clone https://github.com/hosogaya/float_msgs.git

そしたらもう一度コンパイルします．

cd <micro-Ros2をインストールした場所>/micorros_ws
source install/local_setup.bash
ros2 run micro_ros_setup build_firmware.sh
source install/local_setup.bash
ros2 run micro_ros_setup build_agent.sh
source install/local_setup.bash

実行（Custom message)

ここまでできたら先ほどと同じようにマイコンへの書き込みとmicro-ros-agentの起動をします．
そしてtopic echoをしてみると以下のようなメッセージが受け取れていることがわかると思います．

ros2 topic echo /micro_ros_platformio_node_publisher 
x: 2.0
y: 2.0
z: 2.0
---
x: 3.0
y: 3.0
z: 3.0
---
x: 4.0
y: 4.0
z: 4.0

まとめ

今回はPlatformIOとmicro-Ros2を利用してtopicのやり取りを試してみました．またCustom messageの通信も試してみました．注意点として今回のやり方だとmicroros_ws側とPlatformIOのextra_packages側が同じcustom messageのソースを利用する必要があります．Custom messageが編集中で同期を取りながら行いたいという人は，以下の処理をすることでPlatformIOがビルドするときにmicroros_ws側のパッケージを参照するようになるので同期を取りやすくすることができると思います．
1．micro_ros_platformioをlib_depsにリンクを書いて取ってくるのではなく，プロジェクトのlibディレクトリにクローンする
2. micro_ros_platformioにあるextra_script.pyのextra_packages_path = "{}/extra_packages".format(env['PROJECT_DIR'])の部分（33行目）をmciroros_ws内のcustom messageのパッケージへのパスに変更

最後に，micro-ros-agentの立ち上げにはdockerを利用する方法もありますが，Custom messageの登録はどうするんでしょうね？（やる気が出たら調べてみます）．