海洋ロボコンをやっている人です。
今回は、Jetsonに有線接続したM5Stackを使い、micro-ROS-arduino経由でノード間通信を行う手順を備忘録としてメモしていきます。

この記事が皆様のお力添えになれば幸いです。

1. micro-ROS-arduino: JetsonとM5Stackで通信する

この記事を読むことで

のようにmicro-ROS-arduino経由でJetsonとM5Stackの通信を行えるようになります。

今回は、Arduino IDEのライブラリであるmicro_ros_arduinoを使用していますが、firmwareをビルドして開発環境を作る手法もあります。（試したことにて、トライ&エラーのログも記載しておきますが、私は断念しました。）

※ 2023/06/17　
ROS 2 Humbleでmicro-ros-arduinoのROS_DOMAIN_IDを設定する方法を追記しました。

1.1 Jetsonにmicro-ROS環境を準備する

まずはmicro-ros-arduinoを使用するための、環境構築を行っていきます。

開発環境・使用機器は以下です。

Jetson Nano B01 Ubuntu20.04 Foxy
M5stack gray

Arduino IDEのインストールと環境設定

最初に下記を参考に、Jetson側へArduino IDEをインストールしていきます。

私がインストールしたときのバージョンは1.8.19でした。

micro_rosのインストール

続いて、下記記事を参考に

esp32ボードに追加
micro_ros_arduinoライブラリのインストール
micro_ros_setupによるmicro_ros_Agentのビルド

を行います。

micro_ros_setupについては、

のBuilding、Building micro-ROS-Agent通りに行えば問題ありません。

また、micro_ros_arduinoを使用するため、Creating micro-ROS firmwareからFlashing micro-ROS firmwareの手順は省くことができます。

M5Stackの環境構築

Sketch > Include Library > Manage Libraryから「M5Stack by M5stack」を追加

Tools > Boards Managerから「esp32 by Espressif Systems」、「M5Stack by M5stack official」を追加

File > Preferences > Additional boards manager URLsから以下を追加しておくと便利

Additional boards manager URLs

https://dl.espressif.com/dl/package_esp32_index.json
https://m5stack.oss-cn-shenzhen.aliyuncs.com/resource/arduino/package_m5stack_index.json
https://raw.githubusercontent.com/ROBOTIS-GIT/OpenRB-150/master/package_openrb_index.json
https://raw.githubusercontent.com/ROBOTIS-GIT/OpenCR/master/arduino/opencr_release/package_opencr_index.json

1.2 JetsonとM5stack Grayでノード間通信する

Jetson と M5Stackでノード間通信する際に使用するプログラムは以下の「micro_ros_packages
/uros_foxy_subscriber/uros_foxy_subscriber.ino」になります。

その他のプログラムも是非参考にしてください。

M5Stackへmicro-rosプログラムを書き込む

上記でmicro_rosの環境構築が終わったら、M5stack側にプログラムを記述していきます。

micro_ros_subscriber.ino

#include <micro_ros_arduino.h>

#include <M5Stack.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>

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;
rcl_timer_t timer;

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn;}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}


void subscription_callback(const void * msgin)
{  
  // TODO M5stack
  // M5.Lcd.printf("micro ROS2 Data:%d \n",msg.data);

  if (msg.data == 0){
    M5.Lcd.drawJpgFile(SD, "/img/green_status.jpg");  
  }
  else if (msg.data == 1){
    M5.Lcd.drawJpgFile(SD, "/img/yellow_status.jpg");
  }
  else if (msg.data == 2){
    M5.Lcd.drawJpgFile(SD, "/img/blue_status.jpg");
  }

  else if (msg.data == 3){
    M5.Lcd.drawJpgFile(SD, "/img/red_status.jpg");
  }

}

void setup() {
  // TODO M5stack
  M5.begin();
  M5.Lcd.setTextSize(2);
  M5.Lcd.print("micro ROS2 M5Stack START\n");

  set_microros_transports();
  
  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_node", "", &support));

  // create subscriber
  RCCHECK(rclc_subscription_init_default(
    &subscriber,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "/micro_ros_arduino_data"));

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));
}

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

Jetson側で適当なpubノードを作成する

同様に、Jetsonのuros_ws/src下に適当なROSパッケージを作ります。
ここではint32_pubという0~3の整数をpublishするノードとなっています。

int32pub_cpp_node.cpp

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"

using namespace std::chrono_literals;


class Int32Pub : public rclcpp::Node
{
private:
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pubInt_;
    size_t count_;
    size_t cnt_loop_;
    void TimerCallback();

public:
    Int32Pub()
      : Node("int32pub_node"), count_(0), cnt_loop_(0)
    {
      pubInt_ = this->create_publisher<std_msgs::msg::Int32>("/micro_ros_arduino_data",10);
      timer_ = this->create_wall_timer(
      500ms, std::bind(&Int32Pub::TimerCallback, this));
    }

};

