ロボット制御 #16 Arduino UNO R4 Wifi と Raspberry Pi 5 をROS2で繋いでみる

今回は、「Arduino UNO R4 Wifi」と「Raspberry Pi 5」をROS2 Jazzy を使って繋いでみたいと思います。ArduinoでROS2を動かすためのライブラリは幾つかあるようですが、残念ながら公式ライブラリと思われる「ros2arduino」は、gitリポジトリを確認する限り4年前くらいでメンテナンスがストップしており、コンパイルエラーが発生して動かすことが出来ないので断念して、「micro-ROS」を使用します。

ここでの前提となる「Arduino UNO R4 Wifi」のセットアップは、以下の記事を参考にしてください。

ロボット制御⑩ Arduino UNO R4 Wifi を使ってみる

Arduino UNO R4 Wifi を購入したので、使ってみることにしました。いつものArduino IDEではなく、Arduino Cloudを使って、LEDマトリックスであそんだり、WIFI機能を使ってシンプルなWebサーバを立てた...

「Raspberry Pi 5」のセットアップは、以下の記事を参考にしてください。

ロボット制御⑨ Raspberry Pi 5 を使ってみる

昨年の秋ごろに発売された「Raspberry Pi 5」ですが、日本の技適マーク対応を待ってスイッチサイエンスから購入しました。この記事を書いているのが8月なので、随分と時間が経ってしまいました。実は、あまり先頭を走らないタイプなのです。 ...

「Raspberry Pi 5」に「ROS2 Jazzy Jalisco」をセットアップするのは、以下の記事を参考にしてください。

ロボット制御 #12 ROS2を使ってみる(RasPi5編)

WindowsにDockerを使ってROS2 Jazzyを入れましたが、本命はこちらです。結局、他にも利用しているWindowsパソコンだとロボット制御専用にはできないので、実際のプロジェクトは、Raspberry Pi 5の方で進めること...

micro-ROS-arduino のセットアップ
1. Arduino 側のセットアップ
2. Raspberry Pi 側のセットアップ
micro-ROS のデモを動かしてみる
Arduino UNO R4 Wifi でサーボモータを動かしてみる

micro-ROS-arduino のセットアップ

Arduino 側のセットアップ

まずは、「micro-ROS/micro_ros_arduino」をダウンロードします。

GitHub - micro-ROS/micro_ros_arduino: micro-ROS library for Arduino

micro-ROS library for Arduino. Contribute to micro-ROS/micro_ros_arduino development by creating an account on GitHub.

右側の [<> Code]ボタンを押して表示されるメニューの中から「Download ZIP」を選択します。
適当なところに保存して、後で「Arduino IDE」にインポートします。

最新の「Jazzy」を使おうと思うので、「micro_ros_arduino-jazzy.zip」をダウンロードしました。

「Arduino IDE」を起動してメニューの「スケッチ」→「ライブラリをインクルード」→「.ZIP形式のライブラリをインストール…」を選びます。
ファイル選択ウィンドウで先ほどダウンロードしたライブラリを選択して「開く」ボタンを押します。

micro_ros_arduino は、まだ「Arduino UNO R4」に対応していないため、ダウンロードしたライブラリのコードを一部、変更しないといけません。

変更するファイルは、/Arduino/libraries/micro_ros_arduino/src/default_transport.cpp の7行目の「#include <sys/time.h>」を「#include <time.h>」に変更します。

変更前

#include <Arduino.h>

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

  int clock_gettime(clockid_t unused, struct timespec *tp) __attribute__ ((weak));

変更後

#include <Arduino.h>

