Hướng dẫn nhanh về ROS (Robot Operating System)

ROS Introduction (captioned) from Open Robotics on Vimeo.

ROS là gì?

Robot Operating System (ROS) là một framework mã nguồn mở được sử dụng rộng rãi trong nghiên cứu và công nghiệp robot. ROS cung cấp một bộ thư viện và công cụ để giúp các nhà phát triển tạo ra các ứng dụng robot. ROS được thiết kế để hoạt động với nhiều nền tảng robot khác nhau, biến nó thành một công cụ linh hoạt và mạnh mẽ cho các chuyên gia robot.

Các tính năng chính của ROS

  1. Kiến trúc mô-đun: ROS có kiến trúc mô-đun, cho phép các nhà phát triển xây dựng các hệ thống phức tạp bằng cách kết hợp các thành phần nhỏ hơn, có thể tái sử dụng được gọi là node. Mỗi node thường thực hiện một chức năng cụ thể và các node giao tiếp với nhau bằng cách sử dụng tin nhắn qua topic hoặc service.

  2. Middleware giao tiếp: ROS cung cấp cơ sở hạ tầng giao tiếp mạnh mẽ hỗ trợ giao tiếp giữa các tiến trình và điện toán phân tán. Điều này đạt được thông qua mô hình publish-subscribe cho các luồng dữ liệu (topic) và mô hình request-reply cho các lệnh gọi service.

  3. Trừu tượng hóa phần cứng: ROS cung cấp một lớp trừu tượng hóa trên phần cứng, cho phép các nhà phát triển viết mã không phụ thuộc vào thiết bị. Điều này cho phép cùng một đoạn mã được sử dụng với các thiết lập phần cứng khác nhau, tạo điều kiện cho việc tích hợp và thử nghiệm dễ dàng hơn.

  4. Công cụ và tiện ích: ROS đi kèm với một bộ công cụ và tiện ích phong phú để trực quan hóa, gỡ lỗi và mô phỏng. Ví dụ, RViz được sử dụng để trực quan hóa dữ liệu cảm biến và thông tin trạng thái robot, trong khi Gazebo cung cấp một môi trường mô phỏng mạnh mẽ để kiểm thử thuật toán và thiết kế robot.

  5. Hệ sinh thái mở rộng: Hệ sinh thái ROS rất rộng lớn và liên tục phát triển, với nhiều gói có sẵn cho các ứng dụng robot khác nhau, bao gồm điều hướng, thao tác, nhận thức và nhiều hơn nữa. Cộng đồng tích cực đóng góp vào việc phát triển và bảo trì các gói này.

Sự tiến hóa của các phiên bản ROS

Kể từ khi được phát triển vào năm 2007, ROS đã phát triển qua nhiều phiên bản, mỗi phiên bản giới thiệu các tính năng và cải tiến mới để đáp ứng nhu cầu ngày càng tăng của cộng đồng robot. Việc phát triển ROS có thể được phân loại thành hai loạt chính: ROS 1 và ROS 2. Hướng dẫn này tập trung vào phiên bản Hỗ trợ dài hạn (LTS) của ROS 1, được gọi là ROS Noetic Ninjemys, mã cũng sẽ hoạt động với các phiên bản trước đó.

ROS 1 và ROS 2

Trong khi ROS 1 cung cấp nền tảng vững chắc cho sự phát triển robot, ROS 2 giải quyết các thiếu sót của nó bằng cách cung cấp:

  • Hiệu suất thời gian thực: Hỗ trợ cải thiện cho các hệ thống thời gian thực và hành vi tất định.
  • Bảo mật: Các tính năng bảo mật nâng cao để vận hành an toàn và đáng tin cậy trong nhiều môi trường khác nhau.
  • Khả năng mở rộng: Hỗ trợ tốt hơn cho các hệ thống đa robot và triển khai quy mô lớn.
  • Hỗ trợ đa nền tảng: Mở rộng khả năng tương thích với nhiều hệ điều hành ngoài Linux, bao gồm Windows và macOS.
  • Giao tiếp linh hoạt: Sử dụng DDS để giao tiếp giữa các tiến trình linh hoạt và hiệu quả hơn.

