ROS2講座18 actionを使う

# 環境

この記事は以下の環境で動いています。

| 項目 |	値 |
|:--|:--|
| CPU | Core i5-8250U |
| Ubuntu | 22.04 |
| ROS2 | Humble |

# 概要

ROS1ではactionlibとして実装されていた機能です。ROS2からはrclcpp_actionとしてより強いサポートとなりました。
通常のROSトピックと同じようにmsgを定義して、client側、server側のROSノードの間でやり取りをします。topicとは違い双方向のやり取りをすることが特徴です。
構成が複雑になるために使いすぎは厳禁です。actionが必要になる=双方向のやり取りが必要になる=（本来なら）ノードを分けずに一緒に実装すべき です。

## 接続

多くの説明ではactionというROS2の接続の方式があるように記載されていますが、中身は3つのserviceと2つのtopicからなります。それぞれ`_action`という隠し名がnamespaceに入っているために`ros2 topic list`や`ros2 service list`では出てきません。`ros2 service list --include-hidden-services`の様にするとlistに出てきます。

|名前|type|方向|説明|
|:--|:--|:--|:--|
|send_goal|service|client->server|goal指令時にrequest、serverでacceptするとresponse|
|cancel_goal|service|client->server|cancel指令時にrequest、serverでacceptするとresponse|
|get_result|service|client->server|goal指令時にrequest、result決定時にresponse|
|feedback|topic|client<-server|serverでfeedback指定時に送信|
|status|topic|client<-server|serverでの状態変化時に送信


```plantuml
hide empty members

object client_node
object server_node

client_node-->server_node: /server_node/task\n/_action/cancel_goal
client_node-->server_node: /server_node/task\n/_action/get_result
client_node-->server_node: /server_node/task\n/_action/send_goal

client_node<..server_node: /server_node/task\n/_action/feedback
client_node<..server_node: /server_node/task\n/_action/status
```

シーケンスをいかにまとめます。のちに出てくるclient側、server側のAPIも一緒に書き入れています。
* client側はClientGoalHandleを使うcallback型の実装と使わないasync型の実装がありますが、前者の例で書いています。
* client側でgoalを送ると、server側では`handleGoal()`が呼ばれて、そのgoalを受け入れるか拒否するかを選びます。受け入れた場合は`handleAccepted()`が呼ばれます。この2段階のcallbackが呼ばれるシーケンスがactionlibとの一番の違いです。
* serverでfeedbackの送信をすると、最終的にはclient側でcallbackが呼ばれます。
* server側で`success()`を実行すると最終的にclient側で`resultCallback`が呼ばれます。

```plantuml
== goalの指令 ==
participant client_user
participant client_goal_handle
participant client_lib
participant server_lib
participant server_goal_handle
participant server_user
client_user -> client_lib: async_send_goal()
client_lib -> server_lib: goal(send_goal request)
server_lib -> server_user: handleGoal()
server_lib <- server_user: ACCEPT
server_lib -> server_user: handleAccepted()
server_lib <- server_user
activate server_goal_handle

client_lib <- server_lib: accept(send_goal response)
client_user <- client_lib: goalResponseCallback()
activate client_goal_handle

== feedbackの送信 ==
loop
server_goal_handle <-- server_user: publish_feedback()
client_lib <-- server_goal_handle : feedback
client_user <-- client_lib: feedbackCallback()
end

== resultの送信 ==
server_goal_handle <- server_user: succeed()
client_lib <- server_goal_handle : result(SUCCESS)
deactivate server_goal_handle
client_user <-- client_lib: resultCallback()
deactivate client_goal_handle
```

# ソースコード

## actionのメッセージ定義&CMakeListの記述

最初にactionの型を決めるmsgファイルです。拡張子は`.action`にします。
上らか順に、goalの型、resultの型、feedbackの型になります。
後のactionを使うノードとは別のmsg記述専用のpackageを作ります。

```msg:ros2_lecture_msgs/action/WaitTask.action
string name
float32 duration
---
string message
---
float32 left_duration
```

actionメッセージを変換する処理のためにpackage.xmlでは依存の追加、CMakeListではメッセージファイルの指定が必要です。

```xml:ros2_lecture_msgs/package.xmlの一部
<package format="3">

  <depend>std_msgs</depend>
  <build_depend>rosidl_default_generators</build_depend>
  <exec_depend>rosidl_default_runtime</exec_depend>
  <member_of_group>rosidl_interface_packages</member_of_group>
</package>
```

```cmake:ros2_lecture_msgs/CMakeLists.txtの一部
find_package(std_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  action/WaitTask.action
)
```

## server側

ここではactionで受け取ったgoalのdurationで指定された秒数だけ待った後にresultを返すノードを実装しています。resultを返すまでの間はfeedbackを送信します。

