ROS2で任意のメッセージをC++ノードでブリッジする方法

f:id:aptpod-tetsu:20211001141621p:plain

Protocol/Robotics Teamの酒井 (@neko_suki) です。

以前「ROS2メッセージの遠隔リアルタイムデータ伝送を実現する新プロダクトのご紹介」という記事で開発中の intdash_ros2bridge というプロダクトを紹介しました。

今回は、前回記載したとおり、技術にフォーカスして技術的な詳細を含めた進捗をご紹介します。

intdash_ros2bridgeは、ROS2上で「任意のトピック、サービス、アクションのC++実装によるブリッジ」を実現します。ブリッジとは、ROS2空間の内部のデータからインターネット経由で伝送できるメッセージ形式への橋渡しをする処理のことを指します。

intdash_ros2bridgeによって遠隔地のROS2空間をつなぐことが可能になります。その結果ROS1と同様にROS2でも遠隔制御やモニタリングなどが実現できます。

f:id:aptpod_tech-writer:20210916170952p:plain — 構成図

intdash_ros2bridgeではROSメッセージの伝送のために以下の2つの方法でブリッジを行います。

効率的なメッセージ伝送のための Common Data Representation (CDR) 形式という生データに近いフォーマットへのブリッジ
弊社製品のVisual M2M Data Visualizer で可視化するための、JSONへのブリッジ

今回の記事では、最初にトピック、サービス、アクションをブリッジする方法について簡単に紹介します。次に、トピックをブリッジする方法、トピックをJSONに変換して伝送する方法の2点を詳細に掘り下げて説明します。

ROSメッセージをブリッジする方法
トピックのブリッジ
- subscribe
- publish
JSONへのブリッジ
まとめ
Appendix

ROSメッセージをブリッジする方法

intdash_ros2bridge がブリッジするROSメッセージは主にトピック、サービス（サービスリクエスト・サービスレスポンス）、アクション（ゴールリクエスト・レスポンス、キャンセルリクエスト・レスポンス、フィードバック・リザルト）の3つです。汎用的な使用を想定しているため任意のROSメッセージを扱える必要があります。

任意のトピックのブリッジには、ROS2ユーザーにはおなじみのrosbag2 に実装されている rosbag2_transport::GenericSubscription と rosbag2_transport::GenericPublisher の技術を使用しています。*1。

rosbag2は任意のトピックをsubscribeして保存し、保存されたbagファイルに含まれるトピックを再生(=publish)することが可能です。intdash_ros2bridge ではこの技術を応用します。

一方で、技術調査を行った時点では、c++で任意のサービスやアクションをブリッジする実装は見つけられませんでした。

具体的な実装は見つけられませんでしたが、discourse.ros.orgのスレッドで実装方法について議論がされているのを見つけました。スレッドでは、GenericSubscription や GenericPublisher と同じ方針で実装できると言及されています。

そこで、intdash_ros2bridgeはスレッドで言及されている方法で自前でサービスとアクションのブリッジを実装しました。

ここからは現在オープンにアクセスできる情報で紹介が可能なトピックのブリッジとJSONへのブリッジについて、掘り下げて説明します。

トピックのブリッジ

トピックをsubscribeする側は以下のような構成になります。

f:id:aptpod_tech-writer:20210916171015p:plain — subscribeする側の構成

intdash_ros2bridgeは、トピックのブリッジに GenericSubscription という任意のトピックをsubscribeできる技術を使用します。

通常、ROS2でトピックをsubscribeする処理はSubscriptionを使って以下のように実装します。

rclcpp::Node *node = ROSノードのポインタ;
const std::string topic_name = "topic";
auto subscription_callback = [](const std_msgs::msg::String::SharedPtr msg)) {
  // subscribe したトピック msg を元に処理を行う
};
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr> subscription = node->create_subscription<std_msgs::msg::String>(topic_name, 10, subscription_callback);
};

Subscription のインスタンスを生成するときは、create_subscriptionという関数のテンプレートにsubscribeしたいデータ型を記述します。この例では6行目のstd_msgs::msg::String がsubscribeしたいデータ型になります。

トピックをsubscribeしたときに呼び出されるコールバック関数（この場合は3行目の subscription_callback) には、指定したデータ型が引数として渡されます。

ただ、Subscription を製品に使うには2つ課題があります。