extern "C"
{
  #include <stdio.h>
  #include <stdbool.h>
  #include <time.h>

  int clock_gettime(clockid_t unused, struct timespec *tp) __attribute__ ((weak));

Raspberry Pi 側のセットアップ

事前に「ROS2 Jazzy」の開発・実行環境は構築済です。

ロボット制御 #12 ROS2を使ってみる(RasPi5編)

自分の環境は、~/ros2_ws の下に幾つかのワークスペースを分けて作っています。

$ tree -L 1 ~/ros2_ws/
ros2_ws/
├─ examples
└─ tutorials

3 directories, 0 files

micro-ros 用のワークスペースを新たに作ります。
ソースディレクトリに移動して、「git clone」を実行し、micro-ROSセットアップパッケージをダウンロードします。

$ mkdir -p ~/ros2_ws/micro-ros/src
$ cd ~/ros2_ws/micro-ros/src
$ git clone -b jazzy https://github.com/micro-ROS/micro_ros_setup.git

ダウンロードが完了したら１つ上のディレクトリに移動して、依存関係の確認を行います。

$ cd ..
$ rosdep install -i --from-path src --rosdistro jazzy -y

ワークスペースのビルドを行います。

$ colcon build

アンダーレイとしてROS インストール環境設定は、起動スクリプトとしてソースしているので、オーバーレイとして、micro-ros セットアップ環境設定をソースします。

$ cd ~/ros2_ws/micro-ros/
$ source install/local_setup.bash

最後に micro-ROSエージェントのセットアップを実行して完了です。

$ ros2 run micro_ros_setup create_agent_ws.sh
$ ros2 run micro_ros_setup build_agent.sh
$ source ~/ros2_ws/micro-ros/install/setup.bash

micro-ROS のデモを動かしてみる

現時点で「micro-ROS」は、まだ「Arduino UNO R4」ボードに対応していないためコードの一部を修正しました。ただし、GitHubのpullリクエストは出ているようなので対象にノミネートされる可能性はあるのではないかと思います。

fixed time.h import, added Arduino UNO R4 minima (USB), Arduino UNO R4 WiFi (USB and WiFi), Arduino Giga R1 (USB and WiFi) by gbr1 · Pull Request #1712 · micro-ROS/micro_ros_arduino

By importing time.h instead of sys/time.h you can build micro-ros for Arduino UNO R4. It addresses issue: #1461 This PR ...

ライブラリのコードの修正

【修正点①】time.hの修正

「/Arduino/libraries/micro_ros_arduino/src/default_transport.cpp」の7行目の「#include <sys/time.h>」を「#include <time.h>」に変更します。

変更前：「default_transport.cpp」

#include <Arduino.h>

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

  int clock_gettime(clockid_t unused, struct timespec *tp) __attribute__ ((weak));

変更前：「default_transport.cpp」
7行目変更

#include <Arduino.h>

extern "C"
{
  #include <stdio.h>
  #include <stdbool.h>
  #include <time.h>

  int clock_gettime(clockid_t unused, struct timespec *tp) __attribute__ ((weak));

【修正点②】対応ボードの追加

「/Arduino/libraries/micro_ros_arduino/src/micro_ros_arduino.h」と「wifi_transport.cpp」に「defined(ARDUINO_UNOR4_WIFI)」および「#include <WiFiS3.h>」の追加と、「uint」を「uint32_t」に変更します。

変更前：「micro_ros_arduino.h」


#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

extern "C" bool arduino_wifi_transport_open(struct uxrCustomTransport * transport);
extern "C" bool arduino_wifi_transport_close(struct uxrCustomTransport * transport);
extern "C" size_t arduino_wifi_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err);
extern "C" size_t arduino_wifi_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err);
#ifndef TARGET_PORTENTA_H7_M7
struct micro_ros_agent_locator {
	IPAddress address;
	int port;
};
#endif

static inline void set_microros_wifi_transports(char * ssid, char * pass, char * agent_ip, uint agent_port){

変更後：「micro_ros_arduino.h」
98行目変更、109-110行目追加、124行目変更


#if defined(ESP32) || defined(TARGET_PORTENTA_H7_M7) || defined(ARDUINO_NANO_RP2040_CONNECT) || defined(ARDUINO_WIO_TERMINAL) || defined(ARDUINO_UNOR4_WIFI)

#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

extern "C" bool arduino_wifi_transport_open(struct uxrCustomTransport * transport);
extern "C" bool arduino_wifi_transport_close(struct uxrCustomTransport * transport);
extern "C" size_t arduino_wifi_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err);
extern "C" size_t arduino_wifi_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err);
#ifndef TARGET_PORTENTA_H7_M7
struct micro_ros_agent_locator {
	IPAddress address;
	int port;
};
#endif