Thông điệp (Messages) và Topic trong ROS

Trong ROS, giao tiếp giữa các node được thực hiện thông qua messagetopic. Một message là một cấu trúc dữ liệu xác định thông tin được trao đổi giữa các node, trong khi một topic là một kênh có tên mà qua đó các message được gửi và nhận. Các node có thể publish message tới một topic hoặc subscribe message từ một topic, cho phép chúng giao tiếp với nhau. Mô hình publish-subscribe này cho phép giao tiếp không đồng bộ và tách rời giữa các node. Mỗi cảm biến hoặc cơ cấu chấp hành trong một hệ thống robot thường publish dữ liệu lên một topic, sau đó có thể được các node khác tiêu thụ để xử lý hoặc điều khiển. Với mục đích của hướng dẫn này, chúng tôi sẽ tập trung vào các message Image, Depth và PointCloud cũng như các topic camera.

Thiết lập Ultralytics YOLO với ROS

Hướng dẫn này đã được kiểm thử bằng cách sử dụng môi trường ROS này, đây là một nhánh của kho lưu trữ ROSbot ROS. Môi trường này bao gồm gói Ultralytics YOLO, một container Docker để thiết lập dễ dàng, các gói ROS toàn diện và thế giới Gazebo để kiểm thử nhanh. Nó được thiết kế để hoạt động với Husarion ROSbot 2 PRO. Các ví dụ mã được cung cấp sẽ hoạt động trong bất kỳ môi trường ROS Noetic/Melodic nào, bao gồm cả mô phỏng và thực tế.

Husarion ROSbot 2 PRO autonomous robot platform

Cài đặt các phụ thuộc

Ngoài môi trường ROS, bạn sẽ cần cài đặt các phụ thuộc sau:

  • Gói ROS NumPy: Gói này là bắt buộc để chuyển đổi nhanh giữa các message Image của ROS và các mảng NumPy.

    pip install ros_numpy
  • Gói Ultralytics:

    pip install ultralytics

Sử dụng Ultralytics với sensor_msgs/Image trong ROS

The sensor_msgs/Image message type 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.

Detection and Segmentation in ROS Gazebo

Cách sử dụng Image từng bước

Đoạn mã sau đây minh họa cách sử dụng gói Ultralytics YOLO với ROS. Trong ví dụ này, chúng tôi subscribe vào một topic camera, xử lý hình ảnh đến bằng YOLO và publish các đối tượng được phát hiện lên các topic mới để phát hiệnphân đoạn.

Đầu tiên, hãy nhập các thư viện cần thiết và khởi tạo hai model: một cho phân đoạn và một cho phát hiện. Khởi tạo một node ROS (với tên là ultralytics) để cho phép giao tiếp với ROS master. Để đảm bảo kết nối ổn định, chúng tôi bao gồm một khoảng dừng ngắn, cho phép node có đủ thời gian để thiết lập kết nối trước khi tiếp tục.

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)

Khởi tạo hai topic ROS: một cho phát hiện và một cho phân đoạn. Các topic này sẽ được sử dụng để publish các hình ảnh đã chú thích, giúp chúng có thể truy cập được để xử lý thêm. Giao tiếp giữa các node được thực hiện bằng cách sử dụng các message 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)

Cuối cùng, tạo một subscriber lắng nghe các message trên topic /camera/color/image_raw và gọi một hàm callback cho mỗi message mới. Hàm callback này nhận các message loại sensor_msgs/Image, chuyển đổi chúng thành mảng NumPy bằng ros_numpy, xử lý các hình ảnh với các model YOLO đã khởi tạo trước đó, chú thích hình ảnh và sau đó publish chúng trở lại các topic tương ứng: /ultralytics/detection/image cho phát hiện và /ultralytics/segmentation/image cho phân đoạn.

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()
Mã hoàn chỉnh
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()
Gỡ lỗi

