ROS (Robot Operating System) 快速入门指南

ROS Introduction (captioned) from Open Robotics on Vimeo.

什么是 ROS?

Robot Operating System (ROS) 是一个广泛用于机器人研究和工业界的开源框架。ROS 提供了一系列库和工具,以帮助开发者创建机器人应用程序。ROS 旨在与各种机器人平台协作,使其成为机器人开发者灵活且强大的工具。

ROS 的关键特性

  1. 模块化架构:ROS 采用模块化架构,允许开发者通过组合称为节点 (nodes) 的小型、可重用组件来构建复杂的系统。每个节点通常执行特定的功能,并且节点之间通过主题 (topics)服务 (services) 使用消息进行通信。

  2. 通信中间件:ROS 提供了一个强大的通信基础设施,支持进程间通信和分布式计算。这是通过用于数据流(主题)的发布-订阅模型和用于服务调用的请求-回复模型来实现的。

  3. 硬件抽象:ROS 在硬件之上提供了一个抽象层,使开发者能够编写设备无关的代码。这允许相同的代码在不同的硬件配置上使用,从而促进了更轻松的集成和实验。

  4. 工具和实用程序:ROS 附带了一套丰富的工具和实用程序,用于可视化、调试和仿真。例如,RViz 用于可视化传感器数据和机器人状态信息,而 Gazebo 则为测试算法和机器人设计提供了强大的仿真环境。

  5. 丰富的生态系统:ROS 生态系统极其庞大且在不断增长,拥有大量可用于不同机器人应用的软件包,包括导航、操作、感知等。社区积极参与这些软件包的开发和维护。

ROS 版本的演进

自 2007 年开发以来,ROS 经历了多个版本的演进,每个版本都引入了新功能和改进,以满足机器人社区日益增长的需求。ROS 的开发可以分为两个主要系列:ROS 1 和 ROS 2。本指南重点介绍 ROS 1 的长期支持 (LTS) 版本,即 ROS Noetic Ninjemys,代码同样适用于早期版本。

ROS 1 与 ROS 2

虽然 ROS 1 为机器人开发提供了坚实的基础,但 ROS 2 通过提供以下特性解决了其不足之处:

  • 实时性能:改进了对实时系统和确定性行为的支持。
  • 安全性:增强了安全功能,可在各种环境中实现安全可靠的操作。
  • 可扩展性:更好地支持多机器人系统和大规模部署。
  • 跨平台支持:扩展了对 Linux 以外的各种操作系统的兼容性,包括 Windows 和 macOS。
  • 灵活的通信:使用 DDS 以实现更灵活、更高效的进程间通信。

ROS 消息和主题

在 ROS 中,节点之间的通信是通过消息 (messages)主题 (topics) 实现的。消息是一种数据结构,定义了节点之间交换的信息,而主题是一个命名通道,用于发送和接收消息。节点可以将消息发布到主题,或从主题订阅消息,从而实现彼此通信。这种发布-订阅模型允许异步通信和节点之间的解耦。机器人系统中的每个传感器或执行器通常都会将数据发布到主题,然后由其他节点进行处理或控制。在本指南中,我们将重点讨论图像、深度和点云消息以及相机主题。

设置 Ultralytics YOLO 与 ROS

本指南已使用此 ROS 环境进行了测试,它是 ROSbot ROS 存储库的一个分支。此环境包括 Ultralytics YOLO 包、用于快速设置的 Docker 容器、全面的 ROS 包以及用于快速测试的 Gazebo 世界。它旨在与 Husarion ROSbot 2 PRO 一起工作。所提供的代码示例适用于任何 ROS Noetic/Melodic 环境,包括仿真和真实场景。

Husarion ROSbot 2 PRO autonomous robot platform

依赖项安装

除了 ROS 环境外,你还需要安装以下依赖项:

  • ROS NumPy 包:这是在 ROS Image 消息和 NumPy 数组之间进行快速转换所必需的。

    pip install ros_numpy
  • Ultralytics 包

    pip install ultralytics

在 ROS 中使用 sensor_msgs/Image 使用 Ultralytics

