LoginSignup
6
5

More than 1 year has passed since last update.

@ousagi_sama(sama ousagi)

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

Last updated at Posted at 2022-07-12

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

6
5
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
6
5