ROS2 Client Library を作るということ

※ ROS2 Foxy での話です(他のバージョンでも大きくは変わらないと思いますが)

はじめに

ROS2 では ROS から構成が見直され、各言語向けの Client Library に共通する部分を C API として定義した rcl (と rmw ) が提供されており、C++ 向けの rclcpp も Python 向けの rclpy もこの上に実装されています。多くの言語は C API を呼ぶ機構が用意されているため、C++, Python 以外でも Client Library を作りやすくなっています。ドキュメント にある図がわかりやすいでしょう。

実際、Java などの Client Library が サードパーティーによって提供されています。であるならば、開発者は自分の好きな言語で ROS2 をやりたいと思うもの。かく言う私も D言語向けの ROS2 Client Library があればと思いつつも、未だ存在せず、ならば自身で作ろうと考えました。

ROS2 の C API を呼ぶ、という点については難しくありません。というわけで、 D言語で Node を作るだけのサンプルを以下に示します。D言語を知らない方でも雰囲気で読めると思います。

/*
CC=clang dub run -y dpp -- node.dpp \
    --include-path /opt/ros/foxy/include \
    -L-L/opt/ros/foxy/lib \
    -L-lrcl \
    -L-lrcutils
*/
#include <rcl/rcl.h>
import core.runtime;
import core.thread;
import std.exception;

void main(){
    auto args = Runtime.cArgs;

    auto init_options = rcl_get_zero_initialized_init_options();
    auto allocator = rcutils_get_default_allocator();

    enforce(rcl_init_options_init(&init_options, allocator) == 0);

    auto context = rcl_context_t();
    enforce(rcl_init(args.argc, args.argv, &init_options, &context) == 0);

    auto node_handle = rcl_get_zero_initialized_node();
    auto node_options = rcl_node_get_default_options();
    enforce(rcl_node_init(&node_handle, "node", "", &context, &node_options) == 0);

    Thread.sleep(10.seconds);

    rcl_node_fini(&node_handle);
    rcl_shutdown(&context);
}

※ビルドには dpp を使っています(解説記事)。

rcl_ というプレフィックスがついているものが rcl の API です。
ビルドして実行すると、 ros2 node list で "/node" ノードが存在することを確認することができます。簡単ですね。

しかし、ここからメッセージの publish や subscribe を行うための実装をするにはギャップがあるように思えます。Python では、メッセージは std_msgs.msg.String などの構造体として定義されています。では、独自の Client Library では構造体をどうやって定義すれば良いでしょうか？ Publisher/Subscription はどのように作るのか。コールバックはどういう仕組みになっているのか。

そこで、本記事ではこれらの疑問を解決するための指針を提示するべく、以下の3項目について(主に概念的な)ざっくりした解説をします。

• メッセージ生成のやり方
• Publisher の作り方
• Subscription の作り方

説明のために、以降では D言語向けの Client Library を作るものとします。

メッセージ生成のやり方

ROS2 (ROS1 でも) を使っている方なら .msg ファイルから C++ や Python など、各言語向けの構造体定義が生成されているということをご存知か、あるいは容易に想像できるでしょう。この自動生成を行っているのが rosidl_generator です。リポジトリを探してみると、

• rosidl
  • rosidl_generator_c
  • rosidl_generator_cpp
• rosidl_python
  • rosidl_generator_py

が見つかると思います。これらのパッケージが .msg から構造体を自動生成する命令を定義しています。実際に自動生成処理を呼び出すのは rosidl_default_generators のはずです。

rosidl_generator の機能はドキュメント にある図がわかりやすいでしょう。

右側に着目すると、 rosidl_generator は、msg 定義が与えられた時に、

• rosidl_generator_cpp を使って C++ 構造体を
• rosidl_generator_c を使って C 構造体を
• rosidl_generator_py を使って Python 構造体を

作っていることがわかります。
つまり、D言語構造体向けには rosidl_generator_d パッケージを作るということになります。実装の詳細は割愛します(CMake 力が試される)が以下の機能を実装する必要があるでしょう。

• rosidl_default_generators への登録 (CMake)
• D言語向けの構造体定義の自動生成 (Python)
• D言語向け Typesupport の設定及び Typesupport に対応するためのコードの自動生成
  • 次節で説明

Typesupport

Typesupport とは、メッセージの型の構成を表すメタ情報です。ROS2 を使ったアプリケーションの開発ではほとんど登場しないでしょうが、Client Library を作るためには Typesupport を避けて通ることができません。
これについて説明するには、そもそも ROS2 がどのようにトピックの送受信を行っているか説明する必要があります。

