Pololu Zumo Shield付きUNO R4をμROSと動かしてみた

## 序文
![DSC02268.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3861095/cfa9631e-fce2-25d1-3a40-cc93fe52f4a7.jpeg)
これは僕の初めてのブログです。僕は留学生なので、日本語が完璧ではありません。正しくない文法や変な表現があったら、申し訳ありません。

ネットで半値のPololu Zumo Shield for Arduinoを買いました。この頃はROS2で遊んでいたから、これをMicro ROSで動かしたらいいなぁと思っていました。Pololu Zumo Shieldに使用べきUNO R3はMicro ROSでは性能が不足していますが、最近開発されたUNO R4ならできます。

## はじめに
このブログは[スイッチサイエンスの伊藤さんが書かれたブログ](https://www.switch-science.com/blogs/magazine/arduino-uno-r4-minima-ros2 "Arduino Uno R4 Minima & WiFiをROS2と繋いでみる")を基にしています。

まとめると、次のステップになります:
<dl>
    
<dt>ROS2(とColcon)をインストールする</dt>
<dd>
<a href="https://docs.ros.org/en/humble/Installation.html">これについて</a>はもうたくさんの資料がありますので、あまり書きません。僕の環境ではHumbleのブランチを使います。
</dd>
    
<dt>Micro Rosをインストールする</dt>
<dd>
伊藤さんはソースからインストールしてビルドしたが、僕はDockerを使う方が簡単で早いと思います。
    
```shell
docker pull microros/base:humble
```

</dd>

<dt>Arduino IDEをインストールする</dt>
<dd>
ダウンロードは<a href="https://www.arduino.cc/en/software">こちら</a>です。ダウンロードしたらBoard ManagerでUno R4もインストールしてください。

![UNO R4 in boards manager](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3861095/89c42f47-8506-28e7-9abf-1dffe22535f4.png)

</dd>
    <dt>micro_ros_arduinoをダウンロードする</dt>
    <dd>
    <a href="https://github.com/micro-ROS/micro_ros_arduino/releases">こちら</a>からダウンロードして、解凍して<code>Arduino/libraries</code>にコピーしてください。

:::note warn
ROS2のブランチにご注意ください
:::

</dd>

<dt>micro_ros_arduinoを編集</dt>
<dd>
執筆時点で(<strong>v2.0.7</strong>)、Arduino UNO R4はMicro Rosでサポートされていないが、UNO R4のマイコンであるCortex M4プラットフォームはサポートされています。少しライブラリを編集すれば、コンパイルが可能です。次のようにmicro_ros_Arduinoを編集してください。

ESP32 S3 WiFi 機能を加わる

```diff_c:src/micro_ros_arduino.h
...
// 98行目
- #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) || defined(ARDUINO_NANO_RP2040_CONNECT) || defined(ARDUINO_WIO_TERMINAL) || defined(ARDUINO_UNOR4_WIFI)
...
#elif defined(ARDUINO_WIO_TERMINAL)
#include <rpcWiFi.h>
#include <WiFiUdp.h>
// 109行目
+ #elif defined(ARDUINO_UNOR4_WIFI)
+ #include <WiFiS3.h>
#endif
```

```diff_cpp:src/wifi_transport.cpp
- #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) || defined(ARDUINO_NANO_RP2040_CONNECT) || defined(ARDUINO_WIO_TERMINAL) || defined(ARDUINO_UNOR4_WIFI)
#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>
+ #elif defined(ARDUINO_UNOR4_WIFI)
+ #include <WiFiS3.h>
#endif
...
```

UNO R4のマイコンはAVRプラットフォームではないので、sys/timeが使えない。

```diff_cpp:src/default_transport.cpp
#include <Arduino.h>

extern "C"
{
  #include <stdio.h>
  #include <stdbool.h>
-  #include <sys/time.h>
+  #include <time.h>
...
```
必要ではありませんが、例のスケッチも編集したらより便利になります。
```diff_cpp:examples/micro-ros_publisher_wifi/micro-ros_publisher_wifi.ino
...
#include <std_msgs/msg/int32.h>

// 11行目
- #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) && !defined(ARDUINO_NANO_RP2040_CONNECT) && !defined(ARDUINO_WIO_TERMINAL) && !defined(ARDUINO_UNOR4_WIFI)
- #error This example is only avaible for Arduino Portenta, Arduino Nano RP2040 Connect, ESP32 Dev module and Wio Terminal
+ #error This example is only avaible for Arduino Portenta, Arduino Nano RP2040 Connect, ESP32 Dev module, Wio Terminal and Arduino UNO R4 WiFi
#endif
...
```
</dd>
    <dt>コンパイルしてみる</dt>
<dd>
さっきに編集した例を試してみましょう。<br>
<code>File > Examples > micro_ros_arduino > micro-ros_publisher_wifi</code>を開いて、Verifyしてください。

![image_2024-08-14_190302696.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3861095/566a53ad-2c7f-05e6-7d15-02023700f8ff.png)
成功しました！
</dl>

ここまでは伊藤さんのブログで説明されていることとほぼ同じです。次はZumo Shieldの機能を追加します。
## Wire.h と使うこと
PololuのZumoShield.hライブラリはWire.hに依存しています。Wire.hはI2C機能のライブラリーです。Zumo Shieldの場合は、LSM303D加速度計とL3GD20Hジャイロスコープとの通信に使用されます。

![image_2024-08-14_191421705.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3861095/e26acf0d-735d-b5ac-dab6-4990c40b1947.png)

そのままコンパイルすると、このエラーが発生ます。

![image_2024-08-14_191540905.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3861095/3b916bc6-8daf-2ab8-ea36-cb4bde30bc82.png)

原因はメモリ不足です。これを解決するには、`colon_lowmem`の代わりに`colcon_verylowmem`を使用します。ColconはROS2専用のコンパイラです。

`extras/library_generation`にあるコンパイラ設定を含む`library_generation.sh`を編集します。
```diff_shell:extras/library_generation/library_generation.sh
...
######## Build for STM32F4 ########
if [[ " ${PLATFORMS[@]} " =~ " cortex_m4 " ]]; then
    rm -rf firmware/build

    export TOOLCHAIN_PREFIX=/uros_ws/gcc-arm-none-eabi-9-2020-q2-update/bin/arm-none-eabi-
# 102行目
-     ros2 run micro_ros_setup build_firmware.sh /project/extras/library_generation/cortex_m4_toolchain.cmake /project/extras/library_generation/colcon_lowmem.meta
+     ros2 run micro_ros_setup build_firmware.sh /project/extras/library_generation/cortex_m4_toolchain.cmake /project/extras/library_generation/colcon_verylowmem.meta
...
```


micro_ros_arduinoはコンパイル済みライブラリを使用します。この変更は再コンパイルが必要があります。

### micro_ros_arduinoを再コンパイルする
以下は[micro_ros_arduinoのReadme](https://github.com/micro-ROS/micro_ros_arduino?tab=readme-ov-file#how-to-build-the-precompiled-library)から引用しました。

まず、ツールを取得します。
```shell
docker pull microros/micro_ros_static_library_builder:humble
```
そして、再コンパイルします。
```shell
docker run -it --rm -v $(pwd):/project --env MICROROS_LIBRARY_FOLDER=extras microros/micro_ros_static_library_builder:humble -p cortex_m4
```
:::note warn
ライブラリの **ルートディレクトリ** (micro_ros_arduino-2.0.7-humble)で実行してください
:::
:::note warn
Windowsを使っている方は`$(pwd):/project`を`"<micro_ros_arduinoのディレクトリ>:/project"`に変えてください
:::

そしてもう一度例のスケッチをコンパイルしてみて

![image_2024-08-14_203514762.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3861095/2fd361ec-cb96-9f0c-a238-192e24c7c8a4.png)


おめでとう！すべてうまくいきました。
## ZumoShield.h と使うこと
ZumoShield.hはUNO R4と互換性がないPololuBuzzerライブラリに依存しています。このため、PololuBuzzerライブラリを無効にする必要があります。

ZumoShieldライブラリのディレクトリを開いて、次のように編集してください

```diff_c:ZumoShield.h
#pragma once

#include <LSM303.h>
#include <L3G.h>
#include <Pushbutton.h>
#include <QTRSensors.h>
- #include <ZumoBuzzer.h>
#include <ZumoIMU.h>
#include <ZumoMotors.h>
#include <ZumoReflectanceSensorArray.h>
```
そして、この三つのファイルの名を変更してください
`ZumoBuzzer.h`>`ZumoBuzzer.h.bck`
`PololuBuzzer.h`>`PololuBuzzer.h.bck`
`PololuBuzzer.cpp`>`PololuBuzzer.cpp.bck`

これでライブラリが使用できるようになりました。

## 簡単なスケッチを作りましょう！
今回やりたいことはパソコンからWiFiを使ってロボットを操縦することです。システムの概要を見てみましょう。

```plantuml
node "zumo_r4" {
    [twist_subscriber]
    [twist_subscription_callback]
}
frame "パソコン" {
    node "teleop_twist_keyboard" {
        [cmd vel publisher]
    }
}
[cmd vel publisher] --> micro_ros_agent : (geometry_msgs/msg/twist) cmd_vel
micro_ros_agent --> [twist_subscriber]
[twist_subscriber] --> [twist_subscription_callback]

note bottom of [twist_subscription_callback]
    モーターを支配する
end note

```

スケッチはこれです。

```cpp:zumo_code.ino
//------------------------------------micro ros things-------------------------------------
#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 <geometry_msgs/msg/twist.h>

rcl_subscription_t twist_subscriber;
geometry_msgs__msg__Twist twist_msg;
rclc_executor_t executor;
rcl_allocator_t allocator;
rclc_support_t support;
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)){}}

#include "secrets.h" // WiFi and port info

#include "Arduino_LED_Matrix.h"
ArduinoLEDMatrix matrix;

void error_loop(){
  matrix.loadFrame(LEDMATRIX_DANGER);
  Serial.println("Error!");
  while(1){
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

//-----------------------------------------zumo--------------------------------------------

#include <Wire.h>
#include <ZumoShield.h>
ZumoMotors motors;
const int speed_scale = 400;

void twist_subscription_callback(const void * msgin)
{
  const geometry_msgs__msg__Twist * msg = (const geometry_msgs__msg__Twist *)msgin;
  // vx + forward - backward
  // az + left    - right
  float vel_x = constrain(msg->linear.x, -1.0f, 1.0f);
  float angular_z = constrain(msg->angular.z, -1.0f, 1.0f);

  int left_wheel = round(speed_scale * (vel_x - angular_z));
  int right_wheel = round(speed_scale * (vel_x + angular_z));

  motors.setLeftSpeed(left_wheel);
  motors.setRightSpeed(right_wheel);
}

//-----------------------------------------main-------------------------------------------

void setup() {
  Serial.begin(9600);

  Serial.println("Setting up wifi..");

  matrix.loadFrame(LEDMATRIX_CLOUD_WIFI);
  set_microros_wifi_transports(WIFI_SSID, WIFI_PASS, HOST_IP, HOST_PORT);

  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);

  matrix.begin();

  //motors.flipLeftMotor(true);
  //motors.flipRightMotor(true);

  delay(2000);

  allocator = rcl_get_default_allocator();

   //create init_options
  Serial.println("Initializing...");
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // create node
  Serial.println("Creating node...");
  RCCHECK(rclc_node_init_default(&node, "zumo_r4", "zumo", &support));

  // create subscriber
  Serial.println("Initializing subscriber...");
  RCCHECK(rclc_subscription_init_default(
    &twist_subscriber,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
    "cmd_vel"));

  // create executor
  Serial.println("Creating executor...");
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_subscription(&executor, &twist_subscriber, &twist_msg, &twist_subscription_callback, ON_NEW_DATA));
  Serial.println("Setup finished without errors!");
  matrix.loadFrame(LEDMATRIX_EMOJI_HAPPY);
}

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

```

```c:secrets.h
#define WIFI_SSID "<Your wifi ssid>"
#define WIFI_PASS "<Your wifi password>"
#define HOST_IP "<Your host ip>"
#define HOST_PORT 8888
```

試してみましょう！
コードをアップロードしたら、micro_ros_agentを開始します。

```shell
docker run -it --net=host microros/micro-ros-agent:humble udp4 -p 8888
```

こうになると、接続が成功していることになります。
![image_2024-08-14_212533920.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3861095/5e0f812a-61c0-f099-1f42-b794ba90ec59.png)

![image_2024-08-14_212629128.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3861095/d62707b2-642d-998d-b8a8-d183a83c1990.png)

エラーが発生したら、特に`rclc_support_init`のステージで、ネットワーク設定をしっかり確認してください。

ロボットのノードがグラフ上にあるかどうか確認してみましょう。
```shell
ros2 node list
ros2 topic list
```
(ROS2をsourceすることを忘れないように)

![image_2024-08-14_213227318.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3861095/ba0ca570-9fef-740a-3f40-8edfef1b334a.png)

![image_2024-08-14_214028506.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3861095/c3b2e1b3-7f0f-1f51-cf5d-0b4e25616bc4.png)


よし！では、`teleop_twist_keyboad`を起動しましょう。

```shell
ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args --remap cmd_vel:=zumo/cmd_vel
```
こうすればキーボードを使ってロボットを操縦できるようになります。

## 結果
<iframe width="560" height="315" src="https://www.youtube.com/embed/gexEFLRaeIY?si=9b12sIyniPU-MICD" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" referrerpolicy="strict-origin-when-cross-origin" allowfullscreen></iframe>
なかなか楽しかったです！もっと機能を追加したいですが、今のところは時間切れです。今後更に取り組むかもしれません（が、指切りげんまんしない！）

## まとめる
このように、いろいろなArduino UNO R3用プロジェクトはUNO R4を使用して、Micro RosやROS2と統合できます。読んでくれてありがとございます！