액션 서버 및 클라이언트 작성하기 (C++)
목표: C++로 액션 서버 및 클라이언트를 구현합니다.
배경 정보
액션은 ROS에서 비동기 통신의 한 형태입니다. 액션 클라이언트 는 목표 요청을 액션 서버 에 보냅니다. 액션 서버 는 목표 피드백과 결과를 액션 클라이언트 에 보냅니다.
작업
1 액션 서버를 위한 action_tutorials_cpp 패키지 생성
패키지 생성하기 튜토리얼에서 본 것처럼 C++ 및 지원 코드를 보관하기 위한 새 패키지를 생성해야 합니다.
1.1 action_tutorials_cpp 패키지 생성
이전 튜토리얼 에서 생성한 액션 워크스페이스로 이동하십시오 (워크스페이스를 소스로 설정하지 않으면 잊지 마십시오) 그리고 C++ 액션 서버를 위한 새 패키지를 생성하십시오:
cd ~/ros2_ws/src
ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp
1.2 가시성 제어 추가
Windows에서 패키지를 컴파일하고 실행하기 위해 “가시성 제어(visibility control)”를 추가해야 합니다. 자세한 내용은 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::Node
FibonacciActionServer 클래스의 생성자는 노드 이름을 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_server
3 액션 클라이언트 작성
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::Node
FibonacciActionClient 클래스의 생성자는 노드 이름을 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();
}
}; // class FibonacciActionClient
이제 완전한 기능을 갖춘 액션 클라이언트가 있습니다. 이제 빌드하고 실행합시다.
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 2 액션에 대한 자세한 정보는 ROS 2 공식 문서 를 참조하십시오.