【ROS×Python】アクション(Action)通信を実装しよう！

## アクション定義例

# Goal
int32 order
---
# Result
int32[] sequence
---
# Feedback
int32[] sequence

#--------------------------------------------------
# 為替レートリストから単純移動平均を計算した結果を返す
# アクション定義
#--------------------------------------------------

##### Goal #####
# 区切り範囲
int32 window

# 為替レートリスト
float32[] price_raw_list

---
##### Result #####
# 単純移動平均計算結果
float32[] price_sma_list

---
##### Feedback #####
# 進捗[%]
int32 progress

・・・
・・・
・・・
set(act_files
  # "action/***.action"
  "action/SimpleMovingAverage.action"
)
・・・
・・・
・・・

import time
import numpy as np
import rclpy
from rclpy.executors import ExternalShutdownException
from rclpy.node import Node
from rclpy.action import ActionServer
from rclpy.action import GoalResponse
from ros2_example_msgs.action import SimpleMovingAverage


class SimpleMovingAverageServer(Node):
    """
    単純移動平均計算アクションServerノード
    """

    def __init__(self) -> None:
        """
        ノードの初期化
        """
        super().__init__("sma_server")

        # ロガー取得
        self.logger = self.get_logger()

        # smaアクションserverの定義
        self.act_srv = ActionServer(
            self,
            SimpleMovingAverage,
            "sma",
            self._execute_callback,
            goal_callback=self._goal_callback,
        )

    def _goal_callback(self, goal_request) -> GoalResponse:
        """
        アクション・Goal要求コールバック
        """
        self.logger.info("＜アクション：Goal受信＞")
        self.logger.info("　区切り範囲(window):[{}]".format(goal_request.window))
        self.logger.info("　データ長:[{}]".format(len(goal_request.price_raw_list)))

        if goal_request.window < 1:
            self.logger.warn("　Goal Request: REJECT")
            self.logger.warn("　　区切り範囲(window)が0以下です。")
            return GoalResponse.REJECT

        if len(goal_request.price_raw_list) < goal_request.window:
            self.logger.warn("　Goal Request: REJECT")
            self.logger.warn("　　データ長が区切り範囲(window)より小さいです。")
            return GoalResponse.REJECT

        self.logger.info("　Goal Request: ACCEPT")
        return GoalResponse.ACCEPT

    def _execute_callback(self, goal_handle):
        """
        アクション実行コールバック
        """
        # アクションの実行
        self.logger.info("アクション実行中・・・")

        # Feedbackメッセージの定義
        fb_msg = SimpleMovingAverage.Feedback()
        fb_msg.progress = 0

        # Resultの定義
        result = SimpleMovingAverage.Result()

        goal = goal_handle.request
        price_len = len(goal.price_raw_list)
        prg_rate = float(100.0 / (price_len - goal.window + 1))

        # リスト位置 0〜goal.window-1 は計算不可のためNaNで埋める
        for i in range(0, goal.window - 1):
            result.price_sma_list.append(np.nan)

        # リスト位置 goal.window-1 以降で計算
        for i in range(goal.window, price_len + 1):
            # 移動平均の計算
            price_sum = 0.0
            for j in range(i - goal.window, i):
                price_sum += goal.price_raw_list[j]
            result.price_sma_list.append(price_sum / goal.window)

            # 0.5秒待機（重たい処理の代わり）
            time.sleep(0.5)

            # アクションのフィードバックの送信
            fb_msg.progress = int(prg_rate * (i - goal.window + 1))
            goal_handle.publish_feedback(fb_msg)
            self.logger.info("フィードバック送信：進捗{}%".format(fb_msg.progress))

        # アクションステータスに"成功"をセット
        goal_handle.succeed()
        self.get_logger().info("Goal成功")
        # アクション実行結果を送信
        return result


def main(args: list[str] | None = None) -> None:
    # ROSの初期化
    rclpy.init(args=args)
    # sma_serverノードの作成
    mas = SimpleMovingAverageServer()
    mas.get_logger().info("sma_server start!")

    try:
        # ノードの実行開始
        rclpy.spin(mas)
    except (KeyboardInterrupt, ExternalShutdownException):
        pass
    else:
        # ROSのシャットダウン
        rclpy.shutdown()
    finally:
        # ノードの破棄
        mas.destroy_node()

