序文

これは僕の初めてのブログです。僕は留学生なので、日本語が完璧ではありません。正しくない文法や変な表現があったら、申し訳ありません。

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

はじめに

このブログはスイッチサイエンスの伊藤さんが書かれたブログを基にしています。

まとめると、次のステップになります:

ROS2(とColcon)をインストールする

これについてはもうたくさんの資料がありますので、あまり書きません。僕の環境ではHumbleのブランチを使います。

Micro Rosをインストールする

伊藤さんはソースからインストールしてビルドしたが、僕はDockerを使う方が簡単で早いと思います。

docker pull microros/base:humble

Arduino IDEをインストールする

ダウンロードはこちらです。ダウンロードしたらBoard ManagerでUno R4もインストールしてください。

micro_ros_arduinoをダウンロードする

こちらからダウンロードして、解凍してArduino/librariesにコピーしてください。

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

micro_ros_arduinoを編集

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

ESP32 S3 WiFi 機能を加わる

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

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が使えない。

src/default_transport.cpp

#include <Arduino.h>

extern "C"
{
  #include <stdio.h>
  #include <stdbool.h>
-  #include <sys/time.h>
+  #include <time.h>
...

必要ではありませんが、例のスケッチも編集したらより便利になります。

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
...

コンパイルしてみる

さっきに編集した例を試してみましょう。
File > Examples > micro_ros_arduino > micro-ros_publisher_wifiを開いて、Verifyしてください。

成功しました！

ここまでは伊藤さんのブログで説明されていることとほぼ同じです。次はZumo Shieldの機能を追加します。

Wire.h と使うこと

PololuのZumoShield.hライブラリはWire.hに依存しています。Wire.hはI2C機能のライブラリーです。Zumo Shieldの場合は、LSM303D加速度計とL3GD20Hジャイロスコープとの通信に使用されます。

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

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

extras/library_generationにあるコンパイラ設定を含むlibrary_generation.shを編集します。

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から引用しました。

まず、ツールを取得します。

docker pull microros/micro_ros_static_library_builder:humble

そして、再コンパイルします。

docker run -it --rm -v $(pwd):/project --env MICROROS_LIBRARY_FOLDER=extras microros/micro_ros_static_library_builder:humble -p cortex_m4

ライブラリの ルートディレクトリ (micro_ros_arduino-2.0.7-humble)で実行してください

Windowsを使っている方は$(pwd):/projectを"<micro_ros_arduinoのディレクトリ>:/project"に変えてください

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

おめでとう！すべてうまくいきました。

ZumoShield.h と使うこと

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

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

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を使ってロボットを操縦することです。システムの概要を見てみましょう。

スケッチはこれです。

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)));
}

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を開始します。

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

こうになると、接続が成功していることになります。

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

ロボットのノードがグラフ上にあるかどうか確認してみましょう。

ros2 node list
ros2 topic list

(ROS2をsourceすることを忘れないように)

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

ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args --remap cmd_vel:=zumo/cmd_vel

こうすればキーボードを使ってロボットを操縦できるようになります。

結果

なかなか楽しかったです！もっと機能を追加したいですが、今のところは時間切れです。今後更に取り組むかもしれません（が、指切りげんまんしない！）

まとめる

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