Soup's Blog

Back
ROS2_Moveit2_Ur5e_Grasp项目详解（六）：seg_and_det文件详解Blur image
28 min
中文
ros2

ROS2_Moveit2_Ur5e_Grasp项目详解（六）：seg_and_det文件详解

基于ROS2的机械臂仿真抓取

接下来对视觉处理模块的代码seg_and_det.launch.py进行详解 代码如下：

这个launch文件定义了三个视觉处理节点：

  • obj_detect节点 - 负责物体检测功能，识别和定位场景中的物体
  • det_tf节点 - 负责检测坐标变换，计算检测到的物体相对于机器人基座的坐标变换关系
  • point_cloud_processor节点 - 负责点云处理，处理深度相机获取的点云数据 每个节点都来自vision功能包，并且都设置为将输出打印到屏幕，方便调试和监控。

obj_detect节点详解#

由上述代码可知，该节点是vision包下的obj_detect.py文件，内容如下：

参数的设置与读取

self.declare_parameter("model_path", "/home/whisper/ros2_ws/src/ros2_moveit2_ur5e_grasp/src/vision/vision/yolov11/models/best.pt")
self.declare_parameter("depth_topic", "/depth_registered/image_rect")
self.declare_parameter("image_topic", "/color/image_raw")
self.declare_parameter("cam_info_topic", "/color/camera_info")
self.declare_parameter("view_image", True)
self.declare_parameter("publish_result", True)
## 参数读取
model_path = self.get_parameter("model_path").value
self.view_img = self.get_parameter("view_image").value
self.publish_result = self.get_parameter("publish_result").value
python

这是一系列 self.declare_parameter(...) 调用，每个都为当前 ROS 2 节点声明一个可配置的参数。 语法：

self.declare_parameter(...)
python
  • name: 参数的名称（字符串）
  • value: 参数的默认值。如果启动时没有通过外部方式指定该参数，就使用这个值
self.model = YOLO(model_path)
python
  • 从model_path加载 YOLO 目标检测模型
self.names = self.model.names
python
  • 获取模型能够识别的类别名称列表。例如：{0: ‘apple’, 1: ‘cup’, 2: ‘box’}。
self.bridge = CvBridge()
python
  • 创建一个 CvBridge 对象，用于在 ROS 图像消息 和 OpenCV 图像（NumPy 数组） 之间进行转换。
self.tracker = Tracker()
python
  • 创建一个目标跟踪器（Tracker） 对象。
self.depth_instrinsic_inv = np.eye(3)
python
  • 初始化一个 3x3 单位矩阵，用于存放相机内参的逆矩阵。
  • 要将图像中的 2D 像素坐标 (u, v) 和深度值 d 转换为 3D 空间坐标 (x, y, z)，需要使用相机内参矩阵 K，公式如下： z=dz = d

x=(u−cx)⋅zfxx = \frac{(u - c_x) \cdot z}{f_x}

y=(v−cy)⋅zfyy = \frac{(v - c_y) \cdot z}{f_y}

转换成矩阵形式：

[xyz]=K−1[u⋅zv⋅zz]\begin{bmatrix} x \\ y \\ z \end{bmatrix} = K^{-1} \begin{bmatrix} u \cdot z \\ v \cdot z \\ z \end{bmatrix} 
self.create_subscription(Image, self.get_parameter("depth_topic").value, self.depth_callback, 10)
self.create_subscription(Image, self.get_parameter("image_topic").value, self.image_callback, 10)
self.create_subscription(CameraInfo, self.get_parameter("cam_info_topic").value, self.caminfo_callback, 10)
self.detection_pub = self.create_publisher(Detection2DArray, "/detection", 10)
self.image_pub = self.create_publisher(Image, "/detect_track", 10)
python

这段代码是用于 ROS 2 节点初始化的一部分，主要负责订阅话题、发布话题。 例如self.create_subscription(Image, self.get_parameter("depth_topic").value, self.depth_callback, 10)：

  • 作用：创建一个订阅者，监听深度图像话题。
  • 参数：
    • Image：消息类型，表示接收到的是一个图像。
    • self.get_parameter(“depth_topic”).value：从节点参数中获取深度图像的话题名称。
    • self.depth_callback：当接收到新消息时调用的回调函数。
    • 10：队列大小（QoS），限制未处理消息的最大数量。

在介绍话题发布者之前，需要先介绍self.timer = self.create_timer(0.1, self.timer_callback)，因为通过这段代码不断调用self.timer_callback函数生成话题发布所需要的信息。这行代码的作用是：创建一个定时器（Timer），让它每隔 0.1 秒自动调用一次 self.timer_callback 函数。

接下来介绍self.timer_callback 函数：

这个函数是整个目标检测节点的核心处理逻辑，主要功能包括：

  1. 数据检查：

    • 检查深度图像和彩色图像是否有效

  2. 图像处理：

    • 使用CvBridge将ROS图像消息转换为OpenCV格式 分别处理深度图像和彩色图像

  3. 目标检测：

    • 使用YOLO模型对彩色图像进行推理 提取检测结果中的边界框、置信度和类别信息

  4. 目标追踪：

    • 将检测结果传递给追踪器进行目标跟踪
    • 获取追踪结果

  5. 三维位置计算：

    • 从深度图像中获取目标中心点的深度值
    • 结合相机内参逆矩阵计算目标的3D坐标

  6. 结果排序：

    • 按照x坐标对检测结果进行排序，便于机器人按顺序抓取

  7. 可视化处理：

    • 在图像上绘制检测中心点和追踪边界框
    • 添加排序编号标识

  8. 结果发布：

    • 发布处理后的可视化图像
    • 发布目标检测和三维位置信息