sensor_msgs/Image 消息类型 在 ROS 中常用于表示图像数据。它包含编码、高度、宽度和像素数据等字段,非常适合传输相机或其他传感器捕获的图像。图像消息广泛用于机器人应用中,用于视觉感知、目标检测 和导航等任务。

Detection and Segmentation in ROS Gazebo

图像分步使用指南

以下代码片段演示了如何将 Ultralytics YOLO 包与 ROS 结合使用。在此示例中,我们订阅一个相机主题,使用 YOLO 处理传入的图像,并将检测到的对象发布到新的主题中,用于检测分割

首先,导入必要的库并实例化两个模型:一个用于分割,一个用于检测。初始化一个 ROS 节点(名称为 ultralytics)以启用与 ROS 主节点的通信。为确保连接稳定,我们加入了一个简短的暂停,让节点在继续之前有足够的时间建立连接。

import time

import rospy

from ultralytics import YOLO

detection_model = YOLO("yolo26m.pt")
segmentation_model = YOLO("yolo26m-seg.pt")
rospy.init_node("ultralytics")
time.sleep(1)

初始化两个 ROS 主题:一个用于检测,一个用于分割。这些主题将用于发布标注后的图像,使其可供进一步处理。节点之间的通信使用 sensor_msgs/Image 消息完成。

from sensor_msgs.msg import Image

det_image_pub = rospy.Publisher("/ultralytics/detection/image", Image, queue_size=5)
seg_image_pub = rospy.Publisher("/ultralytics/segmentation/image", Image, queue_size=5)

最后,创建一个订阅者,监听 /camera/color/image_raw 主题上的消息,并为每个新消息调用回调函数。该回调函数接收 sensor_msgs/Image 类型的消息,使用 ros_numpy 将其转换为 NumPy 数组,使用之前实例化的 YOLO 模型处理图像,对图像进行标注,然后将它们发布回各自的主题:/ultralytics/detection/image 用于检测,/ultralytics/segmentation/image 用于分割。

import ros_numpy

def callback(data):
    """Callback function to process image and publish annotated images."""
    array = ros_numpy.numpify(data)
    if det_image_pub.get_num_connections():
        det_result = detection_model(array)
        det_annotated = det_result[0].plot(show=False)
        det_image_pub.publish(ros_numpy.msgify(Image, det_annotated, encoding="rgb8"))

    if seg_image_pub.get_num_connections():
        seg_result = segmentation_model(array)
        seg_annotated = seg_result[0].plot(show=False)
        seg_image_pub.publish(ros_numpy.msgify(Image, seg_annotated, encoding="rgb8"))

rospy.Subscriber("/camera/color/image_raw", Image, callback)

while True:
    rospy.spin()
完整代码
import time

import ros_numpy
import rospy
from sensor_msgs.msg import Image

from ultralytics import YOLO

detection_model = YOLO("yolo26m.pt")
segmentation_model = YOLO("yolo26m-seg.pt")
rospy.init_node("ultralytics")
time.sleep(1)

det_image_pub = rospy.Publisher("/ultralytics/detection/image", Image, queue_size=5)
seg_image_pub = rospy.Publisher("/ultralytics/segmentation/image", Image, queue_size=5)

def callback(data):
    """Callback function to process image and publish annotated images."""
    array = ros_numpy.numpify(data)
    if det_image_pub.get_num_connections():
        det_result = detection_model(array)
        det_annotated = det_result[0].plot(show=False)
        det_image_pub.publish(ros_numpy.msgify(Image, det_annotated, encoding="rgb8"))

    if seg_image_pub.get_num_connections():
        seg_result = segmentation_model(array)
        seg_annotated = seg_result[0].plot(show=False)
        seg_image_pub.publish(ros_numpy.msgify(Image, seg_annotated, encoding="rgb8"))

rospy.Subscriber("/camera/color/image_raw", Image, callback)

while True:
    rospy.spin()
调试