# smaアクションserverの定義
self.act_srv = ActionServer(
    self,
    SimpleMovingAverage,
    "sma",
    self._execute_callback,
    goal_callback=self._goal_callback,
)

def _goal_callback(self, goal_request) -> GoalResponse:
    """
    アクション・Goal要求コールバック
    """
    self.logger.info("＜アクション：Goal受信＞")
    self.logger.info("　区切り範囲(window):[{}]".format(goal_request.window))
    self.logger.info("　データ長:[{}]".format(len(goal_request.price_raw_list)))

    if goal_request.window < 1:
        self.logger.warn("　Goal Request: REJECT")
        self.logger.warn("　　区切り範囲(window)が0以下です。")
        return GoalResponse.REJECT

    if len(goal_request.price_raw_list) < goal_request.window:
        self.logger.warn("　Goal Request: REJECT")
        self.logger.warn("　　データ長が区切り範囲(window)より小さいです。")
        return GoalResponse.REJECT

    self.logger.info("　Goal Request: ACCEPT")
    return GoalResponse.ACCEPT

# Resultの定義
result = SimpleMovingAverage.Result()

# 移動平均の計算
price_sum = 0.0
for j in range(i - goal.window, i):
    price_sum += goal.price_raw_list[j]
result.price_sma_list.append(price_sum / goal.window)

# アクション実行結果を送信
return result

# Feedbackメッセージの定義
fb_msg = SimpleMovingAverage.Feedback()

# アクションのフィードバックの送信
fb_msg.progress = int(prg_rate * (i - goal.window + 1))
goal_handle.publish_feedback(fb_msg)
self.logger.info("フィードバック送信：進捗{}%".format(fb_msg.progress))

# アクションステータスに"成功"をセット
goal_handle.succeed()

import matplotlib.pyplot as plt
import rclpy
from rclpy.executors import ExternalShutdownException
from rclpy.node import Node
from rclpy.task import Future
from rclpy.action import ActionClient
from action_msgs.msg import GoalStatus
from ros2_example_msgs.action import SimpleMovingAverage


class SimpleMovingAverageClient(Node):
    """
    単純移動平均計算アクションClientノード
    """

    def __init__(self) -> None:
        """
        ノードの初期化
        """
        super().__init__("sma_client_async")

        # ロガー取得
        self.logger = self.get_logger()

        # smaアクションclientの定義
        self.act_cli = ActionClient(self, SimpleMovingAverage, "sma")

        # アクションServerが有効になるまで待機
        while not self.act_cli.wait_for_server(timeout_sec=1.0):
            self.logger.info("アクションServerが有効になるまで待機中・・・")

        # Future初期化
        self._goal_future = Future()
        self._result_future = Future()

        self._price_raw_list = []
        self._window = 0

    def send_action_goal_async(self, window: int, price_raw_list: list[float]) -> None:
        """
        アクション・ゴールの送信（非同期）
        """
        # smaアクションの引数
        goal_msg = SimpleMovingAverage.Goal()
        goal_msg.window = window
        for price_raw in price_raw_list:
            goal_msg.price_raw_list.append(price_raw)

        # アクションの非同期実行
        self.logger.info("＜アクション：Goal送信＞")
        self._goal_future = self.act_cli.send_goal_async(
            goal_msg, self._feedback_callback
        )
        self._goal_future.add_done_callback(self._goal_response_callback)

        self._price_raw_list = price_raw_list
        self._window = window

    def _feedback_callback(self, feedback) -> None:
        """
        アクション・フィードバックコールバック
        """
        self.logger.info("フィードバック受信：進捗{}%".format(feedback.feedback.progress))

    def _goal_response_callback(self, future) -> None:
        """
        アクション・Goal応答コールバック
        """
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.logger.info("　Goal拒否されました。")
            # Shutdown after receiving a result
            raise ExternalShutdownException

        self.logger.info("　Goal受理されました。")
        self._result_future = goal_handle.get_result_async()
        self._result_future.add_done_callback(self._get_result_callback)

    def _get_result_callback(self, future) -> None:
        """
        アクション・Result応答コールバック
        """
        result = future.result().result
        status = future.result().status
        if status == GoalStatus.STATUS_SUCCEEDED:
            self.logger.info("Goal成功")

            # ********** 描写 **********
            label_sma = "SMA(window:" + str(self._window) + ")"
            plt.plot(self._price_raw_list, label="Raw value")
            plt.scatter(range(len(self._price_raw_list)), self._price_raw_list)
            plt.plot(result.price_sma_list, label=label_sma)
            plt.scatter(range(len(result.price_sma_list)), result.price_sma_list)
            plt.legend()
            plt.title("Simple Moving Average(SMA)")
            plt.grid(linestyle="dashed")
            plt.show()
        else:
            self.logger.info("Goal失敗 (status: {})".format(status))

        # Shutdown after receiving a result
        raise ExternalShutdownException