以下に rclcpp で publish して subscribe するまでの経路について、簡単に示します。

(ここでは単に「シリアライズ」としてしまってますが、DDS 層では実際に送信/受信するために色々やっている。ベンダ依存なので省略)

ミドルウェア層である DDS でメッセージのシリアライズ、デシリアライズが行われていますが、これらの処理を行うためには各メッセージの型に対応するシリアライズ関数とデシリアライズ関数を知っている必要があります。これらの情報を与えるために Typesupport が使われます。実際 rcl で publisher を作る関数は以下のように定義されており、第3引数に Typesupport を与えます。

RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_publisher_init(
  rcl_publisher_t * publisher,
  const rcl_node_t * node,
  const rosidl_message_type_support_t * type_support,
  const char * topic_name,
  const rcl_publisher_options_t * options);

C言語向けには rosidl_typesupport_c のものを指定します。例えば、std_msgs/msg/String であれば、
rosidl_message_type_support_t *rosidl_typesupport_c__get_message_type_support_handle__std_msgs__msg__String() という関数が自動生成されるため、これにより得られるオブジェクトを渡すことになります。C++向けにも同様に rosidl_typesupport_cpp があります。

ということは、D言語向けには rosidl_typesupport_d を用意しシリアライズ関数などを自動生成すれば良い、というこになると思いますが、これはおそらく難しいです。

前に示した図をもう一度見ると、左側に typesupport functions を作っているルートがあるのがわかると思います。

そして、rosidl_typesupport_<dds_vendor>_c と rosidl_typesupport_<dds_vendor>_cpp が typesupport functions を作っていることがわかると思います。dds_vendor とは、 eProsima の FastRTPS であったり、RTI の Connext であったりです。rosidl_typesupport_c と rosidl_typesupport_cpp は各ベンダーが提供する rosidl_typesupport_<dds_vendor>_c と rosidl_typesupport_<dds_vendor>_cpp のエイリアスであり、 どの RMW 実装を使うかにより動的に切り替えられます。実際のシリアライズ関数などの生成は各DDSベンダーが行っています。

つまり、rosidl_typesupport_d を作るには、 rosidl_typesupport_<dds_vendor>_d を作る必要があり、ちょっと大変そうであることがわかります。(いずれ調べてみたいところではある)

実際 ROS2 標準で提供されている Python 向け Client Library である rclpy は、裏側で C言語のメッセージ構造体に変換しており、 rosidl_typesupport_c により通信を行っています。Java 向けの ros2_java も同様に通信は rosidl_typesupport_c により行っているようです。これを踏襲すると、前述の

D言語向け Typesupport の設定及び Typesupport に対応するためのコードの自動生成

は次のような意味になります。

• D <-> C言語のメッセージ型の相互変換の自動生成
• rosidl_typesupport_c を参照できるようにする

相互変換のオーバーヘッドが気になるので何とかしたいところではありますが……。

おまけ: typesupport_introspection

