ROS2C++核心基础

ROS2核心基础

节点

什么是节点（What is Node）

节点的定义和概念

• 节点： ROS2系统中的基本执行单元，是一个独立的进程
• 功能职责： 每个节点负责特定的功能，如传感器处理、运动控制、决策等
• 通信机制： 节点间通过话题、服务、动作进行通信

节点的特性

节点特性：
├── 独立性 # 每个节点是独立的进程，崩溃不影响其他节点
├── 可重用性 # 同一节点可以启动多个实例
├── 分布式 # 节点可以运行在不同的机器上
├── 命名空间 # 支持命名空间隔离
└── 生命周期 # 具有完整的生命周期管理

节点的组成要素

• 基础标识
  • 节点名称和命名空间-确保节点在系统中的唯一识别和组织管理
• 唯一标识符-防止节点名称冲突，支持分布式部署
• 通信接口
• **发布者：**异步发送数据到话题，实现一对多广播通信
• **订阅者：**异步接收话题数据，支持多个订阅者同时监听
• **服务服务器：**提供同步请求-响应服务，处理客户端调用
• **服务客户端：**发起同步请求，等地啊服务器响应
• 动作服务器：处理长时间运行的任务，提供进度反馈
• 动作客户端：发起异步任务请求，接收执行状态和结果
• 配置管理
  • 参数声明和验证：定义节点配置项，确保参数类型和范围正确性
• 参数回调处理：监听参数变化。实现动态配置响应
• 动态参数更新：运行时修改节点行为，无需重启节点
• 时间管理
  • 周期性定时器：按固定间隔执行任务，如传感器数据采集
• 一次性定时器：延迟执行单次任务，用于超时处理
• 时间戳处理：记录和同步事件和时间，确保数据正确
• 日志系统
  • 多级别日志记录：支持DEBUG/INFO/WARN/ERROR等不同重要性级别
• 格式化输出：统一日志格式，便于分析和调试
• 调试信息：提供运行时状态信息，帮助问题定位
• 执行控制
  • 回调组管理：组织和调度不同类型的回调函数
• 优先级控制：确保重要人物优先执行
• 并发处理：支持多线程执行，提高系统响应性能
• 生命周期
  • 状态管理：跟踪节点当前运行状态
• 配置/激活/停用/清理：标准化节点启动、运行、停止流程
• 状态转换：确保节点安全可靠的在不同状态间切换

节点编程方法

C++节点编程基础

• 创建C++功能包

# 进入工作空间src目录
cd ~/ros2_ws/src# 创建简单的C++功能包
ros2 pkg create --build-type ament_cmake hello_cpp_node --dependencies rclcpp

命令解析：

• ros2 pkg create
  • ROS2的包创建工具
• 用于自动生成标准ROS2包结构
• –build-type ament_cmake
• 指定构建系统类型为ament_cmake
• ament_cmake是ROS2推荐的C++包构建系统
• 基于CMake，提供ROS2特点的构建功能
• hello_cpp_node
• 包名称
• 将创建名为hello_cpp_node的ROS2包
• 包名应遵循小写字母和下划线的命名规范
• –dependencies rclcpp
• 声明包依赖项
• rclcpp是ROS2的C++客户端库
• 自动在package.xml和CMakeLists.txt中添加此依赖项

**执行结果：**该命令将创建以下目录结构：

hello_cpp_node/
├── CMakeLists.txt          # CMake构建配置文件
├── package.xml             # 包元数据和依赖声明
├── src/                    # 源代码目录
└── include/hello_cpp_node/ # 头文件目录

编写节点代码

src/hello_cpp_node/src/hello_node.cpp
这个范例启动一个定时器，间隔一秒打印信息。

#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <chrono>
#include <string>// chrono字面量，方便时间单位标识（1s表示1秒）
using namespace std::chrono_literals;class HelloNode : public rclcpp::Node {public:HelloNode() : Node("hello_node"), count_(0) {// 创建墙时钟定时器，每1秒执行一次timer_callback函数// create_wall_timer使用系统时钟，不受ROS时钟影响timer_ = this->create_wall_timer(1s, std::bind(&HelloNode::timer_callback, this));RCLCPP_INFO(this->get_logger(), "Hello ROS2 node setup!");}private:void timer_callback() {RCLCPP_INFO(this->get_logger(), "Hello ROS2! count_: %ld", count_++);}rclcpp::TimerBase::SharedPtr timer_;uint32_t count_;
};int main(int argc, char* argv[]) {rclcpp::init(argc, argv);auto node = std::make_shared<HelloNode>();// 进入ROS2事件循环// 这个函数会被阻塞直到节点关闭rclcpp::spin(node);// 清理ros2资源，关闭通信中间件rclcpp::shutdown();return 0;
}

设置编译选项

src/hello_cpp_node/CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(hello_cpp_node)if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")add_compile_options(-Wall -Wextra -Wpedantic)
endif()# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)if(BUILD_TESTING)find_package(ament_lint_auto REQUIRED)# the following line skips the linter which checks for copyrights# comment the line when a copyright and license is added to all source filesset(ament_cmake_copyright_FOUND TRUE)# the following line skips cpplint (only works in a git repo)# comment the line when this package is in a git repo and when# a copyright and license is added to all source filesset(ament_cmake_cpplint_FOUND TRUE)ament_lint_auto_find_test_dependencies()
endif()add_executable(hello_node src/hello_cpp_node.cpp)
ament_target_dependencies(hello_node rclcpp)install(TARGETShello_nodeDESTINATION lib/${PROJECT_NAME}
)ament_package()

编译运行

# --run--
cd ~/ros2_ws
# 编译指定包
colcon build --packages-select hello_cpp_nodesource install/setup.bash
# 运行 [节点] [节点中的二进制文件]
ros2 run hello_cpp_node hello_node

节点命令行操作

在hello_node运行时的操作命令

节点信息查看

# 查看所有运行的节点
ros2 node list

# 查看节点详细信息
ros2 node info /my_node
# 比如
ros2 node info /hello_node# 实时查看节点信息
watch -n 1 "ros2 node list"# 可以停止hello_node，然后启动hello_node，观察watch信息

通信机制-话题

什么是话题

定义和概念

• **话题：**ROS2中节点间进行异步数据传输的命名通道
• **作用：**作为数据传递的桥梁，连接发布者和订阅者
• **特点：**基于发布-订阅模式，支持一对多、多对一、多对多通信

话题组成要素

话题系统：
├── 话题名称 # 唯一标识符，如 "/sensor_data"
├── 消息类型 # 数据格式定义，如 std_msgs/String
├── 发布者 # 发送数据的节点
└── 订阅者 # 接收数据的节点

话题vs其他通信方式

• **话题：**异步、多对多、持续数据流
• **服务：**同步、一对一、请求-响应
• **动作：**异步、长时间任务、带反馈

话题通信模型

多对多通信

通信模式示例：
发布者1 ──┐
发布者2 ──┼──► 话题 "/data" ──┼──► 订阅者1
发布者3 ──┘                   ├──► 订阅者2└──► 订阅者3

异步通信特点

• 发布者： 发送消息后立刻返回，不等待接收确认
• 订阅者： 通过回调函数异步处理接收数据
• 解耦性： 发布审核和订阅者不需要知道对方的存在

消息接口

• 标准消息类型：std_msgs，geometry_msgs，sensor_msgs等
  • std_msgs：基础数据类型

//基本数据类型
std_msgs::msg::String text_msg;
text_msg.data = "Hallo ROS2"std_msgs::msg::Int32 number_msg;
number_msg.data = 42;// 数组类型
std_msgs::msg::Float64MultiArray array_msg;
array_msg.data = {1.0, 2.0, 3.0, 4.0};
array_msg.layout.dim.push_back(std_msgs::msg::MultiArrayDimension());
array_msg.layout.dim[0].size = 4;
array_msg.layout.dim[0].stride = 4;
array_msg.layout.dim[0].label = "values;

- geometry_msgs-集合和空间信息* 位置信息 geometry_msgs::msg::Point* 四元数标识旋转 geometry_msgs::msg::Quaternion* 位姿信息（位置+旋转） geometry_msgs::msg::Pose* 带时间戳的位姿 geometry_msgs::msg::PoseStamped* 速度信息 geometry_msgs::msgs::msg::Twist* 路径信息 nav_msgs::msg::Path
- sensor_msgs-传感器数据* 图像数据 sensor_msgs::msg::Image* 激光扫描数据 sensor_msgs::msg::LaserScan* 点云数据 sensor_msgs::msg::PointCloud2* IMU数据 sensor_msgs::msg::Imu

• 自定义消息： 用户可以定义特定的消息格式
  • 消息定义文件（.msg） 定义数据结构

# 在msg/目录下创建 RobotStatus.msg
# msg/RobotStatus.msg
string robot_name
geometry_msgs/Pose current_pose
float64 battery_level
bool is_moving
int32[] sensor_readings
RobotMode mode # 自定义枚举# 常量定义
int32 STATUS_IDLE = 0
int32 STATUS_MOVING = 0
int32 STATUS_CHARGING = 2

• QoS（服务质量）： 控制星系传输的可靠性和延迟
  • QoS配置参数节选

#include <rclcpp/qos.hpp>class QosExampleNode: public rclcpp::Node {
public:QosExampleNode() : Node("qos_example") {// 可靠性设置auto reliable_qos = rclcpp::QoS(10).reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE) // 可靠传输.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); // 持久化auto best_effort_qos = rclcpp::Qos(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);...}
}

C++话题编程示例

创建话题通讯功能包

# 创建功能包
cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake topic_example \--dependencies rclcpp std_msgs

命令解析：

• ros2 pkg create
  • ros2：ROS2命令行工具
• pkg：包管理子命令
• create： 创建新包的操作
• –build-type ament_cmake
  • –build-type：指定构建系统类型
• ament_cmake：使用CMake构建系统，这是C++包的标准构建方式
• 其他选项还有：ament_python
• topic_example
  • 这是想要创建的包名
• 包名必须符合ROS2的命名约定
• –dependencies rclcpp std_msgs
• –dependencies：指定包的依赖
• rclcpp：ROS2的C++客户端库，C++节点开发的核心库
• std_msgs：标准消息类型库，包含基本的消息类型如String，Int32等

简单的发布者示例

源文件
src/topic_example/src/simple_publisher.cpp

#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>using namespace std::chrono_literals;class SimplePublisher : public rclcpp::Node {public:SimplePublisher() : Node("simple_publisher"), count_(0) {publisher_ = this->create_publisher<std_msgs::msg::String>("hello_topic", 10);timer_ = this->create_wall_timer(2s, std::bind(&SimplePublisher::timer_callback, this));RCLCPP_INFO(this->get_logger(), "simple_publisher node setup success");}private:void timer_callback() {auto message = std_msgs::msg::String();message.data = "Hello ROS2! message id:" + std::to_string(count_);RCLCPP_INFO(this->get_logger(), "publish topic: '%s'", message.data.c_str());publisher_->publish(message);count_++;}rclcpp::TimerBase::SharedPtr timer_;rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;uint32_t count_;
};int main(int argc, char* argv[]) {rclcpp::init(argc, argv);rclcpp::spin(std::make_shared<SimplePublisher>());rclcpp::shutdown();return 0;
}

