micro-ROS-arduino: JetsonとM5Stackで通信する
海洋ロボコンをやっている人です。
今回は、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から以下を追加しておくと便利
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側にプログラムを記述していきます。
#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するノードとなっています。
#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側のプログラムで設定すれば良いです。
プログラムの差分も示します。
#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の指定方法を変えていきます。
プログラムの差分も示します。
#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のシリアルポートがオープンになっていないことを確認してください。
上述の状態で、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
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