def main(args: list[str] | None = None) -> None:
    # ROSの初期化
    rclpy.init(args=args)
    # sma_client_asyncノードの作成
    mac = SimpleMovingAverageClient()
    logger = mac.get_logger()
    logger.info("sma_client start!")

    # 単純移動平均計算用の生データ
    price_raw_list = [
        100.0,
        102.0,
        105.0,
        112.0,
        120.0,
        122.0,
        118.0,
        110.0,
        98.0,
        88.0,
        85.0,
        90.0,
        110.0,
        125.0,
    ]

    # 区切り範囲をキーボード入力
    window = int(input("windows size: "))

    # アクション・ゴールの送信（非同期）
    mac.send_action_goal_async(window, price_raw_list)

    try:
        # ノードの実行開始
        rclpy.spin(mac)
    except (KeyboardInterrupt, ExternalShutdownException):
        pass
    else:
        # ROSのシャットダウン
        rclpy.shutdown()
    finally:
        # ノードの破棄
        mac.destroy_node()

# smaアクションclientの定義
self.act_cli = ActionClient(self, SimpleMovingAverage, "sma")

# アクションServerが有効になるまで待機
while not self.act_cli.wait_for_server(timeout_sec=1.0):
    self.logger.info("アクションServerが有効になるまで待機中・・・")

# アクションの非同期実行
self.logger.info("アクション：Goal送信")
self._goal_future = self.act_cli.send_goal_async(
    goal_msg, self._feedback_callback
)

def _feedback_callback(self, feedback) -> None:
    """
    アクション・フィードバックコールバック
    """
    self.logger.info("フィードバック受信：進捗{}%".format(feedback.feedback.progress))

self._goal_future.add_done_callback(self._goal_response_callback)

def _goal_response_callback(self, future) -> None:
    """
    アクション・Goal応答コールバック
    """
    goal_handle = future.result()
    if not goal_handle.accepted:
        self.logger.info("　Goal拒否されました。")
        # Shutdown after receiving a result
        raise ExternalShutdownException

    self.logger.info("　Goal受理されました。")
    self._result_future = goal_handle.get_result_async()
    self._result_future.add_done_callback(self._get_result_callback)

def _get_result_callback(self, future) -> None:
    """
    アクション・Result応答コールバック
    """
    result = future.result().result
    status = future.result().status
    if status == GoalStatus.STATUS_SUCCEEDED:
        self.logger.info("Goal成功")

        # ********** 描写 **********
        label_sma = "SMA(window:" + str(self._window) + ")"
        plt.plot(self._price_raw_list, label="Raw value")
        plt.scatter(range(len(self._price_raw_list)), self._price_raw_list)
        plt.plot(result.price_sma_list, label=label_sma)
        plt.scatter(range(len(result.price_sma_list)), result.price_sma_list)
        plt.legend()
        plt.title("Simple Moving Average(SMA)")
        plt.grid(linestyle="dashed")
        plt.show()
    else:
        self.logger.info("Goal失敗 (status: {})".format(status))

    # Shutdown after receiving a result
    raise ExternalShutdownException