由于 ROS (Robot Operating System) 节点的分布式特性,调试它们可能具有挑战性。以下几种工具可以协助此过程:

  1. rostopic echo <TOPIC-NAME>:此命令允许你查看发布在特定主题上的消息,从而帮助你检查数据流。
  2. rostopic list:使用此命令列出 ROS 系统中所有可用的主题,让你概览活动的数据流。
  3. rqt_graph:此可视化工具显示节点之间的通信图,提供有关节点如何互连及其如何交互的见解。
  4. 对于更复杂的可视化(如 3D 表示),你可以使用 RViz。RViz (ROS Visualization) 是一个强大的 ROS 3D 可视化工具。它允许你实时可视化机器人及其环境的状态。使用 RViz,你可以查看传感器数据(例如 sensor_msgs/Image)、机器人模型状态以及各种其他类型的信息,从而更容易调试和理解机器人系统的行为。

使用 std_msgs/String 发布检测到的类别

标准的 ROS 消息还包括 std_msgs/String 消息。在许多应用中,不需要重新发布完整的标注图像;相反,只需要机器人视野中存在的类别。以下示例演示了如何使用 std_msgs/String 消息/ultralytics/detection/classes 主题上重新发布检测到的类别。这些消息更轻量,并提供基本信息,使其对各种应用非常有价值。

用例示例

考虑一个配备相机和对象检测模型的仓库机器人。机器人不必通过网络发送大型标注图像,而是可以将检测到的类别列表作为 std_msgs/String 消息发布。例如,当机器人检测到“箱子”、“托盘”和“叉车”等物体时,它会将这些类别发布到 /ultralytics/detection/classes 主题。这些信息随后可被中央监控系统用于实时跟踪库存、优化机器人的路径规划以避开障碍物,或触发特定的操作(例如拾取检测到的箱子)。这种方法减少了通信所需的带宽,并专注于传输关键数据。

字符串分步使用指南

This example demonstrates how to use the Ultralytics YOLO package with ROS. In this example, we subscribe to a camera topic, process the incoming image using YOLO, and publish the detected objects to new topic /ultralytics/detection/classes using std_msgs/String messages. The ros_numpy package is used to convert the ROS Image message to a NumPy array for processing with YOLO.

import time

import ros_numpy
import rospy
from sensor_msgs.msg import Image
from std_msgs.msg import String

from ultralytics import YOLO

detection_model = YOLO("yolo26m.pt")
rospy.init_node("ultralytics")
time.sleep(1)
classes_pub = rospy.Publisher("/ultralytics/detection/classes", String, queue_size=5)

def callback(data):
    """Callback function to process image and publish detected classes."""
    array = ros_numpy.numpify(data)
    if classes_pub.get_num_connections():
        det_result = detection_model(array)
        classes = det_result[0].boxes.cls.cpu().numpy().astype(int)
        names = [det_result[0].names[i] for i in classes]
        classes_pub.publish(String(data=str(names)))

rospy.Subscriber("/camera/color/image_raw", Image, callback)
while True:
    rospy.spin()

在 ROS 中将 Ultralytics 与深度图像配合使用

除了 RGB 图像外,ROS 还支持深度图像 (depth images),它提供了关于物体距离相机距离的信息。深度图像对于避障、3D 建模和定位等机器人应用至关重要。

深度图像是每个像素表示从相机到物体距离的图像。与捕获颜色的 RGB 图像不同,深度图像捕获空间信息,使机器人能够感知其环境的 3D 结构。

获取深度图像

深度图像可以通过各种传感器获得:

  1. 立体相机 (Stereo Cameras):使用两个相机根据图像视差计算深度。
  2. 飞行时间 (ToF) 相机 (Time-of-Flight Cameras):测量光线从物体返回所需的时间。
  3. 结构光传感器 (Structured Light Sensors):投影图案并测量其在表面上的变形。

将 YOLO 与深度图像结合使用

在 ROS 中,深度图像由 sensor_msgs/Image 消息类型表示,其中包括编码、高度、宽度和像素数据的字段。深度图像的编码字段通常使用诸如“16UC1”之类的格式,表示每个像素为 16 位无符号整数,其中每个值表示到物体的距离。深度图像通常与 RGB 图像结合使用,以提供更全面的环境视图。

使用 YOLO,可以提取并结合 RGB 和深度图像的信息。例如,YOLO 可以在 RGB 图像中检测物体,而此检测可用于精确定位深度图像中的相应区域。这允许提取检测到的物体的精确深度信息,增强机器人理解其三维环境的能力。

