ROS2で一つのプロセスに複数のノードを実装する。

この記事は、ROS2での基本的なノードの作成等チュートリアルを完了している人が対象となります。

ROS1では1プロセスに1ノードを動かすのが基本でしたが、ROS2では1つのプロセスで複数のノードを動かすことが出来ます。今回はその方法を記しながら、1プロセスの中に2つのノードが入ったものを作成していきます。
また、その複数のノードをシングルスレッドで動かすかマルチスレッドで動かすかの設定方法も見ていきます。

環境

・ROS2: foxy
・Ubuntu20.04
【開発言語】
・C++、Python

流れ

・1プロセス1ノードのおさらい
・1プロセス複数ノードの作り方
・シングルスレッド、マルチスレッドの切り替え方

1プロセス1ノードのおさらい

1プロセス1ノードの基本的なパブリッシャーのサンプルを下記に記載します。
publisher_member_function.cpp

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

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

using namespace std::chrono_literals;

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

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

    void timer_callback() {
        auto message = std_msgs::msg::String();
        message.data = "Hello, world1! " + std::to_string(count_++);
        RCLCPP_INFO(this->get_logger(), "Publishing1: '%s'", message.data.c_str());
        publisher_->publish(message);
    }
public:
    MinimalPublisher1() : Node("minimal_publisher1"), count_(0) {
        count_ = 0;
        publisher_ = this->create_publisher<std_msgs::msg::String>("topic1", 10);
        timer_ = this->create_wall_timer(500ms, std::bind(&MinimalPublisher1::timer_callback, this));
    }
};

int main(int argc, char * argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MinimalPublisher1>());
    rclcpp::shutdown();
    return 0;
}

これは0.5秒に一回、文字列をパブリッシュするノードです。
MinimalPublisher1を定義して、main関数でMinimalPublisher1をspinしています。

1プロセス複数ノードの作り方

作り方は簡単です。
まず複数のノードクラスを定義します。そしてmain関数でSingleThreadedExecuterに渡してからspinするだけです。
下記に、1プロセス2ノードのサンプルコードを記載します。

C++の場合

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

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

using namespace std::chrono_literals;

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

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

    void timer_callback() {
        auto message = std_msgs::msg::String();
        message.data = "Hello, world1! " + std::to_string(count_++);
        RCLCPP_INFO(this->get_logger(), "Publishing1: '%s'", message.data.c_str());
        publisher_->publish(message);
    }
public:
    MinimalPublisher1() : Node("minimal_publisher1"), count_(0) {
        count_ = 0;
        publisher_ = this->create_publisher<std_msgs::msg::String>("topic1", 10);
        timer_ = this->create_wall_timer(500ms, std::bind(&MinimalPublisher1::timer_callback, this));
    }
};

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

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

    void timer_callback() {
        auto message = std_msgs::msg::String();
        message.data = "Hello, world2! " + std::to_string(count_++);
        RCLCPP_INFO(this->get_logger(), "Publishing2: '%s'", message.data.c_str());
        publisher_->publish(message);
    }
public:
    MinimalPublisher2() : Node("minimal_publisher2"), count_(0) {
        count_ = 0;
        publisher_ = this->create_publisher<std_msgs::msg::String>("topic2", 10);
        timer_ = this->create_wall_timer(500ms, std::bind(&MinimalPublisher2::timer_callback, this));
    }
};

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

    auto minimal_publisher_node1 = std::make_shared<MinimalPublisher1>();
    auto minimal_publisher_node2 = std::make_shared<MinimalPublisher2>();

    exec.add_node(minimal_publisher_node1);
    exec.add_node(minimal_publisher_node2);
    exec.spin();

    rclcpp::shutdown();
    return 0;
}

1プロセス1ノードとの差分はMinimalPublisher2ノードクラスの定義が追加されていることと、main関数でSingleThreadedExecutorをインスタンス化しており、それに作ったクラスノードをadd_nodeしてからexec.spin()していることです。
このSingleThreadedExecutorで複数ノードの実行を管理しています。
実際にこのプロセスを実行すると、/topic1と/topic2がパブリッシュされているのが確認できると思います。

root@b0a51954602b:/# ros2 topic echo /topic1
data: Hello, world! 50
---
data: Hello, world! 51
---

root@b0a51954602b:/# ros2 topic echo /topic2
data: Hello, world! 60
---
data: Hello, world! 61
---

Pythonの場合

Python も C++ とほぼ同じ方法になります。
Python ではmain関数は下記のようになります
SingleThreadedExecutorをimportするのを忘れないようにしましょう。

こちらのコードサンプルはデバッグしていないので、エラーが出るかもしれないです。

import rclpy
from rclpy.node import Node
from rclpy.executors import SingleThreadedExecutor

def main:
    rclpy.init(args=args)
    exec = SingleThreadedExecutor()

    minimal_publisher_node1 = MinimalPublisher1()
    minimal_publisher_node2 = MinimalPublisher2()
    exec.add_node(minimal_publisher_node1)
    exec.add_node(minimal_publisher_node2)
    exec.spin()

    exec.shutdown()
    minimal_publisher_node1.destroy_node()
    minimal_publisher_node2.destroy_node()
    rclpy.shutdown()

シングルスレッド、マルチスレッドの切り替え方

簡単です。
今までSingleThreadedExecutorとしていたところをMultiThreadedExecutorに変更するだけです。

C++の場合

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

    auto minimal_publisher_node1 = std::make_shared<MinimalPublisher1>();
    auto minimal_publisher_node2 = std::make_shared<MinimalPublisher2>();

    exec.add_node(minimal_publisher_node1);
    exec.add_node(minimal_publisher_node2);
    exec.spin();

    rclcpp::shutdown();
    return 0;
}

Pythonの場合

こちらはデバッグしていません。

import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor

def main:
    rclpy.init(args=args)
    exec = MultiThreadedExecutor()

    minimal_publisher_node1 = MinimalPublisher1()
    minimal_publisher_node2 = MinimalPublisher2()
    exec.add_node(minimal_publisher_node1)
    exec.add_node(minimal_publisher_node2)
    exec.spin()

    exec.shutdown()
    minimal_publisher_node1.destroy_node()
    minimal_publisher_node2.destroy_node()
    rclpy.shutdown()

C++、Python共にMultiThreadedExecutorのインスタンス化時にスレッド数を指定することもできます。詳しくは下記URLを。
C++: rclcpp::executors::MultiThreadedExecutor Class Reference
Python: Execution and Callbacks