ROS2学习之旅（21）——创建一个动作服务和客户节点（C++）
2021/7/16 12:05:23
本文主要是介绍ROS2学习之旅（21）——创建一个动作服务和客户节点（C++），对大家解决编程问题具有一定的参考价值，需要的程序猿们随着小编来一起学习吧！
动作是ROS中的一种异步通信形式，动作客户端向动作服务器发送目标请求，目标服务器向操作客户端发送目标反馈和结果。本文基于前一篇自定义动作博文。
1.创建一个action_turtorials_cpp包
1.1 创建一个action_turtorials_cpp包
在终端运行：
cd ~/action_ws/src ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp
1.2添加可见性控件
为了使包可以在Windows上编译和工作，我们需要添加一些“可见性控制”。有关为什么需要这样做的详细信息，请参见这里。
打开action_tutorials_cpp/include/action_tutorials_cpp/ visbility_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。 - 将一个ROS2节点的动作添加到：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");
}
}
这个工作线程每秒处理一个斐波那契数列序号，为每个步骤发布一个反馈更新。当它完成处理时，它将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)
现在可以编译包了，进入action_ws的根目录，并运行:
colcon build
2.3运行动作服务器
现在已经构建了动作服务器，可以运行它：
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)
前几行包含需要编译的所有头文件。
接下来，创建一个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。 -
将动作客户端添加到的ROS2节点:
this。 -
动作名：
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);
}
这个函数的功能如下:
-
取消计时器(因此只调用一次)。
-
等待动作服务器启动。
-
实例化一个新的
Fibonacci::Goal。 -
设置响应、反馈和结果回调。
-
将目标发送到服务器。
当服务器接收并接受目标时，它将向客户机发送一个响应。该响应由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.2编译动作客户端
打开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)
编译：
colcon build
3.3运行动作客户端
现在已经构建了动作客户端，可以运行它。首先，确保动作服务器在单独的终端中运行：
ros2 run action_tutorials_cpp fibonacci_action_server
运行动作客户端:
ros2 run action_tutorials_cpp fibonacci_action_client
现在可以看到被接受的目标日志消息、打印的反馈和最终的结果。
4.总结
在本文中，编写了一个C++动作服务器和客户端，并为它们配置目标、反馈和结果。
如果给您带来帮助，希望能给点个关注，以后还会陆续更新有关机器人的内容，点个关注不迷路~欢迎大家一起交流学习。
都看到这了，点个推荐再走吧~
未经允许，禁止转载。
这篇关于ROS2学习之旅（21）——创建一个动作服务和客户节点（C++）的文章就介绍到这儿，希望我们推荐的文章对大家有所帮助，也希望大家多多支持为之网！
- 2024-11-15Typescript 类型教程：轻松入门与实践指南
- 2024-11-15AntDesign-icons项目实战：新手入门教程
- 2024-11-14用Scratch编写语言模型：爪爪（Clawed）式简易教程
- 2024-11-14用大型语言模型在Amazon Bedrock上分类Jira工单
- 2024-11-14从数据到行动：亚马逊Bedrock代理如何自动化复杂工作流
- 2024-11-14Databricks与优化后的Snowflake性能大比拼
- 2024-11-14亚马逊 Inspector 解析：提升您的 AWS 负载安全的利器
- 2024-11-14揭秘VS Code for Web - Azure：轻松开发云端应用的新利器
- 2024-11-14揭秘指南：如何让Databricks中的数据为最终用户所用
- 2024-11-14OpenTelemetry扩展进入CI/CD可观测性领域