RGB-D 相机

处理深度图像时,必须确保 RGB 和深度图像正确对齐。RGB-D 相机(例如 Intel RealSense 系列)提供同步的 RGB 和深度图像,从而更容易结合来自两个来源的信息。如果使用单独的 RGB 和深度相机,则必须对其进行校准以确保准确对齐。

深度分步使用指南

在此示例中,我们使用 YOLO 对图像进行分割,并将提取的掩码应用到深度图像中以分割物体。这使我们能够确定目标物体的每个像素距相机焦点的距离。通过获得该距离信息,我们可以计算相机与场景中特定物体之间的距离。首先导入必要的库、创建一个 ROS 节点,并实例化分割模型和一个 ROS 主题。

import time

import rospy
from std_msgs.msg import String

from ultralytics import YOLO

rospy.init_node("ultralytics")
time.sleep(1)

segmentation_model = YOLO("yolo26m-seg.pt")

classes_pub = rospy.Publisher("/ultralytics/detection/distance", String, queue_size=5)

接下来,定义一个处理传入深度图像消息的回调函数。该函数等待深度图像和 RGB 图像消息,将其转换为 NumPy 数组,并将分割模型应用于 RGB 图像。然后,它为每个检测到的对象提取分割掩码,并使用深度图像计算对象距相机的平均距离。大多数传感器都有一个最大距离(称为裁剪距离),超过该距离的值表示为 inf (np.inf)。在处理之前,重要的是滤除这些空值并为其分配 0 的值。最后,它将检测到的对象及其平均距离发布到 /ultralytics/detection/distance 主题。

import numpy as np
import ros_numpy
from sensor_msgs.msg import Image

def callback(data):
    """Callback function to process depth image and RGB image."""
    image = rospy.wait_for_message("/camera/color/image_raw", Image)
    image = ros_numpy.numpify(image)
    depth = ros_numpy.numpify(data)
    result = segmentation_model(image)

    all_objects = []
    for index, cls in enumerate(result[0].boxes.cls):
        class_index = int(cls.cpu().numpy())
        name = result[0].names[class_index]
        mask = result[0].masks.data.cpu().numpy()[index, :, :].astype(int)
        obj = depth[mask == 1]
        obj = obj[~np.isnan(obj)]
        avg_distance = np.mean(obj) if len(obj) else np.inf
        all_objects.append(f"{name}: {avg_distance:.2f}m")

    classes_pub.publish(String(data=str(all_objects)))

rospy.Subscriber("/camera/depth/image_raw", Image, callback)

while True:
    rospy.spin()
完整代码
import time

import numpy as np
import ros_numpy
import rospy
from sensor_msgs.msg import Image
from std_msgs.msg import String

from ultralytics import YOLO

rospy.init_node("ultralytics")
time.sleep(1)

segmentation_model = YOLO("yolo26m-seg.pt")

classes_pub = rospy.Publisher("/ultralytics/detection/distance", String, queue_size=5)

def callback(data):
    """Callback function to process depth image and RGB image."""
    image = rospy.wait_for_message("/camera/color/image_raw", Image)
    image = ros_numpy.numpify(image)
    depth = ros_numpy.numpify(data)
    result = segmentation_model(image)

    all_objects = []
    for index, cls in enumerate(result[0].boxes.cls):
        class_index = int(cls.cpu().numpy())
        name = result[0].names[class_index]
        mask = result[0].masks.data.cpu().numpy()[index, :, :].astype(int)
        obj = depth[mask == 1]
        obj = obj[~np.isnan(obj)]
        avg_distance = np.mean(obj) if len(obj) else np.inf
        all_objects.append(f"{name}: {avg_distance:.2f}m")

    classes_pub.publish(String(data=str(all_objects)))

rospy.Subscriber("/camera/depth/image_raw", Image, callback)

while True:
    rospy.spin()

在 ROS 中使用 sensor_msgs/PointCloud2 使用 Ultralytics

Detection and Segmentation in ROS Gazebo

sensor_msgs/PointCloud2 消息类型 是一种在 ROS 中用于表示 3D 点云数据的结构。该消息类型对于机器人应用不可或缺,支持 3D 建模、目标识别和定位等任务。

