ROS(机器人操作系统)快速入门指南
ROS 简介(带字幕)来自Open RoboticsonVimeo。
什么是ROS?
机器人操作系统(ROS)是一个开源框架,广泛应用于机器人研究和工业领域。ROS 提供一系列库和工具,帮助开发人员创建机器人应用程序。ROS 可与各种机器人平台配合使用,是机器人专家灵活而强大的工具。
ROS的主要特点
模块化架构:ROS 采用模块化架构,允许开发人员通过组合较小的、可重复使用的组件(称为节点)来构建复杂的系统。每个节点通常执行特定的功能,节点之间通过主题或服务上的消息进行通信。
通信中间件: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 中,节点之间的通信是通过消息和主题来实现的。消息是一种数据结构,用于定义节点之间交换的信息,而主题则是一种命名通道,用于发送和接收消息。节点可以向主题发布信息,或从主题订阅信息,从而实现相互通信。这种发布-订阅模式允许节点之间进行异步通信和解耦。机器人系统中的每个传感器或执行器通常都会向一个主题发布数据,然后其他节点可以使用这些数据进行处理或控制。在本指南中,我们将重点介绍图像、深度和点云信息以及摄像机主题。
建立 Ultralytics YOLO 带 ROS
本指南已使用该 ROS 环境进行了测试,该环境是ROSbot ROS 存储库的一个分叉。该环境包括Ultralytics YOLO 软件包、用于轻松设置的 Docker 容器、全面的 ROS 软件包以及用于快速测试的 Gazebo 世界。它旨在与Husarion ROSbot 2 PRO 配合使用。所提供的代码示例可在任何 ROS Noetic/Melodic 环境中运行,包括模拟和现实世界。
依赖项安装
除了 ROS 环境之外,您还需要安装以下依赖项:
ROS Numpy 软件包:这是 ROS 图像信息与 numpy 数组之间快速转换所必需的。
Ultralytics 包装
用 Ultralytics 带 ROS sensor_msgs/Image
"(《世界人权宣言》) sensor_msgs/Image
消息类型 is commonly used in ROS for representing image data. It contains fields for encoding, height, width, and pixel data, making it suitable for transmitting images captured by cameras or other sensors. Image messages are widely used in robotic applications for tasks such as visual perception, object detection, and navigation.
图像分步用法
下面的代码片段演示了如何在 ROS 中使用Ultralytics YOLO 软件包。在这个示例中,我们订阅了一个摄像头主题,使用YOLO 处理传入的图像,并将检测到的对象发布到新的主题中进行检测和分割。
首先,导入必要的库并实例化两个模型:一个用于 分割 和一个 检测.初始化一个 ROS 节点(名称为 ultralytics
) 启用与 ROS 主站的通信。为了确保稳定的连接,我们包括一个短暂的暂停,让节点有足够的时间在继续之前建立连接。
import time
import rospy
from ultralytics import YOLO
detection_model = YOLO("yolov8m.pt")
segmentation_model = YOLO("yolov8m-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
,使用先前实例化的图像处理图像 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("yolov8m.pt")
segmentation_model = YOLO("yolov8m-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 可视化)是一款功能强大的 ROS 3D 可视化工具。通过它,您可以实时可视化机器人及其环境的状态。使用 RViz,您可以查看传感器数据(例如
sensors_msgs/Image
)、机器人模型状态和各种其他类型的信息,从而更轻松地调试和理解机器人系统的行为。
发布检测到的类 std_msgs/String
标准 ROS 消息还包括 std_msgs/String
消息。在许多应用程序中,没有必要重新发布整个带注释的图像;相反,只需要机器人视图中存在的类。以下示例演示如何使用 std_msgs/String
信息 上重新发布检测到的类。 /ultralytics/detection/classes
主题。这些消息更轻量级,并提供基本信息,使其对各种应用程序很有价值。
示例用例
考虑一个配备摄像头和物体的仓库机器人 检测模型.机器人可以将检测到的类别列表发布到网络上,而不是发送大量带注释的图像。 std_msgs/String
消息。例如,当机器人检测到“箱子”、“托盘”和“叉车”等对象时,它会将这些类发布到 /ultralytics/detection/classes
主题。然后,中央监控系统可以使用这些信息来实时跟踪库存,优化机器人的路径规划以避开障碍物,或触发特定操作,例如拾取检测到的箱子。这种方法减少了通信所需的带宽,并专注于传输关键数据。
字符串分步用法
此示例演示如何使用 Ultralytics YOLO 带有 ROS 的包。在此示例中,我们订阅一个相机主题,使用 YOLO,然后将检测到的对象发布到新主题 /ultralytics/detection/classes
用 std_msgs/String
消息。这 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("yolov8m.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 还支持深度图像,深度图像可提供物体与摄像头之间的距离信息。深度图像对于避障、三维制图和定位等机器人应用至关重要。
深度图像是每个像素表示从相机到物体的距离的图像。与捕获颜色的 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("yolov8m-seg.pt")
classes_pub = rospy.Publisher("/ultralytics/detection/distance", String, queue_size=5)
接下来,定义一个回调函数,用于处理传入的深度图像消息。该函数等待深度图像和 RGB 图像消息,将它们转换为 numpy 数组,并将分割模型应用于 RGB 图像。然后,它提取每个检测到的物体的分割掩码,并使用深度图像计算物体与相机的平均距离。大多数传感器都有一个最大距离,称为削波距离,超过该距离的值表示为 inf (np.inf
).在处理之前,请务必筛选出这些 null 值并为其分配值 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)
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
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("yolov8m-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)
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
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扫描技术捕获的物体或场景的外表面。云中的每个点都有 X
, Y
和 Z
坐标,对应于其在空间中的位置,还可以包括颜色和强度等附加信息。
参考系
使用 sensor_msgs/PointCloud2
,必须考虑从中获取点云数据的传感器的参考系。点云最初是在传感器的参考系中捕获的。您可以通过收听 /tf_static
主题。但是,根据您的特定应用要求,您可能需要将点云转换为另一个参考系。这种转换可以使用 tf2_ros
包,它提供了用于管理坐标系和在坐标系之间转换数据的工具。
获取点云
可以使用各种传感器获得点云:
- LIDAR (Light Detection and Ranging): Uses laser pulses to measure distances to objects and create high-precision 3D maps.
- 深度摄像头:捕捉每个像素的深度信息,实现场景的三维重建。
- 立体相机:利用两个或多个摄像头,通过三角测量获取深度信息。
- 结构光扫描仪:将已知图案投射到表面,测量变形以计算深度。
用 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("yolov8m-seg.pt")
创建函数 pointcloud2_to_array
,这将 sensor_msgs/PointCloud2
消息到两个 numpy 数组中。这 sensor_msgs/PointCloud2
消息包含 n
基于点 width
和 height
获取的图像。例如,一个 480 x 640
图像将有 307,200
点。每个点包括三个空间坐标 (xyz
) 和相应的颜色 RGB
格式。这些可以被视为两个独立的信息渠道。
该函数返回 xyz
坐标和 RGB
原始相机分辨率格式的值 (width x height
).大多数传感器都有一个最大距离,称为削波距离,超过该距离的值表示为 inf (np.inf
).在处理之前,请务必筛选出这些 null 值并为其分配值 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 图像和 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 ultralytics import YOLO
rospy.init_node("ultralytics")
time.sleep(1)
segmentation_model = YOLO("yolov8m-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 :
接下来,创建一个 ROS 节点,并订阅一个图像主题来处理传入的数据。下面是一个最简单的示例:
import ros_numpy
import rospy
from sensor_msgs.msg import Image
from ultralytics import YOLO
detection_model = YOLO("yolov8m.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 处理图像,完成检测或分割等任务,并将结果发布到新的主题。
例如,订阅相机主题并处理传入的图像以进行检测:
为什么要在 ROS 中使用Ultralytics YOLO 深度图像?
ROS 中的深度图像,用 sensor_msgs/Image
这些信息提供了物体与摄像机之间的距离,对于避障、三维制图和定位等任务至关重要。通过 使用深度信息 与 RGB 图像一起,机器人可以更好地了解其 3D 环境。
通过YOLO ,您可以从 RGB 图像中提取分割掩码,并将这些掩码应用到深度图像中,从而获得精确的 3D 物体信息,提高机器人导航和与周围环境互动的能力。
如何在 ROS 中使用YOLO 可视化 3D 点云?
在 ROS 中使用YOLO 可视化三维点云:
- 转换
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("yolov8m-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])
这种方法提供了分割对象的三维可视化,对导航和操作等任务非常有用。