0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@ntrlmt

ROS 2でコールバック関数内から別ノードのサービスを同期呼び出しする方法

0
Posted at

はじめに

ROS 2のコールバック関数内で、別ノードのROSサービスを呼び出す方法を紹介する。

ROS 1と違いROS 2では、SingleThreadedExecutorやデフォルトのコールバックグループ構成では、コールバック関数内で同期的にサービス結果を待つとデッドロックが発生することがある。

本記事では、MultiThreadExecutorとコールバックグループの設定を行うことで、コールバック関数内から別ノードのサービスの同期呼び出し方法を紹介する。

動作確認環境

  • Ubuntu 22.04 x86_64/arm64
  • ROS 2 Humble

デフォルト設定でデッドロックする原因

Subscriberやタイマーなどのコールバック関数内で別ノードのサービスを呼び出す場合、コールバックグループを設定しないデフォルト構成や、SingleThreadedExecutorを利用している場合は、サービスコール時にデッドロックが発生し、サービスの結果を取得できない。

図示すると以下のようになる。

Subscriber callback
        │
        ▼
async_send_request()
        │
        ▼
wait_for()
        │
        ▼
Executorのスレッドを占有
        │
        ▼
Service response callback（サービス応答コールバック）を実行したい
        │
        ▼
実行できない（デッドロック）

これを回避するには、MultiThreadExecutorとコールバックグループを適切に設定する必要がある。詳細は以下の方法で説明する。

方法

ノードのExecutorには、MultiThreadExecutorを使い、サービス呼び出しを入れたいコールバックの、コールバックグループに「Reentrant」を指定する。もしくは、サービス呼び出しとコールバックグループ「MutuallyExclusive」にして、別々のコールバックグループを作成する。

  • Reentrant
    • 同じコールバックグループに属する複数のコールバックが同時実行されることを許可する
  • MutuallyExclusive
    • 同じコールバックグループに属するコールバックは同時実行されない（排他処理）

Executorの詳細については、こちらの公式ドキュメントを参照。また、コールバックグループの詳細については、こちらの公式ドキュメントを参照。

ここで、MultiThreadExecutorとコールバックグループの役割を簡単に説明する。MultiThreadExecutorを使うことにより、一つのプロセス上で動作するノードのコールバックを複数スレッドで実行できるようになる。複数スレッドに対して、どのようにコールバックを割り当てるかがコールバックグループの設定となる。つまり、コールバックグループの設定により、各コールバックを同期的に動作させたり、非同期に動作させたり制御が可能となる。

image.png
（公式ドキュメントの図を引用）

サンプルコード

C++、Pythonそれぞれのサンプルコードを示す。

本サンプルでは、サービスクライアントとサービス呼び出しを行うSubscriberを同じReentrantコールバックグループへ所属させている。MutuallyExclusiveを利用する場合は、Subscriberとサービスクライアントをそれぞれ別のMutuallyExclusiveコールバックグループへ割り当てることで、同様の構成を実現できる。

C++の場合

ポイント

  • MultiThreadExecutorを利用する
  • CallbackGroupにReentrantを指定する
  • async_send_request()で非同期にサービスを送信し、future.wait_for()で結果が返るまで同期的に待機する
#include <chrono>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "std_srvs/srv/trigger.hpp"

class SampleNode : public rclcpp::Node
{
public:
    SampleNode()
    : Node("sample_node")
    {
        callback_group_ =
            create_callback_group(rclcpp::CallbackGroupType::Reentrant);

        trigger_client_ =
            create_client<std_srvs::srv::Trigger>(
                "/trigger",
                rmw_qos_profile_services_default,
                callback_group_);

        rclcpp::SubscriptionOptions options;
        options.callback_group = callback_group_;

        subscription_ =
            create_subscription<std_msgs::msg::String>(
                "/topic",
                10,
                std::bind(&SampleNode::callback, this, std::placeholders::_1),
                options);
    }

private:
    void callback(const std_msgs::msg::String::SharedPtr msg)
    {
        auto request =
            std::make_shared<std_srvs::srv::Trigger::Request>();

        auto future = trigger_client_->async_send_request(request);

        if (future.wait_for(std::chrono::seconds(3))
            != std::future_status::ready)
        {
            RCLCPP_ERROR(get_logger(), "Service call timeout");
            return;
        }

        try {
            auto response = future.get();

            if (!response->success) {
                RCLCPP_ERROR(
                    get_logger(),
                    "Service failed: %s",
                    response->message.c_str());
                return;
            }

            RCLCPP_INFO(get_logger(), "Service call succeeded");
        }
        catch (const std::exception &e) {
            RCLCPP_ERROR(
                get_logger(),
                "Service exception: %s",
                e.what());
        }
    }