static inline void set_microros_wifi_transports(char * ssid, char * pass, char * agent_ip, uint32_t agent_port){

変更前：「wifi_transport.cpp」

#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

変更後：「wifi_transport.cpp」
1行目変更、14行目-15行目追加

#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

修正点④ 対応CPUの追加

「/Arduino/libraries/micro_ros_arduino/library.properties」に「renesas_uno」を追加します。

変更前：「library.properties」

url=https://github.com/micro-ROS/micro_ros_arduino
precompiled=true
category=Other
architectures=stm32,OpenCR,Teensyduino,samd,sam,mbed,esp32,mbed_portenta

Java

変更前：「library.properties」
10行目の変更

url=https://github.com/micro-ROS/micro_ros_arduino
precompiled=true
category=Other
architectures=stm32,OpenCR,Teensyduino,samd,sam,mbed,esp32,mbed_portenta,renesas_uno

Java

以上で「Arduino UNO R4 Wifi」ボード対応のコード修正は、終了です。

サンプルコードを動かしてみる

サンプルに「micro-ros_publisher_wifi」があるので、これを動かしてみようと思います。
各サンプルでもボード宣言が先頭にあるので、コードの修正が必要になります。

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

#include "arduino_secrets.h"    // add

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

C++

14行目の最後に「&& !define(ARDUINO_UNOR4_WIFI)」を追加して、エラーが出ないように変更しました。

また、ネットワークに接続するためのSSIDやパスワード、エージェントのIPアドレス、ポートをインクルードファイルを作成して、そちらで定義します。

「aruduino_secrets.h」：

// arduino_secrets.h header file
#define SECRET_SSID "ssid"
#define SECRET_PASS "password"
#define AGENT_IP "XXX.XXX.XXX.XXX"
#define AGENT_PORT 8888

C++

なお、エージェントを動かす Raspberry Pi 5 のIPアドレスを調べるのは、以下のコマンドを実行します。3つ目のアダプタ「wlan0」のところにIPアドレスが書いてあります。

$ ip addr

1: lo: <LOOPBACK,UP,LOWER_UP> mtu 65536 qdisc noqueue state UNKNOWN group default qlen 1000
    link/loopback 00:00:00:00:00:00 brd 00:00:00:00:00:00
    inet 127.0.0.1/8 scope host lo
       valid_lft forever preferred_lft forever
    inet6 ::1/128 scope host noprefixroute 
       valid_lft forever preferred_lft forever
2: eth0: <NO-CARRIER,BROADCAST,MULTICAST,UP> mtu 1500 qdisc pfifo_fast state DOWN group default qlen 1000
    link/ether 2c:cf:67:17:e4:9a brd ff:ff:ff:ff:ff:ff
3: wlan0: <BROADCAST,MULTICAST,UP,LOWER_UP> mtu 1500 qdisc pfifo_fast state UP group default qlen 1000
    link/ether 2c:cf:67:17:e4:9b brd ff:ff:ff:ff:ff:ff
    inet 192.168.0.84/24 brd 192.168.0.255 scope global dynamic noprefixroute wlan0
       valid_lft 6093sec preferred_lft 6093sec
    inet6 fe80::9803:524a:65a2:eea0/64 scope link noprefixroute 
       valid_lft forever preferred_lft forever

「micro-ros_publisher_wifi.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 <std_msgs/msg/int32.h>

#include "arduino_secrets.h"    // add

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

// add ssid, pass, ipad, port start
char ssid[] = SECRET_SSID;    // your network ssid (name)
char pass[] = SECRET_PASS;    // your network password
char ip[] = AGENT_IP;         // micro_ros_agent ip address
uint32_t port =  AGENT_PORT;  // micro_ros_agent port number
// add ssid, pass, ipad, port end

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


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

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    msg.data++;
  }
}

