micro-ROSのsubscribeデモ

# 対象となる読者
:::note info
- micro-ROSを初めて使う人
- m5stackにROS2のtopicを送りたい人
:::

# 参考記事

https://kanpapa.com/today/2022/10/ros2-m5stack-microros-subscriber.html

上記の記事を参考にm5stackへtopicを送信するデモをトライ

# micro-ROSの準備

https://github.com/micro-ROS/micro_ros_setup


:::note info
この記事を書いている時にはjazzyが最新バージョン
:::

## micro-ROSのセットアップ
```bash
source /opt/ros/$ROS_DISTRO/setup.bash

mkdir uros_ws && cd uros_ws

git clone -b $ROS_DISTRO https://github.com/micro-ROS/micro_ros_setup.git src/micro_ros_setup

rosdep update && rosdep install --from-paths src --ignore-src -y

colcon build

source install/local_setup.bash
```

## micro-ROS-Agentのbuild
m5にtopicを送るためにAgentのbuildを実施

```bash
$ ros2 run micro_ros_setup create_agent_ws.sh
...
=== ./uros/drive_base (git) ===
=== ./uros/micro-ROS-Agent (git) ===
=== ./uros/micro_ros_msgs (git) ===
#All required rosdeps installed successfully
```
```bash
$ ros2 run micro_ros_setup build_agent.sh
Building micro-ROS Agent
Starting >>> micro_ros_msgs
Finished <<< micro_ros_msgs [1.32s]                     
Starting >>> micro_ros_agent
Finished <<< micro_ros_agent [0.28s]                
                     
Summary: 2 packages finished [1.80s]
```

buildしたAgentをROS2に認識させる
```bash
$ source install/local_setup.bash 
```
:::note info
terminalを複数立ち上げている場合は各terminalで上記コマンドの実行が必要
:::

# m5stack側の設定

:::note info
Platform IOを使った書き方にトライ
:::

## platformio.iniの設定
m5stack-core-esp32に書き込むことを想定した設定ファイル

```ini
[env:m5stack-core-esp32]
platform = espressif32
board = m5stack-core-esp32
framework = arduino
upload_port = COM3
lib_deps = 
	coryjfowler/mcp_can@^1.5.1
	m5stack/M5Stack@^0.4.6
	https://github.com/NITKK-ROS-Team/micro_ros_arduino_simpler.git#main
    https://github.com/micro-ROS/micro_ros_arduino.git
	Wire
build_flags = 
	-L ./.pio/libdeps/micro_ros_arduino/src/esp32/
	-l microros
```

:::note warn
```build_flags```の設定を忘れるとbuildが通らないので注意
:::

## ```subscriber```のコード
今回はUSB接続によるSerial通信を想定

<details><summary>参考記事のコードからWi-Fi設定をコメントアウトした版</summary>

```cpp
//
// micro-ROS subscriber sample program
// for M5Stack
//
#include <M5Stack.h>
#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>

rcl_subscription_t subscriber;
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 LED_PIN 13

#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(){
  M5.Lcd.print("Error!!\n");  
  //while(1){
  //  digitalWrite(LED_PIN, !digitalRead(LED_PIN));
  //  delay(1000);
  //}
}

void subscription_callback(const void * msgin)
{  
  const std_msgs__msg__Int32 * msg = (const std_msgs__msg__Int32 *)msgin;
  //digitalWrite(LED_PIN, (msg->data == 0) ? LOW : HIGH);
  M5.Lcd.printf("Data: %d\n", msg->data);  
}

void setup() {
  M5.begin();
  M5.Power.begin();
  M5.Lcd.setTextSize(2); 
  M5.Lcd.print("Hello micro-ROS\n");
  M5.Lcd.print("/micro_ros_arduino_subscriber\n");
  
  // for WiFi
//   set_microros_wifi_transports("SSID", "PASS", "192.168.xxx.xxx", 8888);
  // for USB
  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 subscriber
  RCCHECK(rclc_subscription_init_default(
    &subscriber,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "micro_ros_arduino_subscriber"));

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));
}

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

</details>

# デモ
m5stackをつないだ際にできるディレクトリの確認(今回は```/dev/ttyACM0```)
書き込み権限の追加
```bash
$ sudo chmod 666 /dev/ttyACM0
```

:::note info
USBを抜き差しするとここの設定が切れるので```USER```をgroupに追加しておくと楽
```bash
$ sudo usermod -aG dialout $USER
```
:::

Agentの起動
```bash
$ ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0 
```

topicの送信
```bash
$ ros2 topic pub --once /micro_ros_arduino_subscriber std_msgs/Int32 "data: 256"
publisher: beginning loop
publishing #1: std_msgs.msg.Int32(data=256)
```

ちゃんと通信ができていると以下のような結果がm5stackに出てくる

![20241225_141632.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2043769/ddd228ce-0d3e-8340-fc01-81052338645b.jpeg)

# 感想
PlatformIOでコードを書くのが初めてで必要なライブラリのインストールが大変だった．micro-ROSのbuildもちょこちょこErrorでちゃったので困った．そういうときは最悪rebootしてやり直せば何とかなる．