Перейти к содержанию

Краткое руководство по ROS (Robot Operating System)

Введение в ROS (с субтитрами) от Open Robotics на Vimeo.

Что такое ROS?

Robot Operating System (ROS) — это платформа с открытым исходным кодом, широко используемая в робототехнических исследованиях и промышленности. ROS предоставляет набор библиотек и инструментов, помогающих разработчикам создавать роботизированные приложения. ROS предназначена для работы с различными роботизированными платформами, что делает ее гибким и мощным инструментом для специалистов по робототехнике.

Ключевые особенности ROS

  1. Модульная архитектура: ROS имеет модульную архитектуру, позволяющую разработчикам создавать сложные системы, объединяя небольшие, многократно используемые компоненты, называемые нодами. Каждый нод обычно выполняет определенную функцию, и ноды взаимодействуют друг с другом с помощью сообщений через топики или сервисы.

  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 взаимодействие между нодами осуществляется через сообщения и топики. Сообщение - это структура данных, которая определяет информацию, которой обмениваются ноды, а топик - это именованный канал, по которому отправляются и принимаются сообщения. Ноды могут публиковать сообщения в топик или подписываться на сообщения из топика, что позволяет им взаимодействовать друг с другом. Эта модель публикации-подписки обеспечивает асинхронную связь и разделение нод. Каждый датчик или исполнительный механизм в роботизированной системе обычно публикует данные в топик, который затем может быть использован другими нодами для обработки или управления. В рамках данного руководства мы сосредоточимся на сообщениях Image, Depth и PointCloud и топиках камеры.

Настройка Ultralytics YOLO с ROS

Это руководство было протестировано с использованием этой среды ROS, которая является форком репозитория ROSbot ROS. Эта среда включает в себя пакет Ultralytics YOLO, Docker-контейнер для простой настройки, полные пакеты ROS и миры Gazebo для быстрого тестирования. Он предназначен для работы с Husarion ROSbot 2 PRO. Приведенные примеры кода будут работать в любой среде ROS Noetic/Melodic, включая как симуляцию, так и реальный мир.

Husarion ROSbot 2 PRO

Установка зависимостей

Помимо среды ROS, вам необходимо установить следующие зависимости:

  • ROS Numpy package: Это необходимо для быстрого преобразования между сообщениями ROS Image и массивами numpy.

    pip install ros_numpy
    
  • Пакет Ultralytics:

    pip install ultralytics
    

Используйте Ultralytics с ROS sensor_msgs/Image

Параметр sensor_msgs/Image тип сообщения обычно используется в ROS для представления данных изображения. Он содержит поля для кодирования, высоты, ширины и данных пикселей, что делает его подходящим для передачи изображений, полученных с камер или других датчиков. Сообщения изображений широко используются в роботизированных приложениях для таких задач, как визуальное восприятие, обнаружения объектов, и навигации.

Обнаружение и сегментация в ROS Gazebo

Пошаговое использование изображений

Следующий фрагмент кода демонстрирует, как использовать пакет Ultralytics YOLO с ROS. В этом примере мы подписываемся на тему камеры, обрабатываем входящее изображение с помощью YOLO и публикуем обнаруженные объекты в новые темы для обнаружения и сегментации.