    rclcpp::CallbackGroup::SharedPtr callback_group_;

    rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr trigger_client_;

    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);

    auto node = std::make_shared<SampleNode>();

    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);
    executor.spin();

    rclcpp::shutdown();

    return 0;
}

Pythonの場合

ポイント

  • MultiThreadExecutorを利用する
  • CallbackGroupにReentrantCallbackGroupを指定する
  • call_async()で非同期にサービスを送信し、Futureの完了を待って同期的に結果を取得する
    • 10msごとにFutureの完了を確認している
import time

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

from std_msgs.msg import String
from std_srvs.srv import Trigger


class SampleNode(Node):

    def __init__(self):
        super().__init__('sample_node')

        self._callback_group = ReentrantCallbackGroup()

        self._client = self.create_client(
            Trigger,
            '/trigger',
            callback_group=self._callback_group)

        self._subscription = self.create_subscription(
            String,
            '/topic',
            self.callback,
            10,
            callback_group=self._callback_group)

    def callback(self, msg):
        future = self._client.call_async(Trigger.Request())

        deadline = time.monotonic() + 3.0

        while rclpy.ok():

            if future.done():
                break

            if time.monotonic() >= deadline:
                self.get_logger().error('Service call timeout')
                return

            time.sleep(0.01)
    
        try:
            response = future.result()
        except Exception as e:
            self.get_logger().error(f'Service exception: {e}')
            return

        if not response.success:
            self.get_logger().error(
                f'Service failed: {response.message}')
            return

        self.get_logger().info('Service call succeeded')


def main(args=None):
    rclpy.init(args=args)

    node = SampleNode()

    executor = MultiThreadedExecutor()
    executor.add_node(node)

    try:
        executor.spin()
    finally:
        executor.shutdown()
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

注意事項

spin_until_future_completeを使うとうまくいかない。
spin_until_future_complete()は内部でノードをExecutorへ追加する。既に別ExecutorでspinされているノードをグローバルExecutorへ追加しようとするため問題が発生する。そのため、本記事のように既にMultiThreadExecutorでspinしているノードでは利用できない。

spin_unitil_future_completeの中身
def spin_until_future_complete(
    node: 'Node',
    future: Future,
    executor: 'Executor' = None,
    timeout_sec: float = None
) -> None:
    """
    Execute work until the future is complete.

    Callbacks and other work will be executed by the provided executor until ``future.done()``
    returns ``True`` or the context associated with the executor is shutdown.

    :param node: A node to add to the executor to check for work.
    :param future: The future object to wait on.
    :param executor: The executor to use, or the global executor if ``None``.
    :param timeout_sec: Seconds to wait. Block until the future is complete
        if ``None`` or negative. Don't wait if 0.
    """
    executor = get_global_executor() if executor is None else executor
    try:
        executor.add_node(node)
        executor.spin_until_future_complete(future, timeout_sec)
    finally:
        executor.remove_node(node)

まとめ

本記事では、MultiThreadExecutorとコールバックグループの設定を行うことで、コールバック関数内から別ノードのサービスの同期呼び出し方法を紹介した。本記事で紹介した方法は、ROS 1からの移植や同期処理が必要な場面で有効である。

なお、本記事ではROS 1との互換性を考慮して同期呼び出しを紹介したが、新規開発ではサービス完了後の処理をFutureのコールバックで実装するなど、非同期設計を採用する方法も検討するとよい。

参考

0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?