環境

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

項目	値
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での状態変化時に送信

シーケンスをいかにまとめます。のちに出てくるclient側、server側のAPIも一緒に書き入れています。

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

ソースコード

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

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

ros2_lecture_msgs/action/WaitTask.action

string name
float32 duration
---
string message
---
float32 left_duration

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

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>

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を送信します。

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を使ってイベントを扱う例を示します。

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で受け取るパスしかないです。

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の記述

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}
)

ビルド

ビルド

source /opt/ros/humble/setup.bash
cd ros2_ws
colcon build

実行

1つ目のターミナル

source ~/ros2_ws/install/setup.bash 
ros2 run rclcpp_action_lecture server_node

2つ目のターミナル

source ~/ros2_ws/install/setup.bash 
ros2 run rclcpp_action_lecture client_async_node

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講座の目次へのリンク