【micro-ROS for Arduino】簡単なPublisherとSubscriberの書き方

micro-ROS for Arduinoを初めて利用する場合の、PubSubのチュートリアルがほしいところである．

公式にサンプルとチュートリアルが用意されているのだが、少しわかりにくかったので、色々と試した結果をここに残しておく．

前提

ROS2 foxyとmicro-ROSを導入したArduino IDEを準備できていること．

インストール方法は、以前の記事を参照

インクルードとグローバル変数の設定

以下のヘッダファイルをインストールする．

共通インクルード

#include <Arduino.h>
#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 <std_msgs/msg/string.h>

グローバル変数の定義を行う．
ros1のrosserial_arduinoでもそうだが、基本的にROS通信に関係するものは、常にメモリ上に確保される必要がある．
これは、配信・受信問わず、メッセージ型の変数も同様．

グローバル変数の定義

rcl_allocator_t allocator;
rclc_support_t support;
rcl_node_t node;
rclc_executor_t executor;

rcl_publisher_t pub_string;
rcl_subscription_t sub_int32;

std_msgs__msg__Int32 int32_msg;
std_msgs__msg__String string_msg;

setup関数の書き方

起動時に実行されるsetup関数は以下のような構成にすればよい．

setup

void setup(){
	// 通信の初期化
	// Wi-Fi経由の場合
	set_microros_wifi_transports("SSID", "PWD", "IP", Port);
	// USB経由の場合
	// set_microros_transports();

	// 初期化完了までの待機時間
	delay(2000);
	
	// micro-ROSのためのメモリ管理
	allocator = rcl_get_default_allocator();

	// micro-ROSのためのサポートクラス
	rclc_support_init(&support, 0, NULL, &allocator);

	// ノードの生成 (foxy版)
	rcl_node_options_t node_ops = rcl_node_get_default_options();
	node_ops.domain_id = (size_t)(100);		// ドメインIDの設定
	rclc_node_init_with_options(&node, "node_name", "namespace", &support, &node_ops);

	// ノードの生成 (galactic版以降)
	//rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
	//rcl_init_options_init(&init_options, allocator);
	//rcl_init_options_set_domain_id(&init_options, 100);		// ドメインIDの設定
	//rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator); // 前のrclc_support_initは削除する
	//rclc_node_init_default(&node, "node_name", "namespace", &support);

	// Publisher 作成
	// rmw_qos_profile_****を変更することで、qosを変更できる（カスタムも可能）
	rclc_publisher_init(&pub_string, &node, 
						ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String),
						"debug_msg", &rmw_qos_profile_default
	);

	// Subscriber 作成
	rclc_subscription_init(&sub_int32, &node,
							ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
							"test", &rmw_qos_profile_default
	);
	
	// コールバックを管理ためのexecutor
	// Subscriber、Timer、Serviceなどもコールバック関数を設定する
	// Publisherだけなら、以降の処理は必要ない
	int callback_size = 1;	// コールバックを行う数
	executor = rclc_executor_get_zero_initialized_executor();
	rclc_executor_init(&executor, &support.context, callback_size, &allocator);
	rclc_executor_add_subscription(&executor, &sub_int32, &int32_msg, 
								   &callback_int32, ON_NEW_DATA); // callback_int32は関数（後ほど記載）

}

必要に応じて、コメントアウトを外したりすれば、通常のPubSubには利用できるだろう．

Publisherの書き方

基本的には、loop関数内に書けば問題ない．
余談だが、micro-ROS for Arudinoで、文字列を配信するサンプルコードが少なかったので、共有する．

Publisher

void loop() {
	String s = "Hello World!";
    char strBuf[13]; s.toCharArray(strBuf, 13);
	string_msg.data.size = s.length();
	string_msg.data.data = strBuf;

	// 配信
	rcl_publish(&pub_string, &string_msg, NULL);

  	delay(100);
}

Subscriberの書き方

spin関数をloop関数に書き、トピックを受け取ったら実行するコールバック関数に実体を書く．

Subscriber

void loop() {
	// 受信
  	// 第二引数は、Timeout時間を指定する
  	rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));

  	delay(10);
}

