Skip to content

アクションサーバーとクライアントの作成 (C++)

目標: C++でアクションサーバーとクライアントを実装する。

チュートリアルレベル: 中級

所要時間: 15分

背景

アクションはROS における非同期通信の一形態です。 アクションクライアントアクションサーバーにゴールリクエストを送信します。 アクションサーバーはゴールのフィードバックと結果をアクションクライアントに送信します。

前提条件

前のチュートリアル「アクションの作成」で定義したaction_tutorials_interfacesパッケージとFibonacci.actionインターフェースが必要です。

タスク

1 action_tutorials_cppパッケージの作成

パッケージの作成」チュートリアルで見たように、C++とサポートコードを保持するための新しいパッケージを作成する必要があります。

1.1 action_tutorials_cppパッケージの作成

前のチュートリアルで作成したアクションワークスペースに移動し(ワークスペースをソースすることを忘れずに)、C++アクションサーバー用の新しいパッケージを作成します:

Linux/macOS:

bash
cd ~/ros2_ws/src
ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp

Windows:

bash
cd \ros2_ws\src
ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp

1.2 可視性制御の追加

Windowsでパッケージをコンパイルして動作させるために、「可視性制御」を追加する必要があります。 詳細については、WindowsのヒントとコツドキュメントのWindowsシンボル可視性を参照してください。

action_tutorials_cpp/include/action_tutorials_cpp/visibility_control.hを開いて、以下のコードを記述します:

cpp
#ifndef ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_
#define ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_

#ifdef __cplusplus
extern "C"
{
#endif

// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
//     https://gcc.gnu.org/wiki/Visibility

#if defined _WIN32 || defined __CYGWIN__
  #ifdef __GNUC__
    #define ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((dllexport))
    #define ACTION_TUTORIALS_CPP_IMPORT __attribute__ ((dllimport))
  #else
    #define ACTION_TUTORIALS_CPP_EXPORT __declspec(dllexport)
    #define ACTION_TUTORIALS_CPP_IMPORT __declspec(dllimport)
  #endif
  #ifdef ACTION_TUTORIALS_CPP_BUILDING_DLL
    #define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_EXPORT
  #else
    #define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_IMPORT
  #endif
  #define ACTION_TUTORIALS_CPP_PUBLIC_TYPE ACTION_TUTORIALS_CPP_PUBLIC
  #define ACTION_TUTORIALS_CPP_LOCAL
#else
  #define ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((visibility("default")))
  #define ACTION_TUTORIALS_CPP_IMPORT
  #if __GNUC__ >= 4
    #define ACTION_TUTORIALS_CPP_PUBLIC __attribute__ ((visibility("default")))
    #define ACTION_TUTORIALS_CPP_LOCAL  __attribute__ ((visibility("hidden")))
  #else
    #define ACTION_TUTORIALS_CPP_PUBLIC
    #define ACTION_TUTORIALS_CPP_LOCAL
  #endif
  #define ACTION_TUTORIALS_CPP_PUBLIC_TYPE
#endif

#ifdef __cplusplus
}
#endif

#endif  // ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_

2 アクションサーバーの作成

アクションの作成」チュートリアルで作成したアクションを使用してフィボナッチ数列を計算するアクションサーバーの作成に焦点を当てましょう。

2.1 アクションサーバーコードの作成

action_tutorials_cpp/src/fibonacci_action_server.cppを開いて、以下のコードを記述します:

cpp
#include <functional>
#include <memory>
#include <thread>

#include "action_tutorials_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"

#include "action_tutorials_cpp/visibility_control.h"

namespace action_tutorials_cpp
{
class FibonacciActionServer : public rclcpp::Node
{
public:
  using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
  using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;

  ACTION_TUTORIALS_CPP_PUBLIC
  explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : Node("fibonacci_action_server", options)
  {
    using namespace std::placeholders;

    this->action_server_ = rclcpp_action::create_server<Fibonacci>(
      this,
      "fibonacci",
      std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),
      std::bind(&FibonacciActionServer::handle_cancel, this, _1),
      std::bind(&FibonacciActionServer::handle_accepted, this, _1));
  }

private:
  rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;