```c++:rclcpp_action_lecture/src/server.cpp
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <ros2_lecture_msgs/action/wait_task.hpp>

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

class ServerNode : public rclcpp::Node
{
public:
  ServerNode()
  : Node("server_node")
  {
    wait_server_ =
      rclcpp_action::create_server<ros2_lecture_msgs::action::WaitTask>(
      this, "~/task", std::bind(&ServerNode::handleGoal, this, _1, _2),
      std::bind(&ServerNode::handleCancel, this, _1),
      std::bind(&ServerNode::handleAccepted, this, _1));
    interval_timer_ = this->create_wall_timer(1000ms, std::bind(&ServerNode::onIntervalTimer, this));
  }

private:
  rclcpp_action::GoalResponse handleGoal(
    const rclcpp_action::GoalUUID &,
    std::shared_ptr<const ros2_lecture_msgs::action::WaitTask::Goal> goal)
  {
    RCLCPP_INFO(this->get_logger(), "handleGoal %s", goal->name.c_str());
    if (!goal_handle_) {
      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    } else {
      return rclcpp_action::GoalResponse::REJECT;
    }
  }

  rclcpp_action::CancelResponse handleCancel(
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ros2_lecture_msgs::action::WaitTask>> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "cancel %s", goal_handle->get_goal()->name.c_str());
    goal_handle_.reset();
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  void handleAccepted(
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ros2_lecture_msgs::action::WaitTask>> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "accept %s", goal_handle->get_goal()->name.c_str());
    goal_handle_ = goal_handle;
    counter_ = 0;
  }

  void onIntervalTimer(void) {
    if (goal_handle_) {
      counter_++;
      const int threshold_count = goal_handle_->get_goal()->duration * 1.0f;
      RCLCPP_INFO(this->get_logger(), "%d <= %d", threshold_count, counter_);
      if (threshold_count <= counter_) {
        RCLCPP_INFO(this->get_logger(), "success %s", goal_handle_->get_goal()->name.c_str());
        auto result = std::make_shared<ros2_lecture_msgs::action::WaitTask::Result>();
        result->message = "full count";
        goal_handle_->succeed(result);
        goal_handle_.reset();
      } else {
        auto feedback = std::make_shared<ros2_lecture_msgs::action::WaitTask::Feedback>();
        feedback->left_duration = (threshold_count - counter_) * 1.0f;
        goal_handle_->publish_feedback(feedback);
      }
    }
  }

  rclcpp_action::Server<ros2_lecture_msgs::action::WaitTask>::SharedPtr wait_server_;
  rclcpp::TimerBase::SharedPtr interval_timer_{nullptr};
  int counter_{0};

  std::shared_ptr<rclcpp_action::ServerGoalHandle<ros2_lecture_msgs::action::WaitTask>> goal_handle_;
};

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

* clientからgoalを受信すると`handleGoal()`が呼ばれます。この関数でgoalを`ACCEPT_AND_EXECUTE`、`ACCEPT_AND_DEFER`、`REJECT`の3つの内どれにするか決定します。actionは書き方によって、複数のgoalを同時に実行したり、順番に実行するような動作も可能です。今回は1つのgoalのみを実行し、実行中に後から来たものは拒否します、
* 上記で`ACCEPT_AND_EXECUTE`、`ACCEPT_AND_DEFER`のどちらかを選ぶと`handleAccepted()`が実行されます。この関数の引数で渡される`ServerGoalHandle`オブジェクトがこの先重要な働きをします。
    * result、feedbackの送信はこの`ServerGoalHandle`オブジェクトのAPIを使います。
    * このshared_ptrのオブジェクトが消滅したらそのgoalがcancel扱いになってclientにcancelのresultが送信されます。そのためこのオブジェクトをメンバー変数などで保存する必要があります。
    * `goal_handle_->get_goal()`でgoalの値にアクセスできます。

## client側(callbackを使った実装)

clientの書き方は何通りかありますが、最初にcallbackを使ってイベントを扱う例を示します。

```c++:rclcpp_action_lecture/src/client_callback.cpp
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <ros2_lecture_msgs/action/wait_task.hpp>

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

