ROS (机器人操作系统) 快速入门指南
来自 Open Robotics 在 Vimeo 上的ROS 介绍(带字幕)。
什么是 ROS?
机器人操作系统(ROS)是一个广泛应用于机器人研究和行业的开源框架。ROS提供了一系列库和工具,以帮助开发人员创建机器人应用程序。ROS旨在与各种机器人平台配合使用,使其成为机器人专家的灵活而强大的工具。
ROS 的主要特性
-
模块化架构: ROS 具有模块化架构,允许开发人员通过组合称为 节点 的较小、可重用组件来构建复杂的系统。每个节点通常执行特定的功能,节点之间使用 主题 或 服务 通过消息进行通信。
-
通信中间件 (Communication Middleware): ROS 提供了一个强大的通信基础设施,支持进程间通信和分布式计算。这是通过数据流(主题)的发布-订阅模型和服务调用的请求-回复模型来实现的。
-
硬件抽象: ROS 提供了硬件之上的抽象层,使开发人员能够编写与设备无关的代码。这允许相同的代码与不同的硬件设置一起使用,从而促进更容易的集成和实验。
-
工具和实用程序: ROS 配备了一套丰富的工具和实用程序,用于可视化、调试和仿真。例如,RViz 用于可视化传感器数据和机器人状态信息,而 Gazebo 提供了一个强大的仿真环境,用于测试算法和机器人设计。
-
广泛的生态系统: 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 中,节点之间的通信通过 消息 和 话题 来实现。消息是一种数据结构,用于定义节点之间交换的信息;而话题是一个命名的通道,消息通过该通道发送和接收。节点可以向一个话题发布消息,也可以订阅来自一个话题的消息,从而实现彼此间的通信。这种发布-订阅模型允许异步通信和节点之间的解耦。通常,机器人系统中的每个传感器或执行器都会向一个话题发布数据,然后其他节点可以消费这些数据以进行处理或控制。在本指南中,我们将重点介绍图像、深度和点云消息以及相机话题。
使用 ROS 设置 Ultralytics YOLO
本指南已经使用此 ROS 环境进行了测试,该环境是 ROSbot ROS 存储库的一个分支。此环境包括 Ultralytics YOLO 软件包、一个用于轻松设置的 Docker 容器、全面的 ROS 软件包以及用于快速测试的 Gazebo 世界。它旨在与 Husarion ROSbot 2 PRO 配合使用。提供的代码示例适用于任何 ROS Noetic/Melodic 环境,包括模拟和真实环境。
依赖项安装
除了 ROS 环境,您还需要安装以下依赖项:
-
ROS Numpy 包:这是 ROS 图像消息和 numpy 数组之间快速转换所必需的。
pip install ros_numpy
-
Ultralytics 软件包:
pip install ultralytics
将 Ultralytics 与 ROS 结合使用 sensor_msgs/Image
字段 sensor_msgs/Image
消息类型 通常在ROS中用于表示图像数据。它包含编码、高度、宽度和像素数据等字段,使其适用于传输由相机或其他传感器捕获的图像。图像消息广泛用于机器人应用中,例如视觉感知, 对象检测,以及导航。
图像逐步使用指南
以下代码片段演示了如何将 Ultralytics YOLO 软件包与 ROS 一起使用。在此示例中,我们订阅一个相机主题,使用 YOLO 处理传入的图像,并将检测到的对象发布到新的主题,用于检测和分割。
首先,导入必要的库并实例化两个模型:一个用于 分割 以及一个用于 检测。初始化一个 ROS 节点(名称为 ultralytics
),以启用与 ROS 主节点的通信。为了确保连接稳定,我们加入了一个短暂的暂停,让节点有足够的时间来建立连接,然后再继续。
import time
import rospy
from ultralytics import YOLO
detection_model = YOLO("yolo11m.pt")
segmentation_model = YOLO("yolo11m-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
,使用以下方法将它们转换为 numpy 数组 ros_numpy
,使用先前实例化的 YOLO 模型处理图像,注释图像,然后将它们发布回相应的topic: /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("yolo11m.pt")
segmentation_model = YOLO("yolo11m-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(机器人操作系统)节点可能具有挑战性。以下是一些可以协助此过程的工具:
rostopic echo <TOPIC-NAME>
:此命令允许您查看在特定主题上发布的消息,帮助您检查数据流。rostopic list
:使用此命令列出 ROS 系统中所有可用的主题,从而让您全面了解活动数据流。rqt_graph
:此可视化工具显示节点之间的通信图,从而提供对节点如何互连以及它们如何交互的深入了解。- 对于更复杂的可视化,例如 3D 渲染,您可以使用 RViz。RViz (ROS Visualization) 是一个强大的 ROS 3D 可视化工具。它允许您实时可视化机器人的状态及其环境。使用 RViz,您可以查看传感器数据(例如
sensors_msgs/Image
)、机器人模型状态和各种其他类型的信息,从而更容易调试和理解机器人系统的行为。
使用以下方式发布检测到的类别 std_msgs/String
标准 ROS 消息也包括 std_msgs/String
消息。在许多应用中,没有必要重新发布整个带注释的图像;而是只需要机器人视图中存在的类别。以下示例演示了如何使用 std_msgs/String
消息 以重新发布检测到的类 /ultralytics/detection/classes
主题。这些消息更轻量级,并提供基本信息,使其对各种应用都很有价值。
用例示例
考虑一个配备了摄像头和物体的仓库机器人 检测模型。机器人可以发布检测到的类别列表,而不是通过网络发送大型带注释的图像,作为 std_msgs/String
消息。例如,当机器人检测到“box”、“pallet”和“forklift”等对象时,它会将这些类别发布到 /ultralytics/detection/classes
主题。然后,中央监控系统可以使用此信息来实时跟踪库存,优化机器人的路径规划以避开障碍物,或触发特定操作,例如捡起检测到的盒子。这种方法减少了通信所需的带宽,并侧重于传输关键数据。
字符串逐步用法
此示例演示了如何将 Ultralytics YOLO 软件包与 ROS 结合使用。在此示例中,我们订阅一个相机主题,使用 YOLO 处理传入的图像,并将检测到的对象发布到新主题 /ultralytics/detection/classes
使用 std_msgs/String
消息。The ros_numpy
package 用于将 ROS Image 消息转换为 numpy 数组,以便与 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("yolo11m.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()
将 Ultralytics 与 ROS 深度图像结合使用
除了 RGB 图像,ROS 还支持深度图像,它提供关于物体与相机之间距离的信息。 深度图像对于机器人应用至关重要,例如避障、3D 地图构建和定位。
深度图像是一种图像,其中每个像素表示从相机到物体的距离。与捕获颜色的 RGB 图像不同,深度图像捕获空间信息,使机器人能够感知其环境的 3D 结构。
将 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("yolo11m-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("yolo11m-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()
将 Ultralytics 与 ROS 结合使用 sensor_msgs/PointCloud2
字段 sensor_msgs/PointCloud2
消息类型 是ROS中使用的数据结构,用于表示3D点云数据。此消息类型是机器人应用不可或缺的一部分,支持诸如3D地图绘制、对象识别和定位等任务。
点云是在三维坐标系中定义的数据点集合。这些数据点表示物体或场景的外部表面,通过 3D 扫描技术捕获。云中的每个点都有 X
, Y
和 Z
坐标,这些坐标对应于它在空间中的位置,并且还可以包括诸如颜色和强度之类的附加信息。
参考帧
当使用 sensor_msgs/PointCloud2
,必须考虑获取点云数据的传感器的参考坐标系。点云最初是在传感器的参考坐标系中捕获的。您可以通过监听以下内容来确定此参考坐标系 /tf_static
主题。但是,根据您的具体应用需求,您可能需要将点云转换为另一个参考系。这种转换可以使用 tf2_ros
package,它提供了用于管理坐标系并在它们之间转换数据的工具。
获取点云
可以使用各种传感器获取点云:
- 激光雷达(光探测和测距):使用激光脉冲来测量到物体的距离并创建高精度 3D 地图。
- 深度相机: 捕获每个像素的深度信息,从而可以对场景进行 3D 重建。
- 立体相机:利用两个或多个相机通过三角测量获得深度信息。
- 结构光扫描仪:将已知的图案投射到表面上并测量变形以计算深度。
将 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("yolo11m-seg.pt")
创建函数 pointcloud2_to_array
,它转换了一个 sensor_msgs/PointCloud2
消息转换为两个 numpy 数组。The sensor_msgs/PointCloud2
消息包含 n
基于点的 width
和 height
所获取的图像。例如,一个 480 x 640
图像将具有 307,200
点。每个点包括三个空间坐标(xyz
) 以及对应的颜色在 RGB
格式。这些可以被认为是两个独立的信息通道。
该函数返回 xyz
坐标和 RGB
原始相机分辨率格式的值(width x height
)。大多数传感器都有一个最大距离,称为剪裁距离,超过这个距离的值表示为 inf (np.inf
)。在处理之前,重要的是过滤掉这些空值并为它们分配一个值 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
function)。 使用 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("yolo11m-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])
常见问题
什么是机器人操作系统 (ROS)?
机器人操作系统(ROS)是一个常用于机器人领域的开源框架,旨在帮助开发人员创建强大的机器人应用程序。它提供了一系列库和工具,用于构建机器人系统并与之交互,从而更轻松地开发复杂的应用程序。ROS支持使用主题或服务通过消息在节点之间进行通信。
如何将 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("yolo11m.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 topics,它们在 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 对象信息,从而提高机器人导航和与其周围环境交互的能力。
如何在 ROS 中使用 YOLO 可视化 3D 点云?
要在 ROS 中使用 YOLO 可视化 3D 点云:
- 转换
sensor_msgs/PointCloud2
消息转换为 numpy 数组。 - 使用 YOLO 分割 RGB 图像。
- 将分割掩码应用于点云。
以下是使用 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("yolo11m-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])
这种方法提供了分割对象的三维可视化,对于机器人应用中的导航和操作等任务非常有用。