ROS2でプロセス内通信によるゼロコピーを試す。

ROSを開発で使用すると、その提供されたプロセス間通信の機能により、

> 1. 特に何も考えなくてもマルチコアの恩恵を受けられる
> 2. システムの一部(あるプロセス)がバグで死んでも、システム全体は死なない
> 3. 複数マシンでの分散システムに自然と拡張できる
> 4. コードの再利用性が高くなる

といったような恩恵を受けることが出来ます。(引用元: [Qiita - ROSチュートリアル第二回 -- ROSの通信モデル](https://qiita.com/garaemon/items/15c17fd7e216013e81dc))
しかしその代わり、通信するデータの大きさによってはパフォーマンスが落ちる可能性があります。
例えば、高解像度画像や、Lidarのような大規模センサーデータ等をパブリッシュすると、後段のノードがデータを受け取るまでに幾らかの遅延が発生する可能性があります。

そこでROS1ではnodeletといったプロセス内通信の仕組みを使うことでこの問題を解消していました。
ROS2でもいくつか方法が用意されており、
・一つのプロセスに複数ノードを実装し、その中でプロセス内通信をすることによるゼロコピーの実現
・外部ライブラリによるプロセス間通信によるゼロコピーの実現
などがあります。

今回この記事では上の、「一つのプロセスに複数ノードを実装し、その中でプロセス内通信をすることによるゼロコピーの実現」の方法について見ていきます。

公式のドキュメント等にも方法の記載がありますが、パフォーマンスの可視化のためかごちゃごちゃしたサンプルコードとなっており、実際にどこをどうしたらゼロコピーが実現できるか非常に分かりにくいです。なのでこの記事ではパフォーマンス比較等はせず、純粋にゼロコピーの実現方法とゼロコピーできているかどうかの確認のみ行っていきます。
手っ取り早くゼロコピーの方法を知りたい人は、最後の項目だけ見ていただければわかるかと思います。


【参考URL】
[Tire IV Tech Blog - ROS2 rmwを切り替えてpub/sub通信しよう](https://tech.tier4.jp/entry/2020/12/23/160000)
[ROS 2 Documentation - Efficient intra-process communication](https://docs.ros.org/en/foxy/Tutorials/Intra-Process-Communication.html?highlight=intra)
[github -  harihitode/pubsub](https://github.com/harihitode/pubsub)
[ROS 2のプロセス内（intra-process）通信を理解する (2)](https://www.youtalk.jp/2017/06/06/ros2-intra-process-demo.html)
[C++ ROS Client Library API - use_intra_process_comms()](https://docs.ros2.org/dashing/api/rclcpp/classrclcpp_1_1NodeOptions.html#adcad44e6fef3a5d1dcc8e3bc9ccf9e2a)

# 環境
・Ubuntu20.04
・ROS2: foxy

# 流れ
・一つのプロセスに複数ノードを実装し、ゼロコピーではないプロセス内通信を試す
・ゼロコピーのプロセス内通信となるようにコードを変更する

# 一つのプロセスに複数ノードを実装し、ゼロコピーではないプロセス内通信を試す
まずはこちらの記事を参考に一つのプロセスに複数ノードを実装します。
[Qiita - ROS2で一つのプロセスに複数のノードを実装する。](https://qiita.com/shigeharu_shibahata/items/60f547d2ea9ccd8c1b6b)

```cpp
#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;
using std::placeholders::_1;

class MinimalPublisher : public rclcpp::Node {
    size_t count_;

    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;

    void timer_callback() {
        auto msg = std_msgs::msg::String();
        msg.data = "Hello, world! " + std::to_string(count_++);
        RCLCPP_INFO(this->get_logger(), "Publishing: '%s', 0x%x", msg.data.c_str(), &(msg.data));
        publisher_->publish(msg);
    }
public:
    MinimalPublisher() : Node("minimal_publisher"), count_(0) {
        count_ = 0;
        publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
        timer_ = this->create_wall_timer(500ms, std::bind(&MinimalPublisher::timer_callback, this));
    }
};


class MinimalSubscriber : public rclcpp::Node {
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;

    void topic_callback(const std_msgs::msg::String::SharedPtr msg) const {
        RCLCPP_INFO(this->get_logger(), "  I heard: '%s', 0x%x\n", msg->data.c_str(), &(msg->data));
    }
public:
    MinimalSubscriber() : Node("minimal_subscriber") {
        subscription_ = this->create_subscription<std_msgs::msg::String>("topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
    }
};


int main(int argc, char * argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::executors::SingleThreadedExecutor exec;

    auto minimal_publisher_node = std::make_shared<MinimalPublisher>();
    auto minimal_subscriber_node = std::make_shared<MinimalSubscriber>();

    exec.add_node(minimal_publisher_node);
    exec.add_node(minimal_subscriber_node);
    exec.spin();

    rclcpp::shutdown();
    return 0;
}
```

このサンプルコードは、`Hello, world! 0` といった文字列を0.5秒間隔でパブリッシュしてサブスクライブするプログラムです。パブリッシュするときとサブスクライブする時に、送ったり受信したデータのメモリアドレスを一緒に表示するようになっています。
このコードを実行すると、

```sh
[INFO] [1644832817.889490504] [minimal_publisher]: Publishing: 'Hello, world! 0', 0xd9124bd8
[INFO] [1644832817.889892171] [minimal_subscriber]:   I heard: 'Hello, world! 0', 0xe014eac0

[INFO] [1644832818.391199379] [minimal_publisher]: Publishing: 'Hello, world! 1', 0xd9124bd8
[INFO] [1644832818.392608254] [minimal_subscriber]:   I heard: 'Hello, world! 1', 0xe044d420

[INFO] [1644832818.894907546] [minimal_publisher]: Publishing: 'Hello, world! 2', 0xd9124bd8
[INFO] [1644832818.897286005] [minimal_subscriber]:   I heard: 'Hello, world! 2', 0xe044d6f0
```

このように表示されます。パブリッシャー側のデータのメモリアドレスが`0xd9124bd8`、サブスクライバー側のデータのメモリアドレスが`0xe014eac0`等となっており、別のアドレスを参照しているのがわかるかと思います。つまりこれはプロセス内通信であってもゼロコピーになっておらず、データはコピーされているということです。

# ゼロコピーのプロセス内通信となるようにコードを変更する
やることは2つです。
・ノードのコンストラクタで`rclcpp::Node`を継承するときにゼロコピーを有効にするように設定する。
・パブリッシュサブスクライブするメッセージデータ型をユニークポインタ(`UniquePtr`)にする

サンプルコードを下記に示します。

```cpp
#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;
using std::placeholders::_1;

class MinimalPublisher : public rclcpp::Node {
    size_t count_;

    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;

    void timer_callback() {
        std_msgs::msg::String::UniquePtr msg(new std_msgs::msg::String);
        msg->data = "Hello, world! " + std::to_string(count_++);
        RCLCPP_INFO(this->get_logger(), "Publishing: '%s', 0x%x", msg->data.c_str(), &(msg->data));
        publisher_->publish(std::move(msg));
    }
public:
    MinimalPublisher() : Node("minimal_publisher", rclcpp::NodeOptions().use_intra_process_comms(true)), count_(0) {
        count_ = 0;
        publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
        timer_ = this->create_wall_timer(500ms, std::bind(&MinimalPublisher::timer_callback, this));
    }
};


class MinimalSubscriber : public rclcpp::Node {
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;

    void topic_callback(const std_msgs::msg::String::UniquePtr msg) const {
        RCLCPP_INFO(this->get_logger(), "  I heard: '%s', 0x%x\n", msg->data.c_str(), &(msg->data));
    }
public:
    MinimalSubscriber() : Node("minimal_subscriber", rclcpp::NodeOptions().use_intra_process_comms(true)) {
        subscription_ = this->create_subscription<std_msgs::msg::String>("topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
    }
};


int main(int argc, char * argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::executors::SingleThreadedExecutor exec;

    auto minimal_publisher_node = std::make_shared<MinimalPublisher>();
    auto minimal_subscriber_node = std::make_shared<MinimalSubscriber>();

    exec.add_node(minimal_publisher_node);
    exec.add_node(minimal_subscriber_node);
    exec.spin();

    rclcpp::shutdown();
    return 0;
}
```

## `rclcpp::Node`を継承するときにゼロコピーを有効にするように設定する。

変更箇所を見ていきます。
まずは`MinimalPublisher`のコンストラクタの継承部です。

```cpp
    MinimalPublisher() : Node("minimal_publisher", rclcpp::NodeOptions().use_intra_process_comms(true)), count_(0) {
```

ゼロコピーでない方は継承する`Node`の初期化が`Node("minimal_publisher")`となっていましたが、ゼロコピーの方では`Node("minimal_publisher", rclcpp::NodeOptions().use_intra_process_comms(true))`となっています。この2つ目の引数の`true`、`false`でゼロコピーするかどうかを設定できます。

`MinimalSubscriber`も同様です。

```cpp
    MinimalSubscriber() : Node("minimal_subscriber", rclcpp::NodeOptions().use_intra_process_comms(true)) {
```

## メッセージデータ型をユニークポインタ(`UniquePtr`)にする

次に2つ目の変更点、メッセージの型を`UniquePtr`にするところです。下記はパブリッシャー側

```cpp
        std_msgs::msg::String::UniquePtr msg(new std_msgs::msg::String);
        msg->data = "Hello, world! " + std::to_string(count_++);
        RCLCPP_INFO(this->get_logger(), "Publishing: '%s', 0x%x", msg->data.c_str(), &(msg->data));
        publisher_->publish(std::move(msg));
```

ゼロコピーでない方はメッセージの型は`std_msgs::msg::String`となっていましたが、ゼロコピーの方では`std_msgs::msg::String::UniquePtr`となっています。それに伴い`msg`がポインタになるので`data`への参照はアロー演算子によって行われます。また、パブリッシュする際には`msg`を`std::move()`しています。`UniquePtr`の特性上、パブリッシャー側では`std::move()`以降は`msg`を使用できなくなるので気を付けてください。

そしてサブスクライバー側のメッセージの型変更です。

```cpp
    void topic_callback(const std_msgs::msg::String::UniquePtr msg) const {
        RCLCPP_INFO(this->get_logger(), "  I heard: '%s', 0x%x\n", msg->data.c_str(), &(msg->data));
    }
```

コールバック関数の引数の型が`const std_msgs::msg::String::SharedPtr`から`const std_msgs::msg::String::UniquePtr`に変更されています。

型変更で気を付けてほしいのは、`publisher`、`subscriber`自体の型テンプレートには変更がないことです。「パブリッシャーの型、サブスクライバーの型」ではなくて「メッセージの型」のみを`UniquePtr`にする必要があることに気を付けてください。

このコードを実行すると下記のようになります。

```sh
[INFO] [1644834196.930965753] [minimal_publisher]: Publishing: 'Hello, world! 0', 0xd6ccfc40
[INFO] [1644834196.931521170] [minimal_subscriber]:   I heard: 'Hello, world! 0', 0xd6ccfc40

[INFO] [1644834197.429930754] [minimal_publisher]: Publishing: 'Hello, world! 1', 0xd6ccfc40
[INFO] [1644834197.430535754] [minimal_subscriber]:   I heard: 'Hello, world! 1', 0xd6ccfc40

[INFO] [1644834197.929954004] [minimal_publisher]: Publishing: 'Hello, world! 2', 0xd6ccfc40
[INFO] [1644834197.930862962] [minimal_subscriber]:   I heard: 'Hello, world! 2', 0xd6ccfc40
```

パブリッシュ側とサブスクライブ側で同じメモリアドレスを参照しており、ゼロコピーが実現できていることを確認できます。