ROS（机器人操作系统）快速入门指南
ROS介绍（带字幕） 来自 Open Robotics 在 Vimeo。
什么是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 vs. ROS 2
虽然ROS 1为机器人开发提供了坚实的基础，但ROS 2通过提供以下功能解决了其不足：
- 实时性能：改进了对实时系统和确定性行为的支持。
- 安全性：增强了在各种环境中安全可靠操作的安全功能。
- 可扩展性：更好地支持多机器人系统和大规模部署。
- 跨平台支持：扩展了与Linux以外的各种操作系统的兼容性，包括Windows和macOS。
- 灵活的通信：使用DDS实现更灵活和高效的进程间通信。
ROS消息和主题
在ROS中，节点之间的通信通过消息和话题实现。消息是一种定义节点之间交换信息的数据结构，而话题是一个命名通道，消息通过该通道发送和接收。节点可以向话题发布消息或从话题订阅消息，从而实现彼此之间的通信。这种发布-订阅模型允许节点之间进行异步通信和解耦。机器人系统中的每个传感器或执行器通常会将数据发布到一个话题，其他节点可以消费这些数据进行处理或控制。在本指南中，我们将重点关注Image、Depth和PointCloud消息以及相机话题。
使用Ultralytics YOLO与ROS进行设置
本指南已在此ROS环境中进行了测试，该环境是ROSbot ROS仓库的一个分支。此环境包括Ultralytics YOLO包、用于轻松设置的Docker容器、全面的ROS包以及用于快速测试的Gazebo世界。它旨在与Husarion ROSbot 2 PRO配合使用。提供的代码示例将在任何ROS Noetic/Melodic环境中工作，包括模拟和真实世界。
依赖安装
除了ROS环境外，您还需要安装以下依赖项：
-
ROS Numpy包：这是为了快速在ROS Image消息和numpy数组之间进行转换所必需的。
-
Ultralytics包：
在ROS中使用Ultralytics与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("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将其转换为numpy数组，使用之前实例化的YOLO模型处理图像，注释图像，然后将它们发布回各自的话题：/ultralytics/detection/image用于检测，/ultralytics/segmentation/image用于分割。
import ros_numpy
def callback(data):
"""回调函数，用于处理图像并发布标注后的图像。"""
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):
"""回调函数用于处理图像并发布标注后的图像。"""
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处理传入的图像，并使用std_msgs/String消息将检测到的对象发布到新主题/ultralytics/detection/classes。ros_numpy包用于将ROS图像消息转换为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):
"""回调函数用于处理图像并发布检测到的类别。"""
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还支持深度图像，这些图像提供了物体与相机之间距离的信息。深度图像对于机器人应用至关重要，如避障、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("yolov8m-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)
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()
完整代码
```python 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")
```python 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()
```
在ROS中使用Ultralytics与sensor_msgs/PointCloud2
sensor_msgs/PointCloud2 消息类型 是ROS中用于表示3D点云数据的数据结构。这种消息类型对于机器人应用至关重要，支持3D建图、物体识别和定位等任务。
点云是定义在三维坐标系中的一组数据点集合。这些数据点代表通过3D扫描技术捕获的物体或场景的外部表面。云中的每个点都有X、Y和Z坐标，对应其在空间中的位置，还可能包含颜色和强度等额外信息。
参考框架
在使用sensor_msgs/PointCloud2时，必须考虑获取点云数据的传感器的参考框架。点云最初是在传感器的参考框架中捕获的。你可以通过监听/tf_static主题来确定这个参考框架。然而，根据你的具体应用需求，你可能需要将点云转换到另一个参考框架。这可以通过使用tf2_ros包来实现，该包提供了管理坐标框架和在它们之间转换数据的工具。
获取点云
点云可以通过各种传感器获取：
- 激光雷达（Light Detection and Ranging）：使用激光脉冲测量物体距离，创建高精度的3D地图。
- 深度相机：捕获每个像素的深度信息，允许对场景进行3D重建。
- 立体相机：利用两个或多个相机通过三角测量获取深度信息。
- 结构光扫描仪：将已知图案投射到表面上，测量变形以计算深度。
使用YOLO与点云
要将YOLO与sensor_msgs/PointCloud2类型消息集成，我们可以采用类似于深度图的方法。通过利用点云中嵌入的颜色信息，我们可以提取2D图像，使用YOLO对图像进行分割，然后将结果掩码应用于三维点以隔离感兴趣的3D物体。
对于处理点云，我们推荐使用Open3D（pip install open3d），这是一个用户友好的Python库。Open3D提供了强大的工具来管理点云数据结构、可视化它们并执行复杂的操作。该库可以显著简化流程，并增强我们与基于YOLO的分割一起操作和分析点云的能力。
点云使用步骤
导入必要的库并实例化用于分割的YOLO模型。
python
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消息包含基于获取图像的width和height的n个点。例如，一个480 x 640的图像将有307,200个点。每个点包含三个空间坐标（xyz）和相应的RGB格式的颜色。这些可以被视为两个独立的信息通道。
该函数返回 xyz 坐标和 RGB 值，格式为原始相机分辨率（width x height）。大多数传感器都有一个最大距离，称为裁剪距离，超过该距离的值表示为无穷大（np.inf）。在处理之前，重要的是过滤掉这些空值并将它们赋值为 0。
import numpy as np
import ros_numpy
def pointcloud2_to_array(pointcloud2: PointCloud2) -> tuple:
"""
将 ROS PointCloud2 消息转换为 numpy 数组。
参数:
pointcloud2 (PointCloud2): PointCloud2 消息
返回:
(tuple): 包含 (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 坐标，以在三维空间中隔离对象。
处理掩码很简单，因为它由二进制值组成，1 表示对象存在，0 表示对象不存在。要应用掩码，只需将原始通道乘以掩码。此操作有效地在图像中隔离了感兴趣的对象。最后，创建一个 Open3D 点云对象，并在三维空间中可视化带有相关颜色的分割对象。
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("未检测到对象")
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:
"""
将 ROS PointCloud2 消息转换为 numpy 数组。
参数:
pointcloud2 (PointCloud2): PointCloud2 消息
返回:
(tuple): 包含 (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("未检测到对象")
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 表示，提供了物体与相机之间的距离，这对于避障、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("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("未检测到物体")
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 可视化，对于导航和操作等任务非常有用。
