动作是ROS中的一种异步通信形式,动作客户端向动作服务器发送目标请求,目标服务器向操作客户端发送目标反馈和结果。本文基于前一篇自定义动作博文。
action_turtorials_cpp
包action_turtorials_cpp
包在终端运行:
cd ~/action_ws/src ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp
为了使包可以在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_
接下来编写一个动作服务器,使用在前文创建的动作接口来计算斐波那契数列。
打开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
。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
标记为成功,然后退出。
设置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
现在已经构建了动作服务器,可以运行它:
ros2 run action_tutorials_cpp fibonacci_action_server
打开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(); }
打开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
现在已经构建了动作客户端,可以运行它。首先,确保动作服务器在单独的终端中运行:
ros2 run action_tutorials_cpp fibonacci_action_server
运行动作客户端:
ros2 run action_tutorials_cpp fibonacci_action_client
现在可以看到被接受的目标日志消息、打印的反馈和最终的结果。
在本文中,编写了一个C++动作服务器和客户端,并为它们配置目标、反馈和结果。
如果给您带来帮助,希望能给点个关注,以后还会陆续更新有关机器人的内容,点个关注不迷路~欢迎大家一起交流学习。
都看到这了,点个推荐再走吧~
未经允许,禁止转载。