  rclcpp_action::GoalResponse handle_goal(
    const rclcpp_action::GoalUUID & uuid,
    std::shared_ptr<const Fibonacci::Goal> goal)
  {
    RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
    (void)uuid;
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

  rclcpp_action::CancelResponse handle_cancel(
    const std::shared_ptr<GoalHandleFibonacci> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
    (void)goal_handle;
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
  {
    using namespace std::placeholders;
    // this needs to return quickly to avoid blocking the executor, so spin up a new thread
    std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach();
  }

  void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "Executing goal");
    rclcpp::Rate loop_rate(1);
    const auto goal = goal_handle->get_goal();
    auto feedback = std::make_shared<Fibonacci::Feedback>();
    auto & sequence = feedback->partial_sequence;
    sequence.push_back(0);
    sequence.push_back(1);
    auto result = std::make_shared<Fibonacci::Result>();

    for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
      // Check if there is a cancel request
      if (goal_handle->is_canceling()) {
        result->sequence = sequence;
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "Goal canceled");
        return;
      }
      // Update sequence
      sequence.push_back(sequence[i] + sequence[i - 1]);
      // Publish feedback
      goal_handle->publish_feedback(feedback);
      RCLCPP_INFO(this->get_logger(), "Publish feedback");

      loop_rate.sleep();
    }

    // Check if goal is done
    if (rclcpp::ok()) {
      result->sequence = sequence;
      goal_handle->succeed(result);
      RCLCPP_INFO(this->get_logger(), "Goal succeeded");
    }
  }
};  // class FibonacciActionServer

}  // namespace action_tutorials_cpp

RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionServer)

最初の数行では、コンパイルに必要なすべてのヘッダーを含めています。

次に、rclcpp::Nodeの派生クラスであるクラスを作成します:

cpp
class FibonacciActionServer : public rclcpp::Node

FibonacciActionServerクラスのコンストラクタは、ノード名をfibonacci_action_serverとして初期化します:

cpp
explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
: Node("fibonacci_action_server", options)

コンストラクタはまた、新しいアクションサーバーをインスタンス化します:

cpp
this->action_server_ = rclcpp_action::create_server<Fibonacci>(
  this,
  "fibonacci",
  std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),
  std::bind(&FibonacciActionServer::handle_cancel, this, _1),
  std::bind(&FibonacciActionServer::handle_accepted, this, _1));

アクションサーバーには6つのものが必要です:

  1. テンプレート化されたアクションタイプ名:Fibonacci

  2. アクションを追加するROS 2ノード:this

  3. アクション名:'fibonacci'

  4. ゴールを処理するためのコールバック関数:handle_goal

  5. キャンセルを処理するためのコールバック関数:handle_cancel

  6. ゴール受諾を処理するためのコールバック関数:handle_accept

さまざまなコールバックの実装はファイル内の次の部分にあります。 すべてのコールバックは迅速に戻る必要があることに注意してください。そうでなければ、エグゼキュータを飢餓状態にするリスクがあります。

新しいゴールを処理するコールバックから始めます:

cpp
rclcpp_action::GoalResponse handle_goal(
  const rclcpp_action::GoalUUID & uuid,
  std::shared_ptr<const Fibonacci::Goal> goal)
{
  RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
  (void)uuid;
  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}

この実装はすべてのゴールを受け入れます。

次はキャンセルを処理するコールバックです:

cpp
rclcpp_action::CancelResponse handle_cancel(
  const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
  RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
  (void)goal_handle;
  return rclcpp_action::CancelResponse::ACCEPT;
}

この実装は、キャンセルを受け入れたことをクライアントに伝えるだけです。

最後のコールバックは新しいゴールを受け入れて処理を開始します:

cpp
void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
  using namespace std::placeholders;
  // this needs to return quickly to avoid blocking the executor, so spin up a new thread
  std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach();
}

