ROS2でのゼロコピーやShared Memoryによる通信速度の向上

# はじめに

この記事は[Aizu Advent Calendar 2024](https://adventar.org/calendars/10858)の12日目の記事です。
いつも通り誕生日です。

# 前置き
現在、修士2年生である私はROS2を使用して研究手法の実現をしています。
具体的には、複数のRGB-Dカメラから色付き点群を取得し、その点群に対して前処理を行い、最終的に3D SLAMを行い**処理速度の向上**を図っています。
各処理はROS2のノード単位に分割しており、下図のようになっています。
```mermaid:
graph LR
    A(4つのRGB-Dカメラ
      からの色付き点群取得)
    B(IMUによる点群の補正)
    C(点群を複数に分割)
    MCL1(研究手法のアルゴリズム)
    MCL2(研究手法のアルゴリズム)
    MCL3(研究手法のアルゴリズム)
    SELECT(最適な補助情報の抽出)
    ICP(3D SLAM)

    A--**色付き点群**-->B--**色付き点群**-->C
    C--**色付き点群**-->MCL1--補助情報-->SELECT
    C--**色付き点群**-->MCL2--補助情報-->SELECT
    C--**色付き点群**-->MCL3--補助情報-->SELECT

    B--**色付き点群**-->ICP
    SELECT--補助情報-->ICP
```

このシステムでは、4つのRGB-Dカメラから色付き点群を取得するのですが、データサイズが巨大です。
色付き点群ですが、一つの点のデータサイズは色情報と位置情報から**15B**程度になります。
1つのRGB-Dカメラからは1280x720で**92万点**程度取れるでしょう。
なので、4つのRGB-Dカメラからは**368万点の55MB**近いデータが取れます。
(参考: IntelRealsense D435・PCLの[PointXYZRGB型](https://pointclouds.org/documentation/structpcl_1_1_point_x_y_z_r_g_b.html))

そのデータサイズから各ノードの処理速度が遅くなるのは当然ですが、
処理をノードごとに分割しているため**同一マシン内でのノード間通信**もシステム全体の速度に影響を与えることが懸念点となりました。

また、通信のために**点群の型変換**も問題でした。
点群を扱うライブラリとしてPCLライブラリを良く使用しているのですが、通信のためには以下のようにROS2の点群型への変換が必須です。
```mermaid
graph LR
    A(PCLの点群型)
    B(ROS2の点群型)
    C(ROS2の点群型)
    D(PCLの点群型)

    B-.通信.->C
    subgraph 処理1
        A--変換-->B
    end
    
    subgraph 処理2
        C--変換-->D
    end
```

どうにかならないかと調べてみたところ、ROS2には同一マシン内でのノード間通信を高速にする仕組みがありました。
本記事では同一マシン内でのノード間通信の手法を比較することが目的です。

# ROS2での通信
通信するノード同士が「どこで動いているか」によって通信方式が変わります。
- 同一処理内での通信
- 同一ホスト内での通信
- 別ホスト間の通信

## 同一処理内での通信(Intra-process Communications)
ROS2では一つの実行単位(target)に対して複数のノードが実行可能であり、**同一実行単位内でのノード間通信ではゼロコピー通信が可能**です。

基本となる書き方は、SingleThreadedExecutorを使用して一つのプログラムに複数のノードクラスを書く。
(詳しくは[公式ドキュメント](https://docs.ros.org/en/jazzy/Concepts/Intermediate/About-Executors.html)を参照)
```cpp
int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);

  auto options = rclcpp::NodeOptions().use_intra_process_comms(true);

  rclcpp::Node::SharedPtr node1 = std::make_shared<MinimalPublisher>(options);
  rclcpp::Node::SharedPtr node2 = std::make_shared<MinimalSubscriber>(options);

  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node1);
  executor.add_node(node2);
  executor.spin();

  rclcpp::shutdown();

  return 0;
}
```

ただプログラムを一つにすると再利用性が無くなるなどの問題があるため、ノードをComponent化することで別パッケージのノード同士を単一プロセスとして実行ができます。

変更方法としては、以下のようにプログラムを変更してライブラリ化して、launchを使って一つのプロセスにして実行します。
(詳しくは[公式ドキュメント](https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Writing-a-Composable-Node.html)を参照)
```cpp
//main関数をコメントアウト
/*int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalPublisher>());
  rclcpp::shutdown();
  return 0;
}*/

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(MinimalPublisher)
```

## 同一ホスト内での通信(Shared Memory Transport)
同一ホスト内での通信では、**Shared Memoryを通じての通信**が行えます。
ROS2では特に難しい設定をせずとも、この通信がデフォルトで有効となっています。
実行を行うと`/dev/shm`内に新しくファイルが生成されているのを確認できます。
(rmwの種類によって性能が変わると思いますが、今回はデフォルトのFast-RTPSを使用します。)

## 別ホスト間の通信
この通信では、UDPやTCPを使用した通信になります。
デフォルトでは、UDPを使用した通信になるようです。
UDPやTCPを使用して通信をしているのかは`tcpdump`コマンドで確認ができます。

# TypeAdapterクラス
ROS2では送信する際には、基本的にROS2型を用いて通信をお行います。
しかし前置きでも書いたように、実際にプログラム内で使用したい型は別の型のことが多々あります。その場合は、送信前後に変換プログラムをわざわざ各ノードで書く必要がありました。

Humble以降では、この変換を楽にする`TypeAdapter`クラスが用意されました。
**実際に使用する型(カスタム型)とROS2の型のそれぞれの変換**を`TypeAdapter`クラスに定義し、送受信の型として指定します。
そうすると、型の変換を自動で行ってくれるので、送信側ではカスタム型を指定でき、また、受信側でもカスタム型の受け取りを指定できます。

また、「同一処理内での通信」では送る型と受け取る型が合っていれば**変換を行わない通信**も行えます。
詳しくは以下の記事が参考になりました。
https://zenn.dev/hakuturu583/articles/type_adaptation_and_negotiation

# 使用プログラム
以下のようにノードをコンポーネント化してビルドしています。
それらのノードをlaunchファイルから個別に実行するか同一プロセス内で実行するかを定義します。また、同一プロセス内で動かす場合に、Intra-process通信の有無を選択できるので、それぞれを試します。

プログラムの内容としては`vector<uint64>`とROS2の`UInt64MultiArray`のコピーによる相互変換を1億個のデータに対して行い通信をします。

## Publisherノード
```c++:
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include <vector>
#include <climits>
#include <iomanip>

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/u_int64_multi_array.hpp>

rclcpp::Clock system_clockr(RCL_SYSTEM_TIME);

using namespace std::chrono_literals;

template <>
struct rclcpp::TypeAdapter<std::vector<unsigned long long>, std_msgs::msg::UInt64MultiArray>
{
  using is_specialized = std::true_type;
  using custom_type = std::vector<unsigned long long>;
  using ros_message_type = std_msgs::msg::UInt64MultiArray;

  static void convert_to_ros_message(const custom_type &source, ros_message_type &destination)
  {
    for (auto s : source)
    {
      destination.data.push_back(s);
    }
    std::cout << &(destination.data) << " , " << &(destination.data[0]) << ":" << std::fixed << std::setprecision(9) << system_clockr.now().seconds() << std::endl;
  }

  static void convert_to_custom(const ros_message_type &source, custom_type &destination)
  {
    std::cout << &(source.data) << " , " << &(source.data[0]) << ":" << std::fixed << std::setprecision(9) << system_clockr.now().seconds() << std::endl;
    for (auto s : source.data)
    {
      destination.push_back(s);
    }
  }
};

namespace Sample
{
  class MinimalPublisher : public rclcpp::Node
  {
  public:
    MinimalPublisher(const rclcpp::NodeOptions &options)
        : Node("minimal_publisher", options), count_(0)
    {
      publisher_ = this->create_publisher<MyAdaptedType>("topic", 10);
      timer_ = this->create_wall_timer(
          20s, std::bind(&MinimalPublisher::timer_callback, this));

      tmp.clear();

      for (unsigned long long i = 1; i < 100 * 1000 * 1000; i++)
      {
        tmp.push_back(i % 255);
      }
      RCLCPP_INFO_STREAM(this->get_logger(), "start");
    }

  private:
    using MyAdaptedType = rclcpp::TypeAdapter<std::vector<unsigned long long>, std_msgs::msg::UInt64MultiArray>;
    void timer_callback()
    {
      std::unique_ptr<MyAdaptedType::custom_type> data = std::make_unique<MyAdaptedType::custom_type>();
      *data = tmp;
      data->front() = count;
      count++;

      RCLCPP_INFO_STREAM(this->get_logger(), data->front() << " : " << data.get() << " , " << &(*data)[0]);
      publisher_->publish(std::move(data));
    }
    int count = 0;
    MyAdaptedType::custom_type tmp;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<MyAdaptedType>::SharedPtr publisher_;
    size_t count_;
  };
}

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(Sample::MinimalPublisher)
```

## Subscriberノード
```c++
#include <memory>
#include <vector>
#include <iostream>
#include <iomanip>

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/u_int64_multi_array.hpp>

rclcpp::Clock system_clockr(RCL_SYSTEM_TIME);

using std::placeholders::_1;

template <>
struct rclcpp::TypeAdapter<std::vector<unsigned long long>, std_msgs::msg::UInt64MultiArray>
{
  using is_specialized = std::true_type;
  using custom_type = std::vector<unsigned long long>;
  using ros_message_type = std_msgs::msg::UInt64MultiArray;

  static void convert_to_ros_message(const custom_type &source, ros_message_type &destination)
  {
    for (auto s : source)
    {
      destination.data.push_back(s);
    }
    std::cout << &(destination.data) << " , " << &(destination.data[0]) << ":" << std::fixed << std::setprecision(9) << system_clockr.now().seconds() << std::endl;
  }

  static void convert_to_custom(const ros_message_type &source, custom_type &destination)
  {
    std::cout << &(source.data) << " , " << &(source.data[0]) << ":" << std::fixed << std::setprecision(9) << system_clockr.now().seconds() << std::endl;
    for (auto s : source.data)
    {
      destination.push_back(s);
    }
  }
};

namespace Sample
{
  class MinimalSubscriber : public rclcpp::Node
  {
  public:
    MinimalSubscriber(const rclcpp::NodeOptions &options)
        : Node("minimal_subscriber", options)
    {
      subscription_ = this->create_subscription<MyAdaptedType>(
          "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
    }

  private:
    using MyAdaptedType = rclcpp::TypeAdapter<std::vector<unsigned long long>, std_msgs::msg::UInt64MultiArray>;
    void topic_callback(const std::unique_ptr<MyAdaptedType::custom_type> &msg) const
    {
      RCLCPP_INFO_STREAM(this->get_logger(), msg->front() << " : " << msg.get() << " , " << &(*msg)[0]);
    }
    rclcpp::Subscription<MyAdaptedType>::SharedPtr subscription_;
  };
}

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(Sample::MinimalSubscriber)
```

# 比較
Docker上で様々な通信による実行を行い比較します。
比較するのは以下の4パターンです。
- 単一処理での通信(Intra-process有効)
- 単一処理での通信(Intra-process無効)
- 個別処理での通信
- 別ホスト間での通信(別Dockerコンテナを作成)

以下の4つ出力します。
- publish関数を呼び出す直前の時間
- Publishする際の型変換後の時間
- Subscribeした際の型変換前の時間
- Subscribeのコールバック関数が呼ばれた直後の時間

## 単一処理での通信(Intra-process有効)

|  | 経過時間(ms) |
| ---- | ---- |
| Pub前の変換 | 変換しない |
| 通信時間 | 0.0009227449244 |
| Sub前の変換 | 変換しない |
| 合計 | 0.0009227449244 |

さすがゼロコピーですね。
爆速です。

## 単一処理での通信(Intra-process無効)
|  | 経過時間(ms) |
| ---- | ---- |
| Pub前の変換 | 5.777174997 |
| 通信時間 | 0.3939839602 |
| Sub前の変換 | 6.321440983 |
| 合計 | 12.49259994 |

ゼロコピーほどではないですが、通信が速いですね。
これがShared Memoryによる通信だと思います。
しかし、単一処理による影響なのか、後述の通信よりも変換の速度が遅いです。

## 個別処理での通信
|  | 経過時間(ms) |
| ---- | ---- |
| Pub前の変換 | 5.302677035 |
| 通信時間 | 2.281115031 |
| Sub前の変換 | 5.324389982 |
| 合計 | 12.90818205 |

同一ホスト内での通信のはずですが遅いですね。
tcpdumpには通信が見られなかったのですが、Shared Memoryを使っていないかもです...

## 別ホスト間での通信
|  | 経過時間(ms) |
| ---- | ---- |
| Pub前の変換 | 5.535251999 |
| 通信時間 | 2.356626034 |
| Sub前の変換 | 5.343773961 |
| 合計 | 13.23565199 |

さすがに一番遅いですね。
通信時間は「個別処理での通信」よりも遅い気がしますが、単に別ホスト間の影響で0.2秒ほど遅れているだけだと考えられます。

# 感想
比較結果からゼロコピーやShared Memory Transportの早さがわかりました。点群データは今回のデータよりも数が多く、更に変換作業もほぼ必須なので研究でも取り入れていきたいです。
ただ、同一処理内でShared Memory Transportが動いていないことが気になったので、今後はそこを検証したいと思います。