Сначала импортируйте необходимые библиотеки и создайте два экземпляра моделей: один для сегментация и один для обнаружение. Инициализируйте узел ROS (с именем ultralytics) чтобы включить связь с ROS master. Чтобы обеспечить стабильное соединение, мы включаем небольшую паузу, давая узлу достаточно времени для установления соединения, прежде чем продолжить.

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 topic и вызывает функцию обратного вызова для каждого нового сообщения. Эта функция обратного вызова получает сообщения типа sensor_msgs/Image, преобразует их в массив numpy, используя 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("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 (Robot Operating System) может быть сложной из-за распределенной природы системы. В этом процессе могут помочь несколько инструментов:

  1. rostopic echo <TOPIC-NAME> : Эта команда позволяет просматривать сообщения, опубликованные в определенной теме, помогая вам проверять поток данных.
  2. rostopic list: Используйте эту команду, чтобы перечислить все доступные темы в системе ROS, что даст вам обзор активных потоков данных.
  3. rqt_graph: Этот инструмент визуализации отображает граф коммуникаций между узлами, предоставляя понимание того, как узлы взаимосвязаны и как они взаимодействуют.
  4. Для более сложных визуализаций, таких как 3D-представления, вы можете использовать RViz. RViz (ROS Visualization) — это мощный инструмент 3D-визуализации для ROS. Он позволяет визуализировать состояние вашего робота и его окружение в реальном времени. С помощью RViz вы можете просматривать данные с датчиков (например, sensors_msgs/Image), состояния модели робота и различные другие типы информации, что упрощает отладку и понимание поведения вашей роботизированной системы.

Публикация обнаруженных классов с помощью std_msgs/String

Стандартные ROS-сообщения также включают std_msgs/String сообщения. Во многих приложениях нет необходимости повторно публиковать все аннотированное изображение; вместо этого нужны только классы, присутствующие в поле зрения робота. В следующем примере показано, как использовать std_msgs/String сообщения чтобы повторно опубликовать обнаруженные классы на /ultralytics/detection/classes topic. Эти сообщения более легкие и предоставляют важную информацию, что делает их ценными для различных приложений.

Пример варианта использования

Представьте себе складского робота, оснащенного камерой и объектом модель обнаружения. Вместо отправки больших изображений с аннотациями по сети, робот может публиковать список обнаруженных классов в виде std_msgs/String сообщения. Например, когда робот обнаруживает такие объекты, как "box", "pallet" и "forklift", он публикует эти классы в /ultralytics/detection/classes topic. Эта информация может затем использоваться центральной системой мониторинга для отслеживания инвентаря в режиме реального времени, оптимизации планирования пути робота для избежания препятствий или запуска определенных действий, таких как поднятие обнаруженной коробки. Такой подход снижает пропускную способность, необходимую для связи, и фокусируется на передаче критически важных данных.

Пошаговое использование строк

В этом примере показано, как использовать пакет Ultralytics YOLO с ROS. В этом примере мы подписываемся на тему камеры, обрабатываем входящее изображение с помощью YOLO и публикуем обнаруженные объекты в новой теме. /ultralytics/detection/classes используя std_msgs/String сообщения. The ros_numpy Пакет используется для преобразования 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-изображений, которые фиксируют цвет, изображения глубины фиксируют пространственную информацию, позволяя роботам воспринимать трехмерную структуру окружающей среды.

Получение изображений глубины

Изображения глубины можно получить с помощью различных датчиков:

  1. Стереокамеры: Используйте две камеры для вычисления глубины на основе различий в изображениях.
  2. Камеры Time-of-Flight (ToF): Измеряют время, за которое свет возвращается от объекта.
  3. Датчики структурированного света: Проецируют узор и измеряют его деформацию на поверхностях.

Использование 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 topic.

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

Обнаружение и сегментация в ROS Gazebo

Параметр sensor_msgs/PointCloud2 тип сообщения - это структура данных, используемая в ROS для представления данных трехмерного облака точек. Этот тип сообщений является неотъемлемой частью роботизированных приложений, обеспечивая такие задачи, как 3D-картографирование, распознавание объектов и локализация.

Облако точек — это набор точек данных, определенных в трехмерной системе координат. Эти точки данных представляют внешнюю поверхность объекта или сцены, полученные с помощью технологий 3D-сканирования. Каждая точка в облаке имеет X, Yи Z координаты, которые соответствуют его положению в пространстве, а также могут включать дополнительную информацию, такую как цвет и интенсивность.

Опорный кадр

При работе с sensor_msgs/PointCloud2, важно учитывать систему отсчета датчика, с которого были получены данные облака точек. Облако точек изначально захватывается в системе отсчета датчика. Вы можете определить эту систему отсчета, прослушивая /tf_static topic. Однако, в зависимости от ваших конкретных требований к приложению, вам может потребоваться преобразовать облако точек в другую систему координат. Это преобразование может быть достигнуто с использованием tf2_ros Пакет, который предоставляет инструменты для управления системами координат и преобразования данных между ними.

Получение облаков точек

Облака точек можно получить с помощью различных датчиков:

  1. LIDAR (Light Detection and Ranging): Использует лазерные импульсы для измерения расстояний до объектов и создания высоко-точных 3D-карт.
  2. Камеры глубины: Захватывают информацию о глубине для каждого пикселя, позволяя выполнять 3D-реконструкцию сцены.
  3. Стереокамеры: Используйте две или более камеры для получения информации о глубине посредством триангуляции.
  4. Сканеры структурированного света: Проецируют известный узор на поверхность и измеряют деформацию для вычисления глубины.

Использование YOLO с облаками точек

Чтобы интегрировать YOLO с sensor_msgs/PointCloud2 сообщения типа, мы можем использовать метод, аналогичный тому, который используется для карт глубины. Используя информацию о цвете, встроенную в облако точек, мы можем извлечь 2D-изображение, выполнить сегментацию этого изображения с помощью YOLO, а затем применить полученную маску к трехмерным точкам, чтобы выделить интересующий нас трехмерный объект.

Для обработки облаков точек мы рекомендуем использовать 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 полученного изображения. Например, a 480 x 640 изображение будет иметь 307,200 точки. Каждая точка включает в себя три пространственные координаты (xyz) и соответствующий цвет в RGB формат. Их можно рассматривать как два отдельных канала информации.

Функция возвращает xyz координаты и RGB values в формате исходного разрешения камеры (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 topic для получения сообщения облака точек и преобразования sensor_msgs/PointCloud2 сообщение в массивы numpy, содержащие XYZ координаты и значения RGB (используя pointcloud2_to_array функцию). Обработайте RGB-изображение с помощью модели 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 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])