点云是定义在三维坐标系中的数据点集合。这些数据点表示通过 3D 扫描技术捕获的物体或场景的外部表面。云中的每个点都有 XYZ 坐标,对应其在空间中的位置,并且还可能包含诸如颜色和强度等附加信息。

参考框架

处理 sensor_msgs/PointCloud2 时,必须考虑获取点云数据的传感器的参考框架。点云最初是在传感器的参考框架中捕获的。你可以通过监听 /tf_static 主题来确定此参考框架。但是,根据你的特定应用需求,你可能需要将点云转换为另一个参考框架。此转换可以使用 tf2_ros 包来实现,该包提供了管理坐标框架和在它们之间转换数据的工具。

获取点云

点云可以通过各种传感器获得:

  1. LIDAR(激光雷达):使用激光脉冲测量到物体的距离并创建高精度 3D 地图。
  2. 深度相机:捕获每个像素的深度信息,允许场景的 3D 重建。
  3. 立体相机:利用两个或多个相机通过三角测量获得深度信息。
  4. 结构光扫描仪:将已知图案投影到表面并测量变形以计算深度。

将 YOLO 与点云结合使用

为了将 YOLO 与 sensor_msgs/PointCloud2 类型消息集成,我们可以采用类似于深度图使用的方法。利用嵌入在点云中的颜色信息,我们可以提取 2D 图像,使用 YOLO 对此图像进行分割,然后将生成的掩码应用到三维点中,从而隔离感兴趣的 3D 对象。

对于处理点云,我们推荐使用 Open3D (pip install open3d),这是一个用户友好的 Python 库。Open3D 提供了强大的工具,用于管理点云数据结构、可视化它们并无缝执行复杂操作。该库可以显著简化流程,并增强我们在基于 YOLO 的分割中操作和分析点云的能力。

点云分步使用指南

导入必要的库并实例化用于分割的 YOLO 模型。

import time

import rospy

from ultralytics import YOLO

rospy.init_node("ultralytics")
time.sleep(1)
segmentation_model = YOLO("yolo26m-seg.pt")

Create a function pointcloud2_to_array, which transforms a sensor_msgs/PointCloud2 message into two NumPy arrays. The sensor_msgs/PointCloud2 messages contain n points based on the width and height of the acquired image. For instance, a 480 x 640 image will have 307,200 points. Each point includes three spatial coordinates (xyz) and the corresponding color in RGB format. These can be considered as two separate channels of information.

The function returns the xyz coordinates and RGB values in the format of the original camera resolution (width x height). Most sensors have a maximum distance, known as the clip distance, beyond which values are represented as inf (np.inf). Before processing, it is important to filter out these null values and assign them a value of 0.

import numpy as np
import ros_numpy

def pointcloud2_to_array(pointcloud2: PointCloud2) -> tuple:
    """Convert a ROS PointCloud2 message to a numpy array.

    Args:
        pointcloud2 (PointCloud2): the PointCloud2 message

    Returns:
        (tuple): tuple containing (xyz, rgb)
    """
    pc_array = ros_numpy.point_cloud2.pointcloud2_to_array(pointcloud2)
    split = ros_numpy.point_cloud2.split_rgb_field(pc_array)
    rgb = np.stack([split["b"], split["g"], split["r"]], axis=2)
    xyz = ros_numpy.point_cloud2.get_xyz_points(pc_array, remove_nans=False)
    xyz = np.array(xyz).reshape((pointcloud2.height, pointcloud2.width, 3))
    nan_rows = np.isnan(xyz).all(axis=2)
    xyz[nan_rows] = [0, 0, 0]
    rgb[nan_rows] = [0, 0, 0]
    return xyz, rgb

接下来,订阅 /camera/depth/points 主题以接收点云消息,并将 sensor_msgs/PointCloud2 消息转换为包含 XYZ 坐标和 RGB 值的 NumPy 数组(使用 pointcloud2_to_array 函数)。使用 YOLO 模型处理 RGB 图像以提取分割对象。对于每个检测到的对象,提取分割掩码并将其应用于 RGB 图像和 XYZ 坐标,以在 3D 空间中隔离该对象。