void setup() {
  // change network settings start
  // set_microros_wifi_transports("WIFI SSID", "WIFI PASS", "192.168.1.57", 8888);
  set_microros_wifi_transports(ssid, pass, ip, port);
  // change network settings end

  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_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() {
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    msg.data++;
}

C++

Expand

プログラムは完成ですが、プログラムを Arduino UNO R4 Wifi にアップロードする前に Raspberry Pi 5 側でエージェントを起動しておきます。

$ cd ~/ros2_ws/micro-ros/
$ source install/setup.bash
$ ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888 -v6

続いて Arduino UNO R4 Wifi にプログラムをアップロードします。

プログラムをアップロードすると Raspberry Pi 5 のターミナルのコンソールが凄い速さで流れていきます。

サンプルプログラムは、無事実行できました。

Arduino UNO R4 Wifi が対応ボードになっていない理由は何か

ライブラリのコードを修正しないと「micro_ros_arduino」は、Arduino UNO R4 Wifi に書き込むことが出来ませんでした。アップロードしている時に少し思ったのは、RAM領域が結構使用されており、簡単なサンプルプログラムを動かすだけでも圧迫しているように感じました。

最大262144倍とのフラッシュメモリのうち、スケッチが103612バイト（39%）を使っています。
最大32768バイトのRAMのうち、グローバル変数が22404バイト（68%）を使っていて、ローカル変数で10364バイト使うことができます。

ROS2のプログラムは、C++で記述されており、実行時にクラスインスタンスのロードによりメモリをかなり使う印象があります。このため複雑なROS2のインターフェースやサービスなどを実行するとRAM領域が枯渇してしまうのかもしれませんね。

そこで元々対応しているボードを調べてみます。

Portenta H7

Arduino Portenta H7 は、Nanoサイズのマイコンボードですが、２つのMCUを搭載しており、組み込みコンピュータの性能を備えているプロ仕様のボードです。ただし、値段は、日本円で2万円近くします。

項目	仕様
マイクロコントローラー	STM32H747XI デュアル Cortex®-M7+M4 32ビット低消費電力 Arm® MCU
無線モジュール	Murata 1DX デュアル Wifi 802.11b/g/n 65 Mbps および Bluetooth® (Cordio スタック経由の Bluetooth® Low Energy.5、Arduino スタック経由の Bluetooth® Low Energy 4.2)
メモリ	8 MB SDRAM 16 MB QSPIフラッシュ
セキュアエレメント（デフォルト）	NXP SE0502
ボード電源 (USB/VIN)	5V
対応バッテリー	Li-Po 1S 3.7V、700mAh 最小（内臓充電器）
回路動作電圧	3.3V
ディスプレイコネクタ	MIPI DSI ホストと MIPI D-PHY でピン数の少ない大型ディスプレイとインターフェース
グラフィックプロセッサー	Chrom-ART グラフィカルハードウェアアクセラレータ™
タイマー	22倍のタイマーとウォッチドッグ
シリアル	4x ポート (2つはフロー制御付き）
イーサネットPHY	10 / 100 Mbps (拡張ポート経由のみ）
SDカード	SDカードコネクタ用インターフェース（拡張ポート経由のみ）
動作温度	-40℃～+85℃
MKR ヘッダー	既存の工業用MKRシールドのいずれかを使用します。
高密度コネクタ	2つの80ピンコネクタにより、ボードのすべての周辺機器を他のデバイスに公開できます。
カメラインターフェース	8 ビット、最大 80 MHz
アドバンスト	最大 16 ビットの分解能を持つ 3 つの ADC (最大 36 チャネル、最大 3.6 MSPS)
DAC	2×12ビットDAC（1MHz）が利用可能で、そのうち1つのみが外部A6ピンを介してユーザーにアクセスできます。
USB-C	ホスト/デバイス、DisplayPort出力、高速/フルスピード、電力供給

これはハイスペックなマイコンボードです。
8 MBのSDRAM と 16 MB QSPI フラッシュなのでメモリの問題はなさそうです。

ピンはこんなに出ていないので、基本は、以下のような拡張ボードに差し込んで使うのではないかと思います。

Arduino Nano RP2040 Connect

こちらは、まだホビーユースのうちに入るのではないかと思います。公式サイトで5千円弱の値段です。
Raspberry Pi の RP2040 マイコンチップを搭載しています。

項目	仕様
マイクロコントローラ	Raspberry Pi® RP2040
USBコネクタ	マイクロ USB
内臓LEDピン	13
デジタルI/Oピン	20
アナログ入力ピン	8
PWMピン	20 （A6、A7 を除く）
外部割込み	20 （A6、A7 を除く）
Wi-Fi	Nina W102 uBlox モジュール
Blootooth®	Nina W102 uBlox モジュール
セキュアエレメント	ATECC608A-MAHDA-T 暗号 IC
IMUセンサ	LSM6DSOXTR （6軸）
マイクロフォン	MP3DT06JTR
コミュニケーション	シリアル I2C SPI
電源	回路動作電圧: 3.3V 入力電圧(VIN): 5 – 21V IOピンあたりのDC電流: 4mA
クロック速度	プロセッサ　133MHz
メモリ	AT25SF128A-MHB-T: 16MB フラッシュ IC Nina W102 uBlox モジュール: 448KB ROM、520 KB SRAM、16MB フラッシュ
寸法	重さ: 6g 寸法: 18mm x 25mm

スペックと価格は申し分ないのですが、公式サイトでしか手に入らないようで、ハードルが少し高い感じです。

手頃な値段で実用的なマイコンボードは「Espressif」の「ESP32-WROOM-32」あたりになるのではないかと思います。
「ESP32-WROOM-32」については、別の記事に書きたいと思います。ここでは、とりあえず簡単なコードで試してみたいと思います。

Arduino UNO R4 Wifi でサーボモータを動かしてみる

Raspberry Pi 5 をパブリッシャーに、Arduino UNO R4 Wifi をサブスクライバーにして、Raspberry Pi 5 からの指示（角度）をArduino UNO R4 Wifi がネットワーク経由で受け取り、モーターを動かすというものです。

D3ピンはPWM出力可能なので、D3ピンとサーボのPWM、あとは５V、GNDを繋ぎます。
モーターは、TowerPro の MG90S を使いました。

Arduino のコード

Arduino IDE のスケッチは、以下のようにしました。

「ros2_servo_test.ino」:

// Include the required header files
#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>
#include <Servo.h>

// Includes various network settings
#include "arduino_secrets.h"

// Declare variables for ROS2 subscribers
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;

// Declaring the network settings
char ssid[] = SECRET_SSID;    // your network ssid (name)
char pass[] = SECRET_PASS;    // your network password
char ip[] = AGENT_IP;         // micro_ros_agent ip address
uint32_t port =  AGENT_PORT;  // micro_ros_agent port number

// Declare a variable for the servo motor
Servo myservo;
int deg = 0;

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

// What to do when an error occurs
void error_loop() {
  while(1) {
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

// Subscriber callback function
void subscription_callback(const void * msgin)
{
  const std_msgs__msg__Int32 * msg = (const std_msgs__msg__Int32 *)msgin;
  deg = int(msg->data);
  myservo.write(deg);
  Serial.print("Servo motor is move: "); Serial.println(deg);
}

void setup() {
  Serial.begin(115200);
  set_microros_wifi_transports(ssid, pass, ip, port);
  delay(2000);

  // create node
  allocator = rcl_get_default_allocator();
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
  RCCHECK(rclc_node_init_default(&node, "servo", "", &support));

  // create subscriber
  RCCHECK(rclc_subscription_init_default(
    &subscriber, &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "servo/deg"));
  
  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));

  // Attach D3 (PWM) pin for servo motor
  myservo.attach(3, 500, 2500);
  Serial.println("The servo motor is attached to the d3 pin.");
}

uint32_t cnt = 0;
void loop() {
  delay(10);
  Serial.print("loop connt:"); Serial.println(++cnt);
  RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10)));
}