简单的订阅者示例

源文件
src/topic_example/src/simple_subscriber.cpp

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <chrono>class SimpleSubscriber : public rclcpp::Node {public:SimpleSubscriber() : Node("simple_subscriber") {subscription_ = this->create_subscription<std_msgs::msg::String>("hello_topic", 10,std::bind(&SimpleSubscriber::topic_callback, this, std::placeholders::_1));RCLCPP_INFO(this->get_logger(), "simple_subscriber setup success");}private:void topic_callback(const std_msgs::msg::String::SharedPtr msg) const {RCLCPP_INFO(this->get_logger(), "recv message: '%s'", msg->data.c_str());}rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
};int main(int argc, char* argv[]) {rclcpp::init(argc, argv);rclcpp::spin(std::make_shared<SimpleSubscriber>());rclcpp::shutdown();return 0;
}

稍微复杂的发布者示例

源文件
src/topic_example/src/number_publisher

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/int32.hpp>
#include <chrono>
using namespace std::chrono_literals;class NumberPublisher : public rclcpp::Node {
public:NumberPublisher() : Node("number_publisher") {publisher_ = this->create_publisher<std_msgs::msg::Int32>("numbers", 10);timer_ = this->create_wall_timer(1s, std::bind(&NumberPublisher::publisher_number_callback, this));RCLCPP_INFO(this->get_logger(), "number publisher setup success");}private:void publisher_number_callback() {auto msg = std_msgs::msg::Int32();msg.data = number_;RCLCPP_INFO(this->get_logger(), "publish number: %ld", number_);number_++;publisher_->publish(msg);}uint32_t number_;rclcpp::TimerBase::SharedPtr timer_;rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr publisher_;
};int main(int argc, char* argv[]) {rclcpp::init(argc, argv);rclcpp::spin(std::make_shared<NumberPublisher>());rclcpp::shutdown();return 0;
}

稍微复杂的订阅者示例

源文件
src/topic_example/src/number_subscriber.cpp

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/int32.hpp>class NumberSubscriber : public rclcpp::Node {public:NumberSubscriber() : Node("number_subscriber"), sum_(0) {subscription_ = this->create_subscription<std_msgs::msg::Int32>("numbers", 10,std::bind(&NumberSubscriber::number_callback, this, std::placeholders::_1));RCLCPP_INFO(this->get_logger(), "number_subscriber setup success");}private:void number_callback(const std_msgs::msg::Int32::SharedPtr msg) {sum_ += msg->data;RCLCPP_INFO(this->get_logger(), "recv num: %d, sum: %d", msg->data, sum_);}rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr subscription_;uint32_t sum_;
};int main(int argc, char* argv[]) {rclcpp::init(argc, argv);rclcpp::spin(std::make_shared<NumberSubscriber>());rclcpp::shutdown();return 0;
}

编译配置

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(topic_example)if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")add_compile_options(-Wall -Wextra -Wpedantic)
endif()# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)if(BUILD_TESTING)find_package(ament_lint_auto REQUIRED)# the following line skips the linter which checks for copyrights# comment the line when a copyright and license is added to all source filesset(ament_cmake_copyright_FOUND TRUE)# the following line skips cpplint (only works in a git repo)# comment the line when this package is in a git repo and when# a copyright and license is added to all source filesset(ament_cmake_cpplint_FOUND TRUE)ament_lint_auto_find_test_dependencies()
endif()add_executable(simple_publisher src/simple_publisher.cpp)
add_executable(simple_subscriber src/simple_subscriber)
add_executable(number_publisher src/number_publisher)
add_executable(number_subscriber src/number_subscriber)
ament_target_dependencies(simple_publisher rclcpp std_msgs)
ament_target_dependencies(simple_subscriber rclcpp std_msgs)
ament_target_dependencies(number_publisher rclcpp std_msgs)
ament_target_dependencies(number_subscriber rclcpp std_msgs)install(TARGETSsimple_publishersimple_subscribernumber_publishernumber_subscriberDESTINATION lib/${PROJECT_NAME}
)ament_package()

编译运行