一つ目は、subscribeしたいすべてのデータ型について個別の実装が必要になる点です。ROS2では自作のデータ型を作成して使うことが可能です。ユーザーが作成する任意のデータ型に対して Subscription を使用した汎用的な製品を開発することは現実的ではありません。

二つ目は、コールバック関数で渡されるものは構造体という点です。構造体なので何らかの形式でシリアライズしないと、メッセージを受け取った側が正しく解釈できない可能性があります。

そこで、GenericSubscription を使うとこの二つの課題を解決することができます。

rclcpp::Node *node = ROS ノードのポインタ;
const std::string topic_name = "topic";
const std::string topic_type = "std_msgs/msg/String";
// シリアライズされたトピックへのポインタ以外の情報は渡されないので、トピック名、データ型はラムダ式でキャプチャする
auto subscription_callback = [topic_type, topic_name](std::shared_ptr<rclcpp::SerializedMessage> msg) {
  // intdash Edge Agentにデータを渡す
};
rosbag2_transport::GenericSubscription::SharedPtr subscription = create_generic_subscription(node_, topic_name, topic_type, rclcpp::QoS(10), subscription_callback);

GenericSubscriptionとSubscriptionには、大きな違いが2つあります。

一つ目は、create_subscription の代わりに、create_generic_subscription という関数を呼び出す点*2です。 create_generic_subscription は、データ型 std_msgs/msg/String をテンプレートで指定せずに、文字列として引数で渡します。これにより、ユーザーが作成した任意のデータ型を実行時に指定することができます。

二つ目は、コールバック関数の引数です。Subscriptionの場合は、std_msgs/msg/String 型のトピックへのシェアードポインタがコールバック関数の引数でした。GenericSubscription は rclcpp::SerializedMessage というCDR形式にシリアライズされたトピックへのシェアードポインタが渡されます。

シリアライズされた形式なのでそのまま伝送することが可能です。

intdash_ros2bridgeでは、このsubscribeしたシリアライズされたトピック、トピック名、データ型を、intdash Edge Agent に渡し、サーバーに伝送します。

publish

トピックをPublishする側は以下のような構成になります。

f:id:aptpod_tech-writer:20210916171042p:plain — publish側の構成

intdash_ros2bridgeは、intdash Edge Agent から受け取ったシリアライズされたトピックのpublishに、GenericPublisher というシリアライズされたトピックをpublishできる技術を使用します。

GenericPublisher を使用すると、シリアライズされたトピックのpublishは以下のように書くことができます。

const std::string topic_name = "topic";
const std::string topic_type = "std_msgs/msg/String";
rosbag2_transport::GenericPublisher::SharedPtr publisher = create_generic_publisher(node, topic_name, topic_type, rclcpp::QoS(10));
// intdash edge Agent からシリアライズされたメッセージ「serialized_message」を取得する
publisher->publish(serialized_message);

GenericPublisher は create_generic_publisher という関数を呼び出して生成します*3。GenericPublisher のインスタンス生成時にデータ型 std_msgs/msg/String を文字列として引数で渡しています。これにより、ユーザーが作成した任意のデータ型を実行時に指定することが可能になります。

そして、publish() メソッドにシリアライズされたトピックを渡すとそのトピックをpublishします。

こうすることで、intdash Edge Agentを経由して受け取ったシリアライズされたトピックをpublishすることが可能になります。

JSONへのブリッジ

次にシリアライズされたトピックをJSONに変換する方法を解説します。

以下の図のように、弊社製品のVisual M2M Data VisualizerはJSONを可視化することが出来るため、トピックをJSONに変換して送信すると遠隔モニタリングが可能になります。

f:id:aptpod_tech-writer:20210916171102p:plain — JSON可視化の構成

intdash_ros2bridgeはシリアライズされたトピックをJSONに変換してintdash Edge Agentに渡します。渡されたJSONはサーバーを経由し、ブラウザ上で可視化が行えます。

JSONへのブリッジ方法を説明するために以下のようなcustom_msgs/Sample というデータ型を定義します。

int64 a          # 整数
int64 b[]        # 整数の任意長配列
sensor_msgs/Imu c #

もしデータ型がわかればその構成を元に実装ができるので、JSONへのブリッジは以下のように書けます。

auto subscription_callback = [](const std_msgs::msg::String::SharedPtr msg)) {
  // json にメンバa (64bit整数)を追加
  // json にメンバb (64bit整数の配列)を追加
  // json にメンバc (sensor_msgs/Imu) を追加
  // json をintdash Edge Agentに渡す
};