class ClientNode : public rclcpp::Node
{
public:
ClientNode() : Node("client_node") {
  wait_client_ = rclcpp_action::create_client<ros2_lecture_msgs::action::WaitTask>(this, "/server_node/task");
  interval_timer_ = this->create_wall_timer(1000ms, std::bind(&ClientNode::onIntervalTimer, this));

}

private:
  void onIntervalTimer(void) {
    if (!send_goal_) {
      send_goal_ = true;
      if (!wait_client_->wait_for_action_server(2000ms)) {
        RCLCPP_WARN(this->get_logger(), "server not found");
        rclcpp::shutdown();
        return;
      }

      auto goal_msg = ros2_lecture_msgs::action::WaitTask::Goal();
      goal_msg.name = "bbb";
      goal_msg.duration = 9.0f;
      auto send_goal_options = rclcpp_action::Client<ros2_lecture_msgs::action::WaitTask>::SendGoalOptions();
      send_goal_options.goal_response_callback = std::bind(&ClientNode::goalResponseCallback, this, _1);
      send_goal_options.feedback_callback = std::bind(&ClientNode::feedbackCallback, this, _1, _2);
      send_goal_options.result_callback = std::bind(&ClientNode::resultCallback, this, _1);

      RCLCPP_INFO(this->get_logger(), "send goal");
      wait_client_->async_send_goal(goal_msg, send_goal_options);
    }
  }

void goalResponseCallback(rclcpp_action::ClientGoalHandle<ros2_lecture_msgs::action::WaitTask>::SharedPtr goal_handle){
    if (goal_handle) {
      RCLCPP_INFO(this->get_logger(), "get goal_handle");
    } else {
      RCLCPP_WARN(this->get_logger(), "empty goal_handle");
    }
  }

  void feedbackCallback(rclcpp_action::ClientGoalHandle<ros2_lecture_msgs::action::WaitTask>::SharedPtr goal_handle, 
    const std::shared_ptr<const ros2_lecture_msgs::action::WaitTask::Feedback> feedback)
  {
    (void) goal_handle;
    RCLCPP_INFO(this->get_logger(), "feedback %f", feedback->left_duration);
  }

  void resultCallback(const rclcpp_action::ClientGoalHandle<ros2_lecture_msgs::action::WaitTask>::WrappedResult result) {
    RCLCPP_INFO(this->get_logger(), "result %d %s", (int)result.code, result.result->message.c_str());
    rclcpp::shutdown();
  }

  rclcpp_action::Client<ros2_lecture_msgs::action::WaitTask>::SharedPtr wait_client_;
  rclcpp::TimerBase::SharedPtr interval_timer_{nullptr};
  bool send_goal_{false};
};

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

* `rclcpp_action::Client<ros2_lecture_msgs::action::WaitTask>::SharedPtr wait_client_`がactionのclientになります。
* `wait_client_->async_send_goal(goal_msg, send_goal_options)`にてgoalを送信します。
    * goal_msgはgoalを表すオブジェクトです。
    * send_goal_optionsでは種々のcallbackの情報を記載します。


## client側(非同期処理を使った実装)

2つ目にcallbackではなく、featureを使った非同期処理を使う例を示します。std::threadを立てるというROSとしてはご法度なことをしていますがご勘弁を。またfeedbackのやり取りはcallbackで受け取るパスしかないです。

```c++:rclcpp_action_lecture/src/client_async.cpp
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <ros2_lecture_msgs/action/wait_task.hpp>

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

class ClientNode : public rclcpp::Node
{
public:
ClientNode() : Node("client_node") {
  wait_client_ = rclcpp_action::create_client<ros2_lecture_msgs::action::WaitTask>(this, "/server_node/task");
  worker_thread_ = std::thread(std::bind(&ClientNode::workerFunc, this));
}

~ClientNode() {
  worker_thread_.join();
}

private:
  void workerFunc(void) {
    if (!wait_client_->wait_for_action_server(2000ms)) {
      RCLCPP_WARN(this->get_logger(), "server not found");
      rclcpp::shutdown();
      return;
    }

    auto goal_msg = ros2_lecture_msgs::action::WaitTask::Goal();
    goal_msg.name = "aaa";
    goal_msg.duration = 10.0f;
    auto send_goal_options = rclcpp_action::Client<ros2_lecture_msgs::action::WaitTask>::SendGoalOptions();
    send_goal_options.feedback_callback = std::bind(&ClientNode::feedbackCallback, this, _1, _2);

    RCLCPP_INFO(this->get_logger(), "send goal");
    std::shared_future<rclcpp_action::ClientGoalHandle<ros2_lecture_msgs::action::WaitTask>::SharedPtr> goal_feature = wait_client_->async_send_goal(goal_msg, send_goal_options);
    rclcpp_action::ClientGoalHandle<ros2_lecture_msgs::action::WaitTask>::SharedPtr goal_handle = goal_feature.get();
    if (goal_handle) {
      printf("id ");
      for (const auto c : goal_handle->get_goal_id()){
        printf("%x ", c);
      }
      printf("\n");
    } else {
      RCLCPP_WARN(this->get_logger(), "goal_handle empty");
      rclcpp::shutdown();
      return;
    }

    std::shared_future<rclcpp_action::ClientGoalHandle<ros2_lecture_msgs::action::WaitTask>::WrappedResult> result_feature = wait_client_->async_get_result(goal_handle);
    rclcpp_action::ClientGoalHandle<ros2_lecture_msgs::action::WaitTask>::WrappedResult wrapped_result = result_feature.get();
    RCLCPP_INFO(this->get_logger(), "result %d %s", (int)wrapped_result.code, wrapped_result.result->message.c_str());

    rclcpp::shutdown();
  }

  void feedbackCallback(rclcpp_action::ClientGoalHandle<ros2_lecture_msgs::action::WaitTask>::SharedPtr goal_handle, 
    const std::shared_ptr<const ros2_lecture_msgs::action::WaitTask::Feedback> feedback)
  {
    (void) goal_handle;
    RCLCPP_INFO(this->get_logger(), "feedback %f", feedback->left_duration);
  }

  std::thread worker_thread_;
  rclcpp_action::Client<ros2_lecture_msgs::action::WaitTask>::SharedPtr wait_client_;
};

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

* こちらも`wait_client_->async_send_goal()`でgoalを送るのは同じですが、このメソッドの返り値のfeatureを活用します。
* このfeatureはserverからのgoalのresponseがあると`ClientGoalHandle::SharedPtr`をget出来るようになります。
    * responseがREJECTだった場合は空のshared_ptrに、ACCEPT_XXなら実体があります。
* `wait_client_->async_get_result()`でresultを取得するfeatureを取得します。
    * 取得できる実態は`WrappedResult`型で、codeで終了コード(UNKNOWN、SUCCEEDED、CANCELED、ABORTED)を、resultでresultを取得します。

## CMakeListの記述

```cmake:rclcpp_action_lecture/CMakeLists.txt
find_package(rclcpp_action REQUIRED)
find_package(ros2_lecture_msgs REQUIRED)