程序运行后，可以使用`watch -n 1 "ros2 node list"实时查看话题

cd ~/ros2_ws
colcon build --packages-select topic_examplesource install/setup.bashros2 run topic_example simple_publisherros2 run topic_example simple_subscriber

命令行查看话题操作示例

# 查看所有话题
ros2 topic list# 查看话题详细信息
ros2 topic info /hello_topic# 查看话题消息类型
ros2 topic type /hello_topic# 实时监听话题消息
ros2 topic echo /hello_topic# 查看话题发布频率
ros2 topic hz /hello_topic# 查看话题带宽使用
ros2 topic bw /hello_topic

命令行发布消息

# 发布字符串消息
ros2 topic pub /hello_topic std_msgs/msg/String '{data: "命令行发布的消息"}'# 持续发布消息
ros2 topic pub /hello_topic std_msgs/msg/Stirng '{data: "持续消息"}' --rate 0.5# 发布数值消息
ros2 topic pub /numbers std_msgs/msg/Int32 '{data: 42}'# 发布一次后退出
ros2 topic pub --once /hello_topic std_msgs/msg/String '{data: "单次消息"}'

多节点通信测试

# 同时运行多个发布者和订阅者
# 终端1：发布者1
ros2 run topic_example simple_publisher
# 终端2：发布者2（重映射话题名）
ros2 run topic_example simple_publisher --ros-args --remap hello_topic:=hello_topic
# 终端3：订阅者1
ros2 run topic_example simple_subscriber
# 终端4：订阅者2
ros2 run topic_example simple_subscriber
# 终端5：查看话题连接
ros2 topic info /hello_topic

通信机制-服务

什么是服务

服务通信图示

服务通信模型

同步通信特点

异步通信特点

一对多通信模式

• **一个服务端：**提供特定功能的节点
• **多个客户端：**可以同时向同一个服务端发送请求
• 队列处理：服务端按顺序处理客户端请求

服务接口组成

这里的.srv 是重要技术点

服务接口文件 (.srv):
├── 请求部分 (Request)
│ ├── 输入参数1
│ ├── 输入参数2
│ └── ...
├── 分隔符 (---)
└── 响应部分 (Response)├── 输出参数1├── 输出参数2└── ...

服务通信 C++编程示例

创建服务通信功能包

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake service_example \--dependencies rclcpp std_srvs

命令解析：

• std_srvs：标准服务类型库

与 topic_example 的区别：

方面	topic_example	service_example
通信模式	发布-订阅	请求-响应
消息库	std_msgs	std_srvs
通信特点	异步、持续	同步、临时

简单的服务端示例

这里的请求响应格式：

Request:bool data
Response:bool success,string message

源文件

src/service_example/src/simple_service_client.cpp

#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <std_srvs/srv/set_bool.hpp>
#include <memory>class SimpleServiceServer : public rclcpp::Node {public:SimpleServiceServer() : Node("simple_serive_server"), robot_enabled_(false) {service_ = this->create_service<std_srvs::srv::SetBool>("robot_control",std::bind(&SimpleServiceServer::handle_servive, this,std::placeholders::_1, std::placeholders::_2));RCLCPP_INFO(this->get_logger(), "simple_service_server setup success");}private:void handle_servive(const std::shared_ptr<std_srvs::srv::SetBool::Request> request,std::shared_ptr<std_srvs::srv::SetBool::Response> response) {robot_enabled_ = request->data;response->success = true;if (robot_enabled_) {response->message = "robot setup success";RCLCPP_INFO(this->get_logger(), "get request, robot setup success");}else {response->message = "robot down success";RCLCPP_INFO(this->get_logger(), "get request, robot down success");}RCLCPP_INFO(this->get_logger(), "server response send");}rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr service_;bool robot_enabled_;
};int  main(int argc, char* argv[]) {rclcpp::init(argc, argv);rclcpp::spin(std::make_shared<SimpleServiceServer>());rclcpp::shutdown();return 0;
}

简单的客户端示例

源文件

src/service_example/src/simple_service_client.cpp

#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <std_srvs/srv/set_bool.hpp>#include <chrono>
#include <memory>using namespace std::chrono_literals;class SimpleServieClient : public rclcpp::Node {public:SimpleServieClient() : Node("simple_service_client") {client_ = create_client<std_srvs::srv::SetBool>("robot_control");RCLCPP_INFO(this->get_logger(), "simple_service_client setup success");// 等待服务变为可用状态// 这是一个阻塞循环，直到服务上线while (!client_->wait_for_service(1s)) {if (!rclcpp::ok()) {RCLCPP_ERROR(this->get_logger(), "waiting service break...");return;}RCLCPP_INFO(this->get_logger(), "waiting service online...");}send_request(true);std::this_thread::sleep_for(2s);send_request(false);}private:void send_request(bool enable) {auto request = std::make_shared<std_srvs::srv::SetBool::Request>();request->data = enable;RCLCPP_INFO(this->get_logger(), "send request: %s robot", enable ? "enable" : "disable");auto future = client_->async_send_request(request);if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) == rclcpp::FutureReturnCode::SUCCESS) {auto response = future.get();RCLCPP_INFO(this->get_logger(), "get response: success=%s, message='%s'",response->success ? "ture" : "false", response->message.c_str());}else {RCLCPP_ERROR(this->get_logger(), "request server error");}}rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr client_;
};int main(int argc, char* argv[]) {rclcpp::init(argc, argv);rclcpp::spin(std::make_shared<SimpleServieClient>());rclcpp::shutdown();return 0;
}

自定义服务 接口示例

首先创建自定义服务接口：

src/service_example/srv/AddTwoInts.srv

int64 a
int64 b
---
int64 sum
string message

自定义服务 服务端示例

src/service_example/src/add_service_server.cpp

#include <iostream>
#include <service_example/srv/add_two_ints.hpp>
#include <rclcpp/rclcpp.hpp>
#include <memory>class AddServiceServer : public rclcpp::Node {
public:AddServiceServer() : Node("add_service_server") {service_ = create_service<service_example::srv::AddTwoInts>("add_two_ints",std::bind(&AddServiceServer::add_callback, this,std::placeholders::_1, std::placeholders::_2));RCLCPP_INFO(this->get_logger(), "add service setup success");}
private:void add_callback(const std::shared_ptr<service_example::srv::AddTwoInts::Request> request,std::shared_ptr<service_example::srv::AddTwoInts::Response> response) {response->sum = request->a + request->b;response->message = "calc finished";RCLCPP_INFO(this->get_logger(), "get req: %d + %d = %d", request->a, request->b, response->sum);}rclcpp::Service<service_example::srv::AddTwoInts>::SharedPtr service_;
};int main(int argc, char* argv[]) {rclcpp::init(argc, argv);rclcpp::spin(std::make_shared<AddServiceServer>());rclcpp::shutdown();return 0;
}

自定义服务 客户端示例

src/service_example/src/add_service_client.cpp

#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <service_example/srv/add_two_ints.hpp>
#include <chrono>using namespace std::chrono_literals;class AddServiceClient : public rclcpp::Node {
public:AddServiceClient() : Node("add_service_client") {client_ = create_client<service_example::srv::AddTwoInts>("add_two_ints");while (!client_->wait_for_service(1s)){/* code */if (!rclcpp::ok()) {RCLCPP_ERROR(this->get_logger(), "wait for service break...");return;}RCLCPP_INFO(this->get_logger(), "wait for add service...");}}void send_request(int a, int b) {auto request = std::make_shared<service_example::srv::AddTwoInts::Request>();request->a = a;request->b = b;RCLCPP_INFO(this->get_logger(), "send request: %d + %d", a, b);auto future = client_->async_send_request(request);if (rclcpp::spin_until_future_complete(this->get_node_base_interface(),future) == rclcpp::FutureReturnCode::SUCCESS) {auto response = future.get();RCLCPP_INFO(this->get_logger(), "result: %ld, message: %s", response->sum, response->message.c_str());}else {RCLCPP_ERROR(this->get_logger(), "service error");}}private:rclcpp::Client<service_example::srv::AddTwoInts>::SharedPtr client_;
};int main(int argc, char* argv[]) {rclcpp::init(argc, argv);auto node = std::make_shared<AddServiceClient>();node->send_request(2, 3);node->send_request(5, -5);node->send_request(0, 10);rclcpp::shutdown();return 0;
}

编译配置

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(service_example)if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")add_compile_options(-Wall -Wextra -Wpedantic)
endif()# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_srvs REQUIRED)
# 自定义接口依赖包
find_package(rosidl_default_generators REQUIRED)if(BUILD_TESTING)find_package(ament_lint_auto REQUIRED)# the following line skips the linter which checks for copyrights# comment the line when a copyright and license is added to all source filesset(ament_cmake_copyright_FOUND TRUE)# the following line skips cpplint (only works in a git repo)# comment the line when this package is in a git repo and when# a copyright and license is added to all source filesset(ament_cmake_cpplint_FOUND TRUE)ament_lint_auto_find_test_dependencies()
endif()# 生成自定义服务接口
rosidl_generate_interfaces(${PROJECT_NAME}"srv/AddTwoInts.srv"
)add_executable(simple_service_serversrc/simple_service_server.cpp
)
add_executable(simple_service_clientsrc/simple_service_client.cpp
)
add_executable(add_service_clientsrc/add_service_client.cpp
)
add_executable(add_service_serversrc/add_service_server.cpp
)ament_target_dependencies(simple_service_server rclcpp std_srvs)
ament_target_dependencies(simple_service_client rclcpp std_srvs)
ament_target_dependencies(add_service_server rclcpp)
ament_target_dependencies(add_service_client rclcpp)# 链接自定义接口
rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
target_link_libraries(add_service_server "${cpp_typesupport_target}")
target_link_libraries(add_service_client "${cpp_typesupport_target}")
# rosidl_target_interfaces(add_service_server ${PROJECT_NAME} "rosidl_typesupport_cpp")
# rosidl_target_interfaces(add_service_client ${PROJECT_NAME} "rosidl_typesupport_cpp")install(TARGETSsimple_service_serversimple_service_clientadd_service_serveradd_service_clientDESTINATION lib/${PROJECT_NAME}
)# 导出依赖
ament_export_dependencies(rosidl_default_runtime)ament_package()

package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3"><name>service_example</name><version>0.0.0</version><description>TODO: Package description</description><maintainer email="[e-mail]">lenn</maintainer><license>TODO: License declaration</license><buildtool_depend>ament_cmake</buildtool_depend><depend>rclcpp</depend><depend>std_srvs</depend><!-- 自定义接口依赖 --><build_depend>rosidl_default_generators</build_depend><exec_depend>rosidl_default_runtime</exec_depend><member_of_group>rosidl_interface_packages</member_of_group><test_depend>ament_lint_auto</test_depend><test_depend>ament_lint_common</test_depend><export><build_type>ament_cmake</build_type></export></package>

编译运行

cd ~/ros2_ws
colcon build --packages-select service_examplesource install/setup.bashros2 run service_example simple_servie_serverros2 run service_example simple_service_clientros2 run service_example add_service_serverros2 run service_example add_service_client

运行结果

客户端

服务端

服务命令行操作

查看服务信息

# 查看所有可用服务
ros2 service list# 查看服务类型
ros2 service type /simple_service_server# 查看服务接口定义
ros2 interface show std_srvs/srv/SetBool# 查看自定义服务接口
ros2 interface show service_example/srv/AddTwoInts

调用服务

# 调用简单服务 启动机器人
ros2 service call /robot_control std_srvs/srv/SetBool '{data:true}'# 调用简单服务 停止机器人
ros2 service call /robot_control std_srvs/srv/SetBool '{data:false}'# 调用加法服务
ros2 service call /add_two_ints service_example/srv/AddTwoInts '{a: 5, b: 3}'

服务调试和监控

# 查找提供特定服务的节点
ros2 service find std_srvs/srv/SetBool# 实时监控服务调用
ros2 service echo /simple_service_server# 查看服务统计信息
ros2 service info /simple_service_server

话题和服务对比

发布-订阅（Topic）

• 通信模式： 发布者-订阅者模式
• 数据流向： 单向数据流
• 通信方式： 异步
• 连接关系： 多对多

服务（Service）

• 通信模式： 请求-响应模式
• 数据流向： 双向数据交换
• 通信方式： 同步通信
• 连接关系： 多对一（多个客户端，一个服务端）

特别注意：这里的异步是：发送端只管发送数据；这里讲的同步是：发送请求后需要处理响应（这个响应可以是同步也可以是异步）

通信模式图示对比

话题 Topic 通信模式

Topic 通信核心特征：

• 一对多：一个发布者，多个订阅者
• 异步：发布者不等订阅者处理
• 解耦：发布者不需要知道订阅者的存在
• 广播：所有订阅者都能接收到相同的数据

服务 Service 通信模式

Service 通信的核心特征：

• 客户端发送请求后等待响应
• 服务端处理完成返回结果
• 一对一的请求-响应关系
• 同步处理，意思是需要返回结果

选择适合的通信方式是 ROS2 系统设计的关键，应该根据数据特性、频率要求和应用场景来决定。

通信机制-动作

定义和概念

• 动作： ROS2中用于管理长时间运行任务的通信机制
• 特点： 支持目标设置、实时反馈、结果返回和任务取消
• 应用场景： 导航、抓取、充电等需要长时间完成的任务

ROS2动作概念图

动作vs其他通信方式对比

动作通信时序图

C++动作通信接口示例

多动作通信机制数据交互示意图

flowchart LR
subgraph 数据流示意图
A[目标：厨房位置<br/>x=5.0,y=3.0]
B[反馈：距离剩余2.1米<br/>预计时间30s]
C[结果：导航成功<br/>最终误差0.05m]
end

flowchart LR
subgraph 多动作通信系统架构
A[导航服务<br/>Action Server] --> |反馈：当前位置|B[导航控制器<br/>Action Client]
B --> |NavigateToGoal|AC[机械臂控制器<br/>Action Client] --> |PickAndPlace|D[抓取服务<br/>Action Server]
D --> |反馈：抓取进度|CE[任务调度器<br/>Action Client] --> |GoToCharging|F[充电服务<br/>Action Server]
F --> |反馈：充电状态|E
end

动作接口定义的方法

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake action_interfaces \--dependencies rosidl_default_generators rosidl_default_runtime \geometry_msgs builtin_interfaces

参数说明：

• 包名：action_interfaces
  • 约定命名：以_interfaces 结尾，表明这是接口定义包
  • 用途：专门用于定义动作接口
  • 动作：ROS2中用于长时间运行任务的通信模式
• 核心依赖：接口生成工具
  • rosidl_default_generators
• 作用：编译时代码生成器
• 功能：将. action 文件转化为 C++/Python 代码
• 类型：build_depend（构建依赖）
• rosidl_default_runtime
• 作用：运行时支持库
• 功能：提供生成接口的运行时基础设施
• 类型：exec_depend（执行依赖）
• 消息类型依赖：
• geometry_msgs
  • 作用：集合相关的标准消息类型包含：
• Point ,Pose, Twist（位置、姿态、速度）
• Vector3, Quaternion（向量、四元数）
• 用途：机器人导航、定位、运动控制
• builtin_interfaces
• 作用：ROS2内置的基础接口类型，包含：
• Time, Duration （时间相关）
• 基础数据类型支持
• 用途：时间戳、持续时间等

基础动作接口定义

使用 — 区分 目标定义 和 结果定义

action/NavigateToGoal.action

# ============================================================================
# 目标定义 (Goal) - 客户端发送给服务器的任务参数
# ============================================================================# 目标位置和姿态
# 包含坐标系信息、3D位置坐标和四元数姿态
geometry_msgs/PoseStamped target_pose# 最大移动速度限制（单位：米/秒）
# 用于控制机器人的移动速度，确保安全性
float64 max_speed# 是否使用路径规划算法
# true：使用高级路径规划避障算法
# false：使用简单的直线导航
bool use_path_planning# 路径规划算法名称
# 可选值：A_star RRT Dijkstra direct
# 当use_path_planning为true时生效
string planning_algorithm---# ============================================================================
# 结果定义 (Result) - 服务器返回给客户端的最终结果
# ============================================================================# 导航任务是否成功完成
# true：成功到达目标位置
# false：任务失败或被取消
bool success# 详细的结果描述消息
# 例如：导航成功完成 目标不可达 导航被用户取消
string result_message# 最终到目标位置的距离误差（单位：米）
# 用于评估导航精度，越小越精确
builtin_interfaces/Duration total_time# 机器人的最终位置和姿态
# 任务结束时机器人的实际位置
geometry_msgs/PoseStamped final_pose---# ============================================================================
# 反馈定义 (Feedback) - 服务器定期发送给客户端的进度信息
# ============================================================================# 机器人当前位置和姿态
# 实时更新的机器人位置信息
geometry_msgs/PoseStamped current_pose# 到目标位置的剩余距离（单位：米）
# 动态计算的直线距离，用于显示进度
float64 distance_remaining# 机器人当前移动速度（单位：米/秒）
# 实时监测的实际移动速度
float64 current_speed# 任务完成百分比（0-100）
# 基于已完成距离与总距离的比值计算
float64 completion_percentage# 当前状态描述消息
# 例如： 导航中... 避障中... 重新规划路径...
string status_message

action/ChargeBattery.action

# ============================================================================
# 目标定义 (Goal) - 客户端发送给服务器的任务参数
# ============================================================================# 目标位置和姿态
# 包含坐标系信息、3D位置坐标和四元数姿态
geometry_msgs/PoseStamped target_pose# 最大移动速度限制（单位：米/秒）
# 用于控制机器人的移动速度，确保安全性
float64 max_speed# 是否使用路径规划算法
# true：使用高级路径规划避障算法
# false：使用简单的直线导航
bool use_path_planning# 路径规划算法名称
# 可选值：A_star RRT Dijkstra direct
# 当use_path_planning为true时生效
string planning_algorithm---# ============================================================================
# 结果定义 (Result) - 服务器返回给客户端的最终结果
# ============================================================================# 导航任务是否成功完成
# true：成功到达目标位置
# false：任务失败或被取消
bool success# 详细的结果描述消息
# 例如：导航成功完成 目标不可达 导航被用户取消
string result_message# 最终到目标位置的距离误差（单位：米）
# 用于评估导航精度，越小越精确
builtin_interfaces/Duration total_time# 机器人的最终位置和姿态
# 任务结束时机器人的实际位置
geometry_msgs/PoseStamped final_pose---# ============================================================================
# 反馈定义 (Feedback) - 服务器定期发送给客户端的进度信息
# ============================================================================# 机器人当前位置和姿态
# 实时更新的机器人位置信息
geometry_msgs/PoseStamped current_pose# 到目标位置的剩余距离（单位：米）
# 动态计算的直线距离，用于显示进度
float64 distance_remaining# 机器人当前移动速度（单位：米/秒）
# 实时监测的实际移动速度
float64 current_speed# 任务完成百分比（0-100）
# 基于已完成距离与总距离的比值计算
float64 completion_percentage# 当前状态描述消息
# 例如： 导航中... 避障中... 重新规划路径...
string status_message

action/PickAndPlace.action

# @file action_interfaces/action/PickAndPlace.action
# @brief ROS2机械臂抓取放置动作接口定义
# @description 定义了机械臂执行抓取和放置操作的完整动作接口，包括目标参数、执行结果和实时反馈# ============================================================================
# 目标定义 (Goal) - 客户端发送给服务器的抓取任务参数
# ============================================================================# 抓取位置和姿态
# 包含目标物体的空间位置坐标和抓取姿态（四元数表示）
geometry_msgs/PoseStamped pick_pose# 放置位置和姿态
# 机械臂完成抓取后将物体放置到的目标位置和姿态
geometry_msgs/PoseStamped place_pose# 目标物体标识符
# 用于识别和跟踪特定物体的唯一ID字符串
string object_id# 夹爪抓取力度 (单位: 牛顿)
# 控制夹爪的抓取力度，避免损坏脆弱物体或抓取力不足
float64 gripper_force# 接近距离 (单位: 米)
# 机械臂在实际抓取前的预接近距离，用于精确定位
float64 approach_distance# 是否使用视觉引导
# true: 启用视觉系统进行实时位置修正
# false: 仅使用预设的位置坐标
bool use_vision_guidance---# ============================================================================
# 结果定义 (Result) - 服务器返回给客户端的最终执行结果
# ============================================================================# 抓取放置任务是否成功完成
# true: 完整完成抓取和放置操作
# false: 任务失败或被中断
bool success# 详细的结果描述消息
# 例如: "抓取放置成功完成", "物体检测失败", "抓取力度不足"
string result_message# 失败原因详细说明
# 当success为false时，提供具体的失败原因分析
string failure_reason# 实际使用的夹爪力度 (单位: 牛顿)
# 机械臂实际执行时的夹爪力度反馈值
float64 actual_gripper_force# 实际抓取位置和姿态
# 机械臂实际执行抓取时的末端执行器位置（可能与目标位置有偏差）
geometry_msgs/PoseStamped actual_pick_pose# 实际放置位置和姿态
# 机械臂实际放置物体时的末端执行器位置
geometry_msgs/PoseStamped actual_place_pose# 整个任务的执行时间
# 从开始接近物体到完成放置的总耗时
builtin_interfaces/Duration execution_time---# ============================================================================
# 反馈定义 (Feedback) - 服务器定期发送给客户端的执行进度信息
# ============================================================================# 当前执行阶段标识符
# 可能的值: "approaching"(接近中), "grasping"(抓取中), "lifting"(提升中), 
#          "moving"(移动中), "placing"(放置中)
string current_phase# 当前阶段完成百分比 (0-100)
# 表示当前执行阶段的完成程度
float64 phase_completion# 机械臂末端执行器当前位置和姿态
# 实时更新的机械臂末端坐标和姿态信息
geometry_msgs/PoseStamped current_end_effector_pose# 夹爪当前开合位置 (单位: 米或度)
# 夹爪的实时开合状态，用于监控抓取过程
float64 current_gripper_position# 是否检测到目标物体
# true: 传感器成功检测到目标物体
# false: 未检测到物体或物体丢失
bool object_detected# 夹爪力度反馈 (单位: 牛顿)
# 夹爪传感器实时反馈的抓取力度值
float64 grip_force_feedback# 当前状态描述消息
# 例如: "正在接近物体...", "调整抓取姿态...", "物体抓取成功..."
string status_message

编译配置

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(action_interfaces)if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")add_compile_options(-Wall -Wextra -Wpedantic)
endif()# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(rosidl_default_runtime REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(builtin_interfaces REQUIRED)
find_package(std_msgs REQUIRED)# if(BUILD_TESTING)
#   find_package(ament_lint_auto REQUIRED)
#   # the following line skips the linter which checks for copyrights
#   # comment the line when a copyright and license is added to all source files
#   set(ament_cmake_copyright_FOUND TRUE)
#   # the following line skips cpplint (only works in a git repo)
#   # comment the line when this package is in a git repo and when
#   # a copyright and license is added to all source files
#   set(ament_cmake_cpplint_FOUND TRUE)
#   ament_lint_auto_find_test_dependencies()
# endif()# 生成动作接口
rosidl_generate_interfaces(${PROJECT_NAME}"action/NavigateToGoal.action""action/PickAndPlace.action""action/ChargeBattery.action"DEPENDENCIESgeometry_msgsbuiltin_interfacesstd_msgs
)ament_export_dependencies(rosidl_default_runtime)
ament_package()

package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3"><name>action_interfaces</name><version>0.0.0</version><description>TODO: Package description</description><maintainer email="[e-mail]">lenn</maintainer><license>TODO: License declaration</license><buildtool_depend>ament_cmake</buildtool_depend><!-- 接口生成依赖 --><build_depend>rosidl_default_generators</build_depend><exec_depend>rosidl_default_runtime</exec_depend><!-- 接口依赖 不能重复添加--><!-- <depend>rosidl_default_generators</depend><depend>rosidl_default_runtime</depend> --><depend>geometry_msgs</depend><depend>builtin_interfaces</depend><member_of_group>rosidl_interface_packages</member_of_group><test_depend>ament_lint_auto</test_depend><test_depend>ament_lint_common</test_depend><export><build_type>ament_cmake</build_type></export></package>

编译和验证接口

cd ~/ros2_ws
colcon build --packages-select action_interfaces# 设置环境
source install/setup.bash# 验证接口生成
ros2 interface list | grep action_interfaces

执行后显示：

后续需要使用：action_interfaces/action/NavigateToGoal

C++编程方法（使用 NavigateToGoal）

交互逻辑

动作生命周期

创建动作演示功能包

cd ~/ros2_ws/src# 创建action_demo功能包
ros2 pkg create --build-type ament_cmake action_demo \--dependencies rclcpp rclcpp_action action_interfaces geometry_msgs tf2 tf2_geometry_msgs

命令解析：

• action_demo
  • 作用：新创建的 ROS2 包名称
  • 生成：会创建名为 action_demo 的目录
• –dependencies 依赖：
  • rclcpp_action
• 用途：ROS2 C++动作系统库
• 提供：动作客户端和服务器的实现
• 功能：处理长时间运行的任务，支持取消和进度反馈
• action_interfaces （即是我们自己创建的动作接口）
• 用途：自定义动作接口包
• 包含：项目特定的.action 文件定义
• 动作：提供 NavigateToGoal、PickAndPlace 等动作接口
• geometry_msgs
• 用途：几何消息类型标准库
• 提供：PoseStamped、Twist、Point、Quaternion 等
• 用途：处理位置、姿态、速度等几何数据
• tf2
• 用途：坐标变换库核心
• 功能：坐标系变换计算和管理
• 作用：处理机器人各部件之间的空间关系
• tf2_geometry_msgs
• 用途：TF2 与 geometry_msgs 的集成
• 功能：为 geometry_msgs 提供坐标变换功能
• 例如：将 PoseStamped 在不同坐标系间转换

动作服务端编程方法

action_demo/src/navigation_action_server.cpp

#include <iostream>
#include <rclcpp/rclcpp.hpp>
// 包含ROS2动作系统的头文件
#include <rclcpp_action/rclcpp_action.hpp>
// 包含自定义导航动作接口的头文件
#include <action_interfaces/action/navigate_to_goal.hpp>
// 包含集合消息类型，用于位置和姿态的表示
#include <geometry_msgs/msg/pose_stamped.hpp>
// 包含TF2四元数类，用于姿态计算
#include <tf2/LinearMath/Quaternion.hpp>
// 包含TF2集合消息转换工具
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
// 包含数学计算函数
#include <cmath>
// 多线程支持
#include <thread>using NavigateToGoal = action_interfaces::action::NavigateToGoal;
using GoalHandleNavigate = rclcpp_action::ServerGoalHandle<NavigateToGoal>;class NavigationActionServer : public rclcpp::Node {
public:NavigationActionServer() : Node("navigation_action_server") {// 创建动作服务器对象// 模版参数指定动作类型NavigateToGoal// navigate_to_goal是动作名称，客户端通过此名称调用动作// 绑定三个回调函数：目标处理、取消处理、接受处理action_server_ = rclcpp_action::create_server<NavigateToGoal>(this,"navigate_to_goal",std::bind(&NavigationActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2),std::bind(&NavigationActionServer::handle_cancel, this, std::placeholders::_1),std::bind(&NavigationActionServer::handle_accept, this, std::placeholders::_1));// 设置坐标系为map，这是ROS导航中的标准全局坐标系current_pose_.header.frame_id = "map";current_pose_.pose.position.x = 0.0;current_pose_.pose.position.y = 0.0;current_pose_.pose.position.z = 0.0;current_pose_.pose.orientation.w = 1.0; // 四元数w分量，表示无旋转RCLCPP_INFO(get_logger(), "navigation service setup");}private:// 动作服务器对象的智能指针，管理导航动作的生命周期rclcpp_action::Server<NavigateToGoal>::SharedPtr action_server_;// 当前机器人位置geometry_msgs::msg::PoseStamped current_pose_;// 是否接收或拒绝该目标rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, // 目标唯一标识符std::shared_ptr<const NavigateToGoal::Goal> goal // 客户端发送的导航目标 包含目标位置和最大速度) {// 避免未使用变量警告(void)uuid;RCLCPP_INFO(get_logger(),"get navigation target:(%.2f, %.2f), max_speed: %.2f m/s",goal->target_pose.pose.position.x,goal->target_pose.pose.position.y,goal->max_speed);if (goal->max_speed <= 0 || goal->max_speed > 5.0) {return rclcpp_action::GoalResponse::REJECT;}double distance = calculate_distance(current_pose_, goal->target_pose);if (distance > 100.0) { // 距离太远return rclcpp_action::GoalResponse::REJECT;}RCLCPP_INFO(get_logger(), "target accepted, begin navigation");return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;  // 触发handle_accepted的调用}// 客户端请求取消正在进行的导航任务时，此函数被调用rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleNavigate> goal_handle // 要取消的目标句柄) {(void)goal_handle;RCLCPP_INFO(get_logger(), "get cancel request");// 接受取消请求（在实际中需要更复杂的逻辑）return rclcpp_action::CancelResponse::ACCEPT;}void handle_accept(const std::shared_ptr<GoalHandleNavigate> goal_handle // 目标句柄，用于发布反馈和设置结果) {RCLCPP_INFO(get_logger(), "get accepted request");// 新线程中执行导航任务，避免阻塞主线程// 使用detach()让线程独立运行std::thread(std::bind(&NavigationActionServer::execute_navigation, this, goal_handle)).detach();}void execute_navigation(const std::shared_ptr<GoalHandleNavigate> goal_handle) {RCLCPP_INFO(get_logger(), "navigation begin");const auto goal = goal_handle->get_goal();auto feedback = std::make_shared<NavigateToGoal::Feedback>();auto result = std::make_shared<NavigateToGoal::Result>();// 记录任务开始时间auto start_time = this->now();// 计算到目标的总距离double total_distance = calculate_distance(current_pose_, goal->target_pose);const double update_rate = 10.0;const double time_step = 1.0 / update_rate; // 时间步长rclcpp::Rate rate(update_rate);// 主要导航执行循环while (rclcpp::ok()) {// 是否收到取消请求if (goal_handle->is_canceling()) {result->success = false;result->result_message = "navigation canceled";result->final_distance_to_goal = calculate_distance(current_pose_, goal->target_pose);result->total_time = this->now() - start_time;result->final_pose = current_pose_;// 标记目标取消goal_handle->canceled(result);RCLCPP_INFO(this->get_logger(), "navigation canceled");return;}// 模拟机器人向目标移动move_towards_goal(goal->target_pose, goal->max_speed, time_step);double remaining_distance = calculate_distance(current_pose_, goal->target_pose);feedback->current_pose = current_pose_;feedback->distance_remaining = remaining_distance;feedback->completion_percentage = ((total_distance - remaining_distance) / total_distance) * 100.0;feedback->current_speed = goal->max_speed;feedback->estimated_time_remaining = remaining_distance / goal->max_speed;feedback->status_message = "navigating...";goal_handle->publish_feedback(feedback);RCLCPP_DEBUG(this->get_logger(),"current position:(%.2f, %.2f), ramain:%.2f, finish:%.1f%%",current_pose_.pose.position.x,current_pose_.pose.position.y,remaining_distance,feedback->completion_percentage);if (remaining_distance < 0.1) {result->success = true;result->result_message = "navigation success";result->final_distance_to_goal = remaining_distance;result->total_time = this->now() - start_time;result->final_pose = current_pose_;goal_handle->succeed(result);RCLCPP_INFO(get_logger(), "navigation success, 误差:%.3f, total time:%.2f", remaining_distance,result->total_time.sec + result->total_time.nanosec * 1e-9);return;}// 等待下一个控制周期rate.sleep();}}double calculate_distance(const geometry_msgs::msg::PoseStamped& pose1,const geometry_msgs::msg::PoseStamped& pose2) {double dx = pose2.pose.position.x - pose1.pose.position.x;double dy = pose2.pose.position.y - pose1.pose.position.y;return std::sqrt(dx * dx + dy * dy);}void move_towards_goal(const geometry_msgs::msg::PoseStamped& target,double max_speed,double time_step) {double dx = target.pose.position.x - current_pose_.pose.position.x;double dy = target.pose.position.y - current_pose_.pose.position.y;double distance = std::sqrt(dx * dx * dy * dy);if (distance > 0) {double move_distance = std::min(max_speed * time_step, distance);double ratio = move_distance / distance;// 更新位置current_pose_.pose.position.x += dx * ratio;current_pose_.pose.position.y += dy * ratio;current_pose_.header.stamp = this->now();// 更新朝向double yaw = std::atan2(dy, dx); // 计算偏航角tf2::Quaternion q;q.setRPY(0, 0, yaw); // 设置Roll-Pitch-Yaw角度current_pose_.pose.orientation = tf2::toMsg(q); // 转化为消息格式}}
};int main(int argc, char* argv[]) {rclcpp::init(argc, argv);auto  node = std::make_shared<NavigationActionServer>();rclcpp::spin(std::make_shared<NavigationActionServer>());rclcpp::spin(node);rclcpp::shutdown();return 0;
}

动作客户端编程方法

action_demo/src/navigation_action_client.cpp

#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <action_interfaces/action/navigate_to_goal.hpp>
#include <geometry_msgs/msg/pose_stamped.h>
#include <chrono>using namespace std::chrono_literals;
using NavigateToGoal = action_interfaces::action::NavigateToGoal;
using GoalHandleNavigate = rclcpp_action::ClientGoalHandle<NavigateToGoal>;class NavigationActionClient : public rclcpp::Node {public:NavigationActionClient() : Node("navigation_action_client") {action_client_ = rclcpp_action::create_client<NavigateToGoal>(this, "navigate_to_goal");// 等待动作服务可用，超时时间10sif  (!action_client_->wait_for_action_server(10s)) {RCLCPP_ERROR(get_logger(), "action serviec unline");return;}RCLCPP_INFO(get_logger(), "navigation client setup");// 开始测试导航功能send_navigation_goals();}private:rclcpp_action::Client<NavigateToGoal>::SharedPtr action_client_;// 发送多个导航目标// 定义一系列导航点，按顺序发给导航服务器，m模拟机器人按预定路径移动void send_navigation_goals() {std::vector<std::pair<double, double>> waypoints = {{3.0, 2.0},{5.0, 5.0},{1.0, 4.0},{0.0, 0.0}};for (const auto& waypoint : waypoints) {send_goal(waypoint.first, waypoint.second, 1.5);std::this_thread::sleep_for(2s);}}void send_goal(double x, double y, double max_speed) {auto goal_msg = NavigateToGoal::Goal();goal_msg.target_pose.header.stamp = this->now(); // 时间戳设置为当前时间goal_msg.target_pose.header.frame_id = "map"; // 设置坐标系为地图坐标系goal_msg.target_pose.pose.position.x = x;goal_msg.target_pose.pose.position.y = y;goal_msg.target_pose.pose.position.z = 0.0;goal_msg.target_pose.pose.orientation.w = 1.0;goal_msg.max_speed = max_speed;goal_msg.use_path_planning = true;goal_msg.planning_algorithm = "A_star"; // A*算法RCLCPP_INFO(get_logger(), "send navigation target:(%.2f, %.2f), max_speed:%.2f m/s", x, y, max_speed);auto send_goal_options = rclcpp_action::Client<NavigateToGoal>::SendGoalOptions();send_goal_options.goal_response_callback = std::bind(&NavigationActionClient::goal_response_callback, this, std::placeholders::_1);send_goal_options.feedback_callback = std::bind(&NavigationActionClient::feedback_callback, this,std::placeholders::_1, std::placeholders::_2);send_goal_options.result_callback = std::bind(&NavigationActionClient::result_callback, this,std::placeholders::_1);auto goal_handle_future = action_client_->async_send_goal(goal_msg, send_goal_options);}// 当服务器响应目标请求时调用，判断目标是否接受void goal_response_callback(const GoalHandleNavigate::SharedPtr& goal_handle) {if (!goal_handle) {RCLCPP_ERROR(get_logger(), "target refused");}else {RCLCPP_INFO(get_logger(), "target accepted, begin...");}}void feedback_callback(GoalHandleNavigate::SharedPtr,const std::shared_ptr<const NavigateToGoal::Feedback> feedback) {RCLCPP_INFO(get_logger(), "navigation resp: position(%.2f, %.2f), remaining:%.2f,finished:%.2f, state:%s",feedback->current_pose.pose.position.x,feedback->current_pose.pose.position.y,feedback->distance_remaining,feedback->completion_percentage,feedback->status_message.c_str());}void result_callback(const GoalHandleNavigate::WrappedResult& result) {switch (result.code){case rclcpp_action::ResultCode::SUCCEEDED:RCLCPP_INFO(get_logger(), "navigation success:%s, distance:%.3f, total time:%.2f",result.result->result_message.c_str(),result.result->final_distance_to_goal,result.result->total_time.sec + result.result->total_time.nanosec * 1e-9);break;case rclcpp_action::ResultCode::ABORTED:RCLCPP_ERROR(get_logger(), "navigation stoped:%s", result.result->result_message.c_str());break;case rclcpp_action::ResultCode::CANCELED:RCLCPP_WARN(get_logger(), "navigation cancel:%s", result.result->result_message.c_str());break;default:RCLCPP_ERROR(get_logger(), "unexcept code");break;}}public:// 取消当前目标的公共方法// 提供给外部调用的接口，用于取消当前正在执行的目标，这在紧急情况或用户干预时非常有用void cancel_current_goal() {RCLCPP_INFO(get_logger(), "cancel current navigation target");// 异步取消所有正在执行的目标auto cancel_future = action_client_->async_cancel_all_goals();}
};int main(int argc, char* argv[]) {rclcpp::init(argc, argv);auto node = std::make_shared<NavigationActionClient>();rclcpp::spin(node);rclcpp::shutdown();return 0;
}

编译配置

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(action_demo)if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")add_compile_options(-Wall -Wextra -Wpedantic)
endif()# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(action_interfaces REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)if(BUILD_TESTING)find_package(ament_lint_auto REQUIRED)# the following line skips the linter which checks for copyrights# comment the line when a copyright and license is added to all source filesset(ament_cmake_copyright_FOUND TRUE)# the following line skips cpplint (only works in a git repo)# comment the line when this package is in a git repo and when# a copyright and license is added to all source filesset(ament_cmake_cpplint_FOUND TRUE)ament_lint_auto_find_test_dependencies()
endif()add_executable(navigation_action_server src/navigation_action_server.cpp)
add_executable(navigation_action_client src/navigation_action_client.cpp)ament_target_dependencies(navigation_action_serverrclcpprclcpp_actionaction_interfacesgeometry_msgstf2tf2_geometry_msgs
)ament_target_dependencies(navigation_action_clientrclcpprclcpp_actionaction_interfacesgeometry_msgstf2tf2_geometry_msgs
)install(TARGETSnavigation_action_clientnavigation_action_serverDESTINATIONlib/${PROJECT_NAME}
)ament_package()

编译和运行

cd ~/ros2_ws
colcon build --packages-select action_interfaces action_demosource install/setup.bashros2 run action_demo navigation_action_serverros2 run action_demo navigation_action_client

动作的命令行操作

动作命令行操作

# 查看可用动作
ros2 action list# 查看动作接口定义
ros2 interface show acton_interfaces/action/NavigateToGoal# 发送动作目标
ros2 action send_goal /navigate_to_goal action_interfaces/action/NavigateToGoal \'{target_pose:{header: {frame_id: "map"}, pose: {position: {x: 5.0, y: 3.0}}}, max_speed: 2.0}' \--feedback# 查看动作信息
ros2 action info /navigate_to_goal# 监听动作反馈
ros2 topic echo /navigate_to_goal/_action/feedback# 监听动作状态
ros2 topic echo /navigate_to_goal/_action/status# 使用rqt图形界面
rqt_action #动作监控器

动作调试和监控

# 查看动作服务端
ros2 node info /navigation_action_server# 查看动作相关话题
ros2 topic list | grep navigate_to_goal# 查看动作相关服务
ros2 service list | grep navigate_to_goal# 实时监控动作执行
ros2 topic echo /navigate_to_goal/_action/feedback --once

通信接口

什么是通信接口

定义和概念

• 通信接口： 定义 ROS2 节点间数据传输格式的标准结构
• 作用： 确保不同节点间数据交换的一致性和兼容性
• 类型： 消息（.msg）、服务（.srv）、动作（.action）

ROS2 通信接口体系结构图

接口数据类型层次结构

通信接口的定义方法

创建接口功能包

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake my_interfaces \--dependencies rosidl_default_generators rosidl_default_runtime

消息接口（.msg）定义

基础消息定义

msg/RobotStatus.msg：

# 机器人状态消息
# 基本数据类型
bool is_enabled
int32 battery_level
float64 speed
string current_mode# 时间戳
builtin_interfaces/Time timestamp# 数组类型
float64[] joint_positions
string[5] sensor_names# 嵌套消息类型
geometry_msgs/Point position
geometry_msgs/Quaternion orientation

复杂消息定义

msg/SensorData.msg：

# 传感器数据集合
std_msgs/Header header# 传感器信息
string sensor_id
string sensor_type
bool is_active# 测量数据
float64 temperature
float64 humidity
float64 pressure# 位置信息
float64[] raw_data
uint8[] status_flags# 可选字段（使用常量）
uint8 STATUS_OK = 0
uint8 STATUS_WARNING = 1
uint8 STATUS_ERROR = 2
uint8 current_status

服务接口（.srv）定义

基础服务定义

srv/SetRobotMode.srv：

# 请求部分
string mode_name
bool enable_safety
float64 max_speed
---
# 响应部分
bool success
string message
string previous_mode
float64 actual_speed

复杂服务定义

srv/NavigationRequest.srv：

# 导航请求服务
# 请求部分
geometry_msgs/PoseStamped start_pose
geometry_msgs/PoseStamped goal_pose
string planning_algorithm
float64 max_planning_time
bool use_obstacles
string[] obstacle_types
---
# 响应部分
bool success
string result_message
nav_msgs/Path planned_path
float64 path_length
float64 estimated_time
geometry_msgs/PoseStamped[] waypoints

文件操作服务

srv/FileOperation.srv：

# 文件操作服务
# 请求服务
string operation_type # read write delete
string file_path
string file_content # 写操作
---
# 响应部分
bool success
string message
string file_content # 读操作
int64 file_size
builtin_interfaces/Time last_modified

动作接口（.action）定义

基础动作定义

action/NavigateToGoal.action

# 目标定义
geometry_msgs/PoseStamped target_pose
float64 max_speed
bool use_path_planning
---
# 结果定义
bool success
string result_message
float64 final_distance_to_goal
builtin_interfaces/Duration total_time
---
# 反馈定义
geometry_msgs/PoseStamped current_pose
float64 distance_remaining
float64 estimated_time_remaining
float64 current_speed

复杂动作定义

action/ProcessData.action

# 数据处理动作
# 目标定义
string[] input_files
string processing_type
int32 batch_size
bool save_intermediate_results
---
# 结果定义
bool success
string result_message
string[] output_files
int32 processed_items
float64 processing_time
string[] error_logs
---
# 反馈定义
int32 current_item
int32 total_items
float64 completion_percentage
string current_file
builtin_interfaces/Duration elapsed_time
string status_message

编译配置

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(my_interfaces)if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")add_compile_options(-Wall -Wextra -Wpedantic)
endif()# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(rosidl_default_runtime REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(builtin_interfaces REQUIRED)if(BUILD_TESTING)find_package(ament_lint_auto REQUIRED)# the following line skips the linter which checks for copyrights# comment the line when a copyright and license is added to all source filesset(ament_cmake_copyright_FOUND TRUE)# the following line skips cpplint (only works in a git repo)# comment the line when this package is in a git repo and when# a copyright and license is added to all source filesset(ament_cmake_cpplint_FOUND TRUE)ament_lint_auto_find_test_dependencies()
endif()rosidl_generate_interfaces(${PROJECT_NAME}"msg/RobotStatus.msg""msg/SensorData.msg""srv/FileOperation.srv""srv/NavigationRequest.srv""srv/SetRobotMode.srv""action/NavigateToGoal.action""action/ProcessData.action"DEPENDENCIESstd_msgsgeometry_msgssensor_msgsnav_msgsbuiltin_interfaces
)ament_export_dependencies(rosidl_default_runtime)ament_package()

package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3"><name>my_interfaces</name><version>0.0.0</version><description>TODO: Package description</description><maintainer email="[e-mail]">lenn</maintainer><license>TODO: License declaration</license><buildtool_depend>ament_cmake</buildtool_depend><depend>rosidl_default_generators</depend><depend>rosidl_default_runtime</depend><depend>std_msgs</depend><depend>geometry_msgs</depend><depend>sensor_msgs</depend><depend>nav_msgs</depend><depend>builtin_interfaces</depend><member_of_group>rosidl_interface_packages</member_of_group><test_depend>ament_lint_auto</test_depend><test_depend>ament_lint_common</test_depend><export><build_type>ament_cmake</build_type></export></package>

通信接口命令行操作

编译和验证接口

cd ~/ros2_ws
colcon build --packages-select my_interfacessource install/setup.bashros2 interface list | grep my_interfaces

查看接口信息

# 查看所有接口
ros2 interface list
# 查看特定包的接口
ros2 interface package my_interfaces
# 查看消息接口详情
ros2 interface show my_interfaces/msg/RobotStatus
ros2 interface show my_interfaces/msg/SensorData
# 查看服务接口详情
ros2 interface show my_interfaces/srv/SetRobotMode
ros2 interface show my_interfaces/srv/NavigationRequest
# 查看动作接口详情
ros2 interface show my_interfaces/action/NavigateToGoal
ros2 interface show my_interfaces/action/ProcessData

接口类型查询

# 查看标准消息类型
ros2 interface show std_msgs/msg/String
ros2 interface show geometry_msgs/msg/Twist
ros2 interface show sensor_msgs/msg/LaserScan
# 查看标准服务类型
ros2 interface show std_srvs/srv/SetBool
ros2 interface show std_srvs/srv/Trigger
# 搜索包含特定关键字的接口
ros2 interface list | grep -i navigation
ros2 interface list | grep -i sensor

使用接口进行通信测试

source install/setup.bash# 使用自定义消息发布话题
ros2 topic pub /robot_status my_interfaces/msg/RobotStatus \'{is_enabled: true, battery_level: 85, speed: 1.5, current_mode: "navigation"}'# 调用自定义服务
ros2 service call /set_robot_mode my_interfaces/srv/SetRobotMode \'{mode_name: "autonomous", enable_safety: true, max_speed: 2.0}'# 使用rqt图形界面
rqt_msg
rqt_srv

C++服务接口应用示例

创建使用自定义接口的功能包

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake interface_demo \--dependencies rclcpp my_interfaces

服务端实现实例

src/robot_mode_server.cpp

// #include <rclcpp/rclcpp.hpp>
// #include <my_interfaces/srv/set_robot_mode.hpp>
// #include <memory>
// #include <string>
#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <my_interfaces/srv/set_robot_mode.hpp>class RobotModeServer : public rclcpp::Node {
public:RobotModeServer() : Node("robot_mode_server"), current_mode_("manual"), max_speed_(1.0){service_ = create_service<my_interfaces::srv::SetRobotMode>("set_robot_mode",std::bind(&RobotModeServer::handle_set_mode, this, std::placeholders::_1, std::placeholders::_2));RCLCPP_INFO(get_logger(), "robot mode server start");RCLCPP_INFO(get_logger(), "mode: %s, max_speed: %.2f m/s", current_mode_.c_str(), max_speed_);}private:void handle_set_mode(const std::shared_ptr<my_interfaces::srv::SetRobotMode::Request> request,std::shared_ptr<my_interfaces::srv::SetRobotMode::Response> response) {RCLCPP_INFO(get_logger(), "get request: %s, enable_safety: %s, max_speed: %.2f",request->mode_name.c_str(),request->enable_safety ? "true" : "false",request->max_speed);response->previous_mode = current_mode_;// 验证请求参数if (request->mode_name.empty()) {response->success = false;response->message = "模式名称不能为空";response->actual_speed = max_speed_;return;}if (request->max_speed <= 0 || request->max_speed > 5.0) {response->success = false;response->message = "无效的最大速度，应在 0-5.0 m/s 范围内";response->actual_speed = max_speed_;return;}std::vector<std::string> supported_modes = {"manual", "autonomous", "navigation", "teleoperation"};if (std::find(supported_modes.begin(), supported_modes.end(), request->mode_name) == supported_modes.end()) {response->success = false;response->message = "unexcepted mode:" + request->mode_name;response->actual_speed = max_speed_;return;}current_mode_ = request->mode_name;if (request->enable_safety) {max_speed_ = std::min(request->max_speed, 2.0);}else {max_speed_ = request->max_speed;}response->success = true;response->message = "set mode success:" + current_mode_;response->actual_speed = max_speed_;RCLCPP_INFO(get_logger(),"mode changed: %s -> %s, max_speed: %.2f m/s",response->previous_mode.c_str(),current_mode_.c_str(),max_speed_);}rclcpp::Service<my_interfaces::srv::SetRobotMode>::SharedPtr service_;std::string current_mode_;double max_speed_;
};int main(int argc, char* argv[]) {rclcpp::init(argc, argv);rclcpp::spin(std::make_shared<RobotModeServer>());rclcpp::shutdown();return 0;
}

客户端实现实例

src/robot_mode_client.cpp

#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <my_interfaces/srv/set_robot_mode.hpp>
#include <memory>
#include <chrono>using namespace std::chrono_literals;class RobotModeClient : public rclcpp::Node {public:RobotModeClient() : Node("robot_mode_client") {client_ = create_client<my_interfaces::srv::SetRobotMode>("set_robot_mode");while(!client_->wait_for_service(1s)) {if (!rclcpp::ok()) {RCLCPP_ERROR(get_logger(), "service break...");return;}RCLCPP_INFO(get_logger(), "wait for set_robot_mode server...");}RCLCPP_INFO(get_logger(), "robot_mode_client start");send_mode_settings();}private:void send_mode_settings() {// std::vector<std::string> supported_modes = {//     "manual", "autonomous", "navigation", "teleoperation"// };set_robot_mode("autonomous", true, 1.5);std::this_thread::sleep_for(2s);set_robot_mode("manual", false, 3.0);std::this_thread::sleep_for(2s);set_robot_mode("navigation", true, 4.0);// 测试4: 无效模式测试set_robot_mode("invalid_mode", true, 2.0);std::this_thread::sleep_for(2s);// 测试5: 无效速度测试set_robot_mode("manual", true, 10.0);}void set_robot_mode(const std::string& mode_name, bool enable_safety, double max_speed) {auto request = std::make_shared<my_interfaces::srv::SetRobotMode::Request>();request->mode_name = mode_name;request->max_speed = max_speed;request->enable_safety = enable_safety;auto future = client_->async_send_request(request);if (rclcpp::spin_until_future_complete(get_node_base_interface(), future) == rclcpp::FutureReturnCode::SUCCESS) {auto response = future.get();if (response->success) {RCLCPP_INFO(get_logger(),"set mode success: %s, previous: %s, speed: %.2f",response->message.c_str(), response->previous_mode.c_str(), response->actual_speed);}else {RCLCPP_WARN(get_logger(),"set mode failed: %s, speed: %.2f",response->message.c_str(), response->actual_speed);}}else {RCLCPP_ERROR(get_logger(), "service failed");}}rclcpp::Client<my_interfaces::srv::SetRobotMode>::SharedPtr client_;
};int main(int argc, char* argv[]) {rclcpp::init(argc, argv);auto node = std::make_shared<RobotModeClient>();// rclcpp::spin(std::make_shared<RobotModeClient>());rclcpp::shutdown();return 0;
}

编译配置

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(my_interfaces)if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")add_compile_options(-Wall -Wextra -Wpedantic)
endif()# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(rosidl_default_runtime REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(builtin_interfaces REQUIRED)if(BUILD_TESTING)find_package(ament_lint_auto REQUIRED)# the following line skips the linter which checks for copyrights# comment the line when a copyright and license is added to all source filesset(ament_cmake_copyright_FOUND TRUE)# the following line skips cpplint (only works in a git repo)# comment the line when this package is in a git repo and when# a copyright and license is added to all source filesset(ament_cmake_cpplint_FOUND TRUE)ament_lint_auto_find_test_dependencies()
endif()rosidl_generate_interfaces(${PROJECT_NAME}"msg/RobotStatus.msg""msg/SensorData.msg""srv/FileOperation.srv""srv/NavigationRequest.srv""srv/SetRobotMode.srv""action/NavigateToGoal.action""action/ProcessData.action"DEPENDENCIESstd_msgsgeometry_msgssensor_msgsnav_msgsbuiltin_interfaces
)ament_export_dependencies(rosidl_default_runtime)ament_package()

参数

参数是什么

定义和概念

• 参数： ROS2 中用于配置节点行为的键值对系统
• 特点： 动态可配置、类型安全、支持运行时修改
• 作用： 作为机器人系统的全局字典，存储配置信息

ROS2 参数系统概念图

参数在机器人系统中的作用

参数通信模式

参数服务架构图

参数操作时序图

参数生命周期图

参数 C++编程方法

创建参数演示功能包

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake parameter_demo \--dependencies rclcpp rcl_interfaces # ROS2的核心接口包（包含参数相关接口）

基础参数使用示例

src/basic_parameter_node.cpp

#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <rcl_interfaces/msg/parameter_descriptor.hpp> // 参数描述符类型
#include <rcl_interfaces/msg/parameter_event.hpp> // 参数事件类型
#include <string>
#include <memory>
#include <chrono>using namespace std::chrono_literals;class BasicParameterNode : public rclcpp::Node {public:BasicParameterNode() : Node("basic_parameter_node") {// 所有要使用的参数必须先声明declare_basic_parameters();// 读取参数初始化成员变量get_parameters();setup_parameter_callback();timer_ = create_wall_timer(5s,std::bind(&BasicParameterNode::timer_callback, this));}private:void declare_basic_parameters() {// 布尔参数 - 控制是否开启调试模式auto bool_desc = rcl_interfaces::msg::ParameterDescriptor();bool_desc.description = "是否启用调试模式";this->declare_parameter("debug_mode", false, bool_desc);// 整数参数 - 设置循环频率，带有范围约束auto int_desc = rcl_interfaces::msg::ParameterDescriptor();int_desc.description = "循环频率(Hz)";int_desc.integer_range.resize(1);int_desc.integer_range[0].from_value = 1;int_desc.integer_range[0].to_value = 100;declare_parameter("loop_rate", 10, int_desc);// 浮点参数 - 设置最大速度，带有范围约束auto double_desc = rcl_interfaces::msg::ParameterDescriptor();double_desc.description = "最大速度(m/s)";double_desc.floating_point_range.resize(1);double_desc.floating_point_range[0].from_value = 0.0;double_desc.floating_point_range[0].to_value = 5.0;declare_parameter("max_speed", 1.0, double_desc);// 字符串参数 - 设置机器人名称auto string_desc = rcl_interfaces::msg::ParameterDescriptor();string_desc.description = "机器人名称";declare_parameter("robot_name", std::string("robot1"), string_desc);// 数组参数 - 设置关节位置数组（6自由度机械臂）auto array_desc = rcl_interfaces::msg::ParameterDescriptor();array_desc.description = "关节位置数组";std::vector<double> default_joints = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};declare_parameter("joint_positions", default_joints, array_desc);// 字符串数组参数 - 设置传感器名称表auto string_array_desc = rcl_interfaces::msg::ParameterDescriptor();string_array_desc.description = "传感器名称表";// 默认传感器列表std::vector<std::string> default_sensors = {"camera", "lidar", "imu"};declare_parameter("sensor_names", default_sensors, string_array_desc);}void get_parameters() {debug_mode_ = get_parameter("debug_mode_").as_bool();loop_rate_ = get_parameter("loop_rate").as_int();max_speed_ = get_parameter("max_speed").as_double();robot_name_ = get_parameter("robot_name").as_string();joint_positions_ = get_parameter("joint_positions").as_double_array();sensor_names_ = get_parameter("sensor_names").as_string_array();display_parameters();}void setup_parameter_callback() {parameter_callback_handle_ = add_on_set_parameters_callback(std::bind(&BasicParameterNode::on_parameter_change, this, std::placeholders::_1));}// 验证参数是否合法，更新成员变量，记录参数变更日志，返回操作结果rcl_interfaces::msg::SetParametersResult on_parameter_change(const std::vector<rclcpp::Parameter>& parameters) {rcl_interfaces::msg::SetParametersResult result;result.successful = true; // 默认设置成功for (auto& param : parameters) {if (param.get_name() == "debug_mode") {debug_mode_ = param.as_bool();}if (param.get_name() == "loop_rate") {int new_rate = param.as_int();if (new_rate >= 1 && new_rate <= 100) {loop_rate_ = new_rate;}else {result.successful = false;}}if (param.get_name() == "max_speed") {double new_speed = param.as_double();if (new_speed >=0.0 && new_speed <= 5.0) {max_speed_ = new_speed;}else {result.successful = false;}}if (param.get_name() == "robot_name") {robot_name_ = param.as_string();}if (param.get_name() == "joint_positions") {joint_positions_ = param.as_double_array();}if (param.get_name() == "sensor_names") {sensor_names_ = param.as_string_array();}}return result;}void timer_callback() {if (debug_mode_) {display_parameters();}}void display_parameters() {RCLCPP_INFO(get_logger(), "=====current parameters=====");RCLCPP_INFO(get_logger(), "debug_mode: %s", debug_mode_ ? "enable" : "disable");RCLCPP_INFO(get_logger(), "loop_rate: %d Hz", loop_rate_);RCLCPP_INFO(get_logger(), "max_speed: %.2f m/s", max_speed_);RCLCPP_INFO(get_logger(), "robot_name: %s", robot_name_.c_str());std::string joints_str = "joint:[";for (size_t i = 0; i < joint_positions_.size(); i++) {joints_str += std::to_string(joint_positions_[i]);if (i < joint_positions_.size() - 1) {joints_str += ",";}}joints_str += "]";RCLCPP_INFO(get_logger(), "%s", joints_str.c_str());std::string sensors_str = "sensor:[";for (size_t i = 0; i < sensor_names_.size(); i++) {sensors_str += sensor_names_[i];if (i < sensor_names_.size() - 1) {sensors_str += ",";}}sensors_str += "]";RCLCPP_INFO(get_logger(), "%s", sensors_str.c_str());}// ROS2 相关对象rclcpp::TimerBase::SharedPtr timer_;// 参数回调句柄rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;bool debug_mode_;int loop_rate_;double max_speed_;std::string robot_name_;std::vector<double> joint_positions_;std::vector<std::string> sensor_names_;
};int main(int argc, char* argv[]) {rclcpp::init(argc, argv);auto node = std::make_shared<BasicParameterNode>();rclcpp::spin(node);rclcpp::shutdown();return 0;
}

高级参数管理示例

本示例演示了ROS2中参数系统的高级使用方法，包括：

• 参数组的组织和管理
• 从YAML配置文件加载参数
• 参数验证和约束检查
• 参数事件监听和响应
• 参数的保存和导出功能
• 嵌套参数结构的处理
• 复杂参数系统的架构设计

src/advanced_parameter_node.cpp

数据分发服务DDS

DDS是什么

• DDS（Data Distribution Service）： 数据分发服务，是ROS2的底层通信中间件
• 作用：提供高性能、实时、可靠的分布式数据通信
• 标准： 基于OMG DDS规范，支持发布-订阅模式

ROS2系统架构分层图

DDS在机器人系统中的通信场景

不局限以上场景。

DDS通信模式 DDS Communication Models

点对点模式 Peer-to-Peer Model

Broker模式 Broker Model

广播模式 Broadcast Model

数据中心模式 Data-Centric Model

质量服务策略 Quality of Service-QoS

QoS策略概览图

QoS策略应用场景

命令行中配置DDS的QoS

创建QoS配置文件

config/qos_profiles.yaml

qos_profile_sensor_data:history: keep_lastdepth: 5reliability: best_effortdurability: volatiledeadline:sec: 0nsec: 100000000 # 100mslifespan:sec: 1nsec: 0liveliness: automaticliveliness_lease_duration:sec: 3nsec: 0qos_profile_control_commands:history: keep_lastdepth: 1reliability: reliabledurability: volatiledeadline:sec: 0nsec: 50000000lifespan:sec: 0nsec: 500000000liveliness: automaticliveliness_lease_duration:sec: 1nsec: 0
qos_profile_system_config:history: keep_lastdepth: 1reliability: reliabledurability: transient_localdeadline:sec: 2147483647nsec: 999999999lifespan:sec: 2147483647nsec: 999999999 # 无限制liveliness: automaticliveliness_lease_duration:sec: 10nsec: 0
qos_profile_diagnostics:history: keep_lastdepth: 50reliability: reliabledurability: volatiledeadline:sec: 5nsec: 0lifespan:sec: 30nsec: 0liveliness: manual_by_topicliveliness_lease_duration:sec: 10nsec: 0

命令行QoS操作

# 查看话题的QoS设置
ros2 topic info /scan --verbose
ros2 topic info /cmd_vel --verbose# 使用特定QoS发布消息
ros2 topic pub /test_topic std_msgs/msg/String \'{data: "test message"}' \--qos-reliability reliable \--qos-durability transient_local \--qos-history keep_last \--qos-depth 10# 查看QoS兼容性
ros2 topic info /scan --verbose | grep QoS# 使用QoS配置文件启动节点
ros2 run my_package my_node --ros-args \--params-file config/qos_profiles.yaml# 监控QoS匹配状态
ros2 doctor --report | grep -i qos# 设置DDS详细日志
export RCUTILS_LOGGING_SERVERITY=DEBUG
export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
export FASTRTPS_DEFAULT_PROFILES_FILE=config/dds_profiles.xml

参考资料：https://github.com/0voice