void Int32Pub::TimerCallback()
{
      auto msg = std_msgs::msg::Int32();
      cnt_loop_++;
      if (cnt_loop_ >= 5){
        msg.data = count_++;
        cnt_loop_ = 0;
        if (msg.data >= 3){
          count_ = 0;
        }
      }
      else{
        msg.data = count_;
      }
      RCLCPP_INFO(this->get_logger(), "Publishing: '%d'", msg.data);
      pubInt_->publish(msg);
}

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<Int32Pub>());
  rclcpp::shutdown();
  return 0;
}

論よりrun

プログラムを記述できたら、実際に実行し確認してみましょう。

Fisrt shell

cd uros_ws/
. install/setup.bash
ros2 run int32_pub int32pub_cpp

Second shell

cd uros_ws/
. install/setup.bash
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyUSB0 -v6

このように通信ができればOKです。

ノードを確認してみると、こんな感じです。

1.3 オプション：micro-ROSのROS-DOMAIN-IDを指定する(Foxy)

表題について結論から言うと、以下に書いてあることをArduino IDE側のプログラムで設定すれば良いです。

プログラムの差分も示します。

micro_ros_subscriber.ino

#include <micro_ros_arduino.h>

#include <M5Stack.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>

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;
+rcl_node_options_t node_ops;
rcl_timer_t timer;

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn;}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}


void subscription_callback(const void * msgin)
{  
  // TODO M5stack
  // M5.Lcd.printf("micro ROS2 Data:%d \n",msg.data);

  if (msg.data == 0){
    M5.Lcd.drawJpgFile(SD, "/img/green_status.jpg");  
  }
  else if (msg.data == 1){
    M5.Lcd.drawJpgFile(SD, "/img/yellow_status.jpg");
  }
  else if (msg.data == 2){
    M5.Lcd.drawJpgFile(SD, "/img/blue_status.jpg");
  }

  else if (msg.data == 3){
    M5.Lcd.drawJpgFile(SD, "/img/red_status.jpg");
  }

}

void setup() {
  // TODO M5stack
  M5.begin();
  M5.Lcd.setTextSize(2);
  M5.Lcd.print("micro ROS2 M5Stack START\n");

  set_microros_transports();
  
  allocator = rcl_get_default_allocator();

+  // ROS_DOMAIN_ID
+  node_ops = rcl_node_get_default_options();
+  node_ops.domain_id = 89;

  //create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // create node
-  // RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_node", "", &support));
+  RCCHECK(rclc_node_init_with_options(&node, "micro_ros_arduino_node", "", &support, &node_ops));

  // create subscriber
  RCCHECK(rclc_subscription_init_default(
    &subscriber,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "/micro_ros_arduino_data"));

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));
}

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

ただ、こちらのROS_DOMAIN_IDの設定方法はROS2 Foxyで使える設定であり、Humbleでは上記の記述ではIDが設定できません。

上記のHumbleブランチを見ると、node_options.h内にdomain_idの記述が消えており、呼び出せないようです。

HumbleでのID指定方法は、以下のように指定する必要があります。

1.4 オプション：micro-ROSのROS-DOMAIN-IDを指定する(Humble)

以下を参考に、ROS_DOMAIN_IDの指定方法を変えていきます。

プログラムの差分も示します。

micro_ros_subscriber.ino

#include <micro_ros_arduino.h>

#include <M5Stack.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>

rcl_publisher_t publisher;
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;
rcl_timer_t timer;
rcl_node_options_t node_ops; // Foxy
+rcl_init_options_t init_options; // Humble
+size_t domain_id = 89;


#define RCCHECK(fn) { rcl_ret_t temp_rc = fn;}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}


void subscription_callback(const void * msgin)
{  
  // TODO M5stack
  // M5.Lcd.printf("micro ROS2 Data:%d \n",msg.data);

  if (msg.data == 0){
    M5.Lcd.drawJpgFile(SD, "/img/green_status.jpg");  
  }
  else if (msg.data == 1){
    M5.Lcd.drawJpgFile(SD, "/img/yellow_status.jpg");
  }
  else if (msg.data == 2){
    M5.Lcd.drawJpgFile(SD, "/img/blue_status.jpg");
  }

  else if (msg.data == 3){
    M5.Lcd.drawJpgFile(SD, "/img/red_status.jpg");
  }

}

void setup() {
  // TODO M5stack
  M5.begin();
  M5.Lcd.setTextSize(2);
  M5.Lcd.print("micro ROS2 M5Stack START\n");

  set_microros_transports();

  allocator = rcl_get_default_allocator();

+ /* Humble */
+ // create init_options
+ init_options = rcl_get_zero_initialized_init_options();
+ RCCHECK(rcl_init_options_init(&init_options, allocator)); // <--- This was missing on ur side
+ // Set ROS domain id
+ RCCHECK(rcl_init_options_set_domain_id(&init_options, domain_id));
+ // Setup support structure.
+ RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator));
+ // create node
+ RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_node", "", &support));

  // create subscriber
  RCCHECK(rclc_subscription_init_default(
    &subscriber,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "/micro_ros_arduino_data"));

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));
}

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

1.5 トラブルシューティング

シリアルポートに繋がらないとき、Arudion IDEのシリアルポートがオープンになっていないこと / 権限付与されていることを確認してください。