void callback_int32(const void *raw_msg){
  	std_msgs__msg__Int32 *msg = (std_msgs__msg__Int32 *)raw_msg;
  	// *msg->dataで値を取得
  	// もしくは、以上の変換を行わなくてもint32_msg.dataからも取得できる
}

サンプル：Stringのエコープログラム

上記のテンプレートに従って、Subscribreで受け取ったメッセージをPublisherで送信する、エコープログラムを作成する．

エコープログラム

#include <Arduino.h>
#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/string.h>

rcl_allocator_t allocator;
rclc_support_t support;
rcl_node_t node;
rclc_executor_t executor;

rcl_publisher_t pub_string;
rcl_subscription_t sub_string;

std_msgs__msg__String send_string_msg;
std_msgs__msg__String recv_string_msg;
#define STR_SIZE (100) //最大の受信文字数

// Callback
void callback_string(const void *raw_msg){
  	std_msgs__msg__String *msg = (std_msgs__msg__String *)raw_msg;

	String s = "echo : " + String(msg->data.data);

	char strBuf[120]; s.toCharArray(strBuf, 120);
	send_string_msg.data.size = s.length();
	send_string_msg.data.data = strBuf;

	// 配信
	rcl_publish(&pub_string, &send_string_msg, NULL);
}

void setup(){
	// 通信の初期化
	// Wi-Fi経由の場合
	set_microros_wifi_transports("SSID", "PWD", "IP", 8888);
	// USB経由の場合
	// set_microros_transports();

	// 初期化完了までの待機時間
	delay(2000);
	
	// micro-ROSのためのメモリ管理
	allocator = rcl_get_default_allocator();

	// micro-ROSのためのサポートクラス
	rclc_support_init(&support, 0, NULL, &allocator);

	// ノードの生成 (foxy版)
	rcl_node_options_t node_ops = rcl_node_get_default_options();
	node_ops.domain_id = (size_t)(100);		// ドメインIDの設定
	rclc_node_init_with_options(&node, "echo_string_node", "", &support, &node_ops);

	// ノードの生成 (galactic版以降)
	//rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
	//rcl_init_options_init(&init_options, allocator);
	//rcl_init_options_set_domain_id(&init_options, 100);		// ドメインIDの設定
	//rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator); // 前のrclc_support_initは削除する
	//rclc_node_init_default(&node, "echo_string_node", "", &support);

	// Publisher 作成
	// rmw_qos_profile_****を変更することで、qosを変更できる（カスタムも可能）
	rclc_publisher_init(&pub_string, &node, 
						ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String),
						"echo", &rmw_qos_profile_default
	);

	// Subscriber 作成
	rclc_subscription_init(&sub_string, &node,
							ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String),
							"recv", &rmw_qos_profile_default
	);
	
	// コールバックを管理ためのexecutor
	// Subscriber、Timer、Serviceなどもコールバック関数を設定する
	// Publisherだけなら、以降の処理は必要ない
	
	int callback_size = 1;	// コールバックを行う数
	executor = rclc_executor_get_zero_initialized_executor();
	rclc_executor_init(&executor, &support.context, callback_size, &allocator);
	rclc_executor_add_subscription(&executor, &sub_string, &recv_string_msg, 
								   &callback_string, ON_NEW_DATA);
	// char型変数を入れる配列を確保する
	recv_string_msg.data.data = (char * )malloc(STR_SIZE * sizeof(char));
	recv_string_msg.data.size = 0;
	recv_string_msg.data.capacity = STR_SIZE;
}

void loop(){
	// 受信
  	rclc_executor_spin_some(&executor, RCL_MS_TO_NS(200));
  	delay(10);
}

String問わず、配列を用いるメッセージ型の場合は、mallocによるメモリの確保を行う必要がある．
もし、メモリ容量が足りない場合は、Subscriberのコールバックが発火しないので注意すること．

書き込み後、micro-ROS-Agentを起動する．

ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888

別々のターミナルに以下を打ち込み、文字が帰ってくれば成功である．

topic recv

ros2 topic pub --once /recv std_msgs/msg/String "{data: 'hello world'}"  

topic ehco

ros2 topic echo /echo

----
data: 'echo : hello world'
---
data: 'echo : hello world'
---
data: 'echo : hello world'
---
...

参考

micro-ROS Demos
https://github.com/micro-ROS/micro-ROS-demos