处理掩码非常简单,因为它由二进制值组成,1 表示对象存在,0 表示不存在。要应用掩码,只需将原始通道乘以掩码即可。此操作有效地隔离了图像中感兴趣的物体。最后,创建一个 Open3D 点云对象,并在 3D 空间中可视化带有相关颜色的分割对象。

import sys

import open3d as o3d

ros_cloud = rospy.wait_for_message("/camera/depth/points", PointCloud2)
xyz, rgb = pointcloud2_to_array(ros_cloud)
result = segmentation_model(rgb)

if not len(result[0].boxes.cls):
    print("No objects detected")
    sys.exit()

classes = result[0].boxes.cls.cpu().numpy().astype(int)
for index, class_id in enumerate(classes):
    mask = result[0].masks.data.cpu().numpy()[index, :, :].astype(int)
    mask_expanded = np.stack([mask, mask, mask], axis=2)

    obj_rgb = rgb * mask_expanded
    obj_xyz = xyz * mask_expanded

    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(obj_xyz.reshape((ros_cloud.height * ros_cloud.width, 3)))
    pcd.colors = o3d.utility.Vector3dVector(obj_rgb.reshape((ros_cloud.height * ros_cloud.width, 3)) / 255)
    o3d.visualization.draw_geometries([pcd])
完整代码
import sys
import time

import numpy as np
import open3d as o3d
import ros_numpy
import rospy
from sensor_msgs.msg import PointCloud2

from ultralytics import YOLO

rospy.init_node("ultralytics")
time.sleep(1)
segmentation_model = YOLO("yolo26m-seg.pt")

def pointcloud2_to_array(pointcloud2: PointCloud2) -> tuple:
    """Convert a ROS PointCloud2 message to a numpy array.

    Args:
        pointcloud2 (PointCloud2): the PointCloud2 message

    Returns:
        (tuple): tuple containing (xyz, rgb)
    """
    pc_array = ros_numpy.point_cloud2.pointcloud2_to_array(pointcloud2)
    split = ros_numpy.point_cloud2.split_rgb_field(pc_array)
    rgb = np.stack([split["b"], split["g"], split["r"]], axis=2)
    xyz = ros_numpy.point_cloud2.get_xyz_points(pc_array, remove_nans=False)
    xyz = np.array(xyz).reshape((pointcloud2.height, pointcloud2.width, 3))
    nan_rows = np.isnan(xyz).all(axis=2)
    xyz[nan_rows] = [0, 0, 0]
    rgb[nan_rows] = [0, 0, 0]
    return xyz, rgb

ros_cloud = rospy.wait_for_message("/camera/depth/points", PointCloud2)
xyz, rgb = pointcloud2_to_array(ros_cloud)
result = segmentation_model(rgb)

if not len(result[0].boxes.cls):
    print("No objects detected")
    sys.exit()

classes = result[0].boxes.cls.cpu().numpy().astype(int)
for index, class_id in enumerate(classes):
    mask = result[0].masks.data.cpu().numpy()[index, :, :].astype(int)
    mask_expanded = np.stack([mask, mask, mask], axis=2)

    obj_rgb = rgb * mask_expanded
    obj_xyz = xyz * mask_expanded

    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(obj_xyz.reshape((ros_cloud.height * ros_cloud.width, 3)))
    pcd.colors = o3d.utility.Vector3dVector(obj_rgb.reshape((ros_cloud.height * ros_cloud.width, 3)) / 255)
    o3d.visualization.draw_geometries([pcd])

Point Cloud Segmentation with Ultralytics

常见问题 (FAQ)

什么是机器人操作系统 (ROS)?

Robot Operating System (ROS) 是一个常用于机器人技术的开源框架,旨在帮助开发者创建健壮的机器人应用程序。它提供了一系列用于构建和对接机器人系统的库和工具,使得复杂应用程序的开发更加容易。ROS 支持使用主题 (topics)服务 (services) 上的消息在节点之间进行通信。

如何将 Ultralytics YOLO 与 ROS 集成以实现实时目标检测?