Сегментация облака точек с помощью Ultralytics

Часто задаваемые вопросы

Что такое Robot Operating System (ROS)?

Robot Operating System (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 и как они используются в Ultralytics YOLO?

ROS-топики облегчают взаимодействие между нодами в сети ROS, используя модель "издатель-подписчик". Топик — это именованный канал, который ноды используют для асинхронной отправки и получения сообщений. В контексте Ultralytics YOLO можно настроить ноду на подписку к топику изображений, обрабатывать изображения с помощью YOLO для таких задач, как детектирование или сегментация, и публиковать результаты в новые топики.

Например, подпишитесь на тему камеры и обработайте входящее изображение для обнаружения:

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

Зачем использовать изображения глубины с Ultralytics YOLO в ROS?

Изображения глубины в ROS, представленные sensor_msgs/Image, предоставляет расстояние от объектов до камеры, что имеет решающее значение для таких задач, как предотвращение препятствий, 3D-картирование и локализация. С помощью использование информации о глубине наряду с RGB-изображениями, роботы могут лучше понимать свое 3D-окружение.

С помощью YOLO вы можете извлекать маски сегментации из RGB-изображений и применять эти маски к изображениям глубины, чтобы получить точную информацию об объектах в 3D, улучшая способность робота ориентироваться и взаимодействовать с окружающей средой.

Как я могу визуализировать 3D облака точек с помощью YOLO в ROS?

Чтобы визуализировать 3D-облака точек в ROS с помощью YOLO:

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

Этот подход обеспечивает трехмерную визуализацию сегментированных объектов, что полезно для таких задач, как навигация и манипулирование в робототехнических приложениях.



📅 Создано 1 год назад ✏️ Обновлено 6 дней назад

Комментарии