ROS入門 (15) - ROS2のアクションによる通信

ROS2のアクションによる通信をまとめました。

・Foxy

前回

1. アクションによる通信

「アクション」は、サービスと同様に「サーバー-クライアント」関係の通信を行います。サービスと異なる点は、サーバーがクライアントにフィードバックを返すことです。フィードバックとはリクエストに対する処理で発生するイベントで、フィードバックを受信したクライアントは処理の進行状況を知ることができます。

2. ワークスペースのセットアップ

「ROS入門 (9) - ROS2のパッケージの作成」と同様です。

3. アクションファイルの作成

「アクションファイル」は、アクションのデータ型を記述するテキストファイルです。拡張子は「.action」です。パッケージのactionフォルダで保持します。

現在、純粋なPythonパッケージでメッセージファイルを作成する方法はないため、CMakeパッケージでカスタムインターフェイスを作成し、それをPythonノードで利用します。

(1) 「~/colcon_ws/src」に移動し、「hello_interfaces」という名前のパッケージを作成。
「--build-type ament_cmake」で「CMakeパッケージ」を生成しています。

$ ros2 pkg create --build-type ament_cmake hello_interfaces

(2) 「~/colcon_ws/src/hello_interfaces/srv/AddTwoInts.action」を作成し、以下のように編集。

・AddTwoInts.action

int64 a
int64 b
---
int64 sum
---
float32 rate

1行毎にデータ型と変数名を記述します。「---」で区切り、上はリクエスト、中はレスポンス、下はフィードバックの情報になります。

AddTwoInts.actionから、次のメッセージが生成されます。

・AddTwoIntsAction.msg
・AddTwoIntsActionGoal.msg
・AddTwoIntsActionResult.msg
・AddTwoIntsActionFeedback.msg
・AddTwoIntsGoal.msg
・AddTwoIntsResult.msg
・AddTwoIntsFeedback.msg

これらのメッセージは、 ActionClient-ActionServer間の通信のために、 actionlibによって内部的に利用されます。

(3) package.xmlに以下の依存関係を追加。

  <buildtool_depend>rosidl_default_generators</buildtool_depend>
  <depend>action_msgs</depend>
  <member_of_group>rosidl_interface_packages</member_of_group>

(4) CMakeLists.txtを以下のように編集。

・20行目あたり

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)

set(action_files
  "action/AddTwoInts.action"
)
rosidl_generate_interfaces(${PROJECT_NAME}
  ${action_files}
)

(5) hello_interfacesパッケージのビルド。

$ colcon build --packages-select hello_interfaces

4. パッケージの作成

(1) 「~/colcon_ws/src」に移動し、「hello」という名前のパッケージを作成。
「--dependencies hello_interfaces」で依存関係に「hello_interfaces」を追加しています。

$ cd ~/colcon_ws/src
$ ros2 pkg create --build-type ament_python hello --dependencies hello_interfaces

(2) 「setup.py」を以下のように編集。

・entry_points
利用するPythonスクリプト名を指定します。

from setuptools import setup

package_name = "hello"

setup(
       :
    entry_points={
        "console_scripts": [
            "client = hello.client:main",
            "server = hello.server:main",
        ]
    },
)

5. ソースコードの作成

(1) 「~/colcon_ws/src/hello/hello/client.py」を作成し、以下のように編集。

・client.py

import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from hello_interfaces.action import AddTwoInts

# クライアント
class MyClient(Node):
    # 初期化
    def __init__(self):
        super().__init__("my_client")
        self.client = ActionClient(self, AddTwoInts, 'add_two_ints')

    # リクエストの送信
    def send_request(self):
        # メッセージの生成
        self.a = 1
        self.b = 2
        goal = AddTwoInts.Goal()
        goal.a = self.a
        goal.b = self.b

        # サーバー接続の待機
        self.client.wait_for_server()

        # リクエストの送信
        self.future = self.client.send_goal_async(goal, feedback_callback=self.feedback_callback)
        self.future.add_done_callback(self.response_callback)

    # フィードバックの受信時に呼ばれる
    def feedback_callback(self, feedback):
        print("feedback :", feedback.feedback.rate)

    # レスポンスの受信時に呼ばれる
    def response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
           return

        # 結果の取得
        self.result_future = goal_handle.get_result_async()
        self.result_future.add_done_callback(self.result_callback)

    # 結果の受信時に呼ばれる
    def result_callback(self, future):
        result = future.result().result
        print("%s + %s = %s"%(self.a, self.b, result.sum))
        rclpy.shutdown()

# メイン
def main(args=None):
    # ROS通信の初期化
    rclpy.init(args=args)

    # クライアントの生成
    client = MyClient()

    # リクエストの送信
    client.send_request()

    # ノード終了の待機
    rclpy.spin(client)

if __name__ == '__main__':
    main()

(2) 「~/colcon_ws/src/hello/hello/server.py」を作成し、以下のように編集。

・server.py

import rclpy
import time
from rclpy.action import ActionServer
from rclpy.node import Node
from hello_interfaces.action import AddTwoInts

# サーバー
class MyServer(Node):
    # 初期化
    def __init__(self):
        super().__init__('my_server')

        # サーバーの生成
        self.server = ActionServer(self,
            AddTwoInts, "add_two_ints", self.listener_callback)

    # リクエストの受信時に呼ばれる
    def listener_callback(self, goal_handle):
        # フィードバックの返信
        for i in range(10):
            feedback = AddTwoInts.Feedback()
            feedback.rate = i * 0.1
            goal_handle.publish_feedback(feedback)
            time.sleep(0.5)

        # レスポンスの返信
        goal_handle.succeed()
        result = AddTwoInts.Result()
        result.sum = 3
        return result

# メイン
def main(args=None):
    # ROS通信の初期化
    rclpy.init(args=args)

    # サーバーの生成
    server = MyServer()

    # ノード終了の待機
    rclpy.spin(server)

if __name__ == '__main__':
    main()

(3) helloパッケージのビルド。

$ cd ~/colcon_ws
$ colcon build --packages-select hello

(4) ワークスペースのセットアップ。
ワークスペースのセットアップはビルド毎にも必要になります。

$ source ~/colcon_ws/install/setup.bash

6. 実行

(1) ターミナルを開き、「server」を実行。
クライアントからリクエストを受け取る準備ができました。

$ ros2 run hello server

(2) もう1つのターミナルを開き、「client」を実行。
2つの値がサーバーに渡され、計算結果が返されます。

$ ros2 run hello client
feedback : 0.0
feedback : 0.10000000149011612
feedback : 0.20000000298023224
feedback : 0.30000001192092896
feedback : 0.4000000059604645
feedback : 0.5
feedback : 0.6000000238418579
feedback : 0.699999988079071
feedback : 0.800000011920929
feedback : 0.8999999761581421
1 + 2 = 3

7. 参考

Creating an action — ROS 2 Documentation: Foxy documentation

Writing an action server and client (Python) — ROS 2 Documentation: Foxy documentation

Actions — rclpy 0.6.1 documentation

ROS1 の SimpleActionServer を ROS2 で書き換える - Qiita

次回

Unityではじめる ROS・人工知能 ロボットプログラミング実践入門