将 Ultralytics YOLO 与 ROS 集成涉及设置 ROS 环境并使用 YOLO 处理传感器数据。首先安装所需的依赖项,例如 ros_numpy 和 Ultralytics YOLO:

pip install ros_numpy ultralytics

接下来,创建一个 ROS 节点并订阅一个图像主题以处理传入数据。这是一个最小示例:

import ros_numpy
import rospy
from sensor_msgs.msg import Image

from ultralytics import YOLO

detection_model = YOLO("yolo26m.pt")
rospy.init_node("ultralytics")
det_image_pub = rospy.Publisher("/ultralytics/detection/image", Image, queue_size=5)

def callback(data):
    array = ros_numpy.numpify(data)
    det_result = detection_model(array)
    det_annotated = det_result[0].plot(show=False)
    det_image_pub.publish(ros_numpy.msgify(Image, det_annotated, encoding="rgb8"))

rospy.Subscriber("/camera/color/image_raw", Image, callback)
rospy.spin()

什么是 ROS 主题,它们如何在 Ultralytics YOLO 中使用?

ROS 主题通过使用发布-订阅模型促进 ROS 网络中节点之间的通信。主题是一个命名通道,节点使用该通道异步发送和接收消息。在 Ultralytics YOLO 的背景下,你可以让一个节点订阅图像主题,使用 YOLO 处理图像以进行检测分割等任务,并将结果发布到新的主题。

例如,订阅一个相机主题并处理传入的图像以进行检测:

rospy.Subscriber("/camera/color/image_raw", Image, callback)

为什么在 ROS 中将深度图像与 Ultralytics YOLO 配合使用?

ROS 中由 sensor_msgs/Image 表示的深度图像提供了物体距相机的距离,这对于避障、3D 建模和定位等任务至关重要。通过将深度信息与 RGB 图像结合使用,机器人可以更好地了解其 3D 环境。

借助 YOLO,你可以从 RGB 图像中提取分割掩码,并将这些掩码应用到深度图像中以获得精确的 3D 物体信息,从而提高机器人导航和与其周围环境交互的能力。

如何利用 YOLO 在 ROS 中可视化 3D 点云?

要在 ROS 中结合 YOLO 可视化 3D 点云,请:

  1. sensor_msgs/PointCloud2 消息转换为 NumPy 数组。
  2. 使用 YOLO 分割 RGB 图像。
  3. 将分割掩码应用到点云。

这是一个使用 Open3D 进行可视化的示例:

import sys

import open3d as o3d
import ros_numpy
import rospy
from sensor_msgs.msg import PointCloud2

from ultralytics import YOLO

rospy.init_node("ultralytics")
segmentation_model = YOLO("yolo26m-seg.pt")

def pointcloud2_to_array(pointcloud2):
    pc_array = ros_numpy.point_cloud2.pointcloud2_to_array(pointcloud2)
    split = ros_numpy.point_cloud2.split_rgb_field(pc_array)
    rgb = np.stack([split["b"], split["g"], split["r"]], axis=2)
    xyz = ros_numpy.point_cloud2.get_xyz_points(pc_array, remove_nans=False)
    xyz = np.array(xyz).reshape((pointcloud2.height, pointcloud2.width, 3))
    return xyz, rgb

ros_cloud = rospy.wait_for_message("/camera/depth/points", PointCloud2)
xyz, rgb = pointcloud2_to_array(ros_cloud)
result = segmentation_model(rgb)

if not len(result[0].boxes.cls):
    print("No objects detected")
    sys.exit()

classes = result[0].boxes.cls.cpu().numpy().astype(int)
for index, class_id in enumerate(classes):
    mask = result[0].masks.data.cpu().numpy()[index, :, :].astype(int)
    mask_expanded = np.stack([mask, mask, mask], axis=2)

    obj_rgb = rgb * mask_expanded
    obj_xyz = xyz * mask_expanded

    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(obj_xyz.reshape((-1, 3)))
    pcd.colors = o3d.utility.Vector3dVector(obj_rgb.reshape((-1, 3)) / 255)
    o3d.visualization.draw_geometries([pcd])

这种方法提供了分割对象的 3D 可视化,对于机器人应用中的导航和操作等任务非常有用。

评论