Việc gỡ lỗi các node ROS (Robot Operating System) có thể đầy thách thức do tính chất phân tán của hệ thống. Một số công cụ có thể hỗ trợ quá trình này:

  1. rostopic echo <TOPIC-NAME> : Lệnh này cho phép bạn xem các message được publish trên một topic cụ thể, giúp bạn kiểm tra luồng dữ liệu.
  2. rostopic list: Sử dụng lệnh này để liệt kê tất cả các topic có sẵn trong hệ thống ROS, cung cấp cho bạn cái nhìn tổng quan về các luồng dữ liệu đang hoạt động.
  3. rqt_graph: Công cụ trực quan hóa này hiển thị biểu đồ giao tiếp giữa các node, cung cấp thông tin chi tiết về cách các node được kết nối với nhau và cách chúng tương tác.
  4. Đối với các hình ảnh trực quan phức tạp hơn, chẳng hạn như biểu diễn 3D, bạn có thể sử dụng RViz. RViz (ROS Visualization) là một công cụ trực quan hóa 3D mạnh mẽ cho ROS. Nó cho phép bạn trực quan hóa trạng thái của robot và môi trường xung quanh trong thời gian thực. Với RViz, bạn có thể xem dữ liệu cảm biến (ví dụ: sensor_msgs/Image), trạng thái model robot và nhiều loại thông tin khác, giúp việc gỡ lỗi và hiểu hành vi của hệ thống robot của bạn trở nên dễ dàng hơn.

Publish các lớp được phát hiện với std_msgs/String

Standard ROS messages also include std_msgs/String messages. In many applications, it is not necessary to republish the entire annotated image; instead, only the classes present in the robot's view are needed. The following example demonstrates how to use std_msgs/String messages to republish the detected classes on the /ultralytics/detection/classes topic. These messages are more lightweight and provide essential information, making them valuable for various applications.

Trường hợp sử dụng ví dụ

Hãy xem xét một robot kho hàng được trang bị camera và model phát hiện đối tượng. Thay vì gửi các hình ảnh đã chú thích lớn qua mạng, robot có thể publish danh sách các lớp được phát hiện dưới dạng các message std_msgs/String. Ví dụ, khi robot phát hiện các đối tượng như "box", "pallet" và "forklift", nó publish các lớp này lên topic /ultralytics/detection/classes. Thông tin này sau đó có thể được sử dụng bởi một hệ thống giám sát trung tâm để theo dõi hàng tồn kho trong thời gian thực, tối ưu hóa việc lập kế hoạch đường đi của robot để tránh chướng ngại vật hoặc kích hoạt các hành động cụ thể như nhấc một chiếc hộp đã phát hiện. Cách tiếp cận này giảm băng thông cần thiết cho giao tiếp và tập trung vào việc truyền tải dữ liệu quan trọng.

Cách sử dụng String từng bước

Ví dụ này minh họa cách sử dụng gói Ultralytics YOLO với ROS. Trong ví dụ này, chúng tôi subscribe vào một topic camera, xử lý hình ảnh đến bằng YOLO và publish các đối tượng được phát hiện lên topic mới /ultralytics/detection/classes bằng cách sử dụng các message std_msgs/String. Gói ros_numpy được sử dụng để chuyển đổi message ROS Image thành mảng NumPy để xử lý với 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()

Sử dụng Ultralytics với Depth Images trong ROS

Ngoài hình ảnh RGB, ROS hỗ trợ depth image, cung cấp thông tin về khoảng cách của các đối tượng so với camera. Depth image rất quan trọng đối với các ứng dụng robot như tránh chướng ngại vật, lập bản đồ 3D và định vị.

Một depth image là một hình ảnh trong đó mỗi pixel đại diện cho khoảng cách từ camera đến một đối tượng. Khác với hình ảnh RGB thu nhận màu sắc, depth image thu nhận thông tin không gian, cho phép robot nhận thức cấu trúc 3D của môi trường xung quanh.

