本文详细介绍了在ROS 2（Humble）环境下，基于usb_cam驱动采集图像、使用OpenCV进行HSV颜色空间分割与轮廓分析，构建颜色识别服务端/客户端的完整流程。内容涵盖USB摄像头安装与调试、相机标定消除畸变、cv_bridge图像格式转换、红/绿/蓝三色目标检测逻辑（含掩码合并、腐蚀膨胀、质心计算），以及ROS 2服务接口（ColorRecognition.srv）的定义、服务端响应与客户端调用实现。
ROS 2机器人系统开发实践（视频教学版） 梁伟、李园奇、李海慧 清华大学出版社【行情 报价 价格 评测】-京东
【新书介绍】《ROS 2机器人系统开发实践（视频教学版）》-CSDN博客
在机器人应用中，机器视觉是机器人感知周围环境的一种重要能力。机器人可以通过摄像头采集图像，使用OpenCV处理采集到的图像，并通过ROS 2传输数据或封装为服务进行通信与控制。本节将通过一个完整的ROS 2机器视觉示例，介绍如何安装摄像头驱动、如何进行颜色识别服务的封装，以使机器人具备识别指定颜色目标的能力。
4.3.1 安装usb_cam包
在ROS 2生态中，usb_cam是机器人系统常用的USB摄像头驱动包，支持大部分免驱USB摄像头（例如，笔记本内置摄像头、外接USB摄像头等）。该功能包的主要作用是将摄像头硬件采集到的图像转换成ROS 2话题，以供其他节点订阅使用。本小节将介绍usb_cam包的安装方法及安装过程中的问题处理等相关知识。
1. APT安装和源码安装
对于Humble、Jazzy等ROS 2长期支持版本，我们可以直接通过APT包管理器安装usb_cam，命令如下所示：
sudo apt update # 更新软件源
sudo apt install ros-humble-usb-cam # 安装usb_cam
运行以上命令后，我们输入管理员密码开始更新软件源和安装软件包，等待完成安装即可。完成安装后，可以验证usb_cam是否安装成功，命令如下所示：
ros2 pkg list | grep usb_cam #查看ROS 2软件列表并筛选usb_cam
筛选后，如果终端中输出usb_cam，则说明安装成功，如图4.13所示。
图4.13 检查usb_cam包是否安装成功
如果APT仓库中无对应版本的usb_cam包，我们可以从GitHub中获取源码进行编译安装，命令如下所示：
# 进入工作空间下的src目录中
cd src
# 根据ROS版本复制对应的usb_cam源码
git clone https://github.com/ros-drivers/usb_cam.git -b foxy-devel
# 回到工作空间根目录，安装依赖
cd ..
rosdep install --from-paths src --ignore-src -r -y
# 编译usb_cam包
colcon build --packages-select usb_cam
# 更新环境变量
source install/setup.bash
我们在复制usb_cam源码时，需要指定当前对应的ROS 2版本分支。例如，在上述命令中复制的是foxy-devel版本的usb_cam源码。
备注：由于本书从第1章开始讲解即安装的ROS 2版本是Humble，因此，为了方便读者学习和理解，请尽量安装此版本。
2. 安装异常处理
如果我们在安装usb_cam包时出现常见问题，可以使用以下处理方法进行解决：
（1）如果终端中出现提示“E: 无法定位软件包ros-humble-usb-cam”，则表示ROS 2软件源没有配置正确，使用命令sudo apt update更新软件源或者改为源码编译即可。
（2）如果源码编译报“缺少依赖”的错误信息，则表示缺少camera_info_manager、image_transport等依赖项。此时通过执行命令rosdep install --from-paths src --ignore-src -r -y自动安装依赖即可，如图4.14所示。
图4.14 自动安装依赖
本小节主要通过APT安装和源码安装两种安装方式，向读者讲解了安装usb_cam软件包的过程，并且对安装过程中容易遇到的问题及处理方法进行了介绍。通过本小节的学习，读者可以根据自己的ROS 2版本选择适合的安装方式来安装usb_cam软件包。
4.3.2 启动USB摄像头
安装usb_cam软件包后，便可以启动其核心节点usb_cam_node_exe，使摄像头开始采集图像并发布ROS 2话题。本小节主要向读者讲解摄像头节点的启动、查看摄像头发布话题以及使用rqt工具实时查看摄像头数据等相关的操作及命令。
1. 启动摄像头节点
在ROS 2中安装usb_cam包后，可以在终端中使用命令ros2 run启动摄像头节点usb_cam_node。其默认情况下将调用设备/dev/video0，即当前系统中的第一个USB摄像头设备。启动命令如下所示：
# 启动usb_cam_node
ros2 run usb_cam usb_cam_node_exe
摄像头节点启动成功后，终端将会输出节点信息。其中，包括摄像头设备路径、分辨率、帧率等相关的信息，如下所示：
[INFO] [1759101000.123456789] [usb_cam_node]: Start streaming...
[INFO] [1759101000.124567890] [usb_cam_node]: Using video device: /dev/video0
[INFO] [1759101000.125678901] [usb_cam_node]: Resolution: 640x480, FPS: 30
[INFO] [1759101000.126789012] [usb_cam_node]: Publishing image to topic: /image_raw
备注：如果使用虚拟机运行Ubuntu系统，则可能会出现摄像头节点获取数据延迟导致超时退出的情况。
2. 查看摄像头节点发布的话题
启动摄像头节点usb_cam_node_exe后会发布多个与图像相关的话题，通过以下命令可以查看这些话题：
ros2 topic list | grep image #查看与图像相关的话题列表
以上命令的主要作用是查看ROS 2中所有的话题列表，并从列表中筛选出与图像image相关的话题，如图4.15所示。
图4.15 筛选与图像相关的话题列表
如图4.15所示，核心话题为/image_raw，即原始图像数据，具体如下所示：
/image_raw # 原始图像，未压缩，数据量大，适合后续处理
/image_raw/compressed # 压缩图像，JPEG格式，数据量小，适合传输
/image_raw/compressedDepth # 深度图像压缩版（若摄像头支持深度采集）
/image_raw/theora # Theora格式压缩图像（开源压缩格式）
备注：在查看筛选图像相关的话题列表时，一定要保证摄像头节点处于运行状态，否则无法获取图像话题/image_raw。
3. 实时查看图像
在ROS 2中提供了可视化工具rqt_image_view，可以用来查看图像话题数据（即摄像头图像），从而验证摄像头是否正常工作，工具安装和启动命令如下所示：
# 安装rqt_image_view
sudo apt install ros-humble-rqt-image-view
# 启动图像查看工具
ros2 run rqt_image_view rqt_image_view --force-discover
以上命令启动后，将会打开rqt图形窗口，在窗口左上方的话题下拉菜单中选择相应的图像话题，即可显示摄像头实时采集的图像，如图4.16所示。
图4.16 选择图像话题
备注：如果使用虚拟机运行Ubuntu系统，按照上述操作没有在rqt图形窗口中显示摄像头画面，则打开虚拟机菜单“虚拟机设置”中的“USB控制器”选项，选择USB兼容性为USB 3.1即可，如图4.17所示。
图4.17 选择虚拟机的USB兼容性
4. 异常情况处理
（1）启动节点后无图像，终端提示错误信息Could not open video device，则表示摄像头未连接或者硬件故障。重新插拔摄像头后，执行命令ls /dev/video*可以查看设备是否存在，正常情况下会显示当前的设备及序号，如图4.18所示。
图4.18 查看当前连接的设备
（2）如果操作设备权限不足，可以执行命令sudo chmod 666 /dev/video0增加读写权限，或将当前用户添加到video组，命令如下所示：
sudo chmod 666 /dev/video0 # 临时增加读写权限
sudo usermod -aG video $USER # 执行完成后需注销重新登录，永久生效
上述两个命令，任选其一即可。
（3）若rqt窗口无画面，且终端输出Topic /image_raw not available，则表示摄像头节点usb_cam_
node_exe未启动、在rqt窗口中选择的话题错误，或者虚拟机的USB兼容性不适合。排查步骤如下：首先确认摄像头节点是否启动，然后检查rqt窗口中选择的话题名称是否与ros2 topic list显示的图形话题名称一致，最后检查虚拟机设置中的USB兼容性是否为USB 3.1。
本小节主要讲解了如何启动摄像头节点并通过rqt可视化窗口查看图像话题。通过本小节的学习，读者应掌握启动摄像头过程中常见问题的处理方法。
4.3.3 相机标定及使用
通常情况下，普通USB摄像头会存在“光学畸变”的情况。例如，用摄像头拍门框时，边缘会呈现弧形的情况。这种畸变会导致图像中的“实际坐标”与“像素坐标”不匹配，严重影响机器视觉识别的效果。本小节将介绍相机标定的原理及方法。
相机标定的原理是通过诸如“棋盘格”等标准图案，计算出摄像头的焦距、主点坐标等内参矩阵和径向畸变、切向畸变等畸变系数，待使用时再通过这些参数对实际图像进行畸变矫正，从而得到无畸变的正常图像。
我们可以在ROS官方网站上下载棋盘格的标准模板checkerboard.pdf，打印后粘贴在硬纸板上，并确保平整、无褶皱。用尺子测量相邻两个黑白格子的边长并记录此数据。
备注：测量的相邻格子的边长数据一定要准确，后续标定将会使用到该边长数据。
在ROS 2中提供了包camera_calibration专门用于相机标定，安装命令如下所示：
sudo apt install ros-humble-camera-calibration
相机标定的包安装完成后，启动摄像头节点usb_cam_node_exe，并且重新打开一个终端，启动相机标定节点cameracalibrator，两个终端输入命令分别如下所示：
ros2 run usb_cam usb_cam_node_exe #终端1中输入
# 标定格式说明
# --size棋盘格内角数。例如8×6，指横向8个内角、纵向6个内角，不是棋盘的格子数
# --square相邻格子边长（单位：米，0.02表示20mm）
# --ros-args -r image:=/image_raw将标定节点的输入话题重映射为usb_cam的图像话题
ros2 run camera_calibration cameracalibrator --size 8x6 --square 0.02 --ros-args -r image:=/image_raw #终端2中输入
相机标定节点启动后会弹出标定窗口，其中窗口左侧部分将实时显示摄像头画面。窗口上侧部分主要用于操作标定进度的进度条，窗口右侧部分有CALIBRATE、SAVE、COMMIT三个按钮并且初始状态为灰色，如图4.19所示。
图4.19 相机标定窗口
此时我们需要将制作好的棋盘格移动到实时显示窗口的不同位置和角度，使相机标定节点至少能够采集10幅有效图像，使上侧的标定进度条填满并且按钮CALIBRATE变为绿色即可。移动棋盘格的方式如下所示。
（1）位置覆盖：将棋盘格移动到图像的左上角、右上角、左下角、右下角、中心。覆盖范围包括图像的边缘和中心。
（2）角度覆盖：倾斜棋盘格。例如，左高右低、前高后低等操作，以确保棋盘格在不同倾斜角度下均能够被检测到。
（3）大小覆盖：以摄像头为参考点，将棋盘格拉近或者拉远，使棋盘格在图像中呈现不同大小。
备注：每次移动棋盘格后，如果图像窗口显示绿色线条框选棋盘格，则说明检测成功，会自动记录该图像。如果未框选，则我们需要调整棋盘格位置或角度，确保光线充足，避免反光或过暗。
当按钮CALIBRATE变为绿色后，我们就可以单击该按钮开始标定，标定过程大约10~30秒，在标定过程中我们不能关闭窗口。标定完成后，窗口中会显示“矫正前”和“矫正后”的图像对比效果，我们可以直观地看到棋盘格畸变消除的对比效果。
然后，单击SAVE按钮，将标定结果（包括内参矩阵、畸变系数）保存到压缩包/tmp/calibrationdata.tar.gz中，我们可以解压后查看详细的标定参数。单击COMMIT按钮，将相机标定结果写入ROS 2默认的相机参数文件/.ros/camera_info/head_camera.yaml中，用于重新启动摄像头节点usb_cam_node_exe时加载。
标定完成后，重新启动摄像头节点usb_cam_node_exe，节点将会自动加载/.ros/camera_info/head_
camera.yaml中的参数，并对摄像头图像话题/image_raw发布的图像数据进行畸变矫正。
本小节主要介绍了相机标定的作用原理、标定的方法以及标定过程中的一些注意事项。我们在实际进行相机标定时，尽量使用原生Ubuntu系统，避免因虚拟机原因造成摄像头启动失败、无法显示图像以及相机标定失败等问题出现。
4.3.4 集成OpenCV功能
OpenCV（Open Source Computer Vision Library）是开源的计算机视觉库，支持图像滤波、边缘检测、颜色识别、目标跟踪等功能。在ROS 2中，我们可以通过工具cv_bridge将ROS 2的图像消息sensor_msgs/Image转换成OpenCV的图像格式cv::Mat，以便使用OpenCV对摄像头图像数据进行处理。本小节将介绍OpenCV Python的安装、图像转换工具的使用，并通过一个实例实现实时显示图像的灰度化效果。
1. 软件包及工具安装
首先安装OpenCV Python版，Ubuntu系统可以直接安装和验证，命令如下所示：
sudo apt install python3-opencv
python3 -c "import cv2; print(cv2.__version__)" #验证安装是否成功
如果终端验证安装成功，将输出OpenCV Python的版本号。然后安装ROS 2图像转换工具包cv_bridge，命令如下所示：
sudo apt install ros-humble-cv-bridge
2. 核心转换逻辑
转换工具包中的类cv_bridge.CvBridge主要提供了两个核心方法，用于实现ROS 2图像主题消息与OpenCV图像的相互转换，具体方法的功能与参数说明如表4.7所示。
表4.7 图像转换工具中的核心方法说明
| 方 法 | 功能描述 | 关键参数说明 |
| imgmsg_to_cv2(img_msg, desired_encoding) | ROS 2图像主题消息→OpenCV图像 | desired_encoding：目标格式。例如bgr8=OpenCV表示默认格式，rgb8=ROS表示常用格式 |
| cv2img_to_imgmsg(cv_image, encoding) | OpenCV图像→ROS 2图像主题消息 | encoding：表示图像格式，须与OpenCV图像格式匹配，例如bgr8 |
备注：我们使用cv2img_to_imgmsg转换图像时，要注意OpenCV默认使用的是BGR颜色空间，按照蓝绿红通道顺序，而ROS 2则默认使用的是RGB颜色空间，所以转换时我们需要指定正确的encoding（例如bgr8），否则将会出现诸如红色显示为蓝色等颜色错乱的情况出现。
3. 图像灰度化处理示例
工具包和库安装完成后，我们可以接着编写一个Python版本的ROS 2节点，用于订阅摄像头节点发布的话题/image_raw，接收图像数据，并将采集的彩色图像转换为灰度图像后实时显示。
在工作空间chapter4_ws的功能包目录src/service_paramadd_demo/scripts下新建一个Python源文件image_gray.py，代码如下所示：
#!/usr/bin/env python3
# 导入依赖模块
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image # ROS 2图像主题消息类型
import cv2 # 导入OpenCV库
from cv_bridge import CvBridge # 图像格式转换工具
class ImageGrayNode(Node):
def __init__(self):
# 初始化节点并命名为image_gray_node
super().__init__("image_gray_node")
# 初始化CvBridge（用于ROS与OpenCV图像转换）
self.bridge = CvBridge()
# 订阅摄像头图像话题（/image_raw）
self.image_sub = self.create_subscription(
Image, "/image_raw",self.image_callback, # 图像处理回调函数
10 # 消息队列大小（避免消息堆积）
)
# 打印启动日志
self.get_logger().info("图像灰度化节点已启动！")
self.get_logger().info("提示:按ESC键关闭图像窗口")
# 图像处理回调函数（接收ROS图像消息，转换为OpenCV处理）
def image_callback(self, msg):
try:
# ROS图像消息→OpenCV图像（指定格式为bgr8，匹配OpenCV默认格式）
cv_image_color = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8")
# OpenCV处理：彩色图像 →灰度图像（调用cvtColor函数，参数CV_BGR2GRAY）
cv_image_gray = cv2.cvtColor(cv_image_color, cv2.COLOR_BGR2GRAY)
# 显示处理后的灰度图像（窗口标题为Gray Image）
cv2.imshow("Gray Image", cv_image_gray)
# 按键检测：按ESC键（ASCII码27）关闭窗口并退出节点
key = cv2.waitKey(1) & 0xFF
if key == 27:
self.get_logger().info("用户按下ESC键,退出节点")
# 资源清理：关闭所有OpenCV窗口
cv2.destroyAllWindows()
# 销毁节点并关闭ROS 2
self.destroy_node()
rclpy.shutdown()
except Exception as e:
# 异常处理：打印转换或处理错误
self.get_logger().error(f"图像处理失败：{str(e)}")
# 主函数：启动节点
def main(args=None):
rclpy.init(args=args) # 初始化ROS 2
node = ImageGrayNode() # 创建节点实例
rclpy.spin(node) # 持续接收图像消息
# 资源清理
cv2.destroyAllWindows()
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
以上代码实现了一个实时显示图像灰度化的ROS 2节点image_gray_node，其核心功能是订阅ROS 2图像话题/image_raw并将该节点发布的彩色图像实时转换为灰度图像并显示。节点image_
gray_node订阅话题/image_raw，并在图像处理回调函数image_callback中，通过转换工具CvBridge将ROS 2图像主题消息转换为OpenCV格式的彩色图像，然后调用OpenCV库将彩色图像转换为灰度图像并通过主题实时发布、实时显示。
接下来打开配置文件CMakeLists.txt，将Python脚本文件image_gray.py加入安装目录下进行构建编译，配置如下所示：
…
install(PROGRAMS
…
scripts/add_solver_client.py
scripts/image_gray.py #增加image_gray.py到安装目录
DESTINATION lib/${PROJECT_NAME}
)
…
配置完成后，使用组合键Ctrl+Shift+5新打开一个终端，在终端1中启动摄像头节点usb_cam_
node_exe发布图像话题/image_raw，在终端2中更新系统变量后，启动用户示例节点，打开窗口订阅话题/image_raw并实时显示灰度图像，命令如下所示：
ros2 run usb_cam usb_cam_node_exe #在终端1中启动摄像头节点
source install/setup.bash #在终端2中启动用户示例节点
ros2 run service_paramadd_demo image_gray
在两个终端中分别运行上述命令后，摄像头节点usb_cam_node_exe将通过主题/image_raw发布摄像头图像数据。我们可以启动rqt窗口，分别订阅显示原始图像和灰度化窗口（Gray Image）中的图像进行对比，以便直观地看到对比效果，如图4.20所示。
图4.20 原始图像与灰度化图像的对比效果
如图4.20所示，在Gray Image窗口中将实时显示灰度化后的摄像头图像。本小节主要介绍了如何订阅摄像头节点发布的话题/image_raw，并通过OpenCV处理后实时显示灰度化的图像。通过示例的编写与构建编译，讲解了图像转换的逻辑与转换方法的应用等相关知识点。
4.3.5 订阅相机发布节点信息
在4.3.4节中，通过灰度化节点图像实现了“图像处理”的功能。但在实际应用中，项目还要应当具备动态参数的配置功能。例如，是否显示彩色图像、是否保存图像、打印图像关键信息以及使用快捷键保存图像等。本小节将对灰度化示例节点程序进行进一步封装，实现一个相机订阅器节点并支持动态参数的配置。
在功能包下的目录service_paramadd_demo/scripts中新建一个Python源文件camera_subscriber
.py作为相机订阅器节点，实现代码如下所示：
#!/usr/bin/env python3
# 导入依赖模块
import rclpy
from rclpy.node import Node
from rclpy.node import SetParametersResult
from sensor_msgs.msg import Image
import cv2
from cv_bridge import CvBridge
import os # 用于文件路径操作
from datetime import datetime # 用于生成带时间戳的图像文件名
class CameraSubscriberNode(Node):
def __init__(self):
# 初始化节点（命名为camera_subscriber_node）
super().__init__("camera_subscriber_node")
# 声明参数，支持动态配置，默认显示彩色图像
jls_extract_var = "show_color"
self.declare_parameter(jls_extract_var, True)
# 获取初始参数值
self.show_color = self.get_parameter("show_color").value
# 注册参数变化回调函数用于监听参数动态修改
self.add_on_set_parameters_callback(self.parameter_callback)
# 初始化CvBridge和图像保存目录
self.bridge = CvBridge()
# 图像保存路径:用户目录下的ros2_camera_images文件夹
self.save_dir = os.path.expanduser("~/ros2_camera_images")
# 若目录不存在自动创建，参数“exist_ok=True”避免报错
os.makedirs(self.save_dir, exist_ok=True)
# 订阅摄像头图像话题/image_raw
self.image_sub = self.create_subscription(
Image,
"/image_raw", #摄像头节点发布的话题
self.image_callback,
10
)
# 定时器:每1秒打印一次图像信息（分辨率、帧率）
self.info_timer = self.create_timer(
1.0,
self.print_image_info
)
# 保存最后一帧图像消息（用于打印分辨率）
self.last_image_msg = None
# 打印启动日志
self.get_logger().info("相机订阅器节点已启动！")
self.get_logger().info(f"当前参数:show_color={self.show_color}")
self.get_logger().info("操作说明:按s键保存图像，按ESC键退出")
# 辅助函数:打印图像信息（分辨率、帧率）
def print_image_info(self):
if self.last_image_msg is None:
return # 未收到图像前，不打印
# 从图像消息中提取分辨率（width=宽度，height=高度）
width = self.last_image_msg.width
height = self.last_image_msg.height
# 简化帧率计算:话题发布频率≈图像帧率
self.get_logger().info(f"图像信息:分辨率{width}x{height},帧率≈30 FPS")
# 参数变化回调函数，当参数被修改时触发调用
def parameter_callback(self, params):
for param in params:
if param.name == "show_color":
self.show_color = param.value # 更新参数值
self.get_logger().info(f"参数已更新:show_color={self.show_color}")
return SetParametersResult(successful=True)
# 图像处理回调函数用于接收、显示、保存图像
def image_callback(self, msg):
self.last_image_msg = msg # 更新最后一帧图像消息
try:
# ROS图像 →OpenCV图像（bgr8格式）
cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8")
# 根据参数决定显示彩色或灰度图像
if self.show_color:
display_image = cv_image
window_name = "Color Camera" # 彩色图像窗口名
else:
display_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
window_name = "Gray Camera" # 灰度图像窗口名
# 显示图像
cv2.imshow(window_name, display_image)
# 按键处理
key = cv2.waitKey(1) & 0xFF
if key == ord('s'): # 单击s键保存图像
# 生成带时间戳的文件名（避免重名，格式:20240520_153045.jpg）
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
save_path = os.path.join(self.save_dir, f"camera_image_{timestamp}.jpg")
# 保存图像（cv2.imwrite，参数:路径+图像）
cv2.imwrite(save_path, cv_image)
self.get_logger().info(f"图像已保存到:{save_path}")
elif key == 27: # 按ESC键退出
self.get_logger().info("用户按下ESC键，退出节点")
cv2.destroyAllWindows()
self.destroy_node()
rclpy.shutdown()
except Exception as e:
self.get_logger().error(f"图像处理失败:{str(e)}")
# 主函数
def main(args=None):
rclpy.init(args=args)
node = CameraSubscriberNode()
rclpy.spin(node)
# 资源清理
cv2.destroyAllWindows()
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
上述代码主要是在订阅图像话题/image_raw的回调函数image_callback中，根据我们动态配置的参数实现彩色图像和灰度化图像的变换。当配置参数show_color为true时，直接显示转换后的OpenCV彩色图像cv_image；否则通过图像转换方法cv2.cvtColor将彩色图像转成灰度图像。另外两个基本功能是，当我们按下S键后，将会在路径变量save_path中保存文件名带有时间戳的图像，以及每次订阅到图像后，均会显示图像的分辨率、帧率等信息。
在配置文件CMakeLists.txt中配置相机订阅器节点源文件camera_subscriber.py到安装目录下，配置如下所示：
install(PROGRAMS
…
scripts/image_gray.py
scripts/camera_subscriber.py #新增相机订阅器节点源文件到安装目录下
DESTINATION lib/${PROJECT_NAME}
)
配置完成后进行构建编译，再打开两个终端，分别运行摄像头节点usb_cam_node_exe和相机订阅器节点camera_subscriber，命令如下所示：
source install/setup.bash
ros2 run usb_cam usb_cam_node_exe # 在终端1中启动摄像头节点
# 在终端2中启动订阅器节点，默认显示彩色图像
ros2 run service_paramadd_demo camera_subscriber.py
运行后，摄像头节点usb_cam_node_exe负责发布图像话题/image_raw，相机订阅器节点camera_subscriber负责订阅图像话题/image_raw，默认显示实时彩色图像并在终端中输出图像信息。接下来，打开一个新终端，负责修改动态参数show_color以切换为显示灰度图像，命令如下所示：
ros2 param set /camera_subscriber_node show_color false
运行后，终端3中将输出Set parameter successful提示参数修改成功，并且图像窗口标题会从Color Camera切换为Gray Camera，同时实时显示灰度图像。我们按S键将会把当前图像以时间戳命名后保存到指定路径中，如图4.21所示。
图4.21 切换显示灰度图像和保存图像操作
可以看到，当图像窗口处于前端激活状态时，我们按S键，终端中会输出图像保存路径为~/ros2_camera_images/camera_image_20251026_130416.jpg。
本小节主要讲解了如何通过动态参数修改实现原图像和灰度图像的相关切换，以及使用快捷键保存图像的方法。我们可以在下载资源的示例代码中进行修改，实现更多自定义效果。
4.3.6 服务器端分析及实现
我们在4.3.5节中，学习了订阅相机节点发布图像信息的处理方法，以及通过动态参数修改实现图像处理后切换窗口的示例。本小节将以颜色识别服务器为例，创建一个名为color_recognition的服务。客户端向服务器端发送目标颜色，服务器订阅摄像头节点的话题/image_raw，收到客户端的颜色识别请求后进行处理，并返回是否识别到目标、目标的中心坐标及面积等响应信息。识别原理主要是通过HSV颜色空间的阈值分割，提取目标颜色区域，并计算该区域的中心坐标和面积。
1. SRV接口定义
首先在工作空间路径src/service_paramadd_demo/srv下新建一个服务接口文件ColorRecognition.srv，并在该文件中定义服务接口变量，如下所示：
# 请求部分：客户端指定的目标颜色（字符串，例如："red"、"blue"、"green"）
string target_color
---
# 响应部分：识别结果
bool found # 是否找到目标颜色（true=找到，false=未找到）
int64 center_x # 目标中心X坐标（像素，如果没有找到，则为-1）
int64 center_y # 目标中心Y坐标（像素，如果没有找到，则为-1）
float64 area # 目标区域面积（像素，如果没有找到，则为0.0）
string message # 状态消息，例如"找到红色目标""未找到蓝色目标"
接口变量定义完成后，保存即可。注意接口文件中的请求部分和响应部分之间一定要以“---”进行分割，否则该文件定义的服务接口将不会被识别而出现错误。
2. 服务器代码实现
在工作空间chapter4_ws目录src/service_paramadd_demo/scripts下新建一个Python源文件color_recognition_server.py，作为颜色识别服务器节点，代码如下所示：
#!/usr/bin/env python3
# 导入依赖模块
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from service_paramadd_demo.srv import ColorRecognition # 自定义颜色识别SRV
import cv2
from cv_bridge import CvBridge
import numpy as np # 用于图像数组运算（如阈值分割）
class ColorRecognitionServer(Node):
def __init__(self):
# 初始化节点（命名为color_recognition_server）
super().__init__("color_recognition_server")
# 初始化核心组件
self.bridge = CvBridge() # 图像转换
self.latest_image = None # 缓存最新一帧图像（用于识别）
# 预定义颜色的HSV阈值（OpenCV中HSV范围：H[0-179], S[0-255], V[0-255]）
# 键：颜色名，值：HSV阈值范围（红色分两段，其他颜色一段）
self.color_hsv_ranges = {
"red": [
(0, 120, 70), (10, 255, 255), # 红色第一段（低H值）
(170, 120, 70), (180, 255, 255) # 红色第二段（高H值）
],
"blue": [
(110, 50, 50), (130, 255, 255) # 蓝色HSV范围
],
"green": [
(35, 40, 40), (77, 255, 255) # 绿色HSV范围
]
}
# 订阅摄像头图像话题（更新最新图像）
self.image_sub = self.create_subscription(
Image,
"/image_raw",
self.image_callback,
10
)
# 创建颜色识别服务
self.recognition_service = self.create_service(
ColorRecognition,
"color_recognition", # 服务名（客户端需匹配）
self.handle_recognition_request # 服务回调函数
)
# 打印启动日志
supported_colors = ", ".join(self.color_hsv_ranges.keys())
self.get_logger().info(f"颜色识别服务器已启动！")
self.get_logger().info(f"支持识别的颜色：{supported_colors}")
self.get_logger().info(f"服务名：color_recognition，等待客户端请求...")
self.get_logger().info("提示：识别结果窗口按ESC键关闭")
# 图像回调：更新最新图像缓存
def image_callback(self, msg):
try:
# ROS图像 →OpenCV图像（bgr8格式）
self.latest_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8")
except Exception as e:
self.get_logger().error(f"图像接收失败：{str(e)}")
# 根据目标颜色获取HSV阈值掩码（用于分割目标区域）
def get_color_mask(self, hsv_image, target_color):
ranges = self.color_hsv_ranges[target_color]
if target_color == "red":
# 红色分两段，需合并两个掩码
lower1, upper1, lower2, upper2 = ranges
mask1 = cv2.inRange(hsv_image, lower1, upper1)
mask2 = cv2.inRange(hsv_image, lower2, upper2)
mask = cv2.bitwise_or(mask1, mask2) # 合并掩码（或运算）
else:
# 其他颜色：单段阈值
lower, upper = ranges
mask = cv2.inRange(hsv_image, lower, upper)
# 图像滤波：腐蚀→膨胀（去除小噪声，让目标区域更完整）
mask = cv2.erode(mask, None, iterations=2) # 腐蚀（缩小噪声）
mask = cv2.dilate(mask, None, iterations=2) # 膨胀（恢复目标大小）
return mask
# 服务请求处理回调函数
def handle_recognition_request(self, request, response):
# 检查是否有图像缓存
if self.latest_image is None:
response.found = False
response.center_x = -1
response.center_y = -1
response.area = 0.0
response.message = "未接收到摄像头图像，请先启动USB摄像头节点（ros2 run usb_cam usb_cam_node）"
self.get_logger().error(response.message)
return response
# 检查目标颜色是否支持
target_color = request.target_color.lower() # 转为小写（兼容大小写输入）
if target_color not in self.color_hsv_ranges:
supported = ", ".join(self.color_hsv_ranges.keys())
response.found = False
response.center_x = -1
response.center_y = -1
response.area = 0.0
response.message = f"不支持的颜色！当前支持：{supported}"
self.get_logger().error(response.message)
return response
# 颜色识别流程
cv_image = self.latest_image.copy() # 复制图像（避免修改原始缓存）
# BGR图像 → HSV图像（HSV颜色空间更适合颜色分割）
hsv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
# 获取目标颜色的掩码（白色为目标区域，黑色为背景）
color_mask = self.get_color_mask(hsv_image, target_color)
# 查找目标区域的轮廓（contours：轮廓列表）
contours, _ = cv2.findContours(
color_mask.copy(),
cv2.RETR_EXTERNAL, # 只找最外层轮廓
cv2.CHAIN_APPROX_SIMPLE # 简化轮廓点（减少计算量）
)
# 处理轮廓（提取面积最大的轮廓作为目标，过滤小噪声）
if len(contours) > 0:
# 找到面积最大的轮廓
max_contour = max(contours, key=cv2.contourArea)
target_area = cv2.contourArea(max_contour)
# 过滤小面积噪声（面积小于500像素的视为噪声，可根据实际调整）
if target_area > 500:
# 计算轮廓中心坐标
M = cv2.moments(max_contour)
# 中心X坐标：m10/m00，中心Y坐标：m01/m00（m00为轮廓面积，避 # 免除以0）
center_x = int(M["m10"] / M["m00"]) if M["m00"] != 0 else -1
center_y = int(M["m01"] / M["m00"]) if M["m00"] != 0 else -1
# 在图像上绘制目标（便于可视化）
# 绘制轮廓（绿色，线宽2）
cv2.drawContours(cv_image, [max_contour], -1, (0, 255, 0), 2)
# 绘制中心圆点（红色，半径5，填充）
cv2.circle(cv_image, (center_x, center_y), 5, (0, 0, 255), -1)
# 添加文字标注（目标颜色和面积）
text = f"{target_color.capitalize()} (Area: {target_area:.0f})"
cv2.putText(
cv_image, text, (center_x-50, center_y-10),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2
)
# 构造成功响应
response.found = True
response.center_x = center_x
response.center_y = center_y
response.area = float(target_area)
response.message = f"成功识别到{target_color}目标！中心坐标：({center_x}, {center_y})，面积：{target_area:.0f}像素"
self.get_logger().info(f"处理请求：目标颜色={target_color} → {response.message}")
# 显示识别结果图像
cv2.imshow("Color Recognition Result", cv_image)
# 按键检测：按ESC键关闭窗口
if cv2.waitKey(1) & 0xFF == 27:
cv2.destroyWindow("Color Recognition Result")
return response
# 未找到目标的响应值
response.found = False
response.center_x = -1
response.center_y = -1
response.area = 0.0
response.message = f"未在图像中找到{target_color}目标，请调整目标位置或光线"
self.get_logger().warn(f"处理请求：目标颜色={target_color} → {response.message}")
# 显示无目标的图像
cv2.imshow("Color Recognition Result", cv_image)
if cv2.waitKey(1) & 0xFF == 27:
cv2.destroyWindow("Color Recognition Result")
return response
# 主函数
def main(args=None):
rclpy.init(args=args)
server = ColorRecognitionServer()
try:
rclpy.spin(server)
except KeyboardInterrupt:
# 捕获Ctrl+C按键信息后退出并清理资源
cv2.destroyAllWindows()
server.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
以上代码主要基于ROS 2框架创建了一个颜色识别服务端节点类ColorRecognitionServer，该类的核心作用是作为“图像数据处理-服务请求响应”的中间件，实现了“从图像采集到颜色目标分析” 的各个流程。首先通过订阅ROS 2图像话题/image_raw缓存最新的摄像头图像，然后对外提供一个名为color_recognition的服务接口，用于接收客户端发送的“目标颜色”请求，通过计算机视觉算法完成目标识别与特征提取，最后向客户端返回SRV接口变量。
在代码功能上，主要通过方法create_service创建了服务接口color_recognition，并在服务处理回调函数handle_recognition_request中处理客户端请求。在回调逻辑中先完成两个前置校验，分别是校验self.latest_image是否存在，避免没有图像时也执行识别，以及校验客户端请求的目标颜色是否在红、蓝、绿之中，避免不兼容的颜色。
如果前置校验未通过，则会返回错误信息。例如，未接收到摄像头图像、不支持的颜色等响应信息。如果校验通过，则触发后续颜色识别的处理过程，待服务器识别完成后，无论是否找到目标，都会返回包含SRV服务接口变量的响应信息。
备注：代码中有关OpenCV函数方法的参数及作用，我们可参考代码中的注释或者复习OpenCV基础书籍中的相关知识点。
打开配置文件CMakeLists.txt，将SRV文件生成接口文件，并将颜色识别服务器节点代码文件添加到安装目录下，配置如下所示：
…
# 声明需要生成接口的SRV文件
set(srv_files
"srv/AddWithPre.srv"
"srv/ColorRecognition.srv"
)
#生成接口文件
rosidl_generate_interfaces(${PROJECT_NAME}
${srv_files}
DEPENDENCIES std_msgs # 依赖的其他消息包
)
#安装Python脚本（scripts目录下的可执行文件）
install(PROGRAMS
…
scripts/camera_subscriber.py
scripts/color_recognition_server.py
DESTINATION lib/${PROJECT_NAME}
)
…
配置CMakeLists.txt时，我们一定要注意不要遗漏SRV接口文件的生成配置，否则代码中导入模块import ColorRecognition将提示错误信息。构建编译完成后，启动节点命令如下所示：
source install/setup.bash
ros2 run service_paramadd_demo color_recognition_server.py
4.3.7 客户端分析及实现
客户端节点的主要功能是：我们指定目标颜色后，将请求发送到服务器端节点，接收服务器响应消息并显示识别结果。本小节将介绍客户端的工作流程，包括从命令行接收目标颜色作为参数，通过服务color_recognition连接服务器并发送该参数，以及接收服务器返回的识别结果等功能的实现方法。
在工作空间目录src/service_paramadd_demo/scripts下，新建Python源文件color_recognition_client.py作为颜色识别客户端节点，代码如下所示：
#!/usr/bin/env python3
# 导入依赖模块
import rclpy
from rclpy.node import Node
from service_paramadd_demo.srv import ColorRecognition
import sys
class ColorRecognitionClient(Node):
def __init__(self, target_color):
# 初始化节点（命名为color_recognition_client）
super().__init__("color_recognition_client")
# 保存目标颜色
self.target_color = target_color.lower()
# 创建客户端对象连接color_recognition服务
self.client = self.create_client(
ColorRecognition,
"color_recognition"
)
# 等待服务器启动
self.get_logger().info(f"请求识别颜色：{self.target_color}")
self.get_logger().info("等待颜色识别服务器启动...")
while not self.client.wait_for_service(timeout_sec=1.0):
if not rclpy.ok():
self.get_logger().error("客户端被中断，停止等待")
sys.exit(1)
self.get_logger().warn("服务器未就绪，继续等待...")
# 发送识别请求
def send_request(self):
# 构造请求对象并指定目标颜色
request = ColorRecognition.Request()
request.target_color = self.target_color
# 异步发送请求
self.future = self.client.call_async(request)
# 等待响应并处理
rclpy.spin_until_future_complete(self, self.future)
if self.future.done():
try:
response = self.future.result()
# 打印结果
self.get_logger().info("\n" + "="*70)
self.get_logger().info(f"【颜色识别结果】")
self.get_logger().info(f"目标颜色：{self.target_color.capitalize()}")
self.get_logger().info(f"是否找到：{'是' if response.found else '否'}")
if response.found:
self.get_logger().info(f"中心坐标：X={response.center_x}, Y={response.center_y}")
self.get_logger().info(f"目标面积：{response.area:.0f} 像素")
self.get_logger().info(f"状态信息：{response.message}")
self.get_logger().info("="*70 + "\n")
except Exception as e:
self.get_logger().error(f"\n请求失败：{str(e)}\n")
#主函数：处理命令行参数并启动客户端
def main(args=None):
rclpy.init(args=args)
#检查命令行参数
if len(sys.argv) != 2:
print("\n参数错误！正确用法：")
print("ros2 run service_paramadd_demo color_recognition_client目标颜色")
print("支持的颜色：red（红色）、blue（蓝色）、green（绿色）")
print("示例：ros2 run service_paramadd_demo color_recognition_client red\n")
sys.exit(1)
#创建客户端实例并发送请求
client = ColorRecognitionClient(sys.argv[1])
client.send_request()
# 资源清理
client.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
打开配置文件CMakeLists.txt，将客户端源文件添加到安装目录中，配置如下所示：
…
#安装Python脚本（scripts目录下的可执行文件）
install(PROGRAMS
…
scripts/camera_subscriber.py
scripts/color_recognition_server.py
scripts/color_recognition_client.py
DESTINATION lib/${PROJECT_NAME}
)
…
配置完成后，构建编译即可生成客户端节点并启动，命令如下所示：
source install/setup.bash
ros2 run service_paramadd_demo color_recognition_client.py
打开三个终端，分别运行摄像头节点、服务器端节点以及客户端节点，命令分别如下所示：
ros2 run usb_cam usb_cam_node_exe # 在终端1中运行
ros2 run service_paramadd_demo color_recognition_server.py # 在终端2中运行
ros2 run service_paramadd_demo color_recognition_client.py red # 在终端3中运行
分别运行三个节点后，每当客户端发送一次请求，服务器接收到之后，就会显示一次检测结果并在窗口中呈现结果，如图4.22所示。
图4.22 颜色识别检查
可以看到，在服务器窗口Color Recognition Result中，绿色线条框选的是红色目标区域，红色圆点标记目标区域的中心坐标，白色文字Red (Area: 46552)显示的是面积。我们可以尝试除红色以外的另外两种颜色。
扫一扫
3万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
