micro-ros-arduinoライブラリにカスタムメッセージを追加する方法と手順

どうもこんにちは、KRです。
今回はmicro_ros_arduinoライブラリにカスタムメッセージを追加してみたいと思います。

前提条件
環境
カスタムメッセージを作成する準備
カスタムメッセージを作成します。
ビルドの準備
1. CMakeLists.txt
2. package.xml
パッケージのビルド
ライブラリをビルド
サンプルプログラムを改変する

前提条件

ROSがインストールされている。
Dockerがインストールされている。
※上記の条件に該当していても他の原因により動作しない場合があります。

環境

OS : Ubuntu 22.04
ROS Version : humble
Hardware : Raspberry Pi4 , ESP32

カスタムメッセージを作成する準備

上から順にコマンドを適当なコマンドラインで実行してください。

#ライブラリをクローンします。
git clone -b humble https://github.com/micro-ROS/micro_ros_arduino
#カスタムメッセージを作成するフォルダに移動します。
pushd micro_ros_arduino/extras/library_generation/extra_packages
#ROS2を使える状態にします。
source /opt/ros/humble/setup.bash
#パッケージを作成します。mymessageの部分はパッケージ名です。
ros2 pkg create --build-type ament_cmake mymessage
#作成したパッケージに移動、mymessageの部分はパッケージ名です。
pushd mymessage
#ディレクトリを作成します。
mkdir msg
#作成したディレクトリに移動します。
pushd msg

カスタムメッセージを作成します。

.msgファイルを作成し、カスタムメッセージを定義します。
※ファイル名は大文字から始める必要があります。
例:Message等

#RobotPositionの部分はファイル名です。
vim RobotPosition.msg

msgファイルは以下のような感じで書きます。

int64 x
int64 y

詳しくは以下の記事をご覧ください。

msg – ROS Wiki

ビルドの準備

CMakeLists.txt

CMakeLists.txtに以下の文を追加します。
※追加する場所はament_package()という文の上です。

find_package(geometry_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/RobotPosition.msg"
)

依存するパッケージがある場合以下のようになります。

find_package(geometry_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Sphere.msg"
DEPENDENCIES geometry_msgs #上記のメッセージが依存するパッケージを追加します。この場合、Sphere.msgにはgeometry_msgsを使用します。
)

package.xml

< package >の要素内に以下の文を追加します。

<depend>geometry_msgs</depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

パッケージのビルド

以下のコマンドを実行します。

#mymessageの部分はパッケージ名です。
colcon build --packages-select mymessage

ライブラリをビルド

コマンドを上から順に実行してください。

#ワークスペースへ移動
cd ~/micro_ros_arduino
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 -esp32

-esp32という部分はビルドするターゲットを絞るための引数です。
esp32の他にも以下のものがあります。

opencr1 , teensy4 , teensy32 , teensy35 , teensy36 , cortex_m3 , cortex_m0 , cortex_m3 , cortex_m4 , portenta-m4 , portenta-m7 , kakutef7-m7 , esp32

詳細は以下を参照してください。
GitHub – micro-ROS/micro_ros_arduino: micro-ROS library for Arduino

また、次のようなエラーが発生しますが、無視していいようです。

rsync error: some files/attrs were not transferred (see previous errors) (code 23) at main.c(1338) [sender=3.2.7]
rsync: [sender] change_dir "/project/src/tracetools_trace/tracetools_trace" failed: No such file or directory (2)
rsync error: some files/attrs were not transferred (see previous errors) (code 23) at main.c(1338) [sender=3.2.7]
rsync: [sender] change_dir "/project/src/trajectory_msgs/trajectory_msgs" failed: No such file or directory (2)
rsync error: some files/attrs were not transferred (see previous errors) (code 23) at main.c(1338) [sender=3.2.7]
rsync: [sender] change_dir "/project/src/unique_identifier_msgs/unique_identifier_msgs" failed: No such file or directory (2)
rsync error: some files/attrs were not transferred (see previous errors) (code 23) at main.c(1338) [sender=3.2.7]
rsync: [sender] change_dir "/project/src/visualization_msgs/visualization_msgs" failed: No such file or directory (2)
rsync error: some files/attrs were not transferred (see previous errors) (code 23) at main.c(1338) [sender=3.2.7]

