前阵子有人问我ROS2 入门到底做什么小项目最合适。我的答案往往不是打印 Hello World也不是把官方的节点例程跑一遍而是一个特别朴实的目标一个节点去采集图像另一个节点去做推理两个节点之间用一条话题把数据串起来。别小看这个组合它几乎覆盖了机器人开发里最常见的感知链路也是你从会写节点到能搭系统之间最近的一步。这篇文章就把这个案例从零讲一遍。我会用一个 Python 写的采集节点去读摄像头把画面转成 ROS2 的图像消息发到话题上另一个 Python 节点订阅这个话题用训练好的检测模型推理画出框再把标注后的图像发布出去。整个流程不需要机器人本体一台带摄像头的电脑就能跑非常适合刚把 colcon 和 rclpy 跑通的人。读完之后你手头会有一套能直接跑起来的感知 Demo也清楚以后往真机搬的时候哪些地方必须调整。1. 为什么这个采集 推理Demo值得从零写一遍1.1 两个节点到底各自负责什么很多人的第一反应是既然只是做个检测 Demo为什么不能把读摄像头和跑模型写在同一个脚本里能跑但这是典型的短期省事、长期难受。采集节点和推理节点拆开本质上是把数据来源和数据处理这两件事解耦了。采集节点的职责只有一个从硬件拿到图像转成标准消息发布到话题上。它不应该关心下游是谁在订阅、订阅了几个人、推理结果长什么样。推理节点的职责则相反订阅图像消息做预处理、推理、后处理把结论发出去。它也不应该关心图像是从 USB 摄像头来的还是从录像回放来的或者是另一台机器通过网络传过来的。这个拆分在真实机器人项目里价值非常大。我今天想换摄像头只需要换采集节点明天想换一个更准的模型只需要改推理节点后天想把采集放到另一个计算平台话题两端的代码几乎不用动。甚至可以用ros2 bag play把之前录的图像数据重放出来喂给同一个推理节点做离线测试。一个节点一个职责整条链路的调试难度会断崖式下降。1.2 数据在哪里交接话题、消息类型、QoS两个节点之间唯一的约定就是话题。这里我们用两条话题采集节点往camera/raw上发布原始图像推理节点订阅它处理完之后往camera/annotated上发布标注图像检测到的目标信息再通过inference/detections发布成结构化消息。话题上跑的消息类型决定了上下游必须怎么对接。图像消息用的是sensor_msgs/Image它里面包含宽、高、编码方式、每行字节数以及一长串按顺序排列的像素数据。用 CvBridge 转换是最常见的做法后面会细讲。目标检测结果我会定义一个小消息DetectionBox包含类别名、置信度、四个角坐标这样下游不管是做统计、做跟踪还是接可视化都能直接读到结构化数据而不是自己去解析画在图像上的框。还有一个新手极容易忽略的概念叫 QoSQuality of Service。它决定了消息的可靠性、队列深度、消息是不是只保留最新一帧这些行为。图像话题和猛发大量日志的话题对 QoS 的需求是不一样的。这块我会在采样节点和推理节点的实现里分别说明因为很多看起来订阅了但就是收不到数据的诡异问题最后都出在 QoS 不匹配上。2. 工程初始化包结构、依赖与容易踩的版本坑2.1 创建功能包与消息包先建两个功能包一个放两个节点的 Python 包image_inference_demo一个放自定义消息的 CMake 包detection_msgs。用命令创建 Python 包ros2 pkg create image_inference_demo --build-type ament_python --dependencies rclpy sensor_msgs cv_bridge std_msgs创建消息包ros2 pkg create detection_msgs --build-type ament_cmake mkdir -p detection_msgs/msg在detection_msgs/msg/DetectionBox.msg里写string class_name float32 confidence int32 x_min int32 y_min int32 x_max int32 y_max然后打开detection_msgs/CMakeLists.txt确认有这两行find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} msg/DetectionBox.msg )建完包之后先编译一次确认消息接口能正常生成colcon build --packages-select detection_msgs source install/setup.bash用ros2 interface show detection_msgs/msg/DetectionBox能看到字段就说明消息包 OK 了。之后 Python 节点里就能直接from detection_msgs.msg import DetectionBox来引用它。2.2 依赖安装与Python环境对齐在跑这个 Demo 之前机器上需要有 rclpy、CvBridge、OpenCV、ONNX Runtime 这几样。前两样通常跟着 ROS2 桌面版一起装了但 CvBridge 对应的命令行工具有时没装全我习惯先补一遍sudo apt install ros-humble-cv-bridge ros-humble-image-transport pip install onnxruntime opencv-python这里最容易翻车的是 OpenCV 的来源冲突。CvBridge 是调用系统里某个 OpenCV 版本编译出来的而你通过 pip 又装了一个独立的opencv-python两个 OpenCV 同时存在轻则 import 时报警告重则在 CvBridge 做图像转换时直接崩溃。我踩过的具体表现是cv_bridge能导入但imgmsg_to_cv2每次调用都报奇怪的段错误。如果你的环境已经出现了类似症状最稳的处理是不要混着来要么直接用系统包管理装的python3-opencv要么确保cv_bridge和 pip 的 OpenCV 是同一个大版本比如都是 4.x并且把 pip 装在系统 Python 而不是某个虚拟环境里。再补充一句如果是在没有图形界面的机器上跑建议装opencv-python-headless而不是opencv-python否则很可能遇到libGL.so.1: cannot open shared object file这种报错。2.3 setup.py入口点配置Python 包里的节点最终要能被ros2 run和 launch 启动靠的是setup.py里的entry_points。在image_inference_demo/setup.py中配置两个可执行入口entry_points{ console_scripts: [ camera_pub_node image_inference_demo.camera_pub_node:main, inference_node image_inference_demo.inference_node:main, ], },对应的camera_pub_node.py和inference_node.py需要放在image_inference_demo/目录下并且每个文件都要有一个main()函数。这样在colcon build之后系统会自动生成camera_pub_node和inference_node两个可执行命令。需要注意如果改了入口点必须重新colcon build才会生效但如果只是改节点内部的 Python 代码用--symlink-install就可以免去重复编译colcon build --packages-select image_inference_demo --symlink-install3. 采集节点实现读摄像头、转Image消息、控制发布频率3.1 OpenCV读帧与CvBridge转换细节采集节点的主循环很直白定时去读摄像头的一帧转成sensor_msgs/Image发布到camera/raw。核心代码如下import rclpy from rclpy.node import Node import cv2 from sensor_msgs.msg import Image from cv_bridge import CvBridge class CameraPublisher(Node): def __init__(self): super().__init__(camera_publisher) self.publisher self.create_publisher(Image, camera/raw, 10) self.bridge CvBridge() self.cap cv2.VideoCapture(0) if not self.cap.isOpened(): self.get_logger().error(camera open failed) self.timer self.create_timer(1.0 / 30.0, self.timer_callback) def timer_callback(self): ret, frame self.cap.read() if not ret: self.get_logger().warn(read frame failed) return msg self.bridge.cv2_to_imgmsg(frame, encodingbgr8) msg.header.stamp self.get_clock().now().to_msg() msg.header.frame_id camera_link self.publisher.publish(msg) def main(argsNone): rclpy.init(argsargs) node CameraPublisher() rclpy.spin(node) node.destroy_node() rclpy.shutdown()cv2_to_imgmsg的工作是把 OpenCV 的numpy数组转换成图像消息。这里最容易迷糊的是encodingbgr8OpenCV 默认读进来的是 BGR 排列sensor_msgs/Image里的encoding字段必须如实写清楚。后面推理节点取回图像时如果指定desired_encodingbgr8CvBridge 就不会做多余的通道重排少一次拷贝也少一次出错机会。3.2 header时间戳到底该用什么时候很多初学者会直接把header.stamp写成发布时刻的时间这在Demo里看着没问题但在真正的传感器系统里是个隐患。图像时间戳应该尽量反映这一帧是何时被采集的而不是这一帧是何时被发出去的。假如采集节点处理慢、发送队列堵了发布时刻和采集时刻的差距会被拉大下游如果要做多传感器时间同步拿到的就是错乱的时间关系。对于 USB 摄像头的裸驱动来说我们拿不到硬件级的曝光时刻实际做法一般是用cap.read()返回之后立刻取时钟把它当作采到帧的时刻。像我上面代码里msg.header.stamp self.get_clock().now().to_msg()是放在cap.read()成功之后立即执行的这个误差对大多数 Demo 完全够用。真正要求高的场景会用相机驱动 SDK 自带的时间戳甚至让相机在硬件层面打时间标签。写节点的习惯是frame_id也要填比如camera_link即使现在没有 TF 树提前养成习惯总没坏处。3.3 话题命名与QoS策略的选择话题命名我用的是camera/raw而不是image_raw因为 ROS2 社区里许多相机驱动默认话题就叫image_raw我加个camera/前缀是为了以后一台机器上接了多个摄像头时能靠前缀区分比如left_camera/raw、right_camera/raw。不追求和社区默认话题名对齐的 Demo 里叫什么都行关键是发布端和订阅端字符串完全一致。QoS 这里值得多说一句。图像是典型的最新帧才有意义数据你不关心之前每一帧是否被可靠送达只关心能不能尽快拿到最新画面。所以相机发布端很多驱动会用BEST_EFFORT 短队列。但有个坑ROS2 的默认 QoS 是RELIABLE如果你的订阅端沿用了默认值reliabilityReliable而发布端设成了BEST_EFFORT两边 QoS 不兼容订阅端根本不会收到数据日志里还会出现 compatibility 相关的警告。我在这个 Demo 里为了让新手少踩这个坑发布端和订阅端都用默认的RELIABLE队列深度设 10。这对 30Hz、640x480 的图像流量来说完全扛得住而且两端配置一致最不容易出问题。等你想追求低延迟、丢弃旧帧时再一起改回BEST_EFFORTdepth1。记住一个原则修改 QoS 要两端一起改别让一边默认、一边自定义。4. 推理节点实现订阅图像、模型推理、把结果发出去4.1 预处理resize、normalize、像素格式推理节点订阅camera/raw回调里第一件事是把图像消息换回 numpy 数组frame self.bridge.imgmsg_to_cv2(msg, desired_encodingbgr8)这里desired_encodingbgr8告诉 CvBridge我就是要 BGR 格式不用转换如果相机是彩色图这一步几乎就是零拷贝操作。拿到 BGR 的 numpy 数组之后就要做模型输入前的预处理。最常见的检测模型输入规格是 640x640 的 RGB 图需要归一化到 0~1再从 HWC 排布变成 CHW最后加一个 batch 维度。因为原图不一定是正方形我习惯用 letterbox 方式做缩放按比例缩放到目标尺寸四周补灰边这样不会因为直接拉伸变形而破坏目标的宽高比。代码如下def preprocess(self, frame): h, w frame.shape[:2] target 640 scale target / max(h, w) nw, nh int(round(w * scale)), int(round(h * scale)) resized cv2.resize(frame, (nw, nh)) canvas np.zeros((target, target, 3), dtypenp.uint8) pad_x (target - nw) // 2 pad_y (target - nh) // 2 canvas[pad_y:pad_y nh, pad_x:pad_x nw] resized rgb cv2.cvtColor(canvas, cv2.COLOR_BGR2RGB) blob rgb.astype(np.float32) / 255.0 blob blob.transpose(2, 0, 1)[None] return np.ascontiguousarray(blob), scale, (pad_x, pad_y)np.ascontiguousarray这一步很容易被忽略。transpose之后数组的内存布局不是连续的很多推理引擎的输入要求内存连续不转可能会报错或得到不可预期的结果。预处理里的scale和pad也必须记住因为推理得到的坐标是在 640x640 的画布坐标系里的要还原到原始图像坐标系必须减掉灰边、除以缩放比例。4.2 ONNX推理与后处理的取舍模型我用的是 ONNX Runtime因为它跨平台、部署简单而且 CPU 上也能跑得动轻量模型。加载模型只需要在节点初始化时创建一次推理会话self.session ort.InferenceSession( model.onnx, providers[CPUExecutionProvider])在回调里调用推理blob, scale, pad self.preprocess(frame) out self.session.run(None, {self.session.get_inputs()[0].name: blob})[0]ONNX 模型的输出格式和具体网络结构强相关。我这里以常见的检测头为例输出形状是[1, 84, 8400]其中前 4 行是每个候选框的 xywh后面 80 行是类别得分。后处理的简化版可以这样写def postprocess(self, out, scale, pad): preds np.squeeze(out) # [84, 8400] boxes_xywh preds[:4].T scores preds[4:].T confs scores.max(axis1) class_ids scores.argmax(axis1) keep confs 0.4 bx boxes_xywh[keep] cf confs[keep] ci class_ids[keep] cx, cy, bw, bh bx.T x1 (cx - bw / 2 - pad[0]) / scale y1 (cy - bh / 2 - pad[1]) / scale x2 (cx bw / 2 - pad[0]) / scale y2 (cy bh / 2 - pad[1]) / scale results [] for c, conf, x, y, xx, yy in zip(ci, cf, x1, y1, x2, y2): results.append((self.class_names[c], float(conf), int(x), int(y), int(xx), int(yy))) return results注意我这里为了讲清楚主干逻辑省掉了 NMS非极大值抑制。实际项目中同一个目标可能被框出多个重叠候选需要按置信度排序、抑制掉低分的重叠框否则可视化会出现一堆叠在一起的框。建议后处理单独封装成一个函数和预处理、推理解耦换模型时只改这一层。4.3 结果可视化与自定义消息发布推理完成之后我在图像上画矩形框并发布标注图同时把结构化检测结果发布出去annotated frame.copy() for class_name, conf, x, y, xx, yy in results[:3]: cv2.rectangle(annotated, (x, y), (xx, yy), (0, 255, 0), 2) cv2.putText(annotated, f{class_name} {conf:.2f}, (x, y - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2) annotated_msg self.bridge.cv2_to_imgmsg(annotated, encodingbgr8) annotated_msg.header msg.header self.ann_pub.publish(annotated_msg)annotated_msg.header msg.header这行是重点。标注图的原始图像来自于某一条消息它的时间戳和 frame_id 应该保持一致这样下游做同步时能把标注图和原图对应起来。如果重新取一个当前时间原图和标注图的时间戳就对不上了。结构化结果我用DetectionBox消息发到inference/detections。这里可以一条消息发一个目标也可以定义数组消息一次性把整帧的检测结果全发出去。Demo 里我选择发当前帧置信度最高的那个目标写起来直观下游订阅后直接拿到画面里最可能是目标的东西在哪已经足够去接一个简单动作节点了if results: cls_name, conf, x, y, xx, yy res[0] det DetectionBox() det.class_name cls_name det.confidence float(conf) det.x_min x det.y_min y det.x_max xx det.y_max yy self.det_pub.publish(det)5. 启动联调launch文件、日志排查、链路验证5.1 用launch把两个节点跑起来两个节点都写完最方便的不是开两个终端分别ros2 run而是用一个 launch 文件把整条链路一次性拉起来。在image_inference_demo/launch目录下新建demo_launch.pyfrom launch import LaunchDescription from launch_ros.actions import Node def generate_launch_description(): return LaunchDescription([ Node(packageimage_inference_demo, executablecamera_pub_node, outputscreen, parameters[{camera_index: 0}]), Node(packageimage_inference_demo, executableinference_node, outputscreen, parameters[{model_path: model.onnx}]), ])outputscreen会把节点的get_logger()日志打印到当前终端调试时非常有价值。节点里要用declare_parameter接收这些参数比如self.declare_parameter(model_path, model.onnx)然后通过self.get_parameter(model_path).value读取。把摄像头编号、模型路径这类环境相关的东西作为参数提出来是 Demo 到项目过程中很重要的一步否则换个摄像头、换台机器就要改源码。启动前记得编译并 sourcecolcon build --packages-select image_inference_demo detection_msgs --symlink-install source install/setup.bash ros2 launch image_inference_demo demo_launch.py5.2 从topic hz到rqt_image_view的验证三板斧启动之后不要急着看画面先用命令行确认链路通畅我习惯按这个顺序排查ros2 topic list ros2 topic info /camera/raw ros2 topic hz /camera/raw ros2 topic hz /camera/annotatedtopic info能看到发布者和订阅者数量如果/camera/raw显示有两个以上的发布者/订阅者说明节点连接成功。topic hz如果显示频率在 30Hz 附近说明采集链路没问题/camera/annotated的频率会低一些取决于模型推理耗时但如果掉到 2Hz 以下就要回头检查推理是不是把回调整个阻塞了这点下一节专门讲。最后用图工具直接看画面ros2 run rqt_image_view rqt_image_view在图像视图里选择/camera/annotated话题如果能看到带框的画面说明从采集、发布、订阅、推理、再发布这整条链路全部跑通了。如果画面是黑的优先检查摄像头的encoding是不是bgr8以及是否真的读到了非空帧。5.3 我实际遇到过的三个奇葩故障第一次把所有代码组织到完整工程里跑通之前我遇到过三个特别有代表性的坑写在这里给大家省时间。第一个是 QoS 不匹配。当时我把发布端设成了BEST_EFFORT但订阅端用的是默认的RELIABLE结果ros2 topic info显示订阅者存在topic hz却始终是 0日志里有明显的不兼容警告。这个问题的排查思路就是先ros2 topic info看连接状态再确认两端 QoS 策略一致不要想当然。第二个是图像时间戳全为 0。因为我在构造Image消息时没有给header.stamp赋值rqt_image_view还是能显示画面但下游做时间同步的节点全部失效看起来就是一切正常但就是没有输出。这个问题提示我可视化正常不代表消息字段完整订阅端一定要检查 header 是否有效。第三个是 OpenCV 版本混装导致的崩溃这个我在依赖安装一节就写了。那种偶发段错误最折磨人一开始完全想不到是 CvBridge 和 pip 里两个 OpenCV 打架最后把所有 pip 装的 opencv 卸掉、改成系统包才稳定。凡是 CvBridge 相关莫名其妙的崩溃优先怀疑这一条。6. 从Demo到能用线程模型与性能优化的方向6.1 为什么不能让回调在推理期间闲着等Demo 能跑起来之后很多人会问为什么推理节点一忙起来topic hz /camera/annotated的频率就很不稳定因为默认的 rclpy 节点用的是单线程执行器所有回调都在同一个线程里排队执行。图像回调一旦开始跑模型在推理结束之前下一帧的订阅回调不会被调用。摄像头还在以 30fps 发布但回调队列里的消息开始积压表现出来就是推理节点很卡掉帧。最简单的解法是换成多线程执行器from rclpy.executors import MultiThreadedExecutor rclpy.init(argsargs) node InferenceNode() executor MultiThreadedExecutor() executor.add_node(node) executor.spin()但多线程执行器只是让回调并行执行推理回调如果耗时 200ms它还是会占用一个线程 200ms。更稳妥的做法是在节点内部把接收图像和推理拆开回调里只把帧放进一个有界队列后台工作线程负责取帧、推理、发布。队列有界并且满了就丢最旧的帧这样即使推理再慢内存不会无限上涨看到的总会是最新的画面。6.2 降分辨率、异步推理、内部零拷贝性能优化先看数据流。采集节点把 1920x1080 的图发出来推理节点又要先 resize 到 640x640中间白白传了 3 倍的数据量。对于纯推理 Demo直接把采集分辨率降到 640x480或者发布端在发布前先做一次缩放到模型输入尺寸附近延迟能下降不少。注意发布端缩放的图回调里再缩放到模型输入时的质量损失会小一些。推理本身也可以再优化。ONNX Runtime 在 CPU 上有线程池session.run默认会尽量用满 CPU如果模型稍大可以在 launch 参数里把providers换成 GPU 执行器但要保证推理节点所在机器有对应硬件。异步推理是个更进阶的方向把输入交给推理引擎后不阻塞等待先处理别的消息完成后通过回调拿结果。这在 ROS2 节点里实现起来要引入额外的线程安全机制对这个 Demo 来说有点重但如果你后续要做高速摄像头的实时检测建议提前研究。零拷贝方面常规做法是消息在进程间传递时至少做一次复制Python 节点尤其明显。ROS2 提供了use_intra_process_comms选项在 launch 中开启后同一进程内的节点之间可以走内部通信减少序列化开销。不过 Python 节点的收益不如 C 节点明显而且配置不当会引入额外的复杂度Demo 阶段不建议折腾。6.3 下一步可以扩展的几种玩法这套采集 推理链路跑通之后后面能加的东西很多。我建议按这个顺序尝试先把采集节点改成从录制的 bag 包读取图像这样你可以在没有摄像头的情况下复现任何测试场景再往推理节点下游挂一个跟踪节点让检测框有稳定的 ID而不是每一帧重新识别最后让推理结果接一个简单的动作服务比如当检测到目标靠近时让另一个节点发布运动指令。每加一个节点链路就多一个调试维度但基础结构还是这一条话题通路。如果想把推理节点单独部署到另一台更强算力的机器上拓扑也不用变camera/raw的发布端和推理订阅端通过局域网通信话题名、消息类型、QoS 三样保持一致节点代码一行都不用改。这种采集端和推理端可分离的能力正是当初坚持拆两个节点带来的最大红利。按这个思路继续往前推进你会发现自己已经不是在写单一脚本而是在搭一套真正的机器人感知系统了。