micro-ROS on Wio Terminal with WiFi UDP Transport

※2023.06.24更新

- micro-ROS for Arduino本家の`v2.0.6`リリースは[Micro-XRCE-DDS-Client絡みのバグを含んでいる](https://github.com/micro-ROS/micro_ros_arduino/issues/1427)とのことで，すぐに[`v2.0.7`がリリースされている](https://github.com/micro-ROS/micro_ros_arduino/releases/tag/v2.0.7-humble)．

※2023.06.17更新

- micro-ROS for Arduino本家で[`humble`の`v2.0.6`がリリースされた](https://github.com/micro-ROS/micro_ros_arduino/releases/tag/v2.0.6-humble)ことにより，今回のWio Terminal対応がリリースバージョンで使用可能となった
    - `iron`, `rolling`も同時にリリースされている

※2023.02.23更新

- micro-ROS for Arduino本家に[Wio Terminalのサポートがマージ](https://github.com/micro-ROS/micro_ros_arduino/pull/1284)された
- 最新の`foxy`ブランチアーカイブを[ここ](https://github.com/micro-ROS/micro_ros_arduino/archive/refs/heads/foxy.zip)からダウンロードして[Arduino IDEへのインポート手順][9]に則ってインストールすれば`micro-ros_publisher_wifi`をビルドできるようになった
  - リリースタグはまだ打たれていない
  - Wio Terminal固有のLCDサポートなどは入れていないため，使用する場合は本記事中のexampleを参考にされたい
  - `humble`ブランチにもバックポートされているので`humble`でも動作すると思われる

# はじめに

[ROS2 Advent Calendar 2022][1]の記事として，[ROS Japan UG #46 LT大会][2]にてデモンストレーション（飛び込みLT枠のためビデオアーカイブなし）した内容について投稿する．

本記事は，読者が[micro-ROS][3]についての基本的な知識（一部のプラットフォームを除いて，micro-ROS Agentを介してROS2 topicをやり取りすることなど）を持っていることを前提に書かれている．

ライブラリの修正箇所は[micro-ROS for Arduino][4]本家へのPull Requestを予定しており，[Seeed Studio Wio Terminal][5]用のサンプルスケッチはそこからforkした筆者のリポジトリで公開予定である．それらの進捗があり次第，本記事も更新していく．

# 記事概要

Wio TerminalからWiFi経由でROS2 topicをPubするための手順を紹介する．
これにより，ESP32シリーズ以外のMCUでもWirelessでROS2 topicをPub可能であることが示された．

https://twitter.com/K66941741/status/1604849092483776512

# 環境

環境構築の詳細な手順は割愛する．

## ハードウェア

- Wio Terminal
- ホストPC（amd64）
  - 筆者は1台のPCで以下の役割を兼ねさせた
    - ArduinoIDEによる開発
    - Wio TerminalとのUDP通信を受け持つmicro-ROS Agentの実行
    - micro-ROS AgentからのROS2 topicをSubする
- WiFiルータ
  - Wio Terminal側はDHCPを使用してIPアドレスを取得するので，DHCPサーバ機能を持つこと
  - micro-ROS Agentを実行するホストPC側のIPアドレスはArduinoスケッチに埋め込まれるので，静的IPアドレスを割り振っておくこと

## ソフトウェア

- Ubuntu `20.04` amd64
- ROS2 Foxy
- ArduinoIDE `1.8.19`（筆者は`sudo snap install arduino`コマンドによりインストールした）
  - [Wio Terminalボードライブラリのインストール][6]
    - Seeed SAMD Boards `1.8.3`
  - [Wio TerminalのWiFiを使用するために必要なライブラリのインストール][7]
    - Seeed Arduino rpcWiFi `1.0.6`
    - インストール時に`rpcUnified`などの関連するライブラリを一緒にインストールするか聞かれるので，全て一緒にインストールすること
- [WiFiコアのファームウェアアップデート手順][8]を実施しておくこと
  - 筆者は`RTL8720 Firmware Version: 2.1.3`で動作確認している
- micro-ROS for Arduino `v2.0.5-foxy` precompiled
  - [Arduino IDEへのインポート手順][9]

# ビルド手順

Arduinoの各種ライブラリやスケッチを保存するディレクトリがある`Arduino`ディレクトリを起点とする．

## micro-ROS for Arduinoのヘッダとソースの改変

`libraries/micro_ros_arduino-2.0.5-foxy/src/`以下にある`micro_ros_arduino.h`と`wifi_transport.cpp`の一部を以下のように改変する．

``` diff:micro_ros_arduino.h
- #if defined(ESP32) || defined(TARGET_PORTENTA_H7_M7) || defined(ARDUINO_NANO_RP2040_CONNECT)
+ #if defined(ESP32) || defined(TARGET_PORTENTA_H7_M7) || defined(ARDUINO_NANO_RP2040_CONNECT) || defined(ARDUINO_WIO_TERMINAL)

#if defined(ESP32) || defined(TARGET_PORTENTA_H7_M7)
#include <WiFi.h>
#include <WiFiUdp.h>
#elif defined(ARDUINO_NANO_RP2040_CONNECT)
#include <SPI.h>
#include <WiFiNINA.h>
+ #elif defined(ARDUINO_WIO_TERMINAL)
+ #include <rpcWiFi.h>
+ #include <WiFiUdp.h>
#endif

```

``` diff:wifi_transport.cpp
- #if defined(ESP32) || defined(TARGET_PORTENTA_H7_M7) || defined(ARDUINO_NANO_RP2040_CONNECT)
+ #if defined(ESP32) || defined(TARGET_PORTENTA_H7_M7) || defined(ARDUINO_NANO_RP2040_CONNECT) || defined(ARDUINO_WIO_TERMINAL)

#include <Arduino.h>


#if defined(ESP32) || defined(TARGET_PORTENTA_H7_M7)
#include <WiFi.h>
#include <WiFiUdp.h>
#elif defined(ARDUINO_NANO_RP2040_CONNECT)
#include <SPI.h>
#include <WiFiNINA.h>
+ #elif defined(ARDUINO_WIO_TERMINAL)
+ #include <rpcWiFi.h>
+ #include <WiFiUdp.h>
#endif
```

## ボードの選択とスケッチの作成

ボードとしてWio Terminalを選択する．設定は特に変える必要はない．

スケッチを新規作成，以下のソースで上書きして保存する．

`set_microros_wifi_transports("WIFI SSID", "WIFI PASS", "192.168.1.57", 8888);`の第1引数と第2引数をWiFiルータの設定に合わせる．第3引数はmiro-ROS Agentを走らせるホストPCのIPアドレスを指定することに注意．


``` cpp:wio_terminal_wifi_publisher.ino
#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 <TFT_eSPI.h>

#include <std_msgs/msg/int32.h>

#include "Free_Fonts.h"

rcl_publisher_t publisher;
std_msgs__msg__Int32 msg;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;

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

TFT_eSPI tft;

void error_loop(){
  while(1){
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

void setup() {
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW);

  // Setup Wio Terminal's LCD
  tft.begin();
  tft.setRotation(3);
  tft.fillScreen(TFT_BLACK);
  tft.setFreeFont(FMB12);

  // Show Connecting Message to LCD
  const String connecting_message = "Connecting to WiFi ...";
  tft.setCursor((320 - tft.textWidth(connecting_message))/2, 120);
  tft.print(connecting_message);
  
  set_microros_wifi_transports("WIFI SSID", "WIFI PASS", "192.168.1.57", 8888);

  // Show Connected Message to LCD
  tft.fillScreen(TFT_BLACK);
  const String connected_message = "Connected!";
  tft.setCursor((320 - tft.textWidth(connected_message))/2, 120);
  tft.print(connected_message);
  
  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_wifi_node", "", &support));

  // create publisher
  RCCHECK(rclc_publisher_init_best_effort(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "topic_name"));

  msg.data = 0;
}

void loop() {
    delay(1000);
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    msg.data++;
}
```



## フォントファイルのコピー

[Wio TerminalのLCDに適合したフォントファイル][10]をダウンロードして，スケッチを保存したディレクトリにコピーする．

## スケッチのコンパイルと書き込み

Wio TerminalとホストPCをUSBケーブルで接続し「マイコンボードに書き込む」を実行する．以下のようになればOK．

![compile_write.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/574178/20cc79a8-842d-c7f8-a01d-bef8c4b3a70a.png)

# 動作確認

## ホストPCでのmicro-ROS Agentの起動

書き込み終わったWio Terminalの電源スイッチをいったんOFFにしておく．USBケーブルはホストPCに接続したままで良い（後の工程でWio Terminalの電源としてのみ使用するため）．

本記事では[Dockerを使用したmicro-ROS Agent][11]について紹介するが，もちろんネイティブ環境でビルドしたものを使用しても良い（むしろより良い）．

``` sh
$ docker pull microros/base:foxy
$ docker run -it --net=host microros/micro-ros-agent:foxy udp4 -p 8888
```

## Wio Terminalの電源スイッチをONにする

LCDに`Connecting to WiFi...`と表示された後に，WiFiルータやmicro-ROS Agentとの接続に問題なければ`Connected!`と表示される．

micro-ROS Agentを起動した端末では以下のように表示され，接続が確立したことがわかる．

``` sh
$ docker run -it --net=host microros/micro-ros-agent:foxy udp4 -p 8888
[1671409046.554536] info     | UDPv4AgentLinux.cpp | init                     | running...             | port: 8888
[1671409046.555203] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 4
[1671409057.332292] info     | Root.cpp           | create_client            | create                 | client_key: 0x6FCEB7C8, session_id: 0x81
[1671409057.332937] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x6FCEB7C8, address: 192.168.11.7:47138
[1671409057.371542] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x6FCEB7C8, participant_id: 0x000(1)
[1671409057.382665] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x6FCEB7C8, topic_id: 0x000(2), participant_id: 0x000(1)
[1671409057.391553] info     | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0x6FCEB7C8, publisher_id: 0x000(3), participant_id: 0x000(1)
[1671409057.400132] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x6FCEB7C8, datawriter_id: 0x000(5), publisher_id: 0x000(3)

```


## ROS2 topicのSub

``` sh
$ source /opt/ros/foxy/setup.bash
$ ros2 topic list
/parameter_events
/rosout
/topic_name
$ ros2 topic echo /topic_name 
data: 128
---
data: 129
---
data: 130
---
data: 131
---
data: 132
---
```



# 参考資料

1. [ROS2 Advent Calendar 2022][1]
1. [ROS Japan UG #46 LT大会][2]
1. [micro-ROS][3]
1. [micro-ROS for Arduino][4]
1. [Seeed Studio Wio Terminal][5]
1. [Wio Terminalボードライブラリのインストール][6]
1. [Wio TerminalのWiFiを使用するために必要なライブラリのインストール][7]
1. [WiFiコアのファームウェアアップデート手順][8]
1. [Arduino IDEへのインポート手順][9]
1. [Wio TerminalのLCDに適合したフォントファイル][10]
1. [Dockerを使用したmicro-ROS Agent][11]
1. [MAEHARA_Keisukeのポータル記事][12]

[1]:https://qiita.com/advent-calendar/2022/ros2
[2]:https://rosjp.connpass.com/event/244199/
[3]:https://github.com/micro-ROS/micro_ros_setup
[4]:https://github.com/micro-ROS/micro_ros_arduino
[5]:https://wiki.seeedstudio.com/jp/Wio-Terminal-Getting-Started/
[6]:https://wiki.seeedstudio.com/jp/Wio-Terminal-Getting-Started/#_7
[7]:https://wiki.seeedstudio.com/Wio-Terminal-Network-Overview/#needed-libraries-for-wi-fi
[8]:https://wiki.seeedstudio.com/Wio-Terminal-Network-Overview/#update-the-wireless-core-firmware
[9]:https://github.com/micro-ROS/micro_ros_arduino/tree/dc99a9f3663fab6ec5ff809a6b5310e0e40e4130#how-to-use-the-precompiled-library
[10]:https://github.com/Seeed-Studio/Seeed_Arduino_LCD/blob/master/examples/320x240/All_Free_Fonts_Demo/Free_Fonts.h
[11]:https://hub.docker.com/r/microros/micro-ros-agent
[12]:https://qiita.com/MAEHARA_Keisuke/items/31faa68cf5c51831acc6