実行は長時間実行される操作であるため、実際の作業を行うためのスレッドを生成し、handle_acceptedから迅速に戻ります。

さらなる処理と更新はすべて新しいスレッドのexecuteメソッドで行われます:

cpp
void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
  RCLCPP_INFO(this->get_logger(), "Executing goal");
  rclcpp::Rate loop_rate(1);
  const auto goal = goal_handle->get_goal();
  auto feedback = std::make_shared<Fibonacci::Feedback>();
  auto & sequence = feedback->partial_sequence;
  sequence.push_back(0);
  sequence.push_back(1);
  auto result = std::make_shared<Fibonacci::Result>();

  for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
    // Check if there is a cancel request
    if (goal_handle->is_canceling()) {
      result->sequence = sequence;
      goal_handle->canceled(result);
      RCLCPP_INFO(this->get_logger(), "Goal canceled");
      return;
    }
    // Update sequence
    sequence.push_back(sequence[i] + sequence[i - 1]);
    // Publish feedback
    goal_handle->publish_feedback(feedback);
    RCLCPP_INFO(this->get_logger(), "Publish feedback");

    loop_rate.sleep();
  }

  // Check if goal is done
  if (rclcpp::ok()) {
    result->sequence = sequence;
    goal_handle->succeed(result);
    RCLCPP_INFO(this->get_logger(), "Goal succeeded");
  }
}

この作業スレッドは、フィボナッチ数列の1つの数値を毎秒処理し、各ステップでフィードバックの更新を公開します。 処理が完了すると、goal_handleを成功としてマークして終了します。

これで完全に機能するアクションサーバーができました。 それをビルドして実行しましょう。

2.2 アクションサーバーのコンパイル

前のセクションでアクションサーバーコードを配置しました。 コンパイルして実行するために、いくつかの追加作業が必要です。

まず、アクションサーバーがコンパイルされるようにCMakeLists.txtを設定する必要があります。 action_tutorials_cpp/CMakeLists.txtを開いて、find_package呼び出しの直後に以下を追加します:

cmake
add_library(action_server SHARED
  src/fibonacci_action_server.cpp)