C++

Expand

コードを解説します。基本的には「micro_ros_arduino」ライブラリに付属しているスケッチ例の「micro-ros_subscriber」を参考にしています。

先頭には、必要なインクルード文を記述しています。

// Include the required header files
#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>
#include <Servo.h>

C++

ここで「micro_ros_arduino」のサブスクライバーとして実行するために必要なインクルード文を書いています。なお。メッセージは標準から用意されている「std_msgs/msg/int32」を使います。
「Arduino/libraries/micro_ros_arduino/src/std_msgs/msg」の中には、他にも標準提供のメッセージが定義されています。
他の「rcl」で始まるインクルード文は、「micro_ros_arduino」を使う場合は、ほぼ定型としてインクルードしています。
最後の「Servo.h」は、サーボモータを動かすためにインクルードしています。

次の1文は、上の「サンプルコードを動かしてみる」でも使ったネットワーク設定を記述したヘッダーファイルをインクルードしています。

// Includes various network settings
#include "arduino_secrets.h"

C++

「arduino_secrets.h」:

// arduino_secrets.h header file
#define SECRET_SSID "ssid"
#define SECRET_PASS "password"
#define AGENT_IP "XXX.XXX.XXX.XXX"
#define AGENT_PORT 8888

C++

ここに実際のSSID、PASSWORD、エージェントのIPアドレス、ポートを設定します。

