Go to Qiita Advent Calendar 2024 Top
LoginSignup
3
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@srs(project_srs)

ROS2講座18 actionを使う

Posted at

環境

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

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

3
1
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
3
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?