ROS 2 自定义消息与服务实战
系列导航：
前言
ROS 2 里每个节点都靠「发消息、收消息」互相沟通，就像公司里各部门用表单传递信息。ROS 自带的 Twist、JointState 是通用便签，够简单场景用；可一旦你要传「外骨骼关节力矩」「巡检点位列表」这种专属数据，便签就装不下了——你得自己印一套定制表单。
这篇的主线其实就一句话：给机器人设计一门它自己的语言。
先别被 msg / srv / action 这三个词吓到，它们只是你这套语言里的三种「句型」：
- msg 是「单词」——节点之间单向广播的一份数据，比如「我现在在 (1.2, 3.4)」；
- srv 是「问句 + 回执」——一发一收，比如「把巡检计划设为 A，成功了没？」；
- action 是「长任务工单」——发出去后对方干几分钟，中间不断回报进度，最后给结果，比如「去把这 4 个点巡检完」。
我们会做四件事：建一个独立的接口包定义这三种句型；用 C++ 写控制节点（发布/订阅/服务/动作服务端）；用 Python 写监控节点（订阅/客户端/动作客户端）；最后用 Launch 文件一键把整套系统编排起来，并用调试工具链排查通信问题。读完你就能给任何机器人定义专属通信协议，不再被标准消息的类型限制住。
项目结构
先别急着敲命令。
建立一点直觉，能省你后面两小时。
ROS 2 的通信本质只有三种通道。
话题（Topic） 像广播，谁订阅谁就能听，没有回应这回事。
服务（Service） 像问询台，一问一答，问完就结束。
动作（Action） 像外卖订单，下单后骑手边送边回报位置，送完给评价。
而我们自定义 msg / srv / action，就是给这三种通道规定「传什么数据」。
msg 对应话题里传的数据，srv 对应服务的请求加响应，action 对应订单的目标加进度加结果。
后文所有代码，都只是在「生产数据、搬运数据、消费数据」这三件事上打转。
看懂这个，整篇就通了。
真正动手时记住一条心法：先定义「传什么」，再写「谁发谁收」，最后用 Launch 串起来。
动手前先把「房子」的格局看清楚。我们以一个「智能巡检机器人」为场景，需要传三类专属数据：去哪巡检（点位）、环境怎么样（传感器）、现在状态如何（状态报告）。为此拆成四个包：
robot_interfaces/ # 接口定义包（只放 msg/srv/action，不写节点逻辑）
├── msg/
│ ├── PatrolPoint.msg # 巡检点位
│ ├── SensorData.msg # 传感器数据（温度+气体+振动）
│ └── RobotStatus.msg # 机器人状态报告
├── srv/
│ ├── SetPatrolPlan.srv # 设置巡检计划
│ └── EmergencyStop.srv # 紧急停机
├── action/
│ └── ExecutePatrol.action # 执行巡检任务
├── CMakeLists.txt
└── package.xml
robot_control/ # 控制节点包（C++）
robot_monitor/ # 监控节点包（Python）
patrol_launcher/ # Launch 编排包为什么要把接口单独拆成一个包？因为 C++ 和 Python 节点都要 #include / import 同一套类型。接口独立成包后，两边只依赖 robot_interfaces，谁都不用等谁编译完——这是 ROS 2 项目里最常见的目录约定。
一、创建接口包
先建包、再填内容，顺序不能反：CMake 和 package.xml 都依赖你后面要定义的文件清单，所以先把骨架立起来。
1.1 创建工作空间与包
建包之前，先想清楚依赖关系。
接口包 robot_interfaces 是所有人的上游，C++ 和 Python 都依赖它。
所以编译顺序上，接口包必须先于节点包编出来。
ros2 pkg create 的 --dependencies 是在 package.xml 里预填依赖，省得之后手改。
注意 build-type 的差别：ament_cmake 用于要编译 C++ 的包，ament_python 用于纯 Python 脚本包。
接口包用 ament_cmake，因为它要靠 CMake 调用 rosidl 去生成类型。
下面这条命令一口气建出四个包。注意 robot_interfaces 的 build-type 是 ament_cmake——因为它是纯接口包，没有可执行节点，不需要 Python 解释器；而 robot_monitor 用 ament_python，因为里面跑的是 .py 脚本。
# 创建工作空间
mkdir -p ~/ros2_patrol_ws/src && cd ~/ros2_patrol_ws/src
# 创建接口定义包（纯接口，无需节点代码）
# ament_cmake：接口包只生成类型，不产生可执行文件
ros2 pkg create robot_interfaces \
--build-type ament_cmake \
--dependencies rosidl_default_generators
# 创建控制节点包（C++）
# 依赖里带上 robot_interfaces，C++ 才能找到我们自定义的类型
ros2 pkg create robot_control \
--build-type ament_cmake \
--dependencies rclcpp robot_interfaces std_msgs geometry_msgs
# 创建监控节点包（Python）
ros2 pkg create robot_monitor \
--build-type ament_python \
--dependencies rclpy robot_interfaces std_msgs
# 创建 Launch 包（Python）
ros2 pkg create patrol_launcher \
--build-type ament_python建完你会得到几个带 CMakeLists.txt / package.xml 的空包目录。下一步往 robot_interfaces 里塞具体的 .msg / .srv / .action 文件。
1.2 定义自定义消息 (msg)
msg 文件按扩展名扫描，msg / srv / action 必须各放各的文件夹。
每行一个字段，格式是「类型 名字」，井号开头是注释。
基础类型直接用：int32 / float32 / bool / string / uint8 等。
也可以引用别的 msg 作为字段类型，比如 geometry_msgs/Pose2D position。
数组用 类型[] 表示，例如 PatrolPoint[] waypoints 就是「点位列表」。
约定俗成：msg 文件用大驼峰命名，字段用小写下划线。
--- 在 msg 里没有分隔语义，常用来把「枚举常量」和「字段」隔开。
枚举常量写成 uint8 NORMAL=0，生成代码后变成 PatrolPoint::NORMAL，比魔法数字好维护。
先建好放文件的目录。msg / srv / action 必须各放各的文件夹，ROS 2 的构建系统按扩展名去扫描它们。
mkdir -p robot_interfaces/msg
mkdir -p robot_interfaces/srv
mkdir -p robot_interfaces/actionmsg/PatrolPoint.msg — 巡检点位数据：
为什么这些字段这样排：point_id 让监控端能对应回「哪个点」；Pose2D 复用导航标准坐标，下游 Nav2 不用再转换；required_* 是报警阈值，和控制逻辑解耦。
priority 用枚举而非字符串，是为了避免拼写错误——编译器会帮你拦 PatrolPoint::NORMAL。
一个「点位」要回答三件事：它是谁（编号）、在哪（坐标）、到了要判什么（阈值）。这里特意用 geometry_msgs/Pose2D 而不是自己写 x/y，是为了直接复用导航生态里现成的坐标类型，下游 Nav2 能直接吃。
# 巡检点位
std_msgs/Header header
string point_id # 点位编号 "WP-001"
geometry_msgs/Pose2D position # 目标位置
float32 required_temperature # 温度阈值报警值
float32 required_gas_level # 气体浓度阈值
uint8 priority # 优先级 0-普通 1-重要 2-紧急
---
uint8 NORMAL=0
uint8 IMPORTANT=1
uint8 URGENT=2--- 在 msg 里其实没有分隔含义（msg 没有请求/响应之分），但常用来把「常量定义」和「字段」隔开，纯属可读性习惯。下面那三行 uint8 NORMAL=0 是枚举常量，生成代码后会变成 PatrolPoint::NORMAL，比直接写魔法数字 0 好读也好维护。
msg/SensorData.msg — 多传感器融合数据：
为什么打成一个 msg：温度/湿度/气体/振动是同一时刻的环境快照，打包一次发完，订阅方不用凑四条话题。
alarm 标志位让「要不要报警」由采集端判，订阅端只管显示——职责清晰，也省带宽。
这是监控节点最关心的一份数据：机器人把温度、湿度、气体、振动打包广播出来，并自带一个 alarm 标志位，让订阅方一眼知道「这次采样要不要报警」，不用自己再算一遍阈值。
# 传感器采集数据
std_msgs/Header header
string sensor_id
float32 temperature # 温度 (°C)
float32 humidity # 湿度 (%)
float32 gas_concentration # 气体浓度 (ppm)
float32 vibration # 振动幅度 (mm/s)
bool alarm # 是否触发报警
string alarm_reason # 报警原因msg/RobotStatus.msg — 机器人状态报告：
为什么「胖」一点：位置、电量、资源占用、累计计数全是状态面板要的，放一个 msg 订阅一次全拿到。
state 配 5 个枚举，是给控制节点和监控节点一个统一的状态字典，避免两边各写一套 0/1/2。
状态报告比前两份「胖」一些，因为它要同时承载位置、电量、资源占用和累计计数。注意 state 字段配了 5 个枚举常量（空闲/巡检中/充电/故障/急停）——状态机就靠这几个值驱动，控制节点和监控节点对「现在处于哪种状态」必须有统一说法。
# 机器人状态
std_msgs/Header header
string robot_id
uint8 state # 当前状态
float32 battery_level # 电量百分比
float32 cpu_usage # CPU 使用率
float32 memory_usage # 内存使用率
geometry_msgs/Pose2D current_pose # 当前位置
string current_task # 当前任务描述
uint32 total_patrols # 累计巡检次数
---
uint8 IDLE=0
uint8 PATROLLING=1
uint8 CHARGING=2
uint8 ERROR=3
uint8 EMERGENCY_STOP=41.3 定义自定义服务 (srv)
srv 用一条 --- 把文件劈成两半：上半部分是「请求」（客户端发过去），下半部分是「响应」（服务端回过来）。
所以写 srv 时，先想「我要问什么」，再想「对方该回什么」。
请求里尽量只放必要参数，可选项（如 loop、max_speed）也放请求，别写死在代码里。
响应里一定要带一个 success 或等价状态，调用方才知道成没成。
srv 和 msg 最大的不同是它有一条 --- 把「请求」和「响应」劈成两半。上半部分是客户端发过去的，下半部分是服务端回过来的。
srv/SetPatrolPlan.srv — 设置巡检计划：
为什么 loop / max_speed 放在请求里：同一份服务既跑单次也跑循环，参数化比写死灵活得多。
响应回 plan_id，是为了让调用方后续能引用「刚设的是哪份计划」，方便日志和取消。
客户端把一串点位 + 是否循环 + 限速发过来，服务端回一个 success 和 plan_id。把「是否循环」「限速」放进请求而不是写死在代码里，是为了让同一条服务既能跑单次巡检也能跑循环巡检，复用度更高。
# 请求：巡检点位列表
PatrolPoint[] waypoints
bool loop # 是否循环巡检
float32 max_speed # 最大速度限制 (m/s)
---
# 响应
bool success
string message
uint32 plan_id # 计划 IDsrv/EmergencyStop.srv — 紧急停机：
为什么回时间戳：急停后要复盘「几点停的」，Unix 秒方便和传感器日志对齐。
reason / operator_id 让每次急停都可追溯，生产环境里这是审计底线。
急停服务的请求只带「原因 + 操作员」，响应回「停了没 + 时间戳」。时间戳用 float64 存 Unix 秒，方便监控节点事后排错时对齐日志。
# 请求
string reason # 停机原因
string operator_id # 操作员 ID
---
# 响应
bool stopped
string message
float64 stop_timestamp # 停机时间戳1.4 定义自定义动作 (action)
action 一条 --- 隔出三段：目标（任务描述，等于 srv 的请求）、结果（最终汇报，等于 srv 的响应）、反馈（过程中持续回传的进度）。
反馈是 action 独有、服务没有的；频率别太低（进度条会卡），也别太高（几赫兹足够）。
下面这个巡检动作，目标里放点位列表，结果里放统计，反馈里放实时进度。
action 是三者里最复杂的，一条 --- 隔出三段：目标（发出去的任务）、结果（最终汇报）、反馈（过程中持续回传的进度）。
action/ExecutePatrol.action — 执行巡检任务：
为什么目标和设置计划几乎一样：动作只是在「计划」之上加了执行与进度，复用同一份点位结构不重复造。
反馈里带 latest_sensor_data，是让监控面板在一个话题上同时拿到「走到哪了 + 环境如何」。
目标里放的是和 SetPatrolPlan 一样的点位列表；结果是「走完没、走了几个点、总距离、总耗时」；反馈则每到一个点就回传一次「现在第几个点、进度百分比、最新传感器数据」。正是这个反馈机制，让监控面板上能看到实时进度条。
# 目标：巡检计划
PatrolPoint[] waypoints
bool loop
float32 max_speed
---
# 结果：执行报告
bool completed
string result_message
uint32 visited_points
float64 total_distance
float64 total_time
RobotStatus final_status
---
# 反馈：实时进度
uint32 current_waypoint_index
PatrolPoint current_target
float32 progress_percent
SensorData latest_sensor_data
string status_message1.5 配置 CMakeLists.txt
三个句型怎么选？一句话记忆法：一次性广播用 msg；一问一答用 srv；要等很久且要进度用 action。
巡检里「当前状态」是持续广播→msg；「设置计划」是配置请求、要回执→srv；「执行整趟巡检」耗时几十秒、还要进度条→action。
拿不准就问：调用方需不需要「中途知道干到哪了」？需要就 action，否则看要不要回执。
rosidl_generate_interfaces 是接口包的灵魂，把 .msg/.srv/.action 翻译成 C++ 头文件和 Python 模块。
DEPENDENCIES 必须列出 msg 里引用到的外部包（std_msgs / geometry_msgs），漏写是最常见的编译坑。
接口定义好了，得告诉构建系统「去把这几个文件编译成可用的类型」。下面这段是接口包的灵魂：rosidl_generate_interfaces 把 .msg/.srv/.action 翻译成 C++ 头文件和 Python 模块。
cmake_minimum_required(VERSION 3.8)
project(robot_interfaces)
# 不编译节点代码，仅生成接口
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
# 声明所有自定义接口
# 这里每列一个文件，构建时就会生成对应的 C++/Python 类型
# DEPENDENCIES 必须列出 msg 里引用到的外部包（std_msgs / geometry_msgs）
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/PatrolPoint.msg"
"msg/SensorData.msg"
"msg/RobotStatus.msg"
"srv/SetPatrolPlan.srv"
"srv/EmergencyStop.srv"
"action/ExecutePatrol.action"
DEPENDENCIES std_msgs geometry_msgs
)
ament_export_dependencies(rosidl_default_runtime)
ament_package()漏写 DEPENDENCIES 是最常见的坑：如果你的 msg 引用了 geometry_msgs/Pose2D 却在 CMake 里没声明，编译会报「找不到类型」。记住一条规则——msg 里 # 引用了谁，CMake 的 DEPENDENCIES 就带上谁。
1.6 配置 package.xml
package.xml 是给 ROS 包管理器看的「身份证」，ros2 pkg 命令靠它识别包。
<depend> 编译运行时都需要；<buildtool_depend> 只是构建工具；<exec_depend> 仅运行时需要。
接口包必须声明自己是 rosidl_interface_packages 组成员，否则别的包 find_package 时找不到生成的类型。
maintainer 和 license 是必填项，缺了 colcon 直接报错。
package.xml 是给 ROS 包管理器看的「身份证」，构建系统和 ros2 pkg 命令都靠它识别包。
<depend> 表示编译和运行时都需要；<buildtool_depend> 只是构建工具；<exec_depend> 仅运行时需要。
接口包必须声明自己是 rosidl_interface_packages 组的成员，否则别的包 find_package(robot_interfaces) 时找不到生成出来的类型。
maintainer 和 license 是必填项，缺了 colcon 会直接报错。
package.xml 是给ROS包管理器看的「身份证」。接口包必须声明自己是 rosidl_interface_packages 组的成员，否则别的包 find_package(robot_interfaces) 时会找不到生成出来的类型。
<package format="3">
<name>robot_interfaces</name>
<version>0.1.0</version>
<description>自定义消息、服务和动作接口定义</description>
<maintainer email="dev@example.com">Spider</maintainer>
<license>MIT</license>
<!-- buildtool_depend：构建时需要的工具，接口生成靠它 -->
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<!-- depend：编译和运行时都需要的依赖 -->
<depend>std_msgs</depend>
<depend>geometry_msgs</depend>
<!-- exec_depend：运行时才需要（生成的类型库） -->
<exec_depend>rosidl_default_runtime</exec_depend>
<!-- 关键：声明自己是接口包组，别的包才能引用 -->
<member_of_group>rosidl_interface_packages</member_of_group>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>1.7 编译接口包
这一步把「文本定义」变成「代码里能用的类型」。
--packages-select 只编指定包，比全编快，开发期常用。
编译完一定要 source install/setup.bash，否则当前和后续终端都找不到新类型。
ros2 interface show 能直接打印字段，是验证「类型真的生成了」的最快手段。
这一步是把「文本定义」真正变成「代码里能用的类型」。编译完一定要 source，否则当前终端和后续所有终端都找不到新类型。
cd ~/ros2_patrol_ws
colcon build --packages-select robot_interfaces
# 加载环境变量（重要！后续所有终端都需要）
source install/setup.bash
# 验证消息是否注册成功
ros2 interface show robot_interfaces/msg/PatrolPoint
ros2 interface show robot_interfaces/srv/SetPatrolPlan
ros2 interface show robot_interfaces/action/ExecutePatrol编译成功后 ros2 interface show 会把字段原样打印出来。如果这里报错「package not found」，九成是忘了 source install/setup.bash——每开一个新终端都得 source 一次。
接口层完工，机器人的「语言词典」就印好了。接下来用两种语言去说这门语言：先 C++ 写控制端，再 Python 写监控端。
二、C++ 控制节点实现
控制节点是系统的「大脑」：发速度指令、收传感器、响急停、驱动巡检任务。
ROS 2 节点的核心是一个「回调循环（spin）」：你注册好各种回调，框架在循环里挨个调用。
所以写节点 = 构造函数里「注册通信通道」+ 一堆「回调函数」。
先看清编译配置，再读主逻辑，主线就四件：建通道、收数据、跑主循环、发速度。
控制节点是系统的「大脑」：它发出速度指令、接收传感器、响应急停、并驱动整个巡检任务。先看它的编译配置，再看主逻辑。
2.1 CMakeLists.txt 配置
控制节点要编出两个可执行文件：patrol_controller（主控制器）和 emergency_handler（急停处理）。
每个可执行文件都得在 ament_target_dependencies 里声明用到的包，否则链接阶段找不到符号。
add_executable 只负责「从哪个源文件编」，依赖声明负责「去哪找头文件和库」。
install(TARGETS ...) 把二进制装到 lib/，没有这步 ros2 run 找不到节点。
这里依赖带上了 tf2_ros 和 nav_msgs，因为控制节点要读里程计、算坐标变换。
控制节点要编出两个可执行文件：patrol_controller（主控制器）和 emergency_handler（急停处理）。
每个可执行文件都得在 ament_target_dependencies 里声明它用到的包，否则链接阶段找不到符号。
add_executable 只负责「从哪个源文件编」，依赖声明负责「去哪找头文件和库」。
install(TARGETS ...) 把编好的二进制装到 lib/ 下，没有这步 ros2 run 就找不到节点。
注意这里依赖里带上了 tf2_ros 和 nav_msgs，因为控制节点要读里程计、算坐标变换。
控制节点要编出两个可执行文件：patrol_controller（主控制器）和 emergency_handler（急停处理）。每个可执行文件都得在 ament_target_dependencies 里声明它用到的包，否则链接阶段会找不到符号。
cmake_minimum_required(VERSION 3.8)
project(robot_control)
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(robot_interfaces REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(nav_msgs REQUIRED)
# 把源文件编成可执行文件 patrol_controller
add_executable(patrol_controller src/patrol_controller.cpp)
ament_target_dependencies(patrol_controller
rclcpp robot_interfaces std_msgs geometry_msgs tf2_ros nav_msgs
)
# 第二个可执行文件：急停处理
add_executable(emergency_handler src/emergency_handler.cpp)
ament_target_dependencies(emergency_handler
rclcpp robot_interfaces std_msgs
)
# 安装到 lib/ 下，ros2 run 才能找到
install(TARGETS
patrol_controller
emergency_handler
DESTINATION lib/${PROJECT_NAME}
)
ament_package()2.2 巡检控制器节点
下面这个 patrol_controller.cpp 是全文最长的文件，但它干的活其实就四件：建通信通道（发布/订阅/服务/动作）、收传感器和里程计、执行巡检主循环、把速度发出去。读的时候抓主线即可，细节注释已标在关键行。
src/patrol_controller.cpp：
`std::lock_guard` 都保护被多线程共用的变量；navigate_to_waypoint 是简化控制器，重点看它怎么读 RobotStatus 字段、怎么发 Twist。#include <rclcpp/rclcpp.hpp> #include <robot_interfaces/msg/patrol_point.hpp> #include <robot_interfaces/msg/sensor_data.hpp> #include <robot_interfaces/msg/robot_status.hpp> #include <robot_interfaces/srv/set_patrol_plan.hpp> #include <robot_interfaces/srv/emergency_stop.hpp> #include <robot_interfaces/action/execute_patrol.hpp> #include <geometry_msgs/msg/twist.hpp> #include <nav_msgs/msg/odometry.hpp> #include <tf2_ros/transform_listener.h> #include <tf2/LinearMath/Transform.h> #include <mutex> #include <cmath> using namespace std::chrono_literals; // 给自定义类型起别名，下面代码才不会因为名字太长而爆炸 using PatrolPoint = robot_interfaces::msg::PatrolPoint; using SensorData = robot_interfaces::msg::SensorData; using RobotStatus = robot_interfaces::msg::RobotStatus; class PatrolController : public rclcpp::Node { public: PatrolController() : Node("patrol_controller") { // 声明参数 // 参数能从 launch 文件或命令行覆盖，比写死在代码里灵活 this->declare_parameter("max_speed", 0.5f); this->declare_parameter("position_tolerance", 0.3f); this->declare_parameter("loop_patrol", false); // 发布者：速度指令 // 话题名用 /cmd_vel，是差速底盘约定俗成的「油门」话题 cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>( "/cmd_vel", 10); // 发布者：机器人状态 status_pub_ = this->create_publisher<RobotStatus>( "/robot/status", 10); // 订阅者：里程计 // SensorDataQoS 比默认 QoS 更「健谈」，传感器数据不怕丢就用它 odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>( "/odom", rclcpp::SensorDataQoS(), std::bind(&PatrolController::odom_callback, this, std::placeholders::_1)); // 订阅者：传感器数据 sensor_sub_ = this->create_subscription<SensorData>( "/sensors/data", 10, std::bind(&PatrolController::sensor_callback, this, std::placeholders::_1)); // 服务：设置巡检计划 // 用 std::bind 把成员函数绑成回调，_1/_2 是请求和响应的占位符 plan_srv_ = this->create_service<robot_interfaces::srv::SetPatrolPlan>( "/robot/set_patrol_plan", std::bind(&PatrolController::set_plan_callback, this, std::placeholders::_1, std::placeholders::_2)); // 服务：紧急停机 e_stop_srv_ = this->create_service<robot_interfaces::srv::EmergencyStop>( "/robot/emergency_stop", std::bind(&PatrolController::emergency_stop_callback, this, std::placeholders::_1, std::placeholders::_2)); // 动作服务：执行巡检 // 动作服务端要同时给「目标/取消/执行」三个回调 patrol_action_ = rclcpp_action::create_server< robot_interfaces::action::ExecutePatrol>( this, "/robot/execute_patrol", std::bind(&PatrolController::handle_goal, this, std::placeholders::_1, std::placeholders::_2), std::bind(&PatrolController::handle_cancel, this, std::placeholders::_1), std::bind(&PatrolController::handle_accepted, this, std::placeholders::_1)); // 定时器：状态发布 (1Hz) // 1 秒发一次状态，监控面板刷新不卡也不浪费带宽 status_timer_ = this->create_wall_timer(1s, std::bind(&PatrolController::publish_status, this)); // 定时器：控制循环 (20Hz) // 20Hz 是底盘控制的常见节奏，太低走不稳，太高 CPU 吃紧 control_timer_ = this->create_wall_timer(50ms, std::bind(&PatrolController::control_loop, this)); RCLCPP_INFO(this->get_logger(), "巡逻控制器已启动"); } private: // === 状态管理 === enum class State { IDLE, PATROLLING, EMERGENCY_STOP }; State state_ = State::IDLE; std::vector<PatrolPoint> waypoints_; size_t current_wp_index_ = 0; bool loop_patrol_ = false; float max_speed_ = 0.5f; float pos_tolerance_ = 0.3f; float current_x_ = 0.0f, current_y_ = 0.0f, current_yaw_ = 0.0f; std::mutex mutex_; uint32_t total_patrols_ = 0; // === 里程计回调 === void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { std::lock_guard<std::mutex> lock(mutex_); current_x_ = msg->pose.pose.position.x; current_y_ = msg->pose.pose.position.y; // 从四元数提取 yaw // 里程计给的是四元数(x,y,z,w)，机器人朝向只要 yaw 一个角 // 用 atan2 把四元数反算成绕 Z 轴的偏航角 auto &q = msg->pose.pose.orientation; current_yaw_ = std::atan2( 2.0 * (q.w * q.z + q.x * q.y), 1.0 - 2.0 * (q.y * q.y + q.z * q.z)); } // === 传感器回调 === void sensor_callback(const SensorData::SharedPtr msg) { latest_sensor_ = *msg; if (msg->alarm) { // 传感器自己标了 alarm，这里只负责打日志提醒 RCLCPP_WARN(this->get_logger(), "传感器报警 [%s]: %s", msg->sensor_id.c_str(), msg->alarm_reason.c_str()); } } // === 设置巡检计划服务 === void set_plan_callback( const std::shared_ptr<robot_interfaces::srv::SetPatrolPlan::Request> req, std::shared_ptr<robot_interfaces::srv::SetPatrolPlan::Response> res) { std::lock_guard<std::mutex> lock(mutex_); // 正在巡检中不让改计划，避免半路被换目标 if (state_ == State::PATROLLING) { res->success = false; res->message = "机器人正在巡检中，请先停止当前任务"; return; } waypoints_ = req->waypoints; loop_patrol_ = req->loop; // 限速做了兜底：传 0 或负数就退回默认 0.5 max_speed_ = req->max_speed > 0 ? req->max_speed : 0.5f; current_wp_index_ = 0; res->success = true; res->message = "巡检计划设置成功，共 " + std::to_string(waypoints_.size()) + " 个点位"; res->plan_id = ++plan_counter_; RCLCPP_INFO(this->get_logger(), "%s", res->message.c_str()); } // === 紧急停机服务 === void emergency_stop_callback( const std::shared_ptr<robot_interfaces::srv::EmergencyStop::Request> req, std::shared_ptr<robot_interfaces::srv::EmergencyStop::Response> res) { std::lock_guard<std::mutex> lock(mutex_); // 不管之前在干嘛，先切到急停态、发零速度指令刹车 state_ = State::EMERGENCY_STOP; geometry_msgs::msg::Twist stop_cmd; cmd_vel_pub_->publish(stop_cmd); auto now = this->now(); res->stopped = true; res->message = "紧急停机 - 原因: " + req->reason; res->stop_timestamp = now.seconds(); RCLCPP_ERROR(this->get_logger(), "紧急停机! 操作员: %s, 原因: %s", req->operator_id.c_str(), req->reason.c_str()); } // === 动作服务器：目标处理 === rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID&, std::shared_ptr<const robot_interfaces::action::ExecutePatrol::Goal> goal) { std::lock_guard<std::mutex> lock(mutex_); // 急停态或没计划，直接拒绝，别让动作空跑 if (state_ == State::EMERGENCY_STOP) { RCLCPP_WARN(this->get_logger(), "拒绝目标：紧急停机状态"); return rclcpp_action::GoalResponse::REJECT; } if (waypoints_.empty()) { RCLCPP_WARN(this->get_logger(), "拒绝目标：无巡检计划"); return rclcpp_action::GoalResponse::REJECT; } RCLCPP_INFO(this->get_logger(), "接受巡检目标，%zu 个点位", goal->waypoints.size()); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr<rclcpp_action::ServerGoalHandle< robot_interfaces::action::ExecutePatrol>>) { RCLCPP_INFO(this->get_logger(), "收到巡检取消请求"); return rclcpp_action::CancelResponse::ACCEPT; } void handle_accepted( std::shared_ptr<rclcpp_action::ServerGoalHandle< robot_interfaces::action::ExecutePatrol>> goal_handle) { // 开新线程跑巡检，避免阻塞 ROS 的回调循环 std::thread{std::bind(&PatrolController::execute_patrol, this, std::placeholders::_1), goal_handle}.detach(); } // === 执行巡检（独立线程） === void execute_patrol( std::shared_ptr<rclcpp_action::ServerGoalHandle< robot_interfaces::action::ExecutePatrol>> goal_handle) { auto goal = goal_handle->get_goal(); auto feedback = std::make_shared< robot_interfaces::action::ExecutePatrol::Feedback>(); auto result = std::make_shared< robot_interfaces::action::ExecutePatrol::Result>(); { std::lock_guard<std::mutex> lock(mutex_); state_ = State::PATROLLING; } auto start_time = this->now(); double total_distance = 0.0; uint32_t visited = 0; for (size_t i = 0; i < goal->waypoints.size(); ++i) { // 检查是否被取消 if (goal_handle->is_canceling()) { result->completed = false; result->result_message = "巡检被用户取消"; result->visited_points = visited; goal_handle->canceled(result); state_ = State::IDLE; return; } // 检查紧急停机 { std::lock_guard<std::mutex> lock(mutex_); if (state_ == State::EMERGENCY_STOP) { result->completed = false; result->result_message = "巡检被紧急停机中断"; result->visited_points = visited; goal_handle->abort(result); return; } } current_wp_index_ = i; auto& wp = goal->waypoints[i]; // 发送反馈 // 反馈在「到点之前」发，监控面板看到的进度才会平滑 feedback->current_waypoint_index = i; feedback->current_target = wp; feedback->progress_percent = static_cast<float>(i) / goal->waypoints.size() * 100.0f; feedback->latest_sensor_data = latest_sensor_; feedback->status_message = "正在前往点位 " + wp.point_id; goal_handle->publish_feedback(feedback); // 导航到目标点（简化：等待到位） RCLCPP_INFO(this->get_logger(), "前往点位 %s (%.1f, %.1f)", wp.point_id.c_str(), wp.position.x, wp.position.y); navigate_to_waypoint(wp); visited++; total_distance += calculate_distance( current_x_, current_y_, wp.position.x, wp.position.y); // 在点位停留 "检测" RCLCPP_INFO(this->get_logger(), "到达点位 %s，开始检测...", wp.point_id.c_str()); std::this_thread::sleep_for(2s); } // 巡检完成 { std::lock_guard<std::mutex> lock(mutex_); state_ = State::IDLE; total_patrols_++; } auto elapsed = (this->now() - start_time).seconds(); result->completed = true; result->result_message = "巡检完成！"; result->visited_points = visited; result->total_distance = total_distance; result->total_time = elapsed; goal_handle->succeed(result); RCLCPP_INFO(this->get_logger(), "巡检完成! 距离: %.1fm, 耗时: %.1fs", total_distance, elapsed); } // === 导航到目标点（纯追踪控制器） === void navigate_to_waypoint(const PatrolPoint& wp) { auto twist = geometry_msgs::msg::Twist(); rclcpp::Rate rate(20); while (rclcpp::ok()) { std::lock_guard<std::mutex> lock(mutex_); if (state_ == State::EMERGENCY_STOP) break; float dx = wp.position.x - current_x_; float dy = wp.position.y - current_y_; float dist = std::sqrt(dx * dx + dy * dy); // 到了容差范围内就停，避免在原地抖动 if (dist < pos_tolerance_) { twist.linear.x = 0; twist.angular.z = 0; cmd_vel_pub_->publish(twist); break; } // 纯追踪角度计算 float target_yaw = std::atan2(dy, dx); float yaw_error = target_yaw - current_yaw_; // 归一化角度到 [-PI, PI] // 不归一化的话转 350° 会绕远路，而不是反向转 10° while (yaw_error > M_PI) yaw_error -= 2 * M_PI; while (yaw_error < -M_PI) yaw_error += 2 * M_PI; // 分离式控制：先转向，再前进 // 朝向差太多时只转不走的，能少画圈、少撞墙 if (std::abs(yaw_error) > 0.1) { twist.linear.x = 0; twist.angular.z = std::clamp(yaw_error * 2.0f, -max_speed_, max_speed_); } else { twist.linear.x = std::clamp(dist * 1.0f, 0.0f, max_speed_); twist.angular.z = std::clamp(yaw_error * 2.0f, -max_speed_, max_speed_); } cmd_vel_pub_->publish(twist); rate.sleep(); } } // === 控制循环 === void control_loop() { std::lock_guard<std::mutex> lock(mutex_); if (state_ != State::PATROLLING) { // 非巡检状态确保停止 // 兜底刹车：万一别处忘了发零速度，这里补一刀 auto twist = geometry_msgs::msg::Twist(); cmd_vel_pub_->publish(twist); } } // === 发布状态 === void publish_status() { RobotStatus status; status.header.stamp = this->now(); status.robot_id = "PATROL-001"; status.battery_level = 85.0f; status.cpu_usage = 23.5f; status.memory_usage = 45.2f; status.current_pose.x = current_x_; status.current_pose.y = current_y_; status.current_pose.theta = current_yaw_; status.total_patrols = total_patrols_; { std::lock_guard<std::mutex> lock(mutex_); // 把内部状态机的值翻译成 RobotStatus 的枚举 switch (state_) { case State::IDLE: status.state = RobotStatus::IDLE; break; case State::PATROLLING: status.state = RobotStatus::PATROLLING; status.current_task = "巡检中 - 点位 " + std::to_string(current_wp_index_); break; case State::EMERGENCY_STOP: status.state = RobotStatus::EMERGENCY_STOP; status.current_task = "紧急停机"; break; } } status_pub_->publish(status); } // === 工具函数 === static float calculate_distance(float x1, float y1, float x2, float y2) { return std::sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1)); } // === 成员变量 === rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_; rclcpp::Publisher<RobotStatus>::SharedPtr status_pub_; rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_; rclcpp::Subscription<SensorData>::SharedPtr sensor_sub_; rclcpp::Service<robot_interfaces::srv::SetPatrolPlan>::SharedPtr plan_srv_; rclcpp::Service<robot_interfaces::srv::EmergencyStop>::SharedPtr e_stop_srv_; rclcpp_action::Server<robot_interfaces::action::ExecutePatrol>::SharedPtr patrol_action_; rclcpp::TimerBase::SharedPtr status_timer_; rclcpp::TimerBase::SharedPtr control_timer_; SensorData latest_sensor_; uint32_t plan_counter_ = 0; }; int main(int argc, char** argv) { rclcpp::init(argc, argv); // spin 会一直跑回调循环，直到节点被关 rclcpp::spin(std::make_shared<PatrolController>()); rclcpp::shutdown(); return 0; }
读这段代码的诀窍：所有 std::lock_guard 都是为了保护 current_x_ / state_ 这类被多线程同时读写的变量——巡检在子线程跑、里程计在主线程回调，不加锁就会数据竞争。如果你只抄逻辑不改字段，锁可以照抄。
读这段代码的第二个诀窍，是理解「发布 / 订阅」和「服务 / 动作」在控制节点里的分工。
发布者是「我往外说」：速度指令、状态报告。
订阅者是「我听别人说」：里程计、传感器。
服务是「别人问我、我答」：设置计划、急停。
动作服务端是「别人派长活、我边干边报」：执行巡检。
这四个角色合起来，就是控制节点全部对外接口。
QoS 最该留心：传感器用 SensorDataQoS 更「健谈」，状态用 RELIABLE 更「可靠」。
动作服务端必须给「目标 / 取消 / 执行」三个回调，执行逻辑丢进新线程，否则卡死回调循环。
navigate_to_waypoint 是简化版纯追踪控制器：先转朝向、再前进，到位就停。
它没用真实导航栈，是故意简化——重点是演示「怎么用自定义消息驱动行为」。
三、Python 监控节点实现
监控节点是「驾驶舱」：它订阅状态、收传感器、在报警时自动急停，并通过动作客户端把整趟巡检任务发出去。Python 写这类逻辑比 C++ 短得多，适合做上层调度。
robot_monitor/robot_monitor/monitor_node.py：
下面这个文件不用逐行啃，抓三条线就行：① 初始化时建好订阅器/客户端/定时器；② 回调里只做「记数据 + 判断报警」的轻活；③ main 里用独立线程跑 rclpy.spin，主线程负责发任务。注释标在容易踩坑的地方。
只做注册，回调只做轻活（记数据、判报警）；`check_alarms` 统一判报警；`main` 里把 spin 丢进子线程是关键，否则后续发指令的代码永远跑不到。#!/usr/bin/env python3 """巡检机器人监控节点 - 实时监控面板与报警系统""" import rclpy from rclpy.node import Node from rclpy.action import ActionClient from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy from robot_interfaces.msg import ( PatrolPoint, SensorData, RobotStatus ) from robot_interfaces.srv import SetPatrolPlan, EmergencyStop from robot_interfaces.action import ExecutePatrol from geometry_msgs.msg import Pose2D from datetime import datetime from collections import deque import json import threading class PatrolMonitor(Node): def __init__(self): super().__init__('patrol_monitor') # === 状态数据存储 === self.latest_status = None self.sensor_history = deque(maxlen=100) # 最近 100 条，超出自动丢旧的 self.alarm_log = [] # 报警记录 self.patrol_active = False # === QoS 配置 === # 状态这种「丢了也无所谓、但要可靠」的数据用 RELIABLE reliable_qos = QoSProfile( reliability=ReliabilityPolicy.RELIABLE, history=HistoryPolicy.KEEP_LAST, depth=10 ) # === 订阅器 === self.status_sub = self.create_subscription( RobotStatus, '/robot/status', self.status_callback, reliable_qos) self.sensor_sub = self.create_subscription( SensorData, '/sensors/data', self.sensor_callback, 10) # === 客户端 === # 客户端是「主动去问」的那一端，对应控制节点的服务 self.plan_client = self.create_client( SetPatrolPlan, '/robot/set_patrol_plan') self.estop_client = self.create_client( EmergencyStop, '/robot/emergency_stop') self.patrol_action_client = ActionClient( self, ExecutePatrol, '/robot/execute_patrol') # === 定时器：数据摘要日志 (10s) === self.summary_timer = self.create_timer(10.0, self.print_summary) # === 定时器：自动报警判断 (2s) === self.check_timer = self.create_timer(2.0, self.check_alarms) self.get_logger().info('监控节点已启动') # ======== 回调函数 ======== def status_callback(self, msg: RobotStatus): """处理机器人状态更新""" self.latest_status = msg # 把枚举数字翻成中文，日志才看得懂 state_names = { RobotStatus.IDLE: '空闲', RobotStatus.PATROLLING: '巡检中', RobotStatus.CHARGING: '充电中', RobotStatus.ERROR: '故障', RobotStatus.EMERGENCY_STOP: '紧急停机' } state_str = state_names.get(msg.state, '未知') self.get_logger().info( f'状态更新: {state_str} | ' f'电量: {msg.battery_level:.1f}% | ' f'位置: ({msg.current_pose.x:.1f}, {msg.current_pose.y:.1f}) | ' f'累计巡检: {msg.total_patrols} 次' ) # 低电量报警 if msg.battery_level < 20.0 and msg.state != RobotStatus.CHARGING: self.trigger_alarm('LOW_BATTERY', f'电量不足: {msg.battery_level:.1f}%') def sensor_callback(self, msg: SensorData): """处理传感器数据""" # 只存成字典，真正的判断交给 check_alarms 统一做 self.sensor_history.append({ 'time': datetime.now().isoformat(), 'sensor_id': msg.sensor_id, 'temperature': msg.temperature, 'humidity': msg.humidity, 'gas': msg.gas_concentration, 'vibration': msg.vibration, }) def patrol_feedback_callback(self, feedback_msg): """巡检动作的反馈回调""" feedback = feedback_msg.feedback self.get_logger().info( f'巡检进度: {feedback.progress_percent:.1f}% | ' f'当前点位: {feedback.current_target.point_id} | ' f'状态: {feedback.status_message}' ) # ======== 服务调用 ======== def set_patrol_plan(self, waypoints, loop=False, max_speed=0.5): """设置巡检计划""" # 先等服务上线，最多等 5 秒，避免一上来就报错 if not self.plan_client.wait_for_service(timeout_sec=5.0): self.get_logger().error('巡检计划服务不可用') return False request = SetPatrolPlan.Request() request.waypoints = waypoints request.loop = loop request.max_speed = max_speed # call_async 是异步的，用 spin_until_future_complete 等结果 future = self.plan_client.call_async(request) rclpy.spin_until_future_complete(self, future, timeout_sec=5.0) if future.result() is not None: resp = future.result() self.get_logger().info( f'设置计划: {resp.message} (plan_id={resp.plan_id})') return resp.success return False def emergency_stop(self, reason='手动触发', operator='MONITOR'): """触发紧急停机""" if not self.estop_client.wait_for_service(timeout_sec=2.0): self.get_logger().error('紧急停机服务不可用！') return False request = EmergencyStop.Request() request.reason = reason request.operator_id = operator future = self.estop_client.call_async(request) # 紧急停机不需要等完整响应，fire-and-forget 更及时 self.get_logger().warn(f'紧急停机已触发: {reason}') return True # ======== 动作客户端 ======== def start_patrol(self): """启动巡检任务""" if not self.patrol_action_client.wait_for_server(timeout_sec=5.0): self.get_logger().error('巡检动作服务器不可用') return goal_msg = ExecutePatrol.Goal() # 创建示例巡检路线（正方形四角） waypoints = [] for i, (x, y) in enumerate([ (2.0, 0.0), (2.0, 2.0), (0.0, 2.0), (0.0, 0.0) ]): wp = PatrolPoint() wp.point_id = f'WP-{i+1:03d}' wp.position = Pose2D(x=x, y=y, theta=0.0) wp.required_temperature = 60.0 wp.required_gas_level = 100.0 wp.priority = PatrolPoint.NORMAL waypoints.append(wp) goal_msg.waypoints = waypoints goal_msg.loop = False goal_msg.max_speed = 0.5 # 发目标并挂上反馈回调，进度条就靠它更新 send_goal_future = self.patrol_action_client.send_goal_async( goal_msg, feedback_callback=self.patrol_feedback_callback) send_goal_future.add_done_callback(self.goal_response_callback) def goal_response_callback(self, future): goal_handle = future.result() if not goal_handle.accepted: self.get_logger().warn('巡检目标被拒绝') return self.patrol_active = True self.get_logger().info('巡检目标已接受，开始执行...') # 再挂一个回调等最终结果 result_future = goal_handle.get_result_async() result_future.add_done_callback(self.patrol_result_callback) def patrol_result_callback(self, future): result = future.result().result self.patrol_active = False if result.completed: self.get_logger().info( f'巡检完成! 访问 {result.visited_points} 个点位, ' f'总距离: {result.total_distance:.1f}m, ' f'总耗时: {result.total_time:.1f}s') else: self.get_logger().warn(f'巡检未完成: {result.result_message}') # ======== 报警系统 ======== def check_alarms(self): """自动检测异常并报警""" if not self.sensor_history: return latest = self.sensor_history[-1] # 温度异常 if latest['temperature'] > 80.0: self.trigger_alarm('HIGH_TEMP', f'温度过高: {latest["temperature"]:.1f}°C') # 气体泄漏 if latest['gas'] > 200.0: self.trigger_alarm('GAS_LEAK', f'气体浓度异常: {latest["gas"]:.1f}ppm') # 自动触发紧急停机（安全优先） self.emergency_stop( reason=f'气体浓度超标: {latest["gas"]:.1f}ppm', operator='AUTO') # 振动异常 if latest['vibration'] > 15.0: self.trigger_alarm('HIGH_VIBRATION', f'振动过大: {latest["vibration"]:.1f}mm/s') def trigger_alarm(self, alarm_type, message): """记录报警""" alarm = { 'time': datetime.now().isoformat(), 'type': alarm_type, 'message': message, } self.alarm_log.append(alarm) self.get_logger().error(f'[报警] {alarm_type}: {message}') def print_summary(self): """打印系统摘要""" if not self.latest_status: return status = self.latest_status self.get_logger().info( f'\n{"="*50}\n' f' 巡检机器人系统摘要\n' f'{"="*50}\n' f' 状态: {status.current_task}\n' f' 电量: {status.battery_level:.1f}%\n' f' CPU: {status.cpu_usage:.1f}%\n' f' 内存: {status.memory_usage:.1f}%\n' f' 传感器数据: {len(self.sensor_history)} 条\n' f' 报警记录: {len(self.alarm_log)} 条\n' f'{"="*50}' ) def main(): rclpy.init() monitor = PatrolMonitor() # 启动一个线程运行 ROS spin # 因为主线程待会儿要发任务、等键盘，不能让它卡在 spin 里 spin_thread = threading.Thread(target=rclpy.spin, args=(monitor,)) spin_thread.start() try: # 等待系统就绪后自动发送巡检计划 import time time.sleep(3) # 创建巡检路线 waypoints = [] for i, (x, y) in enumerate([ (3.0, 0.0), (3.0, 3.0), (0.0, 3.0), (0.0, 0.0) ]): wp = PatrolPoint() wp.point_id = f'AREA-A-{i+1:03d}' wp.position = Pose2D(x=x, y=y, theta=0.0) wp.required_temperature = 60.0 wp.priority = PatrolPoint.NORMAL waypoints.append(wp) # 设置计划 monitor.set_patrol_plan(waypoints, loop=True, max_speed=0.4) time.sleep(1) # 启动巡检 monitor.start_patrol() # 主线程等待 spin_thread.join() except KeyboardInterrupt: monitor.get_logger().info('监控节点关闭') finally: monitor.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()
注意 main 里把 rclpy.spin 丢进子线程这件事：ROS 2 的 spin 会一直占着调用它的线程，如果放在主线程，后面的「发任务」代码就永远跑不到。这是 Python 节点最常见的「发了任务没反应」根源。
Python 节点和 C++ 干的是同一件事，但写法轻得多。
订阅、客户端、定时器都在 __init__ 里注册，回调里只做轻活：记数据、判报警。
check_alarms 每 2 秒扫一次最新传感器，超阈值就 trigger_alarm，气体超标还会自动急停。
动作客户端用 send_goal_async 发任务，再挂 feedback_callback 收进度，是典型的异步模型。
最容易被坑的是 main 里那行 threading.Thread(target=rclpy.spin, ...)。
ROS 2 的 spin 会一直占着调用它的线程；若放在主线程，后面的「发任务」永远跑不到。
所以把 spin 丢进子线程，主线程才能继续发计划、等键盘。
这也是 Python 节点「发了任务没反应」的头号原因。
四、Launch 文件编排
单个节点手动 ros2 run 太碎，Launch 文件就是「一键拉起全场」的总指挥：先声明可调参数，再按依赖顺序把各节点拼起来。
patrol_launcher/patrol_launcher/launch/patrol_system.launch.py：
这段 launch 里最该看懂的是三件事：① 用 DeclareLaunchArgument 把速度、是否仿真、是否开 RViz 变成启动时可调的旋钮；② 用 TimerAction 让监控节点延迟 2 秒启动，等控制器先就绪；③ 用 remappings 把话题名改掉，避免和别套系统撞名。其余节点照抄即可。
是启动旋钮（命令行可覆盖）；TimerAction 让监控延迟 2 秒起，等控制器先就绪；remappings 改名避免话题冲突；condition#!/usr/bin/env python3 """巡检机器人系统 Launch 文件 - 一键启动全部节点""" from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, LogInfo, TimerAction, GroupAction ) from launch.conditions import IfCondition, LaunchConfigurationEquals from launch.substitutions import LaunchConfiguration, FindExecutable from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue import os def generate_launch_description(): # === 启动参数 === # 这些参数能在命令行用 key:=value 覆盖，比如 max_speed:=1.0 use_sim = DeclareLaunchArgument( 'use_sim', default_value='true', description='是否启用仿真模式') max_speed = DeclareLaunchArgument( 'max_speed', default_value='0.5', description='最大移动速度 (m/s)') verbose = DeclareLaunchArgument( 'verbose', default_value='false', description='详细日志输出') use_rviz = DeclareLaunchArgument( 'use_rviz', default_value='true', description='启动 RViz2 可视化') # === 仿真模式下的虚拟传感器节点 === # condition=IfCondition 让它在非仿真模式下自动不启动 virtual_sensor_node = Node( package='robot_monitor', executable='virtual_sensor_node', name='virtual_sensors', output='screen', parameters=[{ 'publish_rate': 10.0, 'simulate_alarms': False, }], condition=IfCondition(LaunchConfiguration('use_sim')) ) # === 控制器节点 === patrol_controller = Node( package='robot_control', executable='patrol_controller', name='patrol_controller', output='screen', parameters=[{ 'max_speed': LaunchConfiguration('max_speed'), 'position_tolerance': 0.3, 'loop_patrol': False, }], # remappings：把内部 /cmd_vel 改名，方便多机器人并存 remappings=[ ('/cmd_vel', '/robot/cmd_vel'), ] ) # === 监控节点（延迟 2s 启动，等控制器就绪）=== # 用 TimerAction 包一层，否则两个节点同时起可能抢资源 monitor_node = TimerAction( period=2.0, actions=[ Node( package='robot_monitor', executable='monitor_node', name='patrol_monitor', output='screen', ) ] ) # === 日志记录节点 === logger_node = Node( package='robot_monitor', executable='log_recorder_node', name='log_recorder', output='screen', parameters=[{ 'log_dir': os.path.expanduser('~/patrol_logs'), 'log_format': 'json', }] ) # === RViz2 === rviz_node = Node( package='rviz2', executable='rviz2', name='rviz2', arguments=['-d', os.path.join( os.path.dirname(__file__), '..', 'config', 'patrol.rviz')], condition=IfCondition(LaunchConfiguration('use_rviz')) ) # === TF 静态变换（地图→基座）=== # 没有真实 TF 树时，先用静态变换把 map 和 base_link 粘起来 static_tf = Node( package='tf2_ros', executable='static_transform_publisher', name='map_to_base_link', arguments=['0', '0', '0', '0', '0', '0', 'map', 'base_link'], ) return LaunchDescription([ # 参数声明 use_sim, max_speed, verbose, use_rviz, # 启动提示 LogInfo(msg='巡检机器人系统启动中...'), LogInfo(msg='控制器节点启动'), LogInfo(msg='监控节点将在 2s 后启动'), LogInfo(msg='启动参数: sim=%s, speed=%s' % ( LaunchConfiguration('use_sim'), LaunchConfiguration('max_speed'))), # 节点 static_tf, virtual_sensor_node, patrol_controller, monitor_node, logger_node, rviz_node, ])
跑起来后你会看到终端依次打印那几条 LogInfo，RViz 里出现机器人模型——说明总装成功。如果某个节点没起来，output='screen' 会让它的报错直接喷在终端上，比去翻日志快。
跑起来后，终端会依次打印那几条 LogInfo，RViz 里出现机器人模型——说明总装成功。
如果某个节点没起来，output='screen' 让它的报错直接喷在终端上，比翻日志快。
但 Launch 只负责「启动」，不负责「对不对」。
节点都起了、话题却不通，那是通信问题，得靠下一节的命令行探针。
常见 Launch 坑：参数名拼错（启动不报错但节点读到默认值）、remappings 两边不一致、条件启动的 condition 写反。
排 Launch 问题，先 ros2 node list 看谁真的起来了，再往下查。
五、调试工具链
系统跑起来了但「没反应」是最常见的问题。ROS 2 自带一套命令行探针，从「接口存不存在」一路查到「消息通不通」。
排查顺序建议固定为：先看接口、再看节点、再看话题、最后看内容。
别一上来就改代码——八成的「没反应」用命令行三条命令就能定位。
这一节不是背命令，而是建立一套「自顶向下」的排查直觉：越靠前的检查越廉价，先做最便宜的。
5.1 查看消息/服务/动作
ros2 interface list 列出所有接口，grep 你的包名能快速过滤。
ros2 interface show 把字段原样打印，写代码前用它确认字段名和类型最省事。
ros2 topic info --verbose 还能看到话题的发布者、订阅者和 QoS，是查「为什么收不到」的起手式。
先确认你定义的类型真的被注册进了系统。这一步能排掉九成的「import 报错」。
# 列出所有可用的自定义接口
ros2 interface list | grep robot_interfaces
# 查看接口定义
ros2 interface show robot_interfaces/msg/RobotStatus
ros2 interface show robot_interfaces/srv/EmergencyStop
ros2 interface show robot_interfaces/action/ExecutePatrol
# 列出所有话题及其消息类型
ros2 topic list
ros2 topic info /robot/status --verbose5.2 命令行测试通信
topic hz 看发布频率，卡顿或掉帧一眼能看出来。
topic pub --once 能手动灌一条消息，不必等节点发——适合隔离测试「订阅方会不会处理」。
topic delay 看端到端延迟，导航调参时很有用。
手动 pub 一条 /cmd_vel 还能直接验证底盘动不动，是调控制节点的最小用例。
topic echo 是你的「示波器」：把某个话题上的消息实时打印出来，肉眼看数据对不对。topic pub --once 则能手动灌一条消息，不必等节点发。
# 监听机器人状态（实时）
ros2 topic echo /robot/status robot_interfaces/msg/RobotStatus
# 监听传感器数据
ros2 topic echo /sensors/data robot_interfaces/msg/SensorData
# 查看 Topic 频率和带宽
ros2 topic hz /robot/status
ros2 topic delay /sensors/data
# 手动发布测试速度指令
ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist \
"{linear: {x: 0.2}, angular: {z: 0.0}}"5.3 服务调用测试
调用时要按 srv 的字段结构传参，格式是 YAML：{reason: '测试停机', operator_id: 'CLI-USER'}。
ros2 service type 能查服务用哪个 srv 类型，写 call 命令前先确认，免得格式错。
服务调用超时，多半是服务端没起或 QoS 不匹配，下一节会讲怎么查。
服务用 ros2 service call 直接触发，适合验证「一问一答」逻辑。比如急停服务，命令行一发就能看控制节点有没有真的切到急停态。
# 调用紧急停机
ros2 service call /robot/emergency_stop \
robot_interfaces/srv/EmergencyStop \
"{reason: '测试停机', operator_id: 'CLI-USER'}"
# 查看服务类型
ros2 service type /robot/set_patrol_plan
ros2 service list5.4 动作客户端测试
动作的目标参数结构最复杂，常常嵌套 Pose2D 这类子类型，照着 action 文件的字段一层层填。
如果目标被拒绝，别急着改代码，回去检查控制节点是不是在急停态、有没有先 set_patrol_plan。
ros2 action send_goal 是端到端验证整条动作链（目标→执行→反馈→结果）的最小用例。
动作比服务多一层进度反馈。--feedback 会把它执行过程中的每次反馈都打出来，方便确认「卡在第几个点」。
# 发送巡检任务并跟踪进度
ros2 action send_goal --feedback /robot/execute_patrol \
robot_interfaces/action/ExecutePatrol \
"{waypoints: [{point_id: 'TEST-001', position: {x: 1.0, y: 1.0, theta: 0.0}, priority: 0}], loop: false, max_speed: 0.3}"5.5 节点与系统诊断
node info 的输出是一张「节点身份证」：它发布了哪些话题、订阅了哪些、提供了哪些服务。
和 rqt_graph 对照看，能迅速发现「A 发了话题但 B 没订阅」这种断连。
ros2 doctor --report 会扫环境（domain id、网络、QoS），多机通信出问题首选它。
param set 还能不重启就改参数，调速度、调容差特别方便，不必每次重编。
当通信全断时，先 node list 看谁还活着，再 node info 看它的发布/订阅/服务画像，最后用 ros2 doctor 查环境。param set 还能不重启就改参数，调速度特别方便。
# 查看所有运行中的节点
ros2 node list
# 查看节点详细信息（订阅/发布/服务）
ros2 node info /patrol_controller
# 系统资源监控
ros2 doctor --report
ros2 topic delay /robot/status --window 10
# 查看参数
ros2 param list /patrol_controller
ros2 param get /patrol_controller max_speed
# 动态修改参数（运行时）
ros2 param set /patrol_controller max_speed 0.85.6 可视化调试
rqt 系列是图形化探针：rqt_graph 画出节点间连线，一眼看出「谁没连上谁」；rqt_plot 把数值画成曲线，趋势比数字直观。
# 启动 rqt_graph 查看节点通信拓扑
rqt_graph
# 启动 rqt_plot 实时绘制数据曲线
rqt_plot /robot/status/battery_level /robot/status/cpu_usage
# 启动 rqt_console 过滤日志
rqt_console
# 启动 rqt_publisher 手动发布消息
rqt_publisher六、编译与运行
colcon build 是 ROS 2 的编译命令，--symlink-install 让 Python 改了不用重编，直接生效。
编译完记得 source，新终端尤其容易忘这一步，报错往往就从这里来。
如果只改了一个包，用 --packages-select 只编它，能省下大把时间。
调试工具就位，最后把整套系统真正编译跑起来。
6.1 完整编译
第一次全编会慢，因为要拉齐所有依赖，之后增量编就快了。
colcon test-result --verbose 把测试结果展开，哪个用例挂了一目了然。
编不过时别急着 google，先读报错最上面那行，八成是「找不到某某类型」或「链接失败」。
--symlink-install 让 Python 文件改了不用重新 build，直接生效，开发期省时间。colcon test 则是跑包里自带的单元测试。
cd ~/ros2_patrol_ws
# 编译全部包
colcon build --symlink-install
# 加载环境
source install/setup.bash
# 检查编译结果
colcon test --packages-select robot_interfaces robot_control robot_monitor
colcon test-result --verbose6.2 一键启动
use_sim:=false 会关掉虚拟传感器、改用真实驱动，这一步是仿真到真机的分水岭。
max_speed 通过 launch 参数覆盖，不必改代码重编，调速度时最方便。
如果你只想看 RViz 里的机器人模型，用第二条（无仿真）最快，不必等 Gazebo 起来。
三条命令对应三种场景：开发看效果用仿真+RViz；纯逻辑调试关掉 RViz；上真机跑生产就全关只留核心。
# 启动完整系统（仿真模式 + RViz）
ros2 launch patrol_launcher patrol_system.launch.py \
use_sim:=true max_speed:=0.5 use_rviz:=true
# 仅启动核心节点（无仿真）
ros2 launch patrol_launcher patrol_system.launch.py use_sim:=false
# 生产模式（无 RViz，最高速度）
ros2 launch patrol_launcher patrol_system.launch.py \
use_sim:=false use_rviz:=false max_speed:=1.06.3 单独调试某个节点
出问题时别一键全启，单独跑一个节点更容易看到报错。--ros-args -p 能在启动瞬间覆盖参数。
# 只启动控制器
ros2 run robot_control patrol_controller --ros-args \
-p max_speed:=0.3
# 只启动监控
ros2 run robot_monitor monitor_node七、常见问题排查
它们的共同点是：「代码看起来没错，跑起来就是不对」。
根因大多在配置和环境，不在算法逻辑——所以先查配置，再怀疑代码。
每一条都附了可直接复制的命令，照着跑基本能定位。
下面四坑是新手 90% 会踩的，按现象对号入座。
Q1: rosidl_generate_interfaces 编译失败
根因几乎都是「类型引用格式写错」。在 .srv / .action 里引用同包的 msg，必须用完整包名，不能只写类型名。
# 检查 msg/srv/action 文件语法
# 常见错误：自定义类型未用完整包名引用
# 错误写法：
# PatrolPoint[] waypoints
# 正确写法（在 srv/action 中引用同包的 msg）：
# robot_interfaces/msg/PatrolPoint[] waypoints
# 清除缓存后重新编译
colcon build --packages-select robot_interfaces --clean-firstQ2: Python 节点找不到自定义消息
典型症状是 ModuleNotFoundError: robot_interfaces。先确认 source 对了工作空间，再用一行 import 验证类型确实生成了。
# 确保 source 了正确的工作空间
source ~/ros2_patrol_ws/install/setup.bash
# 检查消息是否注册
python3 -c "from robot_interfaces.msg import PatrolPoint; print('OK')"
# 如果报错，检查 Python 路径
echo $PYTHONPATH | tr ':' '\n' | grep robot_interfacesQ3: 服务调用超时
服务端没起、或 QoS 不匹配都会超时。先看 service list 里服务在不在，再比对两端的 QoS 策略是否一致——这是 ROS 2 比 ROS 1 多出来的隐形坑。
# 检查服务是否在线
ros2 service list | grep emergency_stop
# 检查 QoS 配置是否匹配
# 服务端和客户端必须使用相同的 QoS
ros2 topic info /robot/status --verboseQ4: 动作目标被拒绝
目标被拒不是 bug，是 handle_goal 主动挡掉的。三种常见原因：急停态、没有巡检计划、或已有动作在跑。
// 检查 handle_goal 返回值
// 常见原因：
// 1. 机器人处于紧急停机状态
// 2. 未设置巡检计划（waypoints 为空）
// 3. 已有动作在执行中八、进阶技巧
系统能跑之后，想再进一步，得懂 ROS 2 的「执行模型」。
默认情况下，一个节点只有一个回调线程：spin 循环里，回调们排队挨个执行。
这意味着：某个回调卡住（比如 sleep 5 秒），同节点的其他回调全被堵。
所以重活（如巡检主循环）必须丢进独立线程，或用 callback_group 开多线程。
另一个隐形坑是 QoS（服务质量策略），它决定「消息丢了怎么办、收不到历史怎么办」。
默认 QoS 下，发布者用「尽力而为 + 保留最后一条」，订阅者若晚于发布者启动，可能一条都收不到。
传感器、状态这类数据，常改用 SensorDataQoS / TRANSIENT_LOCAL 才能对上。
服务两端也必须 QoS 一致，否则调用直接超时——这是 ROS 2 比 ROS 1 多出来的隐形坑。
理解了执行模型和 QoS，你才真正从「能跑」跨到「跑得稳」。
系统能跑之后，这几招能让它更接近生产级。
8.1 使用 Lifecycle Node
开发期无所谓，但生产环境你往往想「先配好、再激活、出事能停用而不必杀进程」。
LifecycleNode 就是为此设计，它带一套状态机。
状态流转是：Unconfigured → Inactive → Active → Deactivate → Inactive，出错还能走 ErrorProcessing。
用生命周期节点的好处：导航栈启动时能等所有服务齐了再激活，避免「半套系统在跑」。
代价是启动流程变复杂，需要 lifecycle_manager 统一指挥各节点的状态切换。
普通 Node 一启动就全功能在线，关的时候也干脆。生产环境建议用 LifecycleNode，它带状态机：先配好再「激活」，出事能「停用」而不必杀进程。
Inactive → Active → Deactivate → Inactive
↑ ↓
└── Cleanup ← ErrorProcessing ←┘8.2 消息序列化调试
record 时指定话题名，多个话题用空格隔开，录完生成 *.db3 + metadata.yaml。
play 以原始时间戳回放，订阅方完全分不清是实时还是录播，适合复现偶发 bug。
--field 能只 echo 某个字段，数据刷屏时只看关心的那一项最清爽。
ros2 bag 是把话题录下来回放的利器：调试时录一段真实数据，之后离线反复重放，不用每次都让机器人实跑。
# 查看消息的原始数据
ros2 topic echo /robot/status --field battery_level --no-daemon
# 导出话题数据为 bag 文件
ros2 bag record -o patrol_session \
/robot/status /sensors/data /cmd_vel
# 回放 bag 文件
ros2 bag play patrol_session8.3 多机通信配置
两台机器上的节点默认互相看不见，因为它们不在同一个 DDS 域。
换域 ID 或指定发现服务器，才能让多台机器人组队。
ROS_DOMAIN_ID 是 0–232 的整数，同一域才能互相发现，不同域天然隔离——也常用来避免测试干扰生产。
ROS_DISCOVERY_SERVER 让节点通过中心服务器发现彼此，大网络下避免广播风暴。
多机通信排错第一招：先 ros2 topic list 确认两边能看到同一批话题，再谈别的。
两台机器上的节点默认互相看不见，因为它们不在同一个 DDS 域。换域 ID 或指定发现服务器，才能让多台机器人组队。
# 设置 DDS 域 ID（同一域的节点才能通信）
export ROS_DOMAIN_ID=42
# 设置 DDS 发现服务器（大网络下避免广播风暴）
export ROS_DISCOVERY_SERVER=192.168.1.100:11811看到这里，代码你基本能读了。最后把底层原理串一遍## 番外：把原理真正想通
看到这里，代码你基本能读了。最后把底层原理串一遍，以后换个项目也能套。
，以后换个项目也能套。
发布 / 订阅到底发生了什么。
当你 create_publisher 一个话题，ROS 2 在底层做的是：向 DDS（底层通信中间件）注册一个「主题」。
订阅者注册同名主题，DDS 负责把发布者的消息可靠地送到每个订阅者。
所以话题本身不存数据，它只是 DDS 里的一个「频道名」；消息是按需从发布者流到订阅者的。
这也是为什么发布者先起、订阅者后起，默认可能丢开头的几条——频道一直在，但订阅者来晚了。
服务为什么「一问一答」。
服务底层其实是「请求话题 + 响应话题」两个话题的组合，外加一个同步等待。
你 call 的时候，框架发请求、阻塞等响应；服务端收到请求、处理、回响应。
所以它不适合耗时任务——调用方会一直卡着等。
动作为什么「带进度」。
动作服务端在底层帮你建了三个话题：目标（goal）、反馈（feedback）、结果（result）。
你发目标是往 goal 话题发；执行中服务端往 feedback 话题持续发；结束往 result 话题发。
send_goal_async + feedback_callback 就是分别挂在这三个话题上的回调。
理解了这点，action 就不再是黑盒：你甚至能 ros2 topic echo 那三个话题看内部。
一次 set_patrol_plan 的端到端旅程。
- 监控节点构造请求，调用
/robot/set_patrol_plan服务。 - 请求经 DDS 到达控制节点的服务回调
set_plan_callback。 - 回调把 waypoints 存进成员变量，回
success=true和plan_id。 - 响应经 DDS 回监控节点，调用方拿到结果。
全程没有「全局变量」在节点间共享——数据只靠消息流动。这是 ROS 架构最该记住的一点。
调试心法一句话。
「没反应」先查接口在不在、节点起没起、话题通不通、QoS 对不对，逐层往下，别跳步。
接口选型决策树：到底用 msg / srv / action？
三选一是新手最容易卡住的地方——同样「让机器人做某事」，有人用 msg、有人用 srv、有人用 action，选错后期改起来很痛。下面给一套可以照着走的判断流程，不用背定义，顺着问题答就行。
第一步，先看数据要不要「持续往外说」：
- 如果你要的是「不停广播状态 / 不停发传感器读数」，而且根本不关心谁在听、也不期待对方回话——直接用 msg（Topic）。它就像广播喇叭，喊出去就完事。
- 如果这不是持续广播，而是「一次具体的请求」，进入第二步。
第二步，看这次交互要不要「等对方回一句话」：
- 要等，而且通常很快就回（几百毫秒到一秒，结果确定）——用 srv（Service）。典型例子：查询当前电量、请求急停、问一个坐标。一问一答，干净利落。
- 不用等回话、发出去就完事（fire-and-forget）——那还是回到 msg，别硬套 srv。
第三步，如果交互「要做很久」：
- 凡是耗时几秒到几分钟、中间还想汇报「做了百分之多少」——必须用 action。巡检、导航、抓取、移动机械臂都是这类。srv 会一直阻塞等结果，action 把「等」和「进度」拆开了，客户端不会被卡死。
三个容易踩的反例，记住能少走很多弯路：
- 把「启动导航 / 开始巡检」做成 srv：因为要跑很久，服务端会一直占着线程，客户端干等——这是 action 的活。
- 把「心跳 / 实时位姿」做成 srv：高频调用会把服务打爆，且根本不需要回应——这是 msg 的活。
- 把「查询状态」做成 msg：你其实想知道「此刻的值」，用 srv 一问一答更合适，msg 适合广播而非查询。
一个记忆口诀：广播用 msg，快问快答用 srv，费时带进度用 action。拿着它对照上面的三步，九成场景当场就能定。
把接口「翻译成」你项目的三步练习
光看结论不够，动手翻译一次才真会。拿你手头任意机器人项目，按下面三步做，十分钟内就能产出第一版接口草案。
- 先只写「名词」：把所有要传的数据列成单词，别管类型。例如送餐机器人：目标楼层、当前电量、是否到达、剩余路程。
- 再给每个名词贴标签：持续报→msg；问了才答→srv；要跑一段→action。上面四个分别是 msg、msg、msg、action。
- 最后补类型：楼层用 int32、电量用 float32、到达用 bool、路程用 action（目标=楼层，反馈=剩余米数，结果=成功 / 失败）。
你会发现，绝大多数机器人系统的接口都落在这三类的组合里，没有第四种。熟练之后，看到需求就能在纸上画出 msg/srv/action 三栏，这就是「ROS 接口直觉」。
一个小提醒：不要在一开始过度设计。先让系统跑起来，字段不够就加（加字段是向后兼容的），别上来就预留十几个用不上的域——那只会让接口文件越来越难读，也更容易在重构时改出破坏性变更。
最后给一张「套用到你自己的项目」清单：
- 列出节点之间要传的所有东西，每个写一句话说明它是广播、查询还是长任务。
- 广播类→msg；查询类→srv；长任务类→action，直接映射，不要犹豫。
- 字段能复用标准类型（Pose / Twist / Header）就复用，下游工具（RViz、MoveIt）才认。
- 接口文件单独成包、单独 review，因为它一改，所有依赖方都受影响。
按这四步走，你设计的接口从第一天就是「能长大」的结构，而不是后面推倒重来。
调试命令速查卡
把最常用的排查命令收成一张卡，贴手边。
- 看接口：
ros2 interface show <包>/msg/<名> - 看节点：
ros2 node list/ros2 node info /节点名 - 看话题：
ros2 topic list/echo/hz/info --verbose - 发消息：
ros2 topic pub /话题 类型 "{...}" - 调服务：
ros2 service call /服务 类型 "{...}" - 发动作：
ros2 action send_goal --feedback /动作 类型 "{...}" - 可视化：
rqt_graph/rqt_plot/rqt_console - 环境：
ros2 doctor --report
记住顺序：接口→节点→话题→内容，逐层往下别跳步，八成问题三步走完就定位。
踩坑后如何读报错
ROS 报错信息长，但有用的往往在最上面一两行，别被刷屏吓住。
编译错：先看第一个 error，八成是「找不到类型」（DEPENDENCIES 漏了）或「链接失败」（依赖没声明）。
运行错：节点起不来，看是否忘了 source；话题不通，看名字 / QoS。
回调异常：rqt_console 里 ERROR 的栈能定位到具体哪行代码。
别一上来贴全屏报错去搜，先自己读最上面那行，大部分能直接定位。
亲手设计一个消息：以机械臂为例
换个场景练一次，方法就固化了。假设你要让机械臂抓物体。
数据有哪些：目标位姿（Pose）、夹爪开合度（float32）、是否抓取（bool）。
选型：目标位姿是一次性指令→用 srv（设置目标）；执行过程要进度→用 action（抓取动作）。
msg 设计：GraspTarget.msg = geometry_msgs/Pose target + float32 gripper + bool approach。
为什么复用 Pose 而不是自己写 x/y/z：下游 MoveIt 直接吃标准 Pose，不用再转换。
这一套思维和本文巡检完全一致——你现在已经能套到任意机器人了。
消息版本管理与回退
接口也是代码，会改、会改坏，得有退路。
小改（加字段）向后兼容，老节点能跑，直接提交即可。
大改（改名 / 删字段 / 改类型）是破坏性变更，用 v1 / v2 包名共存，老节点暂留。
git 上接口文件改动要 code review，因为它影响所有依赖方，比改一个节点严重。
改完跑一遍 colcon test，确认动作服务端的测试还绿，再合并。
用命名空间做多实例
同一套节点要起两份（比如两台同构机器人），命名空间比改代码快得多。
launch 里给整组节点 push_ros_namespace='robot1'，所有话题自动加前缀。
调度节点用 /robot1/set_patrol_plan 和 /robot2/set_patrol_plan 分别指挥，互不串台。
注意：节点内部互相调用的话题也要在同一命名空间下，否则会出现「自己人找不到自己人」。
上线前的最终核对
提交代码前过一遍，避免把坑带上线。
- 接口 DEPENDENCIES 声明全了吗？
- 所有节点启动都 source 了环境吗（CI 里尤其容易漏）？
- 话题名 / 服务名两端一致吗？
- QoS 两端一致吗？
- launch 的 remappings 和代码里的话题名对得上吗？
- 关键路径（急停、设置计划）有测试覆盖吗？
全打勾，基本就不会出现「我本地能跑、上线就挂」。
参数化：用 param 文件统一管理
节点多了，每个 declare_parameter 都靠命令行覆盖太累。把参数抽到 yaml，launch 里一次性 load 最省心。
yaml 里按节点名分组：patrol_controller: {max_speed: 0.5, position_tolerance: 0.3}。
launch 里用 parameters=[os.path.join(pkg,'config','params.yaml')] 加载，节点启动时自动读。
好处有三：改参数不重编；多环境（仿真/真机）用不同 yaml；接手的人一眼看全配置。
注意 yaml 里的缩进就是层级，缩错一格参数就加载不到，排错先看缩进。
多机器人：同域隔离与命名空间
多台机器人组队，第一件事是别让它们互相干扰。
最省事：每台设不同 ROS_DOMAIN_ID，天然隔离，互不可见，也常用来避免测试干扰生产。
要协同：用 namespace 给话题加前缀，如 /robot1/cmd_vel、/robot2/cmd_vel，避免撞名。
launch 里 push_ros_namespace 或 remappings 都能加前缀，选一种全队统一。
注意：namespace 要前后一致，发布端和订阅端前缀不匹配照样收不到。
给节点写单元测试
ROS 2 节点也能测，别等上线才发现问题。
用 rclcpp::Executor + rclcpp::Node 在测试里起节点，直接 call 服务 / 发消息验证行为。
Python 侧用 rclpy + unittest，起节点、等回调、断言结果。
colcon test 会把测试结果收口，CI 里跑一遍，接口改了立刻知道谁挂。
最小实践：至少给动作服务端写一条「正常目标→成功」的测试，回归时最有价值。
日志级别：怎么用才不刷屏
RCLCPP / RCLPY 有 5 个级别：DEBUG < INFO < WARN < ERROR < FATAL。
开发期用 INFO 看流程，联调用 DEBUG 看细节，上线调成 WARN 只留异常。
别用 std::cout / print 打日志——那样没法按级别过滤，rqt_console 也管不到。
DEBUG 打得太多会拖慢实时节点，上线前关掉或降级别。
报警一定用 ERROR / WARN，正常心跳用 INFO，别反过来，否则真出事时日志被淹没。
从 ROS 1 迁移要注意的 5 件事
老 ROS 1 项目转 ROS 2，这几个坑必踩。
roscpp→rclcpp，回调模型从轮询变自旋，写法整体换血。catkin→colcon，包结构差不多但编译命令全变。rosmsg→ros2 interface，话题/服务/动作统一成 interface 命令。- 通信默认可靠（ROS 1 的 TCPROS），ROS 2 要自己管 QoS，最容易翻车。
tf→tf2，API 类似但底层更稳，老代码直接换包名大多能跑。
巡检场景的扩展玩法
本文是最小骨架，真实巡检系统往往还要加几块。
加传感器：在 SensorData.msg 里追加字段，或新建一个 msg，控制节点订阅新话题即可，不动旧逻辑。
加动作：除了「执行巡检」，还可以有「回充」「自检」动作，每个动作一个 .action，服务端加一个回调。
多机协作巡检：每台机器人一个 namespace，调度节点用一组动作客户端下发任务，结果汇总到中心。
接 Web / 手机：用 rosbridge_suite 把话题桥成 WebSocket，前端用 roslibjs 订阅状态、发急停，手机就能监控。
性能调优 checklist
系统卡顿或消息延迟高，按这个顺序查。
先看 ros2 topic hz：发布频率是不是预期，掉帧说明发布端卡了。
再看 CPU：ros2 topic delay 和 rqt_plot 看处理延迟，单回调慢就丢线程。
话题带宽大：用 QoS depth 限流，或对不重要的数据降频发布。
跨机器卡：查 ROS_DOMAIN_ID 有没有冲突，或换 discovery server 减少广播。
进程间开销大：把多个节点合成 Composable Node，省掉序列化往返。
常见术语英中对照（补）
补几组前面没列、但文档里常出现的词。
- executor：执行器，负责驱动节点的回调循环，spin 背后就是它。
- callback group：回调组，决定多个回调能不能并行跑，重活要放可重入组。
- transient local：一种 QoS，晚订阅也能拿到发布过的历史，地图/静态数据常用。
- lifecycle manager：生命周期管理器，统一指挥一组生命周期节点的状态切换。
- bag：话题录制文件，ros2 bag 录下来回放，调试复现神器。
从零搭一个最小可运行系统的 10 步
把全文的命令串成一条可复制的执行线，你照着敲就能跑起来。每一步都标了「你会看到什么」。
- 建工作空间：
mkdir -p ~/ros2_patrol_ws/src && cd ...。你会看到一个空的 src 目录。 - 建四个包：
ros2 pkg create ...。你会得到带 CMakeLists.txt / package.xml 的空包。 - 写 msg/srv/action 三个文件。用
ros2 interface show验证类型已生成。 - 写 CMakeLists.txt 和 package.xml。漏了 DEPENDENCIES 编译会报「找不到类型」。
- 编译接口包：
colcon build --packages-select robot_interfaces && source install/setup.bash。先看接口能不能 show 出来。 - 写 C++ 控制节点 + CMake。你会得到 patrol_controller 可执行文件。
- 写 Python 监控节点。先看
__init__注册了哪些通信通道。 - 写 launch 文件。用 TimerAction 让监控晚 2 秒起，避免抢资源。
- 一键启动：
ros2 launch patrol_launcher patrol_system.launch.py use_sim:=true。RViz 里出现机器人模型即成功。 - 调与查：用
ros2 topic echo /robot/status看状态，ros2 node info看断连。
为什么按这个顺序：接口是上游，先编；节点依赖接口，后编；launch 把节点串起来，最后跑。反过来会满屏「找不到类型」。
接口演进：改了 msg 老节点怎么办
项目跑起来后，十有八九要改接口：加字段、改类型、删字段。ROS 2 的接口是「强类型契约」，改动有代价，提前知道不慌。
加字段最安全：在 msg 末尾追加新字段，老节点读不到新字段时它取默认值，不会崩。
改字段类型要谨慎：把 int32 改成 int64 会破坏 ABI，所有依赖方必须重编。
删字段或改名是破坏性改动：所有引用方都得同步改，否则编译或运行直接报错。
生产环境建议：接口稳定后再冻结，用版本号（v1/v2）区分大改，老节点暂时留着。
用 rqt 五分钟定位一次断连
节点都在、却收不到消息，别猜，用 rqt 三连确认。
第一步 rqt_graph：看节点之间的连线。没连线 = 话题名拼错或 remap 不一致。
第二步 rqt_topic / rqt_plot：挑一个话题看有没有数据在流。有数据但你的节点没反应 = 订阅端 QoS 不对。
第三步 rqt_console：按 ERROR 过滤日志，常常能直接看到回调里的异常栈。
这三步比改代码快十倍，养成习惯能省大量时间。
常见 QoS 组合速查
QoS 是 ROS 2 最隐蔽的坑，记几组常用组合直接套。
传感器流（/scan、/odom）：发布用 SensorDataQoS（可靠+历史），订阅也用 SensorDataQoS，晚订阅也能拿到最近一条。
状态广播（/robot/status）：发布 RELIABLE + KEEP_LAST(10)，监控端不丢关键状态。
命令（/cmd_vel）：发布 BEST_EFFORT 即可，丢一帧速度无妨，换来低延迟。
服务 / 动作：两端必须同名 QoS，否则 call 直接超时，排查先 ros2 topic info --verbose 看两端策略。
名词对照与常见缩写
写 ROS 时这些词会反复出现，先认个脸。
- msg / srv / action：自定义数据的三种「句型」，对应话题 / 服务 / 动作。
- QoS：服务质量策略，决定消息可靠性与历史，两端必须一致。
- DDS：底层通信中间件，ROS 2 的「消息总线」，话题只是它上面的频道名。
- TF / TF2：坐标变换树，负责 map / odom / base_link 之间的坐标换算。
- odom：里程计坐标系，由轮子/IMU 积分得到，会随时间漂移。
- base_link：机器人本体的坐标系，固定在机器人中心。
- map：地图坐标系，全局固定，AMCL 负责把 odom 对齐到 map。
- lifecycle：生命周期节点，带状态机的节点，生产环境常用。
- composable：组合节点，多个节点塞进一个进程，减少通信开销。
- colcon：ROS 2 的编译工具，类似 ROS 1 的 catkin。
调试实战：复现一个真实 bug
讲一个新手十有八九会遇到的场景，帮你把前面工具串起来。
现象：监控节点 ros2 service call /robot/set_patrol_plan 一直超时，但 ros2 node list 里 patrol_controller 明明在。
第一步：ros2 topic list 看 /robot/set_patrol_plan 话题在不在——在，说明服务注册了。
第二步：ros2 topic info /robot/set_patrol_plan --verbose 看 QoS——发现两端都是 RELIABLE，对得上。
第三步：回头查代码，发现控制节点里服务名写成了 /set_patrol_plan（少了 /robot/ 前缀），客户端却叫 /robot/set_patrol_plan。
根因：服务名不一致，客户端在跟一个不存在的服务对话，所以静默超时。
修法：统一话题名，或在 launch 里用 remappings 对齐。
教训：服务/话题「超时但节点在」第一反应永远是——名字对不对、QoS 对不对。
一键复跑：把命令存成脚本
调试阶段反复敲启动命令很烦，把常用组合存成脚本最省力。
建一个 run_patrol.sh：里面 source 环境、build、再 launch，下次一个 ./run_patrol.sh 全跑。
脚本里加 set -e，任何一步失败就停，避免「前面挂了后面还跑」误导你。
把 launch 参数（use_sim / max_speed）也写成脚本参数，不同场景传不同值。
等真机部署时，这份脚本就是你的「启动说明书」，接手的人照着跑就行。
上手前最后记住三件事
写代码之前，先把这三句话刻进脑子，能避开八成的新手坑：
- 接口是「公共契约」：它一改，所有依赖方都得跟着改，所以改接口比改节点更慎重，务必走 review，别在本地偷偷改完就提交。
- 名字和 QoS 是隐形杀手：节点都在跑却「没反应」，第一反应永远是——话题名对不对、QoS 匹不匹配，而不是去重写逻辑。
- 先跑通再优化：字段不够就加（加字段向后兼容），别在一开始塞满用不上的域；系统能跑起来，比「设计完美但跑不起来」重要一百倍。
把这三句和前面的决策树一起用，你写的第一个 ROS 2 接口就能直接进生产，而不是写完就推倒重来。验证也很简单：接口编译过、节点能 echo 到数据、调用能拿到回执，这三步绿灯，接口就算站住了。
接口文档：写给三个月后的自己
接口写完了，最容易烂在「没人记得它为什么这样设计」。花五分钟给每个接口写三行注释，省下三个月后重读代码的半天：
- 这一栏（字段）当初是为了解决什么场景加的——比如「remaining_distance 是为了让监控端预判到达时间」。
- 哪些字段是兼容可加的，哪些一动就破坏性变更——用注释标清楚，后人改的时候就不敢乱动。
- 谁在依赖它——列上订阅 / 调用方的包名，改之前先去戳对方。
ROS 2 的接口文件本身就是文档载体：msg / srv / action 顶部的注释会进生成的 API 文档。把「为什么」写进注释，比写进微信聊天记录靠谱一万倍。
上手前最后记住的稳定性红线
还有一点常被忽略：自定义接口一旦被别的包依赖，就不要轻易删字段或改类型。如果必须大改，用 v1 / v2 两个包共存过渡，给老节点留退路。接口的稳定性，比接口的「好看」重要得多。
写代码时回头翻这张表，比从头读快：
| 知识点 | 要点 |
|---|---|
| 自定义接口 | msg/srv/action 分别定义，rosidl_generate_interfaces 生成 |
| C++ 节点 | rclcpp + action server + 多线程（巡检放子线程） |
| Python 节点 | rclpy + ActionClient + 异步回调（spin 丢子线程） |
| Launch 文件 | 参数声明、条件启动、延迟启动、remappings 改名 |
| 调试工具 | topic echo/hz/delay、service call、action send_goal、rqt_* |
| 常见坑 | QoS 不匹配、消息类型引用格式、环境变量未 source |
下一篇我们将进入 ROS 2 Nav2 自主导航实战，在真实 SLAM 环境中实现自主建图与导航避障。
本文采用 署名-非商业性使用-相同方式共享 4.0 国际 许可协议，转载请注明出处。