しかし、GenericSubscription でsubscribeしたデータはシリアライズされています。そのままでは「データ型に含まれるメンバ構成の定義」や「それぞれのメンバの定義」がわからないため、JSONへのブリッジはできません。

rclcpp::Node *node = ROS ノードのポインタ;
const std::string topic_name = "topic";
const std::string topic_type = "std_msgs/msg/String";
// シリアライズされたトピックへのポインタ以外の情報は渡されないので、トピック名、データ型はラムダ式でキャプチャする
auto subscription_callback = [topic_type, topic_name](std::shared_ptr<rclcpp::SerializedMessage> msg) {
  // JSONに変換するためにはデータ型に含まれるメンバ構成の定義と、それぞれのメンバの定義が必要
};
rosbag2_transport::GenericSubscription::SharedPtr subscription = create_generic_subscription(node_, topic_name, topic_type, rclcpp::QoS(10), subscription_callback);

そこでROS2で提供されている「データ型に含まれるメンバ構成の定義」を含む rosidl_typesupport_introspection_cpp::MessageMembers (以下MessageMembers)と「それぞれのメンバの定義」を含む rosidl_typesupport_introspection_cpp::MessageMember (以下 MessageMember) という構造体を活用します。

データ型の定義とMessageMembers、 MessageMember の関係は以下の図のようになっています。

f:id:aptpod_tech-writer:20210917160859p:plain — MessageMembersとMessageMemberの関係

ROS2ではデータ型の定義を元に、共有ライブラリが生成されます。MessageMembers と MessageMember は共有ライブラリに含まれます。

MessageMembersはデータ型に含まれるメンバの数、MessageMemberの配列へのポインタを含んでいます。

MessageMembers は以下のように定義されています。

typedef struct ROSIDL_TYPESUPPORT_INTROSPECTION_CPP_PUBLIC MessageMembers
{
  const char * message_namespace_;
  const char * message_name_;
  uint32_t member_count_;
  size_t size_of_;
  const MessageMember * members_;
  void (* init_function)(void *, rosidl_runtime_cpp::MessageInitialization);
  void (* fini_function)(void *);
} MessageMembers;

custom_msgs/msg/Sample に対して生成された MessageMembersは以下のようになります。 (staticで定義されていますが外部から取得可能です。詳細な取得方法はAppendix を参照してください。)

custom_msgs/Sample の場合はメンバa, b, c の定義を含む MessageMember の配列へのポインタを持ちます（下記プログラムの6行目）。

static const ::rosidl_typesupport_introspection_cpp::MessageMembers Sample_message_members = {
  "custom_msgs::msg",  // message namespace
  "Sample",  // message name
  3,  // number of fields  // メンバの数
  sizeof(custom_msgs::msg::Sample),
  Sample_message_member_array,  // message members // それぞれのメンバの定義を含む MessageMember の配列
  Sample_init_function,  // function to initialize message memory (memory has to be allocated)
  Sample_fini_function  // function to terminate message instance (will not free memory)
};

custom_msgs/msg/Sample の場合は、member_count_から3つのメンバがいることがわかります。そして、Sample_message_member_array から custom_msgs/Sampleを構成するそれぞれのメンバの定義を参照できます。

メンバの定義を記述している MessageMember は以下のように定義されています。

typedef struct ROSIDL_TYPESUPPORT_INTROSPECTION_CPP_PUBLIC MessageMember
{
  const char * name_;  // メンバの名前
  uint8_t type_id_;    // メンバの型
  size_t string_upper_bound_;
  const rosidl_message_type_support_t * members_; // sensor_msgs/Imu など別のデータ型の場合、ハンドラを含む
  bool is_array_;      // 配列かどうかの判定
  size_t array_size_;
  bool is_upper_bound_;
  uint32_t offset_;
  const void * default_value_;
  size_t (* size_function)(const void *);
  const void * (*get_const_function)(const void *, size_t index);
  void * (*get_function)(void *, size_t index);
  void (* resize_function)(void *, size_t size);
} MessageMember;

そして、それぞれのメンバの定義を含むSample_message_member_array は以下のように定義されます。