经过上述代码处理之后的结果如下：

在这里插入图片描述 接下来介绍发布话题

self.detection_pub = self.create_publisher(Detection2DArray, "/detection", 10)
python
  • 创建一个发布者（Publisher），用于将目标检测的结果（如物体类别、边界框、置信度等）发布到名为 /detection 的话题上。## det_tf节点详解
self.image_pub = self.create_publisher(Image, "/detect_track", 10)
python
  • 创建一个发布者，用于将带有检测的图像发布到 /detect_track 话题上。

det_tf节点详解#

代码内容如下：

self.subscription = self.create_subscription(
    Detection2DArray,                 ## 消息类型：二维检测结果数组
    'detection',                      ## 订阅话题名称
    self.det_callback,                ## 回调函数
    10                                ## 队列大小
)
python
  • 创建一个订阅者（Subscriber），订阅话题detection，用于接收来自其他节点发布的 2D 目标检测结果。
self.subscription  ## prevent unused variable warning
python
  • 防止代码分析工具或编译器报 “未使用变量” 警告。
  • 在 Python 中，如果你只是把 create_subscription 的返回值赋给一个变量但没有读取或使用它，某些 Linter（如 pylint）会认为这是一个“未使用变量”，并发出警告。
  • 实际上，你必须保存这个订阅者对象，否则它会被 Python 的垃圾回收机制销毁，导致订阅失败。
  • 所以这行代码只是“读取”了一下 self.subscription，告诉工具：“我确实用了这个变量”，从而消除警告。
self.tf_broadcaster = TransformBroadcaster(self)
python
  • 创建一个 TF2 广播器（Transform Broadcaster），用于向 ROS 2 的 TF 树（Transform Tree） 发布坐标变换。
  • 这个操作就可以是Base_link或Rviz知道这个物体的相对自己的具体位置

接下来介绍逻辑处理函数det_callback：

将目标检测的结果位置信息发布为TF变换，值得注意的是这些Cube的位置信息是相对于camera_color_optical_frame 坐标系的。

工作流程如下：

接收到 Detection2DArray 消息
        ↓
筛选出类别为 'cube' 的检测结果
        ↓
提取每个 cube 的 3D 位置 (x, y, z)
        ↓
按 x 坐标从小到大排序（从左到右）
        ↓
为每个 cube 创建一个 TF 坐标系：cube1, cube2, ...
        ↓
广播这些坐标系相对于相机的变换
python

point_cloud_processor节点详解#

对于这个代码，我目前感觉对我们的这个项目没有什么用（可能看的还不够深入，不知道在哪里使用了），但是还是要解释一下，代码如下：

这个文件实现了一个ROS 2节点，主要功能是对点云数据进行过滤处理。详细说明如下：

  1. PointCloudProcessor类：

    • 继承自ROS 2 Node类，实现点云处理功能
    • 初始化时声明参数、创建点云订阅者和发布者

  2. 初始化方法：

    • 声明两个参数：proportion（点云保留比例，默认0.3）和start_from_top（是否从顶部开始截取，默认True）
    • 创建对/depth/points话题的订阅者，用于接收原始点云数据
    • 创建对/depth/points_filtered话题的发布者，用于发布过滤后的点云数据

  3. 点云处理逻辑：

    • listener_callback：处理接收到的点云数据
    • 根据点云的组织形式（有组织或无组织）分别处理：
      • 有组织点云（height > 1）：按行进行截取，保持宽度不变，调整高度
      • 无组织点云（height = 1）：按点进行截取，调整宽度
    • 根据start_from_top参数决定是从起始位置还是末尾位置开始截取
    • 根据proportion参数确定截取的比例

  4. 点云数据处理：

    • 计算新的点云尺寸（高度或宽度）
    • 计算截取的起始位置和数据大小
    • 从原始点云数据中截取相应部分
    • 构建新的PointCloud2消息并发布

原始点云数据效果： 在这里插入图片描述

过滤后的点云效果： 在这里插入图片描述

总结#

这些节点共同构成了机器人视觉系统的核心部分，用于实现物体识别、定位和三维点云处理等功能，为后续的抓取规划提供必要的环境感知信息。

到此，我们的抓取的准备工作已经完成，也就是simulation.launch.py代码执行结束，接下来将执行start_grasp.launch.py代码。

ROS2_Moveit2_Ur5e_Grasp项目详解（六）：seg_and_det文件详解
http://www.soupcola.top/blog/ros2_blogs/ros2_blogs-6
Author Soup Cola
Published at 2026年2月4日
Copyright CC BY-NC-SA 4.0

ROS2_Moveit2_Ur5e_Grasp项目详解（七）：simulation文件小结

ROS2_Moveit2_Ur5e_Grasp项目详解（五）：register_depth文件详解