続いて、ROS2サブスクライバーに必要な変数を定義しています。

// Declare variables for ROS2 subscribers
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;

C++

汎用的なサブスクライバーを変数として「rcl_subscription_t subscriber;」を定義、
メッセージを変数として「std_msgs__msg__Int32 msg;」を定義、
残りは、基本的な定義で「micro_ros_arduino」を使う場合は、定型的に定義しています。

続いて、ネットワーク設定に必要な各種設定を宣言しています。
ここに直接ローカルネットワーク設定を指定しても構いません。

// Declaring the network settings
char ssid[] = SECRET_SSID;    // your network ssid (name)
char pass[] = SECRET_PASS;    // your network password
char ip[] = AGENT_IP;         // micro_ros_agent ip address
uint32_t port =  AGENT_PORT;  // micro_ros_agent port number

C++

続いて、サーボモータ制御用の変数を定義しています。

// Declare a variable for the servo motor
Servo myservo;
int deg = 0;

C++

以下は、サンプルコードをそのまま持ってきています。
エラー判定用のマクロと、エラーが発生した場合には、ビルトインのLEDをチカチカ点滅するようになっています。

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

// What to do when an error occurs
void error_loop() {
  while(1) {
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

C++

次は、サンプルスケッチには無いコードです。
サブスクライバーがメッセージを受け取った時に呼び出される関数です。

// Subscriber callback function
void subscription_callback(const void * msgin)
{
  const std_msgs__msg__Int32 * msg = (const std_msgs__msg__Int32 *)msgin;
  deg = int(msg->data);
  myservo.write(deg);
  Serial.print("Servo motor is move: "); Serial.println(deg);
}

C++

受け取ったメッセージをint型に変換して「deg」に設定しています。
「myservo.write(deg);」でサーボモーターを「deg」°に動かします。その後、シリアルポートにメッセージを送信しています。

次は、セットアップ処理です。

void setup() {
  Serial.begin(115200);
  set_microros_wifi_transports(ssid, pass, ip, port);
  delay(2000);

  // create node
  allocator = rcl_get_default_allocator();
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
  RCCHECK(rclc_node_init_default(&node, "servo", "", &support));

  // create subscriber
  RCCHECK(rclc_subscription_init_default(
    &subscriber, &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "servo/deg"));
  
  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));

  // Attach D3 (PWM) pin for servo motor
  myservo.attach(3, 500, 2500);
  Serial.println("The servo motor is attached to the d3 pin.");
}

C++

「set_microros_wifi_transports(ssid, pass, ip, port);」は、WiFi でメッセージを受信する形態にするためのコードです。
残りのほとんどのコードは、定型名です。その中で62行目の「"servo"」はノード名で、68行目の「"servo/deg"」は、トピック名になります。

75行目の「myservo.attach(3, 500, 2500);」は、サーボモーターをアタッチしています。
最初の引数の「3」はピンの指定で、次の「500」はサーボモーターの最小の「μ秒」、次の「2500」は最大の「μ秒」を指定しています。

uint32_t cnt = 0;
void loop() {
  delay(10);
  Serial.print("loop connt:"); Serial.println(++cnt);
  RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10)));
}

C++

最後のこの部分は、実行処理の登録になります。
「rclc_executor_spin_some」で、サブスクライバーがメッセージ待ちの状況になります。
「RCL_MS_TO_NS(10)」で10ミリ秒間隔で信号をチェックするはずですが、動作確認する限りは、信号のチェックは「10秒毎」でした。
※ここは、別の人の動作確認では、触れられていませんでしたので、使い方が間違っている等の原因があるのかもしれません。（要調査）