Thu nhận Depth Images

Depth image có thể thu được bằng cách sử dụng nhiều cảm biến khác nhau:

  1. Camera Stereo: Sử dụng hai camera để tính toán độ sâu dựa trên sự chênh lệch hình ảnh.
  2. Camera Time-of-Flight (ToF): Đo thời gian ánh sáng mất để quay trở lại từ một đối tượng.
  3. Cảm biến ánh sáng cấu trúc: Chiếu một mẫu hình và đo độ biến dạng của nó trên các bề mặt.

Sử dụng YOLO với Depth Images

Trong ROS, depth image được biểu diễn bằng loại message sensor_msgs/Image, bao gồm các trường cho mã hóa, chiều cao, chiều rộng và dữ liệu pixel. Trường mã hóa cho depth image thường sử dụng định dạng như "16UC1", biểu thị một số nguyên 16-bit không dấu trên mỗi pixel, trong đó mỗi giá trị đại diện cho khoảng cách đến đối tượng. Depth image thường được sử dụng kết hợp với hình ảnh RGB để cung cấp cái nhìn toàn diện hơn về môi trường.

Sử dụng YOLO, có thể trích xuất và kết hợp thông tin từ cả hình ảnh RGB và depth image. Ví dụ, YOLO có thể phát hiện các đối tượng trong hình ảnh RGB, và phát hiện này có thể được sử dụng để xác định chính xác các vùng tương ứng trong depth image. Điều này cho phép trích xuất thông tin độ sâu chính xác cho các đối tượng được phát hiện, nâng cao khả năng của robot trong việc hiểu môi trường xung quanh theo ba chiều.

Camera RGB-D

Khi làm việc với depth image, điều quan trọng là phải đảm bảo rằng hình ảnh RGB và độ sâu được căn chỉnh chính xác. Camera RGB-D, chẳng hạn như dòng Intel RealSense, cung cấp các hình ảnh RGB và độ sâu đồng bộ, giúp dễ dàng kết hợp thông tin từ cả hai nguồn. Nếu sử dụng các camera RGB và độ sâu riêng biệt, việc hiệu chỉnh chúng là rất quan trọng để đảm bảo sự căn chỉnh chính xác.

Cách sử dụng Depth từng bước

Trong ví dụ này, chúng tôi sử dụng YOLO để phân đoạn một hình ảnh và áp dụng mask đã trích xuất để phân đoạn đối tượng trong depth image. Điều này cho phép chúng tôi xác định khoảng cách của từng pixel của đối tượng quan tâm từ tâm tiêu cự của camera. Bằng cách thu được thông tin khoảng cách này, chúng tôi có thể tính toán khoảng cách giữa camera và đối tượng cụ thể trong cảnh. Hãy bắt đầu bằng cách nhập các thư viện cần thiết, tạo một node ROS và khởi tạo một model phân đoạn và một topic 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)

Tiếp theo, hãy xác định hàm callback xử lý message depth image đến. Hàm này đợi các message depth image và RGB image, chuyển đổi chúng thành các mảng NumPy và áp dụng model phân đoạn cho hình ảnh RGB. Sau đó, nó trích xuất segmentation mask cho từng đối tượng được phát hiện và tính toán khoảng cách trung bình của đối tượng từ camera bằng cách sử dụng depth image. Hầu hết các cảm biến đều có khoảng cách tối đa, được gọi là clip distance, vượt quá giá trị đó được biểu thị là inf (np.inf). Trước khi xử lý, việc lọc bỏ các giá trị null này và gán cho chúng giá trị 0 là rất quan trọng. Cuối cùng, nó publish các đối tượng được phát hiện cùng với khoảng cách trung bình của chúng lên topic /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()
Mã hoàn chỉnh
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()

Sử dụng Ultralytics với sensor_msgs/PointCloud2 trong ROS

Detection and Segmentation in ROS Gazebo

