概要:这篇内容主要介绍如何使用C++来编写动作服务器和客户端
环境:ubuntu20.04,ros2-foxy,vscode
最后如果没有陈述实操过程中碰到问题的话,则表示该章节都可被本人正常复现.
3.2编写动作服务器和客户端(C++)(原文:https://docs.ros.org/en/foxy/Tutorials/Actions/Writing-a-Cpp-Action-Server-Client.html
)
>>
教程>>
编写动作服务器和客户端(C++
)
你正阅读的是ros2
较老版本(Foxy
),但仍然支持的说明文档.想查看最新版本的信息,请看galactic
版本链接( https://docs.ros.org/en/galactic/Tutorials.html
)
编写动作服务器和客户端(C++
)
目标:实现c++
版的动作服务器和客户端.
时长:15min
目录
1.背景
2.预备知识
3.步骤
3.1创建action_tutorials_cpp包
3.2编写动作服务器
3.3编写动作客户端
4.总结
5.相关内容
动作是ros异步通信的方式.动作客户端发送目标请求到动作服务器,动作服务器发送目标反馈以及结果到动作客户端.
你需要前面创建一个动作教程里面创建的action_tutorials_interfaces
包以及Fibonacci.action
接口.
参考创建第一个ros2
包课程,我们需要创建一个包来放c++
源码的,并提供支持的.
进入前面课程创建动作的工作空间(记得source
一下环境变量),然后创建一个装c++动作服务器的包:
linux:
cd ~/action_ws/src ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp
(本人练习时,还是在之前dev_ws
工作空间,所以进入工作空间会跟官方有点出入)
为了保证包的编译以及在窗口有效,我们需要添加明显需要的内容.为啥需要这些的细节缘由,可以看这里(https://docs.microsoft.com/en-us/cpp/cpp/dllexport-dllimport?view=msvc-160
).
打开并创建路径文件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_
现在,让我们专注于计算Fibonacci
数列的动作服务器编写,这会用到前面创建动作课程所创建的动作.
打开路径并创建action_tutorials_cpp/src/fibonacci_action_server.cpp
,然后复制以下代码到里面:
1#include <functional> 2#include <memory> 3#include <thread> 4 5#include "action_tutorials_interfaces/action/fibonacci.hpp" 6#include "rclcpp/rclcpp.hpp" 7#include "rclcpp_action/rclcpp_action.hpp" 8#include "rclcpp_components/register_node_macro.hpp" 9 10#include "action_tutorials_cpp/visibility_control.h" 11 12namespace action_tutorials_cpp 13{ 14class FibonacciActionServer : public rclcpp::Node 15{ 16public: 17 using Fibonacci = action_tutorials_interfaces::action::Fibonacci; 18 using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>; 19 20 ACTION_TUTORIALS_CPP_PUBLIC 21 explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) 22 : Node("fibonacci_action_server", options) 23 { 24 using namespace std::placeholders; 25 26 this->action_server_ = rclcpp_action::create_server<Fibonacci>( 27 this, 28 "fibonacci", 29 std::bind(&FibonacciActionServer::handle_goal, this, _1, _2), 30 std::bind(&FibonacciActionServer::handle_cancel, this, _1), 31 std::bind(&FibonacciActionServer::handle_accepted, this, _1)); 32 } 33 34private: 35 rclcpp_action::Server<Fibonacci>::SharedPtr action_server_; 36 37 rclcpp_action::GoalResponse handle_goal( 38 const rclcpp_action::GoalUUID & uuid, 39 std::shared_ptr<const Fibonacci::Goal> goal) 40 { 41 RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order); 42 (void)uuid; 43 return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; 44 } 45 46 rclcpp_action::CancelResponse handle_cancel( 47 const std::shared_ptr<GoalHandleFibonacci> goal_handle) 48 { 49 RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); 50 (void)goal_handle; 51 return rclcpp_action::CancelResponse::ACCEPT; 52 } 53 54 void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle) 55 { 56 using namespace std::placeholders; 57 // this needs to return quickly to avoid blocking the executor, so spin up a new thread 58 std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach(); 59 } 60 61 void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle) 62 { 63 RCLCPP_INFO(this->get_logger(), "Executing goal"); 64 rclcpp::Rate loop_rate(1); 65 const auto goal = goal_handle->get_goal(); 66 auto feedback = std::make_shared<Fibonacci::Feedback>(); 67 auto & sequence = feedback->partial_sequence; 68 sequence.push_back(0); 69 sequence.push_back(1); 70 auto result = std::make_shared<Fibonacci::Result>(); 71 72 for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) { 73 // Check if there is a cancel request 74 if (goal_handle->is_canceling()) { 75 result->sequence = sequence; 76 goal_handle->canceled(result); 77 RCLCPP_INFO(this->get_logger(), "Goal canceled"); 78 return; 79 } 80 // Update sequence 81 sequence.push_back(sequence[i] + sequence[i - 1]); 82 // Publish feedback 83 goal_handle->publish_feedback(feedback); 84 RCLCPP_INFO(this->get_logger(), "Publish feedback"); 85 86 loop_rate.sleep(); 87 } 88 89 // Check if goal is done 90 if (rclcpp::ok()) { 91 result->sequence = sequence; 92 goal_handle->succeed(result); 93 RCLCPP_INFO(this->get_logger(), "Goal succeeded"); 94 } 95 } 96}; // class FibonacciActionServer 97 98} // namespace action_tutorials_cpp 99 100 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(
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
样东西:
1.模板动作类名:Fibonacci 2.用于添加动作的ros2节点:this 3.动作名称:'fibonacci' 4.负责目标的回调函数:handle_goal 5.负责取消的回调函数:handle_cancel 6.负责目标取消的回调函数: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(); }
由于execute
是一个需要长时间运行的操作,我们开一个线程来做实际的工作,并且可以很快地从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"); } }
工作线程每秒都会处理Fibonacci
数列的连串数字,并发布信息反馈每一步的更新.当完成处理过程,它会标记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
现在我们构建好动作服务器了,可以运行它了.source
一下工作空间环境变量,然后尝试运行动作服务器:
ros2 run action_tutorials_cpp fibonacci_action_server
打开路径并新建action_tutorials_cpp/src/fibonacci_action_client.cpp
,把下面代码放到里面:
1#include <functional> 2#include <future> 3#include <memory> 4#include <string> 5#include <sstream> 6 7#include "action_tutorials_interfaces/action/fibonacci.hpp" 8 9#include "rclcpp/rclcpp.hpp" 10#include "rclcpp_action/rclcpp_action.hpp" 11#include "rclcpp_components/register_node_macro.hpp" 12 13namespace action_tutorials_cpp 14{ 15class FibonacciActionClient : public rclcpp::Node 16{ 17public: 18 using Fibonacci = action_tutorials_interfaces::action::Fibonacci; 19 using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>; 20 21 explicit FibonacciActionClient(const rclcpp::NodeOptions & options) 22 : Node("fibonacci_action_client", options) 23 { 24 this->client_ptr_ = rclcpp_action::create_client<Fibonacci>( 25 this, 26 "fibonacci"); 27 28 this->timer_ = this->create_wall_timer( 29 std::chrono::milliseconds(500), 30 std::bind(&FibonacciActionClient::send_goal, this)); 31 } 32 33 void send_goal() 34 { 35 using namespace std::placeholders; 36 37 this->timer_->cancel(); 38 39 if (!this->client_ptr_->wait_for_action_server()) { 40 RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); 41 rclcpp::shutdown(); 42 } 43 44 auto goal_msg = Fibonacci::Goal(); 45 goal_msg.order = 10; 46 47 RCLCPP_INFO(this->get_logger(), "Sending goal"); 48 49 auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions(); 50 send_goal_options.goal_response_callback = 51 std::bind(&FibonacciActionClient::goal_response_callback, this, _1); 52 send_goal_options.feedback_callback = 53 std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2); 54 send_goal_options.result_callback = 55 std::bind(&FibonacciActionClient::result_callback, this, _1); 56 this->client_ptr_->async_send_goal(goal_msg, send_goal_options); 57 } 58 59private: 60 rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_; 61 rclcpp::TimerBase::SharedPtr timer_; 62 63 void goal_response_callback(std::shared_future<GoalHandleFibonacci::SharedPtr> future) 64 { 65 auto goal_handle = future.get(); 66 if (!goal_handle) { 67 RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); 68 } else { 69 RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); 70 } 71 } 72 73 void feedback_callback( 74 GoalHandleFibonacci::SharedPtr, 75 const std::shared_ptr<const Fibonacci::Feedback> feedback) 76 { 77 std::stringstream ss; 78 ss << "Next number in sequence received: "; 79 for (auto number : feedback->partial_sequence) { 80 ss << number << " "; 81 } 82 RCLCPP_INFO(this->get_logger(), ss.str().c_str()); 83 } 84 85 void result_callback(const GoalHandleFibonacci::WrappedResult & result) 86 { 87 switch (result.code) { 88 case rclcpp_action::ResultCode::SUCCEEDED: 89 break; 90 case rclcpp_action::ResultCode::ABORTED: 91 RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); 92 return; 93 case rclcpp_action::ResultCode::CANCELED: 94 RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); 95 return; 96 default: 97 RCLCPP_ERROR(this->get_logger(), "Unknown result code"); 98 return; 99 } 100 std::stringstream ss; 101 ss << "Result received: "; 102 for (auto number : result.result->sequence) { 103 ss << number << " "; 104 } 105 RCLCPP_INFO(this->get_logger(), ss.str().c_str()); 106 rclcpp::shutdown(); 107 } 108}; // class FibonacciActionClient 109 110} // namespace action_tutorials_cpp 111 112 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
部分内容:
1.动作类模板名:Fibonacci 2.用于添加动作客户端的ros2节点:this 3.动作名:'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); }
这个函数所做的内容如下:
1.取消定时器(所以它只是调用一次) 2.等待动作服务器启动 3.实例化一个新的Fibonacci::Goal 4.设置(目标)响应,反馈和结果的回调的回调函数 5.发送目标到服务器
当服务器接收并接受目标,它会给一个响应到客户端,这个响应由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"); } }
当服务器处理完成,它会返回一个结果到客户端.这个结果(反馈)由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(); }
现在我们有了一个完整功能性的动作客户端,让我们开始编译运行吧.
在上一小节,我们写好了动作客户端的代码.为了让它可以编译并运行,我们需要做一些额外工作.
首先,我们需要配置一下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)
现在,我们可以编译这个包了.回到所在工作空间的根目录,运行:
colcon build
(如果是有多个包情况,建议添加指令--packages-select
来指定编译,提高编译速度)
上面的指令会编译整个工作空间,包括action_tutorials_cpp
包里面的fibonacci_action_client
.
现在我们构建好了动作客户端,我们可以运行了.首先保证动作服务器正在别的终端运行,source
一下工作空间环境变量,然后尝试运行动作客户端:
ros2 run action_tutorials_cpp fibonacci_action_client
你会看见日志信息,目标被接受,反馈被打印和最后结果.
在本课程,你用c++逐行一起实现动作服务器和动作客户端,并且配置它们以实现传递目标,传递反馈和传递结果的功能.
这里(https://github.com/ros2/examples/tree/foxy/rclcpp
)有几种方式,你可以用来编写c++
版本的动作服务器和动作客户端,并且可以校核minimal_action_server
和 minimal_action_client
包.
想知道更多关于ros
动作细节,你可以参考设计文档(http://design.ros2.org/articles/actions.html
).
个人认为重点:
动作服务器,动作客户端分别对应的功能函数的每一行句子的含义理解;配置文件,这里具体指的是CMakeLists.txt
的编写;如何才能用到上一节课创建的动作接口(开始创建包时候,作为依赖添加上去).
这课程是在等毕业证那十几天搞的,室友问,现在在线翻译这么强大,为啥还在这里瞎折腾呢?我说,我的目地是好好认真看一下,了解一下,自己折腾,目前是我想到最好的办法来获得最佳效果,即使这翻译有点别扭,哈哈哈.
#####################
不积硅步,无以至千里
好记性不如烂笔头
感觉有点收获的话,麻烦大大们点赞收藏哈