编写动作服务器和客户端 (c ++) [待校准@6753]
Goal目标: Goal在C++ 中实现动作服务器和客户端。 [待校准@6754]
Tutorial教程级别: Intermediate中级 [待校准@6713]
时间: 15分钟 [Alyssa@6755]
背景
[需手动修复的语法]Action是ROS中的一种异步通讯形式。* [需手动修复的语法]Action客户端 * 向 * 动作服务器 * 发送目标请求。* [需手动修复的语法]Action服务器 * 向 * 动作客户端 * 发送目标反馈和结果。 [待校准@6756]
先决条件
您将需要上一教程 “ 创建动作 [待校准@6711] ” 中定义的 action_tutorials_interfaces
包和 Fibonacci.action
接口。 [待校准@6757]
任务
1创建action_tutorials_cpp包 [机器人@6758]
正如我们在 创建您的第一个ROS 2包 [待校准@7117] 教程中看到的,我们需要创建一个新的包来保存我们的C++ 和支持性代码。 [待校准@6759]
1.1创建动作 _ 教程 _ cpp包 [待校准@6760]
进入您在 previous tutorial 中创建的动作工作区 (请记住为工作区源文件),并为c动作服务器创建一个新包: [待校准@6761]
cd ~/action_ws/src
ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp
cd ~/action_ws/src
ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp
cd \dev\action_ws\src
ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp
1.2添加可见性控制 [待校准@6762]
为了使包编译并在Windows上工作,我们需要添加一些 "visibility control" 。有关为什么需要这样做的详细信息,请参阅 here 。 [待校准@6763]
打开 action_tutorials_cpp/include/action_tutorials_cpp/visibility_control.h
,放入以下代码: [待校准@6764]
#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编写动作服务器 [待校准@6765]
让我们集中精力编写一个动作服务器,使用我们在 创建动作 [待校准@6711] 教程中创建的动作来计算斐波那契序列。 [待校准@6766]
2.1编写动作服务器代码 [待校准@6767]
打开 action_tutorials_cpp/src/fibonacci_action_server.cpp
,放入以下代码: [待校准@6768]
#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)
前几行包括我们需要编译的所有标头。 [待校准@6769]
接下来,我们创建一个类,该类是 “rclcpp:: node” 的派生类: [待校准@6770]
class FibonacciActionServer : public rclcpp::Node
[需手动修复的语法] FibonacciActionServer
类的构造函数将节点名称初始化为 fibonacci_action_server
: [待校准@6771]
explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
: Node("fibonacci_action_server", options)
构造函数还实例化新的动作服务器: [待校准@6772]
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件事: [待校准@6773]
模板化动作类型名称:
Fibonacci
。 [待校准@6774]一个ROS 2节点,用于将动作添加到:
this
。 [待校准@6775]动作名称: “'fibonacci'”。 [待校准@6776]
用于处理目标的调用返回函数:
handle_goal
[待校准@6777]用于处理取消的调用返回函数:
handle_cancel
。 [待校准@6778]用于处理目标接受的调用返回函数:
handle_accept
。 [待校准@6779]
接下来是文件中各种调用的实现。请注意,所有的调用都需要快速返回,否则我们有可能使执行程序挨饿。 [待校准@6780]
我们首先调用back来处理新目标: [待校准@6781]
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;
}
此实现仅接受所有目标。 [待校准@6782]
接下来是处理取消的调用: [待校准@6783]
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;
}
这个实现只是告诉客户端它接受了取消。 [待校准@6784]
最后一个调用back接受一个新目标并开始处理它: [待校准@6785]
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
返回。 [待校准@6786]
所有进一步的处理和更新都在新线程的 execute
方法中完成: [待校准@6787]
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
标记为成功,然后退出。 [待校准@6788]
我们现在有了一个功能齐全的动作服务器。让我们构建并运行它。 [待校准@6789]
2.2编译动作服务器 [待校准@6790]
在上一节中,我们将动作服务器代码放在适当的位置。为了让它编译并运行,我们需要做一些额外的事情。 [待校准@6791]
首先,我们需要设置CMakeLists.txt,以便编译动作服务器。打开 action_tutorials_cpp/CMakeLists.txt
,在 find_package
调用后添加以下内容: [待校准@6792]
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)
现在我们可以编译这个包了。转到 action_ws
的顶层,然后运行: [待校准@6793]
colcon build
这应该编译整个工作区,包括 fibonacci_action_server
在 action_tutorials_cpp
包。 [待校准@6794]
2.3运行动作服务器 [待校准@6795]
现在我们已经构建了动作服务器,我们可以运行它了。获取我们刚刚构建的工作区 ( action_ws
),并尝试运行动作服务器: [待校准@6796]
ros2 run action_tutorials_cpp fibonacci_action_server
3编写动作客户端 [待校准@6797]
3.1编写动作客户端代码 [待校准@6798]
打开 action_tutorials_cpp/src/fibonacci_action_client.cpp
,放入以下代码: [待校准@6799]
#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)
前几行包括我们需要编译的所有标头。 [待校准@6769]
接下来,我们创建一个类,该类是 “rclcpp:: node” 的派生类: [待校准@6770]
class FibonacciActionClient : public rclcpp::Node
[需手动修复的语法] FibonacciActionClient
类的构造函数将节点名称初始化为 fibonacci_action_client
: [待校准@6800]
explicit FibonacciActionClient(const rclcpp::NodeOptions & options)
: Node("fibonacci_action_client", options)
构造函数还实例化新的动作客户端: [待校准@6801]
this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
this,
"fibonacci");
一个动作客户端需要三件事: [待校准@6802]
模板化动作类型名称:
Fibonacci
。 [待校准@6774]一个ROS 2节点,用于将动作客户端添加到:
this
。 [待校准@6803]动作名称: “'fibonacci'”。 [待校准@6776]
我们还实例化了一个ROS计时器,该计时器将启动唯一调用 send_goal
: [待校准@6804]
this->timer_ = this->create_wall_timer(
std::chrono::milliseconds(500),
std::bind(&FibonacciActionClient::send_goal, this));
当计时器到期时,它将调用 send_goal
: [待校准@6805]
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);
}
此函数执行以下操作: [待校准@6806]
取消计时器 (因此只调用一次)。 [待校准@6807]
等待动作服务器启动。 [待校准@6808]
实例化一个新的 “斐波那契:: 目标”。 [待校准@6809]
设置响应、反馈和结果调用。 [待校准@6810]
将目标发送到服务器。 [待校准@6811]
当服务器接收并接受目标时,它将向客户端发送响应。该回应由 goal_response_callback
处理: [待校准@6812]
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
处理: [待校准@6813]
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
处理: [待校准@6814]
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();
}
我们现在有了一个功能齐全的动作客户端。让我们构建并运行它。 [待校准@6815]
3.2编译动作客户端 [待校准@6816]
在上一节中,我们将动作客户端代码放置到位。为了让它编译并运行,我们需要做一些额外的事情。 [待校准@6817]
首先,我们需要设置CMakeLists.txt,以便编译动作客户端。打开 action_tutorials_cpp/CMakeLists.txt
,在 find_package
调用后添加以下内容: [待校准@6818]
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)
现在我们可以编译这个包了。转到 action_ws
的顶层,然后运行: [待校准@6793]
colcon build
这应该编译整个工作区,包括 fibonacci_action_client
在 action_tutorials_cpp
包。 [待校准@6819]
3.3运行动作客户端 [待校准@6820]
现在我们已经构建了动作客户端,我们可以运行它了。首先,确保动作服务器在单独的终端中运行。现在源文件我们刚刚构建的工作区 ( action_ws
),并尝试运行动作客户端: [待校准@6821]
ros2 run action_tutorials_cpp fibonacci_action_client
您应该会看到已记录的目标被接受的消息、正在打印的反馈以及最终结果。 [待校准@6822]
总结
在本教程中,您将c动作服务器和动作客户端逐行组合在一起,并将它们配置为交换目标、反馈和结果。 [待校准@6823]