sudo chmod 666 /dev/ttyACM0

上述の状態で、ros2 runコマンドを実行するとアクセス権を取得できずにエラーになります。

その他、試したこと

他に試したこととして、メモ書き程度でトライ&エラーのログを残しておきます。

having trouble in installing ESP-IDF tools (jetson nano, esp32)　によると、ESP-IDFのインストールとmicro-ROS component for ESP-IDFを使用すれば、Arduino IDEを使用せずとも開発環境を作れるようです。

ESP-IDFをインストールするために、ESP-IDF GEt Startedを見てみると、インストール方法が

IDE
Manual Installation

とありマニュアルインストールでもできるかも！と思いトライ&エラーしてみました。
結論は、撃沈しましたが...

また、ESP-IDF開発環境を試みた際の使用機器は以下です。

Jetson Nano B01 Ubuntu20.04 Foxy
M5stack gray

注意点として

となっているので、Jetson Nanoの場合はUbuntu20.04にアップデートするか、以下のイメージファイルを使用する必要があります。

ESP32 Toolchainの準備

JetsonやRasperryPi等でESP32 micro-ROSのファームウェアを作成しようとすると、以下のようなエラーが発生します。

Installing ESP-IDF tools
Installing tools : xtensa-esp32-elf, xtensa-esp32s2-elf, esp32ulp-elf, esp32s2ulp-elf, openocd-esp32
ERROR : tool xtensa-esp32-elf does not have versions compatible with platform linux-arm64

Set up of Toolchain for Linux

ESP32 - PI の開発環境

RaspberryPi4上にESP32用のmicro-ROS開発環境を作る

http://isl.gforge.inria.fr が落ちててESP32のToolchainがビルドできない問題　解決法

上記をまとめると、以下のコマンドを叩いていけば準備ができるそうなので確認してみる...

必要なパッケのインストール

sudo apt-get install git wget libncurses-dev flex bison gperf python3 python3-pip python3-setuptools python3-serial python3-cryptography python3-future python3-pyparsing python3-pyelftools cmake ninja-build ccache libffi-dev libssl-dev dfu-util libusb-1.0-0
sudo apt-get install gawk gperf grep gettext python python-dev automake bison flex texinfo help2man libtool libtool-bin make

Toolchainの準備

mkdir -p ~/esp
cd ~/esp

git clone https://github.com/espressif/crosstool-NG.git
cd crosstool-NG
+git checkout esp-2022r1
-git checkout esp-2020r2
git submodule update --init
./bootstrap && ./configure --enable-local && make
./ct-ng xtensa-esp32-elf

+unset LD_LIBRARY_PATH

./ct-ng build
chmod -R u+w builds/xtensa-esp32-elf
cp -r ~/esp/crosstool-NG/builds/xtensa-esp32-elf ~/esp/xtensa-esp32-elf
export PATH="$HOME/esp/xtensa-esp32-elf/bin:$PATH"

git checkoutをesp-2021r2以降にすることで、crosstool-NG/.configのCT_EXPAT_VERSION="2.4.1"がデフォルトバージョンとなります。

またgit checkout esp-2021r2以前では.configのCT_ISL_MIRRORSがCT_ISL_MIRRORS="http://isl.gforge.inria.fr"となっていますが、esp-2022r1ではCT_ISL_MIRRORS="http://libisl.sourceforge.io"へ変更されています。

esp-2021r2でビルドすると途中までは進みますが、途中で以下のようなエラーとなり終了します。

[ERROR]    make[6]: *** [Makefile:580: install-toolexeclibLTLIBRARIES] Error
[ERROR]    make[5]: *** [Makefile:784: install-am] Error 2
[ERROR]    make[4]: *** [Makefile:643: install-recursive] Error 1
[ERROR]    make[3]: *** [Makefile:512: install-recursive] Error 1
[ERROR]    make[2]: *** [Makefile:10960: install-target-libstdc++-v3] Error 2
[ERROR]    make[1]: *** [Makefile:2280: install] Error 2
[ERROR]  | 
[ERROR]  >>
[ERROR]  >>  Build failed in step 'Installing cross-gdb'
[ERROR]  >>        called in step '(top-level)'
[ERROR]  >>
[ERROR]  >>  Error happened in: CT_DoExecLog[scripts/functions@376]
[ERROR]  >>        called from: do_gdb_backend[scripts/build/debug/300-gdb.sh@281]
[ERROR]  >>        called from: do_debug_gdb_build[scripts/build/debug/300-gdb.sh@59]
[ERROR]  >>        called from: do_debug[scripts/build/debug.sh@35]
[ERROR]  >>        called from: main[scripts/crosstool-NG.sh@695]

あまりここで時間を使用したくなかったので、心残りではありますが、断念しました。

また、firmwareをビルドしてESP32をmicro_ROSで通信するチュートリアルを公開している方もいたので、こちらも勝手に記載させていただきます。

ROS2 connect ESP32 Basic Install | micro-ROS

Raspberry Pi やjetsonでfirmwareをビルドし、ESP32をmicro-rosで通信できている方がいましたら、是非ご教授いただければ幸いです。

以上。

Discussion