詳細は以下のページに記載されています。
Fail to build using micro_ros_static_library_builder docker · Issue #1159 · micro-ROS/micro_ros_arduino · GitHub

ライブラリをビルドすることができたら圧縮してzipファイルにして、ArduinoIDEにインストールしましょう。

サンプルプログラムを改変する

micro-ros-arduinoライブラリのサンプルプログラムのmicro_ros_publisherというプログラムを改変していきます。
変更後のプログラムは次のようになります。

#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 <mymessage\msg\robot_position.h> //変更箇所<[パッケージ名]\msg\[msgファイル名をすべて小文字にした形].h>
rcl_publisher_t publisher;
mymessage__msg__RobotPosition msg;//変更箇所
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;
#define LED_PIN 2
#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.x++;//変更箇所
msg.y++;//変更箇所
}
}
void setup() {
set_microros_transports();
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_node", "", &support));
// create publisher
RCCHECK(rclc_publisher_init_default(
&publisher,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(mymessage, msg, RobotPosition),//変更箇所
"micro_ros_arduino_node_publisher"));
// create timer,
const unsigned int timer_timeout = 1000;
RCCHECK(rclc_timer_init_default(
&timer,
&support,
RCL_MS_TO_NS(timer_timeout),
timer_callback));
// create executor
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
RCCHECK(rclc_executor_add_timer(&executor, &timer));
msg.x = 0;//変更箇所
msg.y = 0;//変更箇所
}
void loop() {
delay(100);
RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}

このソースコードはAPACHE LICENSE, VERSION 2.0で配布された制作物が含まれます。
micro_ros_arduino/examples/micro-ros_publisher/micro-ros_publisher.ino at iron · micro-ROS/micro_ros_arduino · GitHub
より

変更箇所についての説明

変更者:KR

include <mymessage\msg\robot_position.h>

9行目に存在するこのコードは以下のような変更の仕方をしている。

<[パッケージ名]\msg\[msgファイル名をすべて小文字にした形].h>

mymessagemsgRobotPosition msg;

12行目に存在するこのコードは以下のような変更の仕方をしている。
ヘッダーファイルのファイルパスは以下のようなものになっている。

mymessage\msg\robot_position.h

上記のファイルパスの \ の部分を__に置き換え、ヘッダーファイル名をmsgファイル名にしたものの後ろに
msgをつけた形が12行目のコードになる。

mymessage__msg__RobotPosition msg;

簡単に表すと以下のようになる。

[パッケージ名]__msg__[msgファイル名] msg;

msg.x++; , msg.y++;

37、38行目に存在するこのコードはmsgファイルに記載したカスタムメッセージに1足しているコードだ。
これを応用すると以下のような使い方ができる。

msg.x = 100
msg.y = 100

上記のコードではmsgファイルに記載したx,yというカスタムメッセージに100を代入している。

ROSIDL_GET_MSG_TYPE_SUPPORT(mymessage, msg, RobotPosition),

62行目に存在するこのコードは以下のページを参照するとわかりやすいと思われる。

docs.ros.org

msg.x = 0;,msg.y = 0;

先ほどmsg.x++; , msg.y++;の部分で説明した通りである。

ROS2と通信してみる。

ROS2にmicro-ROS-Agentがインストールされていることを確認し、以下のコードを実行する。

#USBでESP32と接続した状態で実行する。
sudo chmod 666 /dev/ttyUSB0
#通信が成功した場合数十行のメッセージが表示される。失敗した場合、USBポートの位置を変えてみるといい。
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyUSB0 -v6
#通信が成功したことを確認し、別のコマンドラインで実行する。
ros2 topic list
ros2 topic echo /micro_ros_arduino_node_publisher

以下の記事を参照すると詳細が理解できます。
[ROS2 foxy] ESP32を使ってmicro-ROS for Arduinoで遊ぼう #ROS – Qiita