ESP32マイコンとROS2で無線通信（micro-ROS）

今回は無線通信が可能なESP32マイコンを使ってROS2との通信を実装する。環境は以下の通り。
 - ROS2Humble
 - Ubuntu22.04
 - [ESP32 DEVKIT V1](https://www.amazon.co.jp/gp/product/B07MH58JS2/ref=ppx_yo_dt_b_asin_title_o00_s00?ie=UTF8&psc=1)

ESP32 DEVKIT V1のピン配置は画像の通り。
<img width="1600" alt="image.png (804.5 kB)" src="https://img.esa.io/uploads/production/attachments/19744/2023/05/10/142382/bf6761da-f8bb-4dfc-a1c6-605ecd22aba0.png">

# ArduinoIDEのインストール
[公式サイト](https://www.arduino.cc/en/software)からLinux用ArduinoIDE(今回はver2.1.0)のZIPファイルをダウンロードして解凍する。解凍したフォルダの中に`arduino-ide`というものがあるのでダブルクリックして開く。
左上から`File`>`Preferences...`とクリックして環境設定を開く。

<img width="780" alt="image.png (53.3 kB)" src="https://img.esa.io/uploads/production/attachments/19744/2023/05/10/142382/efc4f2ad-b9fb-4930-9be1-650620e95c99.png">

`Additional boards manager URLs:`の欄に以下のURLを貼り付けて，ArduinoIDEにESP32マイコンの情報を追加する。
```
https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_index.json
```
ついでに僕のような英語弱者は`Language:日本語`とすると良いだろう。
`OK`を選択して設定画面を閉じる。

次に`ツール`>`ボード`>`ボードマネージャ...`からボードマネージャを開き，`esp32`で検索する。表示された物をインストールしておく。

<img width="258" alt="image.png (23.5 kB)" src="https://img.esa.io/uploads/production/attachments/19744/2023/05/10/142382/c1b7b3e7-a4d5-44bc-851e-dedfe3a3de5d.png">

# とりあえずLチカ(余興)
LEDをpin10(GPIO32)に繋げる。

<img width="2147" alt="image.png (5.3 MB)" src="https://img.esa.io/uploads/production/attachments/19744/2023/05/10/142382/0bfb7ca2-e0f9-4b23-b9cf-13cf860f20a3.png">

コードをこのように変更する。
```c++
#define LED 32

void setup() {
  pinMode(LED, OUTPUT);
}

void loop() {
  digitalWrite(LED, HIGH);
  delay(500);
  digitalWrite(LED, LOW);
  delay(500);
}
```
上の方のボード選択から`DOIT ESP32 DEVKIT V1`を選び，左上のチェックマーク`✓`を選択してコンパイル。

<img width="662" alt="image.png (12.4 kB)" src="https://img.esa.io/uploads/production/attachments/19744/2023/05/10/142382/4c73049e-47f3-4f05-97bb-e3088e9b9848.png">

このようなエラーが出た。自分の場合はPython3がPythonにリンクされていないことが原因であったようだ。ターミナルを開いて以下の操作を行う。
```
$ which python3
/usr/bin/python3
$ sudo ln -s /usr/bin/python3 /usr/bin/python
```
再びコンパイルを行うと別のエラーメッセージが出た。

<img width="918" alt="image.png (25.5 kB)" src="https://img.esa.io/uploads/production/attachments/19744/2023/05/10/142382/370abaf9-a5de-4cad-b7ce-8ae2a9b56047.png">

どうやらpyserialというライブラリが入っていない事が原因らしい。
```
$ sudo apt install python3-pip
$ pip install pyserial
```
再びコンパイルを行うとようやくうまく行った。早速書き込んでみる......とエラーが出た。

<img width="1073" alt="image.png (145.3 kB)" src="https://img.esa.io/uploads/production/attachments/19744/2023/05/10/142382/57aac70b-8122-402a-9eaa-0c3c079513c3.png">

これはUSBポートへのアクセス権限がないことが原因らしい。
```
$ sudo nano /lib/udev/rules.d/50-udev-default.rules
```
以下の行を探して修正する。
```
KERNEL=="tty[A-Z]*[0-9]|pppox[0-9]*|ircomm[0-9]*|noz[0-9]*|rfcomm[0-9]*", GROUP="dialout"
　　　↓　　　　↓　　　　↓　　　　↓　　　　↓
KERNEL=="tty[A-Z]*[0-9]|pppox[0-9]*|ircomm[0-9]*|noz[0-9]*|rfcomm[0-9]*", GROUP="dialout", MODE="0777"
```
`Ctrl`+`S`で保存,`Ctrl`+`X`で終了。
PCにつながっているUSBケーブルを挿し直して，再び書き込んでみる。

<img width="2231" alt="image.png (5.8 MB)" src="https://img.esa.io/uploads/production/attachments/19744/2023/05/10/142382/f85fad33-74fe-4085-aabb-1f2548404f08.png">

LEDがチカチカした。

# micro-ROS関連
閑話休題。
先ずArduinoIDEにmicro-ROS for Arduinoを導入する。
[このリンク](https://github.com/micro-ROS/micro_ros_arduino/archive/refs/heads/humble.zip)からmicro-ROS for Arduinoのzipファイルをダウンロードする。
ArduinoIDEを開き，左上の`スケッチ`>`ライブラリをインクルード`>`.ZIP形式のライブラリをインストール...`を順に選択。
先ほどダウンロードしたZIPファイルを選択して`開く`。
コンソールに`Library installed`と表示されれば成功。

次にmicro-ROS-Agentを導入する。これは簡単に言えばマイコンからのメッセージをROS2で認識可能にする役割を持っている。
以下のコマンドを順に実行する。
```
$ cd ~/ros2_ws (ROS2のワークスペースに移動する)
$ source /opt/ros/humble/setup.bash
$ git clone -b humble 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
$ ros2 run micro_ros_setup create_agent_ws.sh
$ ros2 run micro_ros_setup build_agent.sh
```
ここで下の画像のような警告が出た場合は[ここ](https://github.com/leethomason/tinyxml2)からZIPファイルをダウンロードして解凍する。
フォルダ名を`tinyxml2`に変更してホーム下に配置して，`colcon build`を行う。
<img width="1080" alt="image.png (820.6 kB)" src="https://img.esa.io/uploads/production/attachments/19744/2023/05/10/142382/ad918cab-e275-4155-8afb-655df9e4fe83.png">
Agentが動作するか確認してみる。
```
$ source install/local_setup.sh
$ ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888
```

<img width="778" alt="image.png (151.2 kB)" src="https://img.esa.io/uploads/production/attachments/19744/2023/05/10/142382/2c733704-9f5b-4d52-a21d-506512c3791f.png">

こんな感じになっていればうまく行っていると思われる。

# 実際に使ってみる
ポテンショメータの値を読み込んで，それをWiFi経由でPC上のROS2に送ってみる事にする。
それぞれいい感じに配線する（適当）。
<img width="2132" alt="image.png (7.1 MB)" src="https://img.esa.io/uploads/production/attachments/19744/2023/05/10/142382/e038af27-eb3e-40d2-a639-a4f6dd5f328d.png">

今回はアナログ・デジタル変換（ADC）にADC1_CH6(pin12)を使う。ちなみにADC2はWiFiドライバで使用されており、WiFiドライバが動作していないときしか使えないらしいので注意が必要だ。

ArduinoIDEを開き，以下のコードをコピペする。基本的にはmicro_ros_arduinoの`micro-ros_publisher_wifi`というスケッチ例に準拠している。
```c
#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>

#if !defined(ESP32) && !defined(TARGET_PORTENTA_H7_M7) && !defined(ARDUINO_NANO_RP2040_CONNECT) && !defined(ARDUINO_WIO_TERMINAL)
#error This example is only avaible for Arduino Portenta, Arduino Nano RP2040 Connect, ESP32 Dev module and Wio Terminal
#endif

#define LED_PIN 2 //マイコン上の青いLEDを指定
#define AD_PIN 34 //ADCのピンを指定

//エラーハンドリング用のマクロ
#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)){}}

//micro-ROS関連で必要となる変数を宣言しておく
rcl_publisher_t publisher;
std_msgs__msg__Int32 msg;
rcl_init_options_t init_options;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;

//micro-ROS関連でエラーが出るとマイコン上の青色LEDが点滅するように設定している
void error_loop(){
  while(1){
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

void setup() {
  /*
  wifiの設定を行う。
  マイコンが接続すべきWiFi(つまりmicro_ros_agentを立ち上げるPCが接続されているWiFi)のSSID，
  そのWiFiのパスワード，micro_ros_agentを立ち上げているPCのIPアドレス，port番号を
  指定する必要がある。PCのIPは事前に固定しておいた方が良い。
  */
  set_microros_wifi_transports("WIFI_SSID", "PASSWORD", "192.168.0.xxx", 8888);

  //LEDの設定
  pinMode(LED_PIN, OUTPUT);
  //ADCの設定
  analogSetAttenuation(ADC_6db); //追加
  pinMode(AD_PIN, ANALOG); //追加

  delay(2000);

  //micro-ROSの設定
  allocator = rcl_get_default_allocator();
  init_options = rcl_get_zero_initialized_init_options();
  //初期化オプションの作成
  RCCHECK(rcl_init_options_init(&init_options, allocator));
  RCCHECK(rcl_init_options_set_domain_id(&init_options, 123)); //ROS_DOMAIN_IDを設定できる。今回は123としている。
  RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator));
  //ノードの作成
  RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_wifi_node", "", &support));
  //publisherの作成
  RCCHECK(rclc_publisher_init_best_effort(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "/ADC_data"));

  msg.data = 0;
  //セットアップが完了するとLEDが点灯する
  digitalWrite(LED_PIN, HIGH);
}

void loop() {
  msg.data = analogReadMilliVolts(AD_PIN);
  RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
}
```
普通のC++版のROS2のようにpublisherはtimerで実行することもできるが，今回はこのような形としている。

上のボード選択から`DOIT ESP32 DEVKIT V1`を選択してから，コンパイル（✓）して書き込む（→）。
<img width="561" alt="image.png (19.0 kB)" src="https://img.esa.io/uploads/production/attachments/19744/2023/05/10/142382/6c4b1a79-d97d-40bd-8071-1a5ace48dc4f.png">

ターミナルを開いてAgentを起動する。
```
$ ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888
```
マイコンボード上のENスイッチを押して，すでに書き込んであるコードを再度読み込むとAgentにmicro-ROSが認識される。

<img width="763" alt="image.png (233.9 kB)" src="https://img.esa.io/uploads/production/attachments/19744/2023/05/10/142382/b4cd7138-a3c8-4674-8fe0-0ecaf9189028.png">

マイコンからデータが送られてきている事がわかる。

<img width="865" alt="image.png (18.9 kB)" src="https://img.esa.io/uploads/production/attachments/19744/2023/05/10/142382/5bb38e3a-30a7-46a6-8fc5-eb43a855a62b.png">

rqtでもしっかり認識されている。

# 終わり
　今回はpublisherのみの実装だったのでexecutorを使いませんでしたが，使う場合には`rclc_executor_init`関数の第三引数が登録したいコールバック関数の個数を表していることに注意してください。また2023年9月現在，micro-ROSでサービスサーバーを二つ以上設定するのは難しいようですので，そのことにも注意してください。
　また，カスタムメッセージの実装については[こちらのサイト](https://klab.hateblo.jp/entry/2023/05/22/203251)が分かりやすいのですが，自分の環境ではビルドターゲットを指定するとライブラリの生成に失敗したので，同様の現象が発生している人はビルドターゲットを指定せずにビルドを行うとよいかもしれません。
```
sudo docker run -it --rm -v $(pwd):/project --env MICROROS_LIBRARY_FOLDER=extras microros/micro_ros_static_library_builder:humble
```

以上です。

[とても参考になった動画](https://youtu.be/cLlK7uqFC2A)