The sensor_msgs/PointCloud2 message type is a data structure used in ROS to represent 3D point cloud data. This message type is integral to robotic applications, enabling tasks such as 3D mapping, object recognition, and localization.

Một point cloud là một tập hợp các điểm dữ liệu được xác định trong hệ tọa độ ba chiều. Các điểm dữ liệu này đại diện cho bề mặt bên ngoài của một đối tượng hoặc một cảnh, được thu thập thông qua các công nghệ quét 3D. Mỗi điểm trong cloud có tọa độ X, YZ, tương ứng với vị trí của nó trong không gian và cũng có thể bao gồm thông tin bổ sung như màu sắc và cường độ.

Hệ quy chiếu

Khi làm việc với sensor_msgs/PointCloud2, việc xem xét hệ quy chiếu của cảm biến mà từ đó dữ liệu point cloud được thu thập là rất cần thiết. Point cloud ban đầu được thu thập trong hệ quy chiếu của cảm biến. Bạn có thể xác định hệ quy chiếu này bằng cách lắng nghe topic /tf_static. Tuy nhiên, tùy thuộc vào yêu cầu ứng dụng cụ thể của bạn, bạn có thể cần chuyển đổi point cloud sang một hệ quy chiếu khác. Phép biến đổi này có thể đạt được bằng cách sử dụng gói tf2_ros, cung cấp các công cụ để quản lý các hệ tọa độ và biến đổi dữ liệu giữa chúng.

Thu nhận Point cloud

Point cloud có thể thu được bằng cách sử dụng nhiều cảm biến khác nhau:

  1. LIDAR (Light Detection and Ranging): Sử dụng các xung laser để đo khoảng cách đến các đối tượng và tạo bản đồ 3D chính xác cao.
  2. Camera độ sâu: Thu nhận thông tin độ sâu cho từng pixel, cho phép tái tạo 3D của cảnh.
  3. Camera Stereo: Sử dụng hai hoặc nhiều camera để thu được thông tin độ sâu thông qua phép tam giác đạc.
  4. Máy quét ánh sáng cấu trúc: Chiếu một mẫu hình đã biết lên bề mặt và đo độ biến dạng để tính toán độ sâu.

Sử dụng YOLO với Point cloud

Để tích hợp YOLO với các message loại sensor_msgs/PointCloud2, chúng ta có thể áp dụng phương pháp tương tự như phương pháp được sử dụng cho bản đồ độ sâu. Bằng cách tận dụng thông tin màu sắc được nhúng trong point cloud, chúng ta có thể trích xuất một hình ảnh 2D, thực hiện phân đoạn trên hình ảnh này bằng YOLO, và sau đó áp dụng mask kết quả cho các điểm ba chiều để cô lập đối tượng 3D quan tâm.

Để xử lý point cloud, chúng tôi khuyên dùng Open3D (pip install open3d), một thư viện Python thân thiện với người dùng. Open3D cung cấp các công cụ mạnh mẽ để quản lý các cấu trúc dữ liệu point cloud, trực quan hóa chúng và thực hiện các thao tác phức tạp một cách liền mạch. Thư viện này có thể đơn giản hóa đáng kể quá trình và nâng cao khả năng thao tác và phân tích point cloud kết hợp với phân đoạn dựa trên YOLO.

Cách sử dụng Point cloud từng bước

Nhập các thư viện cần thiết và khởi tạo model YOLO cho phân đoạn.

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

Tiếp theo, subscribe vào topic /camera/depth/points để nhận message point cloud và chuyển đổi message sensor_msgs/PointCloud2 thành các mảng NumPy chứa tọa độ XYZ và giá trị RGB (sử dụng hàm pointcloud2_to_array). Xử lý hình ảnh RGB bằng model YOLO để trích xuất các đối tượng đã phân đoạn. Đối với mỗi đối tượng được phát hiện, trích xuất segmentation mask và áp dụng nó cho cả hình ảnh RGB và tọa độ XYZ để cô lập đối tượng trong không gian 3D.