前述の typesupport はベンダ固有の静的なもので、ビルド時に typesupport functions が生成される。 他方、動的な typesupport_introspection というものも存在する。これは、メッセージの構造(型、要素数など)を [この辺の型](https://github.com/ros2/rosidl/blob/foxy/rosidl_typesupport_introspection_cpp/include/rosidl_typesupport_introspection_cpp/message_introspection.hpp) で動的に表現し、メッセージを解釈するものである。型の大きさ、要素数に従って都度メッセージを走査するため、当然ながら静的な typesupport よりもパフォーマンスが劣る。 (あまり使ったことがないのでよく知らない)

Publisher の作り方

Publisher を作りメッセージを publish するためには最低限以下のようなコードを書けば良いでしょう。前述のサンプルからは(ヘッダとビルド方法も変わりますが) auto pub_handle = rcl_get_zero_initialized_publisher(); 以降が増えています。このサンプルでは std_msgs/msg/String のメッセージを publish します。

/*
CC=clang dub run -y dpp -- talker.dpp \
    --include-path /opt/ros/foxy/include  \
    -L-L/opt/ros/foxy/lib \
    -L-lrcl  \
    -L-lrcutils \
    -L-lrosidl_typesupport_c \
    -L-lrosidl_runtime_c \
    -L-lstd_msgs__rosidl_generator_c \
    -L-lstd_msgs__rosidl_typesupport_c
*/
#include <rcl/rcl.h>
#include <std_msgs/msg/string.h>
#include <rosidl_runtime_c/string.h>
#include <rosidl_runtime_c/string_functions.h>
import core.runtime;
import core.thread;
import std.exception;
import std.string;
import std.format;
import std.stdio;

void main(){
    auto args = Runtime.cArgs;

    auto init_options = rcl_get_zero_initialized_init_options();
    auto allocator = rcutils_get_default_allocator();

    enforce(rcl_init_options_init(&init_options, allocator) == 0);

    auto context = rcl_context_t();
    enforce(rcl_init(args.argc, args.argv, &init_options, &context) == 0);

    auto node_handle = rcl_get_zero_initialized_node();
    auto node_options = rcl_node_get_default_options();
    enforce(rcl_node_init(&node_handle, "talker", "", &context, &node_options) == 0);

    auto pub_handle = rcl_get_zero_initialized_publisher();
    auto pub_options = rcl_publisher_get_default_options();
    auto ts = rosidl_typesupport_c__get_message_type_support_handle__std_msgs__msg__String();
    enforce(rcl_publisher_init(&pub_handle, &node_handle, ts, "/chatter", &pub_options) == 0);

    Thread.sleep(1.seconds); // 最初のメッセージが Drop しないように

    foreach(i;0..10){
        auto c_msg = std_msgs__msg__String__create();
        auto str = format("Hello D %d", i);
        rosidl_runtime_c__String__assign(&c_msg.data, str.toStringz);

        writefln("Send:[%s]", str);
        enforce(rcl_publish(&pub_handle, cast(const(void)*) c_msg, null) == 0);

        std_msgs__msg__String__destroy(c_msg);

        Thread.sleep(1.seconds);
    }

    enforce(rcl_publisher_fini(&pub_handle, &node_handle) == 0);

    rcl_node_fini(&node_handle);
    rcl_shutdown(&context);
}

D言語はC言語と ABI 互換があるため、 直接 C言語の構造体のオブジェクトを作り(std_msgs__msg__String__create)、値を設定しています。
しかし、これはC関数であり、スマートポインタのようなメモリ管理の仕組みはなく create したものは自分で destroy する必要があります。ユーザはそんなことをしたくないはずですし、さらに言えば D言語構造体を使いたいはずです。
そのため、実際に D Client Library を作る時は、 D言語構造体を渡せば、 create、 D構造体→C構造体変換、 rcl_publish、 destory を裏側でやってくれるように設計することになるでしょう。言語によってはこの部分をC言語で実装する必要があるかもしれません。

(オーバーヘッドを考慮すると、サンプルのようにC構造体を直接扱った方が効率的ではある)

Subscription

C++ で Subscription を作る時、コールバック関数を登録しますが、このコールバック関数は一体誰が、どのように呼んでいるのでしょうか？

Subscription を実現するのは、 Publisher を実現するのに比べて手間がかかります。ただ1つの Subscription を作り、メッセージを受信するには最低限以下のようなコードとなるでしょう。

/*
CC=clang dub run -y dpp -- listener.dpp \
    --include-path /opt/ros/foxy/include  \
    -L-L/opt/ros/foxy/lib \
    -L-lrcl  \
    -L-lrcutils \
    -L-lrosidl_typesupport_c \
    -L-lrosidl_runtime_c \
    -L-lstd_msgs__rosidl_generator_c \
    -L-lstd_msgs__rosidl_typesupport_c
*/
#include <rcl/rcl.h>
#include <std_msgs/msg/string.h>
#include <rosidl_runtime_c/string.h>
#include <rosidl_runtime_c/string_functions.h>
import core.runtime;
import core.thread;
import std.exception;
import std.string;
import std.format;
import std.stdio;

void main(){
    auto args = Runtime.cArgs;

    auto init_options = rcl_get_zero_initialized_init_options();
    auto allocator = rcutils_get_default_allocator();

    enforce(rcl_init_options_init(&init_options, allocator) == 0);

    auto context = rcl_context_t();
    enforce(rcl_init(args.argc, args.argv, &init_options, &context) == 0);

    auto node_handle = rcl_get_zero_initialized_node();
    auto node_options = rcl_node_get_default_options();
    enforce(rcl_node_init(&node_handle, "talker", "", &context, &node_options) == 0);

    auto sub_handle = rcl_get_zero_initialized_subscription();
    auto sub_options = rcl_subscription_get_default_options();
    auto ts = rosidl_typesupport_c__get_message_type_support_handle__std_msgs__msg__String();

    enforce(rcl_subscription_init(&sub_handle, &node_handle, ts, "/chatter", &sub_options) == 0);

    auto wait_set = rcl_get_zero_initialized_wait_set();
    enforce(rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, &context, allocator) == 0);

    foreach (i; 0..10) {
        enforce(rcl_wait_set_clear(&wait_set) == 0);
        enforce(rcl_wait_set_add_subscription(&wait_set, &sub_handle, null) == 0);

        enforce(rcl_wait(&wait_set, -1 /*nanoseconds*/) == 0);

        rmw_message_info_t message_info;
        auto c_msg = std_msgs__msg__String__create();
        enforce(rcl_take(&sub_handle, cast(void*) c_msg, &message_info, null) == 0); 

        writefln("Receive:[%s]", fromStringz(c_msg.data.data));

        std_msgs__msg__String__destroy(c_msg);
    }

    enforce(rcl_subscription_fini(&sub_handle, &node_handle) == 0);

    rcl_node_fini(&node_handle);
    rcl_shutdown(&context);
}