target_include_directories(action_server PRIVATE
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
target_compile_definitions(action_server
  PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL")
ament_target_dependencies(action_server
  "action_tutorials_interfaces"
  "rclcpp"
  "rclcpp_action"
  "rclcpp_components")
rclcpp_components_register_node(action_server PLUGIN "action_tutorials_cpp::FibonacciActionServer" EXECUTABLE fibonacci_action_server)
install(TARGETS
  action_server
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin)

これでパッケージをコンパイルできます。 ros2_wsのトップレベルに移動して実行します:

bash
colcon build

これにより、action_tutorials_cppパッケージ内のfibonacci_action_serverを含む、ワークスペース全体がコンパイルされるはずです。

2.3 アクションサーバーの実行

アクションサーバーがビルドされたので、実行できます。 先ほどビルドしたワークスペース(ros2_ws)をソースして、アクションサーバーを実行してみてください:

bash
ros2 run action_tutorials_cpp fibonacci_action_server

3 アクションクライアントの作成

3.1 アクションクライアントコードの作成

action_tutorials_cpp/src/fibonacci_action_client.cppを開いて、以下のコードを記述します:

cpp
#include <functional>
#include <future>
#include <memory>
#include <string>
#include <sstream>

#include "action_tutorials_interfaces/action/fibonacci.hpp"

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"

namespace action_tutorials_cpp
{
class FibonacciActionClient : public rclcpp::Node
{
public:
  using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
  using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;

  explicit FibonacciActionClient(const rclcpp::NodeOptions & options)
  : Node("fibonacci_action_client", options)
  {
    this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
      this,
      "fibonacci");

    this->timer_ = this->create_wall_timer(
      std::chrono::milliseconds(500),
      std::bind(&FibonacciActionClient::send_goal, this));
  }

  void send_goal()
  {
    using namespace std::placeholders;

    this->timer_->cancel();

    if (!this->client_ptr_->wait_for_action_server()) {
      RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
      rclcpp::shutdown();
    }

    auto goal_msg = Fibonacci::Goal();
    goal_msg.order = 10;

    RCLCPP_INFO(this->get_logger(), "Sending goal");

    auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
    send_goal_options.goal_response_callback =
      std::bind(&FibonacciActionClient::goal_response_callback, this, _1);
    send_goal_options.feedback_callback =
      std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2);
    send_goal_options.result_callback =
      std::bind(&FibonacciActionClient::result_callback, this, _1);
    this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
  }

private:
  rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_;
  rclcpp::TimerBase::SharedPtr timer_;

  void goal_response_callback(const GoalHandleFibonacci::SharedPtr & goal_handle)
  {
    if (!goal_handle) {
      RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
    } else {
      RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
    }
  }

  void feedback_callback(
    GoalHandleFibonacci::SharedPtr,
    const std::shared_ptr<const Fibonacci::Feedback> feedback)
  {
    std::stringstream ss;
    ss << "Next number in sequence received: ";
    for (auto number : feedback->partial_sequence) {
      ss << number << " ";
    }
    RCLCPP_INFO(this->get_logger(), ss.str().c_str());
  }

  void result_callback(const GoalHandleFibonacci::WrappedResult & result)
  {
    switch (result.code) {
      case rclcpp_action::ResultCode::SUCCEEDED:
        break;
      case rclcpp_action::ResultCode::ABORTED:
        RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
        return;
      case rclcpp_action::ResultCode::CANCELED:
        RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
        return;
      default:
        RCLCPP_ERROR(this->get_logger(), "Unknown result code");
        return;
    }
    std::stringstream ss;
    ss << "Result received: ";
    for (auto number : result.result->sequence) {
      ss << number << " ";
    }
    RCLCPP_INFO(this->get_logger(), ss.str().c_str());
    rclcpp::shutdown();
  }
};  // class FibonacciActionClient

}  // namespace action_tutorials_cpp

RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionClient)

最初の数行では、コンパイルに必要なすべてのヘッダーを含めています。

次に、rclcpp::Nodeの派生クラスであるクラスを作成します:

cpp
class FibonacciActionClient : public rclcpp::Node

FibonacciActionClientクラスのコンストラクタは、ノード名をfibonacci_action_clientとして初期化します:

cpp
explicit FibonacciActionClient(const rclcpp::NodeOptions & options)
: Node("fibonacci_action_client", options)

コンストラクタはまた、新しいアクションクライアントをインスタンス化します:

cpp
this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
  this,
  "fibonacci");

アクションクライアントには3つのものが必要です:

  1. テンプレート化されたアクションタイプ名:Fibonacci

  2. アクションクライアントを追加するROS 2ノード:this

  3. アクション名:'fibonacci'

また、send_goalへの唯一の呼び出しを開始するROSタイマーもインスタンス化します:

cpp
this->timer_ = this->create_wall_timer(
  std::chrono::milliseconds(500),
  std::bind(&FibonacciActionClient::send_goal, this));

タイマーが期限切れになると、send_goalを呼び出します:

cpp
void send_goal()
{
  using namespace std::placeholders;

  this->timer_->cancel();

  if (!this->client_ptr_->wait_for_action_server()) {
    RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
    rclcpp::shutdown();
  }

  auto goal_msg = Fibonacci::Goal();
  goal_msg.order = 10;

  RCLCPP_INFO(this->get_logger(), "Sending goal");

  auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
  send_goal_options.goal_response_callback =
    std::bind(&FibonacciActionClient::goal_response_callback, this, _1);
  send_goal_options.feedback_callback =
    std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2);
  send_goal_options.result_callback =
    std::bind(&FibonacciActionClient::result_callback, this, _1);
  this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
}

この関数は以下を実行します:

  1. タイマーをキャンセルします(一度だけ呼び出されるように)。

  2. アクションサーバーが立ち上がるまで待機します。

  3. 新しいFibonacci::Goalをインスタンス化します。

  4. レスポンス、フィードバック、結果のコールバックを設定します。

  5. ゴールをサーバーに送信します。

