Перейти к содержимому

Краткое руководство по ROS (операционной системе робота)

ROS Introduction (captioned) from Open Robotics on Vimeo.

Что такое ROS?

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

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

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

  2. Коммуникационное программное обеспечение (Communication Middleware): ROS предлагает надежную коммуникационную инфраструктуру, которая поддерживает межпроцессное взаимодействие и распределенные вычисления. Это достигается за счет модели публикации-подписки для потоков данных (тем) и модели запроса-ответа для вызовов сервисов.

  3. Аппаратная абстракция: ROS обеспечивает уровень абстракции над аппаратным обеспечением, позволяя разработчикам писать код, не зависящий от устройства. Это позволяет использовать один и тот же код с разными аппаратными установками, облегчая интеграцию и эксперименты.

  4. Инструменты и утилиты: ROS поставляется с богатым набором инструментов и утилит для визуализации, отладки и моделирования. Например, RViz используется для визуализации данных датчиков и информации о состоянии робота, а Gazebo представляет собой мощную среду моделирования для тестирования алгоритмов и конструкций роботов.

  5. Обширная экосистема: Экосистема ROS обширна и постоянно развивается, в ней доступно множество пакетов для различных робототехнических приложений, включая навигацию, манипуляции, восприятие и многое другое. Сообщество активно участвует в разработке и сопровождении этих пакетов.

Эволюция версий ROS

С момента своего создания в 2007 году ROS прошла через множество версий, в каждой из которых появлялись новые функции и улучшения для удовлетворения растущих потребностей робототехнического сообщества. Развитие ROS можно разделить на две основные серии: ROS 1 и ROS 2. Это руководство фокусируется на версии ROS 1, известной как ROS Noetic Ninjemys, которая находится на долгосрочной поддержке (LTS), однако код должен работать и с более ранними версиями.

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: Он нужен для быстрого преобразования между сообщениями ROS Image и массивами numpy.

    pip install ros_numpy
    
  • Ultralytics пакет:

    pip install ultralytics
    

Использование Ultralytics с ROS sensor_msgs/Image

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

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

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

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

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

Рассмотрим складского робота, оснащенного камерой и объектом модель обнаружения. Вместо того чтобы отправлять по сети большие аннотированные изображения, робот может опубликовать список обнаруженных классов в виде std_msgs/String Сообщения. Например, когда робот обнаруживает такие объекты, как «коробка», «поддон» и «вилочный погрузчик», он публикует эти классы в /ultralytics/detection/classes тема. Затем эта информация может быть использована центральной системой мониторинга для отслеживания запасов в режиме реального времени, оптимизации планирования траектории робота, чтобы избежать препятствий, или инициировать определенные действия, такие как поднятие обнаруженной коробки. Такой подход снижает пропускную способность, необходимую для связи, и фокусируется на передаче критически важных данных.

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

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

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

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

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

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

Использование YOLO с изображениями глубины

В ROS изображения глубины представлены sensor_msgs/Image message type, который включает поля для кодировки, высоты, ширины и пиксельных данных. В поле кодирования для изображений глубины часто используется формат «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

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

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

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

Система отсчета

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

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

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

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

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

Для интеграции YOLO с sensor_msgs/PointCloud2 type, мы можем использовать метод, аналогичный тому, который используется для карт глубины. Используя цветовую информацию, встроенную в облако точек, мы можем извлечь 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 message в два массива numpy. Тем sensor_msgs/PointCloud2 Сообщения содержат n баллов, основанных на width и height полученного изображения. Например, 480 x 640 image будет иметь 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 в массивы 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 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])

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

ВОПРОСЫ И ОТВЕТЫ

Что такое операционная система для роботов (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("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 для таких задач, как обнаружение или сегментация, и публиковать результаты в новых темах.

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

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

Такой подход обеспечивает 3D-визуализацию сегментированных объектов, что полезно для таких задач, как навигация и манипуляции.



Создано 2024-06-19, Обновлено 2024-07-05
Авторы: glenn-jocher (2), RizwanMunawar (1), ambitious-octopus (3)

Комментарии