ROS2入门教程—编写动作服务器和客户端(C++版)
动作是ROS 2中异步通信的一种形式。动作客户端将目标请求发送到动作服务器。动作服务器将目标反馈和结果发送到动作客户端。
1 创建功能包
开启一个新终端,设置好环境变量,然后进入到dev_ws/src
目录下,使用以下命令创建新的功能包action_tutorials_cpp
:
ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp
2 编写动作服务器
首先,让我们来编写一个动作服务器,该动作服务器使用我们在ROS2入门教程—创建一个动作消息(action)一文中创建的用来计算斐波那契序列的动作。
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)
2.2 代码解释
前几行包括我们需要编译的所有头文件。
接下来,我们创建一个类,它是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个参数:
∙
\bullet
∙ 模板化的动作类型名称:Fibonacci
∙
\bullet
∙ 一个可以将动作服务端添加进来的ROS 2节点:this
∙
\bullet
∙ 动作名称:"fibonacci"
∙
\bullet
∙ 一个用于处理目标的回调函数:handle_goal
∙
\bullet
∙ 一个用于处理取消的回调函数:handle_cancel
∙
\bullet
∙ 一个用于处理目标接受的回调函数:handle_accepted
接下来是三个回调函数的实现,请注意,所有回调函数都需要快速返回,否则可能会堵塞住执行器。
首先是处理新目标的回调函数:
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();
}
action
是可抢占式的,由于需要执行一段时间,因此开启一个线程来执行实际工作,并且从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");
}
}
这个工作线程每秒处理斐波纳契数列的一个序列数,并且发布一个反馈更新。完成处理后,将目标句柄goal_handle
标记为成功状态,然后退出。
2.3 编译动作服务器
首先,需要设置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)
接下来就可以编译该功能包了,进入到工作空间根目录下,执行下列命令:
colcon build
2.4 运行动作服务器
打开一个新终端,进入到工作空间根目录,然后运行如下命令:
. install/setup.bash
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(std::shared_future<GoalHandleFibonacci::SharedPtr> future)
{
auto goal_handle = future.get();
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)
3.2 代码解释
前几行包括我们需要编译的所有头文件。
接下来,我们创建一个类,它是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个参数:
∙
\bullet
∙ 模板化的动作类型名称:Fibonacci
∙
\bullet
∙ 一个可以将动作客户端添加进来的ROS 2节点:this
∙
\bullet
∙ 动作名称:"fibonacci"
接着实例化了一个ROS计时器,用于启动send_goal
函数:
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);
}
该函数会执行以下操作:
∙
\bullet
∙ 取消计时器(这样该函数仅被调用一次);
∙
\bullet
∙ 等待动作服务器启动;
∙
\bullet
∙ 实例化一个新目标Fibonacci :: Goal
;
∙
\bullet
∙ 设置响应、反馈和结果回调
∙
\bullet
∙ 将目标发送到服务器
当动作服务器接收到目标并接受该目标时,它会向客户端发送响应。该响应由下面的回调函数goal_response_callback
处理:
void goal_response_callback(std::shared_future<GoalHandleFibonacci::SharedPtr> future)
{
auto goal_handle = future.get();
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.3 编译动作客户端
首先,需要设置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)
接下来就可以编译该功能包了,进入到工作空间根目录下,执行下列命令:
colcon build
3.4 运行动作客服端
打开一个新终端,进入到工作空间根目录,然后运行如下命令:
. install/setup.bash
ros2 run action_tutorials_cpp fibonacci_action_client
运行动作服务器和动作客户端之后,在动作客户端中输出如下信息:
[INFO] [fibonacci_action_client]: Sending goal
[INFO] [fibonacci_action_client]: Goal accepted by server, waiting for result
[INFO] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2
[INFO] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3
[INFO] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3 5
[INFO] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3 5 8
[INFO] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3 5 8 13
[INFO] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3 5 8 13 21
[INFO] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3 5 8 13 21 34
[INFO] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3 5 8 13 21 34 55
[INFO] [fibonacci_action_client]: Result received: 0 1 1 2 3 5 8 13 21 34 55
同时,在动作服务器中输出如下信息:
[INFO] [fibonacci_action_server]: Received goal request with order 10
[INFO] [fibonacci_action_server]: Executing goal
[INFO] [fibonacci_action_server]: Publish feedback
[INFO] [fibonacci_action_server]: Publish feedback
[INFO] [fibonacci_action_server]: Publish feedback
[INFO] [fibonacci_action_server]: Publish feedback
[INFO] [fibonacci_action_server]: Publish feedback
[INFO] [fibonacci_action_server]: Publish feedback
[INFO] [fibonacci_action_server]: Publish feedback
[INFO] [fibonacci_action_server]: Publish feedback
[INFO] [fibonacci_action_server]: Publish feedback
[INFO] [fibonacci_action_server]: Goal succeeded