static const ::rosidl_typesupport_introspection_cpp::MessageMember Sample_message_member_array[3] = {
  { // メンバ a の情報
    "a",  // name
    ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64,  // type // メンバの型
    0,  // upper bound of string
    nullptr,  // members of sub message
    false,  // is array
    0,  // array size
    false,  // is upper bound
    offsetof(custom_msgs::msg::Sample, a),  // bytes offset in struct
    nullptr,  // default value
    nullptr,  // size() function pointer
    nullptr,  // get_const(index) function pointer
    nullptr,  // get(index) function pointer
    nullptr  // resize(index) function pointer
  },
  { // メンバbの情報
    "b",  // name
    ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64,  // type // メンバの型
    0,  // upper bound of string
    nullptr,  // members of sub message
    true,  // is array
    0,  // array size
    false,  // is upper bound
    offsetof(custom_msgs::msg::Sample, b),  // bytes offset in struct
    nullptr,  // default value
    size_function__Sample__b,  // size() function pointer
    get_const_function__Sample__b,  // get_const(index) function pointer
    get_function__Sample__b,  // get(index) function pointer
    resize_function__Sample__b  // resize(index) function pointer
  },
  { // メンバcの情報
    "c",  // name
    ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE,  // type // メンバの型
    0,  // upper bound of string
    ::rosidl_typesupport_introspection_cpp::get_message_type_support_handle<sensor_msgs::msg::Imu>(),  // members of sub message
    false,  // is array
    0,  // array size
    false,  // is upper bound
    offsetof(custom_msgs::msg::Sample, c),  // bytes offset in struct
    nullptr,  // default value
    nullptr,  // size() function pointer
    nullptr,  // get_const(index) function pointer
    nullptr,  // get(index) function pointer
    nullptr  // resize(index) function pointer
  }
};

custom_msgs/Sampleの場合、MessageMembers 経由で取得した MessageMember 配列の情報を走査すると以下のことがわかります。

メンバaは64bit整数
メンバbは64bit整数の配列
メンバcは別途定義されたデータ型である。

このようにMessageMembers と MessageMember を組みわせると、実行時に動的に custom_msgs/Sampleの構成を動的に取得することが可能になります。

長くなるため詳細は書きませんが、eProsima/Fast-CDR というソフトウェアにはCDR形式にシリアライズされたメッセージから所定の型を読み取るAPIがあります。このソフトウェアを使うと、MessageMember に書かれている型の値をシリアライズされたメッセージから読みとれます。

f:id:aptpod_tech-writer:20210917160923p:plain — シリアライズされたメッセージの読み込み

custom_msgs/Sample の場合は、最初に64ビット整数を読み込みます。次に、64bit整数の配列を読み込みます。最後に、sensor_msgs/Imu 型のメッセージを読み込みます。

読み取ったそれぞれのメンバの値はJSONに追加します。

// トピック名、トピック型をキャプチャしておく
auto subscription_callback = [topic_type, topic_name](std::shared_ptr<rclcpp::SerializedMessage> msg) {
  MessageMembers members;// MessageMembers を取得
  for(size_t i = 0;i < members.member_count_; ++i){
    // members.members_[i] 型のデータを読み込む
    // JSONに要素を追加する
  }
  // JSONに変換されたトピックをintdash Edge Agent に渡す
};
subscription = create_generic_subscription(node_, topic_name, topic_type, rclcpp::QoS(10), subscription_callback);

このようにJSONにブリッジしたトピックはintdash Edge Agent、intdash Server を経由し、Visual M2M Data Visualizer で可視化することが可能になります。

f:id:aptpod_tech-writer:20210930104628p:plain — Visual M2M Data Visualizer で可視化したROSトピック

まとめ

今回は弊社が開発しているROS2メッセージの遠隔リアルタイムデータ伝送を実現するintdash_ros2bridgeというプロダクトを実現する方法について紹介しました。

本記事では、トピックのブリッジについて扱いましたが、サービスやアクションも同様の方法でブリッジすることが可能です。

弊社では現在のintdash_ros2bridgeの利用拡大やROSコミュニティへの貢献を視野に入れたOSS化に向けた計画も進めています。

プロダクト開発の進捗やOSS化の進捗がありましたらまた続報をお届けできればと思います。

最後までご覧いただきありがとうございました。

Appendix

create_generic_subscriptionの実装

foxyではGenericSubscriptionを生成する関数は、Rosbag2Node のメンバとして実装されていますが、Rosbag2Nodeを使う必要は無さそうだったので独自に生成用の関数を定義しています。