サーバーがゴールを受信して受け入れると、クライアントにレスポンスを送信します。 そのレスポンスはgoal_response_callbackによって処理されます:

cpp
void goal_response_callback(const GoalHandleFibonacci::SharedPtr & goal_handle)
{
  if (!goal_handle) {
    RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
  } else {
    RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
  }
}

ゴールがサーバーによって受け入れられたと仮定すると、処理が開始されます。 クライアントへのフィードバックはfeedback_callbackによって処理されます:

cpp
void feedback_callback(
  GoalHandleFibonacci::SharedPtr,
  const std::shared_ptr<const Fibonacci::Feedback> feedback)
{
  std::stringstream ss;
  ss << "Next number in sequence received: ";
  for (auto number : feedback->partial_sequence) {
    ss << number << " ";
  }
  RCLCPP_INFO(this->get_logger(), ss.str().c_str());
}

サーバーが処理を完了すると、クライアントに結果を返します。 結果はresult_callbackによって処理されます:

cpp
void result_callback(const GoalHandleFibonacci::WrappedResult & result)
{
  switch (result.code) {
    case rclcpp_action::ResultCode::SUCCEEDED:
      break;
    case rclcpp_action::ResultCode::ABORTED:
      RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
      return;
    case rclcpp_action::ResultCode::CANCELED:
      RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
      return;
    default:
      RCLCPP_ERROR(this->get_logger(), "Unknown result code");
      return;
  }
  std::stringstream ss;
  ss << "Result received: ";
  for (auto number : result.result->sequence) {
    ss << number << " ";
  }
  RCLCPP_INFO(this->get_logger(), ss.str().c_str());
  rclcpp::shutdown();
}

これで完全に機能するアクションクライアントができました。 それをビルドして実行しましょう。

3.2 アクションクライアントのコンパイル

前のセクションでアクションクライアントコードを配置しました。 コンパイルして実行するために、いくつかの追加作業が必要です。

まず、アクションクライアントがコンパイルされるようにCMakeLists.txtを設定する必要があります。 action_tutorials_cpp/CMakeLists.txtを開いて、find_package呼び出しの直後に以下を追加します:

cmake
add_library(action_client SHARED
  src/fibonacci_action_client.cpp)
target_include_directories(action_client PRIVATE
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
target_compile_definitions(action_client
  PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL")
ament_target_dependencies(action_client
  "action_tutorials_interfaces"
  "rclcpp"
  "rclcpp_action"
  "rclcpp_components")
rclcpp_components_register_node(action_client PLUGIN "action_tutorials_cpp::FibonacciActionClient" EXECUTABLE fibonacci_action_client)
install(TARGETS
  action_client
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin)

これでパッケージをコンパイルできます。 ros2_wsのトップレベルに移動して実行します:

bash
colcon build

これにより、action_tutorials_cppパッケージ内のfibonacci_action_clientを含む、ワークスペース全体がコンパイルされるはずです。

3.3 アクションクライアントの実行

アクションクライアントがビルドされたので、実行できます。 まず、別のターミナルでアクションサーバーが実行されていることを確認してください。 先ほどビルドしたワークスペース(ros2_ws)をソースして、アクションクライアントを実行してみてください:

bash
ros2 run action_tutorials_cpp fibonacci_action_client

ゴールが受け入れられ、フィードバックが印刷され、最終結果が表示されるログメッセージが表示されるはずです。

概要

このチュートリアルでは、C++アクションサーバーとアクションクライアントを一行ずつ組み立て、ゴール、フィードバック、結果を交換するように設定しました。

関連コンテンツ

  • C++でアクションサーバーとクライアントを書く方法はいくつかあります。ros2/examplesリポジトリのminimal_action_serverminimal_action_clientパッケージをチェックしてください。

  • ROSアクションについてのより詳細な情報については、設計記事を参照してください。

関連リンク

Released under the MIT License.