アクションサーバーとクライアントの作成 (C++)
目標: C++でアクションサーバーとクライアントを実装する。
チュートリアルレベル: 中級
所要時間: 15分
背景
アクションはROS における非同期通信の一形態です。 アクションクライアントはアクションサーバーにゴールリクエストを送信します。 アクションサーバーはゴールのフィードバックと結果をアクションクライアントに送信します。
前提条件
前のチュートリアル「アクションの作成」で定義したaction_tutorials_interfacesパッケージとFibonacci.actionインターフェースが必要です。
タスク
1 action_tutorials_cppパッケージの作成
「パッケージの作成」チュートリアルで見たように、C++とサポートコードを保持するための新しいパッケージを作成する必要があります。
1.1 action_tutorials_cppパッケージの作成
前のチュートリアルで作成したアクションワークスペースに移動し(ワークスペースをソースすることを忘れずに)、C++アクションサーバー用の新しいパッケージを作成します:
Linux/macOS:
cd ~/ros2_ws/src
ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cppWindows:
cd \ros2_ws\src
ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp1.2 可視性制御の追加
Windowsでパッケージをコンパイルして動作させるために、「可視性制御」を追加する必要があります。 詳細については、WindowsのヒントとコツドキュメントのWindowsシンボル可視性を参照してください。
action_tutorials_cpp/include/action_tutorials_cpp/visibility_control.hを開いて、以下のコードを記述します:
#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を開いて、以下のコードを記述します:
#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の派生クラスであるクラスを作成します:
class FibonacciActionServer : public rclcpp::NodeFibonacciActionServerクラスのコンストラクタは、ノード名をfibonacci_action_serverとして初期化します:
explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
: Node("fibonacci_action_server", options)コンストラクタはまた、新しいアクションサーバーをインスタンス化します:
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つのものが必要です:
テンプレート化されたアクションタイプ名:
Fibonacci。アクションを追加するROS 2ノード:
this。アクション名:
'fibonacci'。ゴールを処理するためのコールバック関数:
handle_goalキャンセルを処理するためのコールバック関数:
handle_cancel。ゴール受諾を処理するためのコールバック関数:
handle_accept。
さまざまなコールバックの実装はファイル内の次の部分にあります。 すべてのコールバックは迅速に戻る必要があることに注意してください。そうでなければ、エグゼキュータを飢餓状態にするリスクがあります。
新しいゴールを処理するコールバックから始めます:
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();
}実行は長時間実行される操作であるため、実際の作業を行うためのスレッドを生成し、handle_acceptedから迅速に戻ります。
さらなる処理と更新はすべて新しいスレッドのexecuteメソッドで行われます:
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呼び出しの直後に以下を追加します:
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のトップレベルに移動して実行します:
colcon buildこれにより、action_tutorials_cppパッケージ内のfibonacci_action_serverを含む、ワークスペース全体がコンパイルされるはずです。
2.3 アクションサーバーの実行
アクションサーバーがビルドされたので、実行できます。 先ほどビルドしたワークスペース(ros2_ws)をソースして、アクションサーバーを実行してみてください:
ros2 run action_tutorials_cpp fibonacci_action_server3 アクションクライアントの作成
3.1 アクションクライアントコードの作成
action_tutorials_cpp/src/fibonacci_action_client.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の派生クラスであるクラスを作成します:
class FibonacciActionClient : public rclcpp::NodeFibonacciActionClientクラスのコンストラクタは、ノード名をfibonacci_action_clientとして初期化します:
explicit FibonacciActionClient(const rclcpp::NodeOptions & options)
: Node("fibonacci_action_client", options)コンストラクタはまた、新しいアクションクライアントをインスタンス化します:
this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
this,
"fibonacci");アクションクライアントには3つのものが必要です:
テンプレート化されたアクションタイプ名:
Fibonacci。アクションクライアントを追加するROS 2ノード:
this。アクション名:
'fibonacci'。
また、send_goalへの唯一の呼び出しを開始するROSタイマーもインスタンス化します:
this->timer_ = this->create_wall_timer(
std::chrono::milliseconds(500),
std::bind(&FibonacciActionClient::send_goal, this));タイマーが期限切れになると、send_goalを呼び出します:
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);
}この関数は以下を実行します:
タイマーをキャンセルします(一度だけ呼び出されるように)。
アクションサーバーが立ち上がるまで待機します。
新しい
Fibonacci::Goalをインスタンス化します。レスポンス、フィードバック、結果のコールバックを設定します。
ゴールをサーバーに送信します。
サーバーがゴールを受信して受け入れると、クライアントにレスポンスを送信します。 そのレスポンスはgoal_response_callbackによって処理されます:
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によって処理されます:
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によって処理されます:
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呼び出しの直後に以下を追加します:
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のトップレベルに移動して実行します:
colcon buildこれにより、action_tutorials_cppパッケージ内のfibonacci_action_clientを含む、ワークスペース全体がコンパイルされるはずです。
3.3 アクションクライアントの実行
アクションクライアントがビルドされたので、実行できます。 まず、別のターミナルでアクションサーバーが実行されていることを確認してください。 先ほどビルドしたワークスペース(ros2_ws)をソースして、アクションクライアントを実行してみてください:
ros2 run action_tutorials_cpp fibonacci_action_clientゴールが受け入れられ、フィードバックが印刷され、最終結果が表示されるログメッセージが表示されるはずです。
概要
このチュートリアルでは、C++アクションサーバーとアクションクライアントを一行ずつ組み立て、ゴール、フィードバック、結果を交換するように設定しました。
関連コンテンツ
C++でアクションサーバーとクライアントを書く方法はいくつかあります。ros2/examplesリポジトリの
minimal_action_serverとminimal_action_clientパッケージをチェックしてください。ROSアクションについてのより詳細な情報については、設計記事を参照してください。