std::shared_ptr<rosbag2_transport::GenericSubscription> create_generic_subscription(
  rclcpp::Node * node, const std::string & topic, const std::string & type, const rclcpp::QoS & qos,
  std::function<void(std::shared_ptr<rclcpp::SerializedMessage>)> callback, rclcpp::CallbackGroup::SharedPtr callback_group
  )
{
  auto library_generic_subscriptor =
    rosbag2_cpp::get_typesupport_library(type, "rosidl_typesupport_cpp");
  auto type_support = rosbag2_cpp::get_typesupport_handle(
    type, "rosidl_typesupport_cpp", library_generic_subscriptor);
  auto subscription = std::shared_ptr<rosbag2_transport::GenericSubscription>();

  try {
    subscription = std::make_shared<rosbag2_transport::GenericSubscription>(
      node->get_node_base_interface().get(), *type_support, topic, qos, callback);
    node->get_node_topics_interface()->add_subscription(subscription, callback_group);
  } catch (const std::runtime_error & ex) {
    RCLCPP_ERROR(node->get_logger(),
      "Error subscribing to topic %s, Error: %s", topic.c_str(), ex.what());
  }
  return subscription;
}

create_generic_publisherの実装

create_generic_publisher も同様に、独自に生成用の関数を実装しています。

std::shared_ptr<rosbag2_transport::GenericPublisher> create_generic_publisher(
  rclcpp::Node * node, const std::string & topic, const std::string & type, const rclcpp::QoS & qos)
{
  auto library_generic_publisher_ =
    rosbag2_cpp::get_typesupport_library(type, "rosidl_typesupport_cpp");
  auto type_support =
    rosbag2_cpp::get_typesupport_handle(type, "rosidl_typesupport_cpp", library_generic_publisher_);
  return std::make_shared<rosbag2_transport::GenericPublisher>(
    node->get_node_base_interface().get(), *type_support, topic, qos);
}

`MessageMembers` の取得方法

MessageMembers は build/custom_msgs/rosidl_typesupport_introspection_cpp/custom_msgs/msg/detail/sample__type_support.cpp というファイルに生成されています。

厳密には、rosidl_typesupport_introspection_cpp__get_message_type_support_handle__custom_msgs__msg__Sample() という関数が定義されます。これを呼び出すことで、rosidl_message_type_support_t 型のハンドラ Sample_message_type_support_handle へのポインタが取得できます。

MessageMembers へのポインタがここに含まれているため、これを使用します。

static const rosidl_message_type_support_t Sample_message_type_support_handle = {
  ::rosidl_typesupport_introspection_cpp::typesupport_identifier,
  &Sample_message_members,
  get_message_typesupport_handle_function,
};

// ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME(rosidl_typesupport_introspection_cpp, custom_msgs, msg, Sample)は
// rosidl_typesupport_introspection_cpp__get_message_type_support_handle__custom_msgs__msg__Sample に展開される
ROSIDL_TYPESUPPORT_INTROSPECTION_CPP_PUBLIC
const rosidl_message_type_support_t *
ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME(rosidl_typesupport_introspection_cpp, custom_msgs, msg, Sample)() {
  return &::custom_msgs::msg::rosidl_typesupport_introspection_cpp::Sample_message_type_support_handle;
}

ハンドラの取得方法は、rclcppのtypesupport_helpers.cpp で実装されています。

*1:galacticではROS2のc++クライアントであるrclcppに取り込まれています。rclcpp::GenericSubscription、rclcpp::GenericPublication

*2:galacticではnode->create_generic_subscription() と呼び出せます。

*3:galacticではnode->create_generic_publisher() と呼び出します。

aptpod Tech Blog

株式会社アプトポッドのテクノロジーブログです

ROS2で任意のメッセージをC++ノードでブリッジする方法

ROSメッセージをブリッジする方法

トピックのブリッジ

publish

JSONへのブリッジ

まとめ

Appendix

create_generic_subscriptionの実装

create_generic_publisherの実装

`MessageMembers` の取得方法

ROSメッセージをブリッジする方法

トピックのブリッジ

subscribe

publish

JSONへのブリッジ

まとめ

Appendix

create_generic_subscriptionの実装

create_generic_publisherの実装

MessageMembers の取得方法

`MessageMembers` の取得方法