Việc xử lý mask rất đơn giản vì nó bao gồm các giá trị nhị phân, với 1 biểu thị sự hiện diện của đối tượng và 0 biểu thị sự vắng mặt. Để áp dụng mask, chỉ cần nhân các kênh gốc với mask. Thao tác này cô lập hiệu quả đối tượng quan tâm trong hình ảnh. Cuối cùng, tạo một đối tượng point cloud Open3D và trực quan hóa đối tượng đã phân đoạn trong không gian 3D với các màu sắc liên quan.

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])
Mã hoàn chỉnh
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

Câu hỏi thường gặp (FAQ)

Robot Operating System (ROS) là gì?

Robot Operating System (ROS) là một framework mã nguồn mở thường được sử dụng trong lĩnh vực robot để giúp các nhà phát triển tạo ra các ứng dụng robot mạnh mẽ. Nó cung cấp một bộ thư viện và công cụ để xây dựng và kết nối với các hệ thống robot, cho phép phát triển các ứng dụng phức tạp dễ dàng hơn. ROS hỗ trợ giao tiếp giữa các node bằng cách sử dụng các message qua topic hoặc service.

Làm thế nào để tích hợp Ultralytics YOLO với ROS cho việc phát hiện đối tượng thời gian thực?

Việc tích hợp Ultralytics YOLO với ROS bao gồm việc thiết lập môi trường ROS và sử dụng YOLO để xử lý dữ liệu cảm biến. Bắt đầu bằng cách cài đặt các phụ thuộc cần thiết như ros_numpy và Ultralytics YOLO:

pip install ros_numpy ultralytics

Tiếp theo, tạo một node ROS và subscribe vào một topic hình ảnh để xử lý dữ liệu đến. Đây là một ví dụ tối giản:

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()

Topic ROS là gì và chúng được sử dụng như thế nào trong Ultralytics YOLO?

Các topic ROS tạo điều kiện cho giao tiếp giữa các node trong mạng ROS bằng cách sử dụng mô hình publish-subscribe. Một topic là một kênh có tên mà các node sử dụng để gửi và nhận các message không đồng bộ. Trong bối cảnh của Ultralytics YOLO, bạn có thể tạo một node để subscribe vào một topic hình ảnh, xử lý các hình ảnh bằng YOLO cho các tác vụ như phát hiện hoặc phân đoạn và publish kết quả lên các topic mới.

Ví dụ, subscribe vào một topic camera và xử lý hình ảnh đến để phát hiện:

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

Tại sao nên sử dụng depth image với Ultralytics YOLO trong ROS?

Depth image trong ROS, được biểu diễn bởi sensor_msgs/Image, cung cấp khoảng cách của các đối tượng từ camera, rất quan trọng đối với các tác vụ như tránh chướng ngại vật, lập bản đồ 3D và định vị. Bằng cách sử dụng thông tin độ sâu cùng với hình ảnh RGB, robot có thể hiểu rõ hơn về môi trường 3D của chúng.

Với YOLO, bạn có thể trích xuất segmentation mask từ hình ảnh RGB và áp dụng các mask này cho depth image để thu được thông tin đối tượng 3D chính xác, cải thiện khả năng điều hướng và tương tác với môi trường xung quanh của robot.

Làm thế nào tôi có thể trực quan hóa point cloud 3D với YOLO trong ROS?

Để trực quan hóa point cloud 3D trong ROS với YOLO:

  1. Chuyển đổi các message sensor_msgs/PointCloud2 thành các mảng NumPy.
  2. Sử dụng YOLO để phân đoạn hình ảnh RGB.
  3. Áp dụng segmentation mask vào point cloud.

Đây là một ví dụ sử dụng Open3D để trực quan hóa:

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])

Cách tiếp cận này cung cấp một hình ảnh trực quan 3D của các đối tượng được phân đoạn, hữu ích cho các tác vụ như điều hướng và thao tác trong các ứng dụng robot.

Bình luận