sub_handle 周りが Subscription のコードになります。 rcl_subscription_init で初期化し、 rcl_take で受信したメッセージを受信します。しかし、コード上にはこれだけでなく wait_set が出てきます。

WaitSet の役割は、「イベント待ち」と言えると思います。イベントとは、 トピックの受信、サービスのリクエスト/レスポンスの受信、そして、タイマーです(アクションもあるはずだが……)。コード上では、rcl_wait_set_add_subscription によって Subscription を wait_set に登録し、 rcl_wait で受信イベントが発生するのを待ち、そして rcl_take でメッセージを取得します。今回は Subscription が 1 つなので省略していますが、rcl_wait の後で wait_set のメンバー変数を見ればどの Subscription、 タイマーなどでイベントが発生したか確認することができます。 このあたり が参考になるでしょう。

実際どのようなロジックを組めば良いのかは rclcpp の executor 周りを参考にしてみると良いと思います。 WaitSet に必要な要素を登録、rcl_wait で待つ、イベントが発生したものについては登録されているコールバック関数を呼ぶ、というのが基本的な流れになっていると思います。

さて、実のところメッセージの受信は WaitSet がなくても動きます。実際 WaitSet の記述を削除し、 while(rcl_take(&sub_handle, cast(void*) c_msg, &message_info, null) != 0) {} とすれば問題なく受信することができました。 Subscription が複数あったとしても、上手いことロジックを組めば対応可能でしょう。独自のコールバックの仕組みを作っても動くはずです。ROS1 ではぱっと見 WaitSet を使っていないようです。

とは言え、 WaitSet は ROS2 がいきなり提案したものではなく、 DDS の文脈で出てくるようです。詳細は調べないといけませんが、 ROS2 が DDS を採用している以上、WaitSet を使うのが自然ですし、イベントを待つ仕組みというのが共通 API として提供されているのにわざわざ作り直すのも変でしょう。

さいごに

ほとんどの ROS2 ユーザにとって Client Library 開発というのは (ROS1 に比べて簡単になったとは言え) 触れることのない要素だと思います。とは言え、Client Library を開発したいと思う人は少しはいると思いますし、ROS2 の裏側を知りたいと思う人はもう少し多くいると思います。そういった人々向けにこの記事を書きました。もともと D言語 Advent Calendar のネタの予定でしたが、これほとんど ROS2 の話だなと思い、分割したというのもあります。ここで書いたサンプルはほとんど C 言語と変わらない内容ですが、来週 D言語の方で記事を書く頃には D言語向け Client Library としての最低限の形を整えたものを公開できればと思います。

2020/12/22 更新

D言語向け Client Library の記事。

参考文献

• http://docs.ros2.org/foxy/developer_overview.html
  • そもそものデザインとか知るのに
• https://github.com/ros2/rcl
  • あんま見てないけど
• https://github.com/ros2/rclcpp
  • node/publisher/subscription どうやって作ってるの、とか
  • 機能でかいので最初は他を見た方が迷子にならないと思う
• https://github.com/ros2/rosidl
  • rosidl_generator_d を作るのに参考にした
• https://github.com/ros2-rust/ros2_rust
  • 最初に参考にしてたやつ
• https://github.com/ros2/rclpy
  • Typesupport 周りで？？？ってなった時に見て rosidl_typesupport_c を使っていることを知った
• https://github.com/ros2/rclc (追記）
  • 機能小さい分読みやすい