micro-Ros2を試してみる

# 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で以下を実行していただければインストールが終了すると思います．
```sh
# 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](https://github.com/micro-ROS/micro_ros_platformio)にあるサンプルをとりあえず使います．
<details><summary>PlatformIOで書くマイコン側のプログラム</summary><div>

```cpp: 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)));
}

```
</div></details>

次にmicro-Ros2を利用するために`platformio.ini`を書き換えます．重要なのは最後の`lib_deps`の部分でここでmicro-Ros2のライブラリを取ってくるようにしています．なおデフォルトでマイコンとの通信方法が`serial`, Ros2のバージョンが`humble`に設定されています．他の通信方法やRos2バージョンを利用したい場合は，[ここで](https://github.com/micro-ROS/micro_ros_platformio)サポートされている通信方法やRos2バージョンを確認してから変更してください．
```sh
[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を立ち上げます．これは，マイコンとの通信を管理してくれるものです．

```sh
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ずつ値が大きくなるようにデータが送られているのが確認できると思います．
```sh
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が定義されているのパッケージを入れてあげます．

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

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

```cpp: 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のパッケージを入れてあげます．
```sh
cd <micro-Ros2をインストールした場所>/micorros_ws/src
git clone https://github.com/hosogaya/float_msgs.git
```

そしたらもう一度コンパイルします．
```sh
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`をしてみると以下のようなメッセージが受け取れていることがわかると思います．

```sh
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](https://github.com/micro-ROS/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の登録はどうするんでしょうね？（やる気が出たら調べてみます）．