add_executable(server_node 
  src/server.cpp)
ament_target_dependencies(server_node
  rclcpp
  rclcpp_action
  ros2_lecture_msgs
)
install(TARGETS server_node
  DESTINATION lib/${PROJECT_NAME}
)

add_executable(client_async_node 
  src/client_async.cpp)
ament_target_dependencies(client_async_node
  rclcpp
  rclcpp_action
  ros2_lecture_msgs
)
install(TARGETS client_async_node
  DESTINATION lib/${PROJECT_NAME}
)

add_executable(client_callback_node 
  src/client_callback.cpp)
ament_target_dependencies(client_callback_node
  rclcpp
  rclcpp_action
  ros2_lecture_msgs
)
install(TARGETS client_callback_node
  DESTINATION lib/${PROJECT_NAME}
)
```

# ビルド

```shell:ビルド
source /opt/ros/humble/setup.bash
cd ros2_ws
colcon build
```

# 実行

```shell:1つ目のターミナル
source ~/ros2_ws/install/setup.bash 
ros2 run rclcpp_action_lecture server_node 
```

```shell:2つ目のターミナル
source ~/ros2_ws/install/setup.bash 
ros2 run rclcpp_action_lecture client_async_node 
```

```text:2つ目のターミナルの結果
[INFO] [1708147624.254845375] [client_node]: send goal
id 52 52 23 4e 9e d9 b8 f6 29 a f1 9b 81 bb f3 b8 
[INFO] [1708147625.240671738] [client_node]: feedback 9.000000
[INFO] [1708147626.245140618] [client_node]: feedback 8.000000
[INFO] [1708147627.248166072] [client_node]: feedback 7.000000
[INFO] [1708147628.250885640] [client_node]: feedback 6.000000
[INFO] [1708147629.253070312] [client_node]: feedback 5.000000
[INFO] [1708147630.240964293] [client_node]: feedback 4.000000
[INFO] [1708147631.245789658] [client_node]: feedback 3.000000
[INFO] [1708147632.247191639] [client_node]: feedback 2.000000
[INFO] [1708147633.246573327] [client_node]: feedback 1.000000
[INFO] [1708147634.251678922] [client_node]: result 4 full count
```

# コメント
* actionはclient-server型の設計になっているために、clientが複数という作りにすることもできます。
* 死活監視の仕組みが無いために、通信中にclientやserverがexceptionで落ちた場合にそれを知る手段がactionにはありません。feeebackが途切れないか監視するなどの仕組みを入れる必要があります。

# 参考

* [ROS2 design](https://design.ros2.org/articles/actions.html)
* [サンプルコード](https://github.com/ros2/examples/tree/humble/rclcpp/actions)
* [tutorial](https://docs.ros.org/en/humble/Tutorials/Intermediate/Writing-an-Action-Server-Client/Cpp.html)

# 目次ページへのリンク
[ROS2講座の目次へのリンク](https://qiita.com/srs/items/5f0d765343ca78f524ed)