ROS2 学习记录（三）——ROS2 参数服务器与动作通信
文章目录
ROS2 参数服务器
ROS2 中的参数服务器与 ROS1 中一样，用于存储和管理 ROS2 节点的配置参数。但是 ROS2 中的参数节点也需要通过继承节点类实现。相比于其他类型的节点，参数节点所需要的功能更少，代码复杂度也更低。
C++ 实现参数节点
1#include "rclcpp/rclcpp.hpp"
2
3using namespace std;
4
5class MinimalParameterNode : public rclcpp::Node {
6 public:
7 // 构造函数，初始化节点并声明参数
8 MinimalParameterNode() : Node("minimal_parameter_node") {
9 this->declare_parameter("custom_parameter", "Hello, World!");
10 timer_ = this->create_wall_timer(1000ms, bind(&MinimalParameterNode::callback, this));
11 }
12
13 private:
14 rclcpp::TimerBase::SharedPtr timer_;
15
16 // 定时器回调函数，读取并打印参数值
17 void callback() {
18 string param = this->get_parameter("custom_parameter").as_string();
19 RCLCPP_INFO(this->get_logger(), "Custom parameter: %s", param.c_str());
20 }
21};
22
23int main(int argc, char **argv) {
24 // 初始化 ROS 2
25 rclcpp::init(argc, argv);
26 // 创建并运行节点
27 rclcpp::spin(std::make_shared<MinimalParameterNode>());
28 // 关闭 ROS 2
29 rclcpp::shutdown();
30 return 0;
31}
Python 实现参数节点
1import rclpy
2from rclpy.node import Node
3
4
5class MinimalParameterNode(Node):
6 # 初始化节点并声明一个自定义参数
7 def __init__(self):
8 super().__init__("minimal_parameter_node")
9 self.declare_parameter("custom_parameter", "Hello, World!")
10 self.__timer = self.create_timer(1.0, self.__callback)
11
12 # 定时器回调函数，用于获取并打印自定义参数的值
13 def __callback(self):
14 param = self.get_parameter("custom_parameter")
15 self.get_logger().info(f"Custom parameter: {param.value}")
16
17
18def main(args=None):
19 # 初始化 ROS 2 客户端库
20 rclpy.init(args=args)
21 # 创建节点并开始处理事件
22 rclpy.spin(MinimalParameterNode())
23 # 关闭 ROS 2 客户端库
24 rclpy.shutdown()
25
26
27if __name__ == "__main__":
28 main()
ROS2 动作通信与自定义动作
ROS1 中也有动作通信，但是我以前并未了解。ROS2 的动作通信是各种通信模式中最复杂的一种。
动作通信模型
ROS2 中的动作通信通过服务通信实现，也分为客户端与服务端。首先客户端向服务端发送动作目标（Goal），随后服务端决定是否执行目标，如果服务端执行，则向客户端发送一个 ACCEPT 信号，否则发送 REJECT 信号。服务端执行目标时，每达到一定进度，就会将反馈（Feedback）发送给客户端。当整个目标执行完毕后，服务端向客户端发送结果（Result）。
自定义动作
继续在 custom_interfaces 包中创建 action 目录，并在该目录下创建 Fibonacchi.action 用于编写自定义动作。
1mkdir -p custom_interfaces/action
2touch custom_interfaces/action/Fibonacci.action
动作文件结构如下：
1目标 Goal
2---
3结果 Result
4---
5反馈 Feedback
例如我们希望服务端求出斐波那契数列的前若干项，并实时反馈当前计算结果，则这样编写动作文件：
1int32 order
2---
3int32[] sequence
4---
5int32[] partial_sequence
为了能够编译动作，需要在 CMakeLists.txt 中添加如下内容：
1rosidl_generate_interfaces(${PROJECT_NAME}
2 "msg/Coordinate.msg"
3 "srv/CoordinateTransformation.srv"
4 "action/Fibonacci.action"
5)
运行 colcon build --packages-select custom_interfaces 编译包，即可生成对应 C++ 头文件和 Python 库文件。
C++ 实现动作通信
编写服务端节点
1#include "custom_interfaces/action/fibonacci.hpp"
2#include "rclcpp/rclcpp.hpp"
3#include "rclcpp_action/rclcpp_action.hpp"
4
5using custom_interfaces::action::Fibonacci;
6using std::placeholders::_1;
7using std::placeholders::_2;
8using namespace std::chrono_literals;
9
10// 斐波那契数列动作服务器类，继承自rclcpp::Node
11class FibonacciActionServer : public rclcpp::Node {
12 public:
13 // 构造函数，初始化节点并创建动作服务器
14 FibonacciActionServer() : Node("fibonacci_action_server") {
15 this->action_server_ =
16 rclcpp_action::create_server<Fibonacci>(this,
17 "fibonacci",
18 std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),
19 std::bind(&FibonacciActionServer::handle_cancel, this, _1),
20 std::bind(&FibonacciActionServer::handle_accepted, this, _1));
21 }
22
23 private:
24 // 动作服务器的共享指针
25 rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;
26
27 // 处理目标请求的方法
28 rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID &uuid, std::shared_ptr<const Fibonacci::Goal> goal) {
29 RCLCPP_INFO(this->get_logger(), "Received goal request: %d", goal->order);
30
31 if (goal->order < 0) {
32 RCLCPP_ERROR(this->get_logger(), "%d is an invalid order, order must be non-negative! Rejecting...", goal->order);
33 return rclcpp_action::GoalResponse::REJECT;
34 }
35
36 RCLCPP_INFO(this->get_logger(), "Received a goal: %d", goal->order);
37 RCLCPP_DEBUG(this->get_logger(), "Goal ID: %s", rclcpp_action::to_string(uuid).c_str());
38 return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
39 }
40
41 // 处理取消请求的方法
42 rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<rclcpp_action::ServerGoalHandle<Fibonacci>>) {
43 RCLCPP_INFO(this->get_logger(), "Received cancel request.");
44 return rclcpp_action::CancelResponse::ACCEPT;
45 }
46
47 // 处理已接受目标的方法
48 void handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<Fibonacci>> goal_handle) {
49 std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach();
50 }
51
52 // 执行目标的动作
53 void execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<Fibonacci>> goal_handle) {
54 RCLCPP_INFO(this->get_logger(), "Excuting goal...");
55
56 const auto goal = goal_handle->get_goal();
57 auto feedback = std::make_shared<Fibonacci::Feedback>();
58 auto &sequence = feedback->partial_sequence;
59 sequence.push_back(0);
60 sequence.push_back(1);
61
62 auto result = std::make_shared<Fibonacci::Result>();
63
64 for (int i = 1; i < goal->order && rclcpp::ok(); i++) {
65 if (goal_handle->is_canceling()) {
66 result->sequence = sequence;
67 goal_handle->canceled(result);
68 RCLCPP_INFO(this->get_logger(), "Goal canceled.");
69 return;
70 }
71
72 sequence.push_back(sequence[i] + sequence[i - 1]);
73 goal_handle->publish_feedback(feedback);
74 RCLCPP_INFO(this->get_logger(), "Publishing feedback...");
75 std::this_thread::sleep_for(200ms);
76 }
77
78 if (rclcpp::ok()) {
79 result->sequence = sequence;
80 goal_handle->succeed(result);
81 RCLCPP_INFO(this->get_logger(), "Goal succeeded.");
82 }
83 }
84};
85
86// 主函数，初始化ROS 2，创建并运行动作服务器，最后关闭ROS 2
87int main(int argc, char **argv) {
88 rclcpp::init(argc, argv);
89 rclcpp::spin(std::make_shared<FibonacciActionServer>());
90 rclcpp::shutdown();
91 return 0;
92}
编写客户端节点
1#include "custom_interfaces/action/fibonacci.hpp"
2#include "rclcpp/rclcpp.hpp"
3#include "rclcpp_action/rclcpp_action.hpp"
4
5using custom_interfaces::action::Fibonacci;
6using FibonacciGoalHandle = rclcpp_action::ClientGoalHandle<Fibonacci>;
7using std::placeholders::_1;
8using std::placeholders::_2;
9using namespace std::chrono_literals;
10
11// 斐波那契动作客户端类，继承自 rclcpp::Node
12class FibonacciActionClient : public rclcpp::Node {
13 public:
14 // 构造函数，初始化节点并创建动作客户端和定时器
15 FibonacciActionClient() : Node("fibonacci_action_client") {
16 this->action_client_ = rclcpp_action::create_client<Fibonacci>(this, "fibonacci");
17 timer_ = this->create_wall_timer(200ms, std::bind(&FibonacciActionClient::send_goal, this));
18 }
19
20 // 检查目标是否完成
21 bool is_goal_done() { return goal_done_; }
22
23 // 发送动作目标
24 void send_goal() {
25 this->timer_->cancel();
26
27 if (!this->action_client_->wait_for_action_server(5s)) {
28 RCLCPP_ERROR(this->get_logger(), "Action server unavailable.");
29 this->goal_done_ = true;
30 return;
31 }
32
33 auto goal = Fibonacci::Goal();
34 goal.order = this->random_order();
35 RCLCPP_INFO(this->get_logger(), "Sending goal with order %d", goal.order);
36
37 auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
38 send_goal_options.goal_response_callback = std::bind(&FibonacciActionClient::goal_response_callback, this, _1);
39 send_goal_options.feedback_callback = std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2);
40 send_goal_options.result_callback = std::bind(&FibonacciActionClient::result_callback, this, _1);
41
42 this->future_ = this->action_client_->async_send_goal(goal, send_goal_options);
43 }
44
45 // 生成一个随机的斐波那契序列长度
46 int random_order() { return time(nullptr) % 20 - 9; }
47
48 private:
49 rclcpp_action::Client<Fibonacci>::SharedPtr action_client_; // 动作客户端指针
50 rclcpp::TimerBase::SharedPtr timer_; // 定时器指针
51 std::shared_future<std::shared_ptr<FibonacciGoalHandle>> future_; // 发送目标的未来结果
52 bool goal_done_ = false; // 标志目标是否完成
53
54 // 处理目标响应的回调函数
55 void goal_response_callback(std::shared_future<FibonacciGoalHandle::SharedPtr> future) {
56 auto goal_handle = future.get();
57 if (!goal_handle) {
58 RCLCPP_ERROR(this->get_logger(), "Goal rejected.");
59 this->goal_done_ = true;
60 return;
61 }
62
63 RCLCPP_INFO(this->get_logger(), "Goal accepted.");
64 }
65
66 // 处理目标反馈的回调函数
67 void feedback_callback(FibonacciGoalHandle::SharedPtr, const std::shared_ptr<const Fibonacci::Feedback> feedback) {
68 std::stringstream ss;
69 for (auto number : feedback->partial_sequence) {
70 ss << number << " ";
71 }
72 RCLCPP_INFO(this->get_logger(), "Received feedback: %s", ss.str().c_str());
73 }
74
75 // 处理目标结果的回调函数
76 void result_callback(const FibonacciGoalHandle::WrappedResult &result) {
77 this->goal_done_ = true;
78
79 switch (result.code) {
80 case rclcpp_action::ResultCode::SUCCEEDED:
81 RCLCPP_INFO(this->get_logger(), "Goal succeeded!");
82 break;
83 case rclcpp_action::ResultCode::ABORTED:
84 RCLCPP_ERROR(this->get_logger(), "Goal aborted.");
85 return;
86 case rclcpp_action::ResultCode::CANCELED:
87 RCLCPP_ERROR(this->get_logger(), "Goal canceled.");
88 return;
89 default:
90 RCLCPP_ERROR(this->get_logger(), "Unknown result code,");
91 return;
92 }
93
94 std::stringstream ss;
95 for (auto number : result.result->sequence) {
96 ss << number << " ";
97 }
98 RCLCPP_INFO(this->get_logger(), "Received result: %s", ss.str().c_str());
99 }
100};
101
102// 主函数，初始化 ROS 2，创建动作客户端实例并发送目标，等待目标完成
103int main(int argc, char **argv) {
104 rclcpp::init(argc, argv);
105 auto action_client = std::make_shared<FibonacciActionClient>();
106 action_client->send_goal();
107 while (!action_client->is_goal_done()) {
108 rclcpp::spin_some(action_client);
109 }
110 rclcpp::shutdown();
111 return 0;
112}
修改 package.xml 和 CMakeLists.txt
由于使用了 rclcpp_action 包，我们需要在对应配置中添加 rclcpp_action 依赖：
1<depend>rclcpp_action</depend>
1find_package(rclcpp_action REQUIRED)
其余部分正常修改即可。
Python 实现动作通信
编写服务端节点
1import rclpy
2from rclpy.action import ActionServer
3from rclpy.node import Node
4
5from custom_interfaces.action import Fibonacci
6
7
8class FibonacciActionServer(Node):
9 # 初始化FibonacciActionServer节点
10 def __init__(self):
11 super().__init__("fibonacci_action_server")
12 self.__action_server = ActionServer(
13 self, Fibonacci, "fibonacci", self.__execute
14 )
15
16 async def __execute(self, goal_handle):
17 # 执行Fibonacci序列生成的逻辑
18 self.get_logger().info("Executing goal...")
19 sequence = [0, 1]
20
21 if goal_handle.request.order < 0:
22 # 如果请求的Fibonacci序列的阶数小于0，记录错误并中止目标
23 self.get_logger().error("Invalid order, aborting goal...")
24 goal_handle.abort()
25 result = Fibonacci.Result()
26 result.sequence = sequence
27 return result
28
29 for i in range(1, goal_handle.request.order):
30 # 检查目标是否被取消，如果是则记录并返回当前结果
31 if goal_handle.is_cancel_requested:
32 self.get_logger().info("Goal canceled.")
33 result = Fibonacci.Result()
34 result.sequence = sequence
35 return result
36
37 # 生成Fibonacci序列
38 sequence.append(sequence[i] + sequence[i - 1])
39
40 # 发布反馈信息
41 feedback = Fibonacci.Feedback()
42 feedback.partial_sequence = sequence
43 goal_handle.publish_feedback(feedback)
44 self.get_logger().info(f"Sending feedback: {sequence}")
45 rclpy.spin_once(self, timeout_sec=0.1)
46
47 # 目标成功完成
48 goal_handle.succeed()
49
50 # 设置并返回结果
51 result = Fibonacci.Result()
52 result.sequence = sequence
53 self.get_logger().info(f"Result: {sequence}")
54 return result
55
56
57def main(args=None):
58 # 主函数，初始化ROS2节点并运行ActionServer
59 rclpy.init(args=args)
60 rclpy.spin(FibonacciActionServer())
61 rclpy.shutdown()
62
63
64if __name__ == "__main__":
65 main()
编写客户端节点
1from random import randint
2
3import rclpy
4from rclpy.action import ActionClient
5from rclpy.node import Node
6
7from custom_interfaces.action import Fibonacci
8
9
10class FibonacciActionClient(Node):
11 # 初始化节点，设置节点名称，并创建一个异步操作客户端
12 def __init__(self):
13 super().__init__("fibonacci_action_client")
14 self.__action_client = ActionClient(self, Fibonacci, "fibonacci")
15 self.__goal_handle = None
16 self.__resul_future = None
17 self.goal_done = False
18
19 # 发送目标到服务器，并设置回调函数以处理反馈和目标响应
20 def send_goal(self):
21 if not self.__action_client.wait_for_server(timeout_sec=2.0):
22 self.get_logger().error("Action server unavailable.")
23 self.goal_done = True
24 return
25
26 goal = Fibonacci.Goal()
27 goal.order = randint(-10, 10)
28
29 self.get_logger().info(f"Sending goal with order {goal.order}")
30
31 self.__action_client.send_goal_async(
32 goal, feedback_callback=self.__feedback_callback
33 ).add_done_callback(self.__goal_response_callback)
34
35 # 处理反馈消息的回调函数
36 def __feedback_callback(self, feedback_message):
37 feedback = feedback_message.feedback
38 self.get_logger().info(f"Received feedback: {feedback.partial_sequence}")
39
40 # 处理目标响应的回调函数
41 def __goal_response_callback(self, future):
42 goal_handle = future.result()
43
44 if not goal_handle.accepted:
45 self.get_logger().error("Goal rejected.")
46 self.goal_done = True
47 return
48
49 self.get_logger().info("Goal accepted.")
50 self.__goal_handle = goal_handle
51
52 self.__goal_handle.get_result_async().add_done_callback(self.__result_callback)
53
54 # 处理结果消息的回调函数
55 def __result_callback(self, future):
56 result = future.result().result
57 self.get_logger().info(f"Result: {result.sequence}")
58 self.goal_done = True
59
60
61# 主函数，初始化 ROS2 系统，创建 FibonacciActionClient 实例，发送目标，并等待目标完成
62def main(args=None):
63 rclpy.init(args=args)
64 action_client = FibonacciActionClient()
65 action_client.send_goal()
66
67 while not action_client.goal_done:
68 rclpy.spin_once(action_client)
69
70 rclpy.shutdown()
71
72
73if __name__ == "__main__":
74 main()