🤖

Type Adaptation/Negotiationによる型変換とコピーから開放されたPub/Sub通信

2024/04/16に公開

ROS

ROS2

tech

はじめに

ROS 2でPub/Subを書いているときに毎回記述している気がする処理、それが型変換です。
例えばROS 2のメッセージでは点群はsensor_msgs::msg::PointCloud2型ですが、点群処理をするときにはそれをpcl::PointCloud<pcl::PointXYZ>::Ptr型などに変換すると思います。

部品性を考慮したROS 2プログラミングを行うと、何度も点群をPublishしてSubscribeして処理するのを繰り返すことになると思います。
そこまで重たい処理ではないとはいえ、何回も繰り返すとパフォーマンスに悪影響を与えます。
特にGPUメモリにデータを確保していた場合などでは一度メモリ転送が必要になり更に影響が大きくなります。

そのような場合に使えるのがType AdaptationとType Negotiationです。

なお、今回説明する機能はHumble以降のROS 2で使用可能です。

Type Adaptationとは？

Type Adaptationの仕様はこちらにREP2007として規定されています。
Type Adaptationは型変換とそれに伴う無駄なコピーを省いてくれます。

挙動としてはこちらの図がわかりやすいです。

Type Adaptation

Type Adaptationを使っていない場合、ROS 2の画像データsensor_msgs::msg::Imageはノード内部でcv::Matに変換されて処理が行われ、後段のノードに送られます。
つまりこの時、ノード間の通信のためだけに一度無駄な変換が実行されています。
ROS 2ではExecutorという仕組みで複数ノードを一つのプロセスに束ねる機能があります。
cv::Matのままメモリでノード間のデータ転送が行えれば非常に計量となります。
それを実現するのが今回紹介するType AdaptationとType Negotiationです。

Type Adaptation機能を利用するには下記のように自作アダプタを記述します。

template<>
struct rclcpp::TypeAdapter<cv::Mat, sensor_msgs::msg::Image>
{
  using is_specialized = std::true_type;
  using custom_type = cv::Mat;
  using ros_message_type = sensor_msgs::msg::Image;

  static void convert_to_ros_message(const custom_type & source, ros_message_type & destination)
  {
    destination.height = source.cv_mat().rows;
    destination.width = source.cv_mat().cols;
    const auto & encoding_override = source.encoding_override();
    if (encoding_override.has_value() && !encoding_override.value().empty()) {
      destination.encoding = encoding_override.value();
    } else {
      switch (source.cv_mat().type()) {
        case CV_8UC1:
          destination.encoding = "mono8";
          break;
        case CV_8UC3:
          destination.encoding = "bgr8";
          break;
        case CV_16SC1:
          destination.encoding = "mono16";
          break;
        case CV_8UC4:
          destination.encoding = "rgba8";
          break;
        default:
          throw std::runtime_error("unsupported encoding type");
      }
    }
    destination.step = static_cast<sensor_msgs::msg::Image::_step_type>(source.cv_mat().step);
    size_t size = source.cv_mat().step * source.cv_mat().rows;
    destination.data.resize(size);
    memcpy(&destination.data[0], source.cv_mat().data, size);
    destination.header = source.header();
  }

  static void convert_to_custom(const ros_message_type & source, custom_type & destination)
  {
    destination = cv_bridge::ROSCvMatContainer(source);
  }
};

アダプタにはconvert_to_ros_message関数とconvert_to_custom関数が必要です。
上記アダプタを実装した上で、下記のようにPublisher/Subscriberを書き換えるとType Adaptationが使用可能になります。

using MyAdaptedType = TypeAdapter<cv::Mat, sensor_msgs::msg::Image>;
auto pub = node->create_publisher<MyAdaptedType>("topic", 10);
auto sub = node->create_subscription<MyAdaptedType>("topic", 10, [](const cv::Mat & msg) {});

上記のようにPublisher/Subscriberを書き換えることで毎回ROS 2のメッセージ型からデータを処理するための型に変換する部分をい実装する必要がなくなりバグの混入を防止できます。
その上で、Executorのuse_intra_process_commsオプションをTrueにすることでPublish/Subscribeに伴うシリアライズ、デシリアライズ処理をスキップできます。
このテストコードは自分が記述した点群のデータをpublish/subscribeして点群のポインタが同一で有ることを確認しています。

Type Negotiation

Type Negotiationの仕様はこちらにREP2009として規定されています。

Type Negotiation

Type NegotiationのためにROS 2アプリケーション開発者はType Adaptationを使用する以外の作業は必要ありません。
Type Negotiationに対応したROS 2ノードはPublisherがサポートする型のリストを共有し、Subscriberは好みを示す度合いを共有することができます。
ROS 2クライアントライブラリは効率が最も良いと考えられる型を自動で選定し、データをPublish/Subscribeします。

これにより、ROS 2ではType Adaptationを設定するだけでコピーやデータの変換回数を減らしながら最も効率よく通信を行うことが可能になります。

はじめに

Type Adaptationとは？

Type Negotiation

Discussion