Raspberry Pi 5 のエージェント起動

Raspberry Pi 5 側は、以下のコマンドで「ros_micro_agent」を起動します。

$ cd ~/ros2_ws/micro-ros/
$ . install/setup.bash
$ ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888 -v6

実行すると以下のように表示されます。

Arduino UNO R4 Wifi にスケッチをアップロードする

先ほど書いたコードを「Arduino UNO R4 Wifi」にアップロードします。

シリアルモニタには、約10秒毎に「loop()」が呼び出されているようです。

これで、Arduino UNO R4 Wifi でサブスクライバーが起動しました。

Raspberry Pi 5 でパブリッシャーを動かす

Arduino UNO R4 Wifi でサブスクライバーを起動すると「micoro_ros_agent」を実行したターミナルにメッセージが流れます。

最初は、サーボモーターは以下の状態になっています。

以下のコマンドを入力してサーボモーターを動かします。

$ ros2 topic pub --once /servo/deg std_msgs/msg/Int32 "{data: 0}"

これでサーボモーターを0°の位置に動かします。

$ ros2 topic pub --once /servo/deg std_msgs/msg/Int32 "{data: 180}"

続けてサーボモーターを180°の位置に動かします。

最後にサーボモーターを90°の位置に動かして終了します。

$ ros2 topic pub --once /servo/deg std_msgs/msg/Int32 "{data: 90}"

Arduino UNO R4 Wifi でサブスクライバーが動く

Raspberry Pi 5 からのパブリッシュメッセージを受け取って Arduino UNO R4 Wifi は、サーボモータを動かしてみましたが、その際に表示したシリアルモニタのメッセージを確認してみます。

12:02:31.764 -> The servo motor is attached to the d3 pin.
12:02:31.764 -> loop connt:1
12:02:31.810 -> loop connt:2
12:02:31.810 -> loop connt:3
12:02:31.842 -> loop connt:4
12:02:41.860 -> loop connt:5
12:02:51.907 -> loop connt:6
12:03:01.962 -> Servo motor is move: 0
12:03:01.962 -> loop connt:7
12:03:02.007 -> loop connt:8
12:03:02.054 -> loop connt:9
12:03:02.101 -> loop connt:10
12:03:02.147 -> loop connt:11
12:03:02.191 -> loop connt:12
12:03:02.191 -> loop connt:13
12:03:02.224 -> loop connt:14
12:03:12.251 -> loop connt:15
12:03:22.283 -> Servo motor is move: 180
12:03:22.329 -> loop connt:16
12:03:22.374 -> loop connt:17
12:03:22.417 -> loop connt:18
12:03:22.417 -> loop connt:19
12:03:22.500 -> loop connt:20
12:03:22.543 -> loop connt:21
12:03:22.543 -> loop connt:22
12:03:22.578 -> loop connt:23
12:03:32.626 -> loop connt:24
12:03:42.679 -> Servo motor is move: 90
12:03:42.679 -> loop connt:25
12:03:42.725 -> loop connt:26
12:03:42.771 -> loop connt:27
12:03:42.803 -> loop connt:28
12:03:42.850 -> loop connt:29
12:03:42.896 -> loop connt:30
12:03:42.942 -> loop connt:31
12:03:42.942 -> loop connt:32
12:03:52.966 -> loop connt:33

動くには動きましたが、トピックを送信してから動き出しまでに約20秒くらいの間隔が空いてしまいます。とりあえず、Raspberry Pi 5 と Arduino UNO R4 Wifi が ROS2 Jazzy のmicro-ROSを利用して繋がるところまでは出来ました。

次は、もっと本格的に Interface を実装して、四足歩行ロボットの制御をしていきたいと思います。

以上です。

ロボット制御 #16 Arduino UNO R4 Wifi と Raspberry Pi 5 をROS2で繋いでみる
Raspberry Pi 5 と Arduino UNO R4 Wifi を ROS2 で繋いで MicroServo MG90S を動かしてみた話