Guide de démarrage rapide de ROS (Robot Operating System)

ROS Introduction (captioned) from Open Robotics on Vimeo.

Qu'est-ce que ROS ?

Le Robot Operating System (ROS) est un framework open-source largement utilisé dans la recherche en robotique et dans l'industrie. ROS fournit une collection de bibliothèques et d'outils pour aider les développeurs à créer des applications robotiques. ROS est conçu pour fonctionner avec diverses plates-formes robotiques, ce qui en fait un outil flexible et puissant pour les roboticiens.

Fonctionnalités clés de ROS

  1. Architecture modulaire : ROS possède une architecture modulaire, permettant aux développeurs de construire des systèmes complexes en combinant des composants plus petits et réutilisables appelés nœuds. Chaque nœud effectue généralement une fonction spécifique, et les nœuds communiquent entre eux en utilisant des messages via des topics ou des services.

  2. Middleware de communication : ROS offre une infrastructure de communication robuste qui prend en charge la communication inter-processus et l'informatique distribuée. Cela est réalisé via un modèle de publication-abonnement pour les flux de données (topics) et un modèle de requête-réponse pour les appels de service.

  3. Abstraction matérielle : ROS fournit une couche d'abstraction sur le matériel, permettant aux développeurs d'écrire du code agnostique au périphérique. Cela permet d'utiliser le même code avec différentes configurations matérielles, facilitant l'intégration et l'expérimentation.

  4. Outils et utilitaires : ROS est livré avec un riche ensemble d'outils et d'utilitaires pour la visualisation, le débogage et la simulation. Par exemple, RViz est utilisé pour visualiser les données des capteurs et les informations sur l'état du robot, tandis que Gazebo fournit un environnement de simulation puissant pour tester des algorithmes et des conceptions de robots.

  5. Écosystème étendu : L'écosystème ROS est vaste et en croissance continue, avec de nombreux paquets disponibles pour différentes applications robotiques, incluant la navigation, la manipulation, la perception, et plus encore. La communauté contribue activement au développement et à la maintenance de ces paquets.

Évolution des versions de ROS

Depuis son développement en 2007, ROS a évolué à travers plusieurs versions, chacune introduisant de nouvelles fonctionnalités et améliorations pour répondre aux besoins croissants de la communauté robotique. Le développement de ROS peut être classé en deux séries principales : ROS 1 et ROS 2. Ce guide se concentre sur la version à support à long terme (LTS) de ROS 1, connue sous le nom de ROS Noetic Ninjemys ; le code devrait également fonctionner avec des versions antérieures.

ROS 1 vs. ROS 2

Bien que ROS 1 ait fourni une base solide pour le développement robotique, ROS 2 résout ses lacunes en offrant :

  • Performance en temps réel : Prise en charge améliorée des systèmes temps réel et comportement déterministe.
  • Sécurité : Fonctionnalités de sécurité améliorées pour une utilisation sûre et fiable dans divers environnements.
  • Évolutivité : Meilleure prise en charge des systèmes multi-robots et des déploiements à grande échelle.
  • Prise en charge multiplateforme : Compatibilité étendue avec divers systèmes d'exploitation au-delà de Linux, incluant Windows et macOS.
  • Communication flexible : Utilisation de DDS pour une communication inter-processus plus flexible et efficace.

Messages et topics ROS

Dans ROS, la communication entre les nœuds est facilitée par des messages et des topics. Un message est une structure de données qui définit les informations échangées entre les nœuds, tandis qu'un topic est un canal nommé sur lequel les messages sont envoyés et reçus. Les nœuds peuvent publier des messages sur un topic ou s'abonner à des messages provenant d'un topic, ce qui leur permet de communiquer entre eux. Ce modèle de publication-abonnement permet une communication asynchrone et un découplage entre les nœuds. Chaque capteur ou actionneur dans un système robotique publie généralement des données sur un topic, qui peut ensuite être consommé par d'autres nœuds pour le traitement ou le contrôle. Pour les besoins de ce guide, nous nous concentrerons sur les messages Image, Depth et PointCloud et sur les topics de caméra.

Configuration d'Ultralytics YOLO avec ROS

Ce guide a été testé en utilisant cet environnement ROS, qui est un fork du dépôt ROSbot ROS. Cet environnement inclut le paquet Ultralytics YOLO, un conteneur Docker pour une installation facile, des paquets ROS complets et des mondes Gazebo pour des tests rapides. Il est conçu pour fonctionner avec le Husarion ROSbot 2 PRO. Les exemples de code fournis fonctionneront dans n'importe quel environnement ROS Noetic/Melodic, qu'il s'agisse de simulation ou de conditions réelles.

Husarion ROSbot 2 PRO autonomous robot platform

Installation des dépendances

En dehors de l'environnement ROS, tu devras installer les dépendances suivantes :

  • Paquet ROS NumPy : Il est requis pour une conversion rapide entre les messages ROS Image et les tableaux NumPy.

    pip install ros_numpy
  • Paquet Ultralytics :

    pip install ultralytics

Utilise Ultralytics avec sensor_msgs/Image de 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

Utilisation étape par étape de l'image

L'extrait de code suivant démontre comment utiliser le paquet Ultralytics YOLO avec ROS. Dans cet exemple, nous nous abonnons à un topic de caméra, traitons l'image entrante en utilisant YOLO, et publions les objets détectés sur de nouveaux topics pour la détection et la segmentation.

D'abord, importe les bibliothèques nécessaires et instancie deux modèles : un pour la segmentation et un pour la détection. Initialise un nœud ROS (avec le nom ultralytics) pour permettre la communication avec le maître ROS. Pour assurer une connexion stable, nous incluons une brève pause, donnant au nœud suffisamment de temps pour établir la connexion avant de continuer.

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)

Initialise deux topics ROS : un pour la détection et un pour la segmentation. Ces topics seront utilisés pour publier les images annotées, les rendant accessibles pour un traitement ultérieur. La communication entre les nœuds est facilitée par l'utilisation de messages 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)

Enfin, crée un abonné qui écoute les messages sur le topic /camera/color/image_raw et appelle une fonction de rappel pour chaque nouveau message. Cette fonction de rappel reçoit des messages de type sensor_msgs/Image, les convertit en un tableau NumPy en utilisant ros_numpy, traite les images avec les modèles YOLO précédemment instanciés, annote les images, puis les publie sur les topics respectifs : /ultralytics/detection/image pour la détection et /ultralytics/segmentation/image pour la segmentation.

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()
Code complet
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()
Débogage

Le débogage des nœuds ROS (Robot Operating System) peut être difficile en raison de la nature distribuée du système. Plusieurs outils peuvent t'aider dans ce processus :

  1. rostopic echo <TOPIC-NAME> : Cette commande te permet de voir les messages publiés sur un topic spécifique, t'aidant à inspecter le flux de données.
  2. rostopic list : Utilise cette commande pour lister tous les topics disponibles dans le système ROS, te donnant une vue d'ensemble des flux de données actifs.
  3. rqt_graph : Cet outil de visualisation affiche le graphe de communication entre les nœuds, fournissant des aperçus sur la manière dont les nœuds sont interconnectés et comment ils interagissent.
  4. Pour des visualisations plus complexes, comme des représentations 3D, tu peux utiliser RViz. RViz (ROS Visualization) est un puissant outil de visualisation 3D pour ROS. Il te permet de visualiser l'état de ton robot et de son environnement en temps réel. Avec RViz, tu peux voir les données des capteurs (par exemple, sensor_msgs/Image), les états du modèle de robot et divers autres types d'informations, facilitant le débogage et la compréhension du comportement de ton système robotique.

Publier les classes détectées avec 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.

Exemple de cas d'utilisation

Considère un robot d'entrepôt équipé d'une caméra et d'un modèle de détection. Au lieu d'envoyer de grandes images annotées sur le réseau, le robot peut publier une liste de classes détectées sous forme de messages std_msgs/String. Par exemple, lorsque le robot détecte des objets comme "box", "pallet" et "forklift", il publie ces classes sur le topic /ultralytics/detection/classes. Cette information peut ensuite être utilisée par un système de surveillance central pour suivre l'inventaire en temps réel, optimiser la planification de chemin du robot pour éviter les obstacles, ou déclencher des actions spécifiques comme le ramassage d'une boîte détectée. Cette approche réduit la bande passante requise pour la communication et se concentre sur la transmission de données critiques.

Utilisation étape par étape des chaînes

Cet exemple démontre comment utiliser le paquet Ultralytics YOLO avec ROS. Dans cet exemple, nous nous abonnons à un topic de caméra, traitons l'image entrante en utilisant YOLO, et publions les objets détectés sur un nouveau topic /ultralytics/detection/classes en utilisant des messages std_msgs/String. Le paquet ros_numpy est utilisé pour convertir le message Image ROS en un tableau NumPy pour le traitement avec 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()

Utiliser Ultralytics avec les images de profondeur ROS

En plus des images RGB, ROS prend en charge les images de profondeur, qui fournissent des informations sur la distance des objets par rapport à la caméra. Les images de profondeur sont cruciales pour les applications robotiques telles que l'évitement d'obstacles, la cartographie 3D et la localisation.

Une image de profondeur est une image où chaque pixel représente la distance de la caméra à un objet. Contrairement aux images RGB qui capturent la couleur, les images de profondeur capturent des informations spatiales, permettant aux robots de percevoir la structure 3D de leur environnement.

Obtention d'images de profondeur

Les images de profondeur peuvent être obtenues en utilisant divers capteurs :

  1. Caméras stéréo : Utilisent deux caméras pour calculer la profondeur en fonction de la disparité d'image.
  2. Caméras à temps de vol (ToF) : Mesurent le temps que la lumière met pour revenir d'un objet.
  3. Capteurs à lumière structurée : Projettent un motif et mesurent sa déformation sur les surfaces.

Utiliser YOLO avec des images de profondeur

Dans ROS, les images de profondeur sont représentées par le type de message sensor_msgs/Image, qui inclut des champs pour l'encodage, la hauteur, la largeur et les données de pixels. Le champ d'encodage pour les images de profondeur utilise souvent un format comme "16UC1", indiquant un entier non signé de 16 bits par pixel, où chaque valeur représente la distance jusqu'à l'objet. Les images de profondeur sont couramment utilisées en conjonction avec les images RGB pour fournir une vue plus complète de l'environnement.

Avec YOLO, il est possible d'extraire et de combiner des informations à partir des images RGB et de profondeur. Par exemple, YOLO peut détecter des objets dans une image RGB, et cette détection peut être utilisée pour identifier les régions correspondantes dans l'image de profondeur. Cela permet l'extraction d'informations de profondeur précises pour les objets détectés, améliorant la capacité du robot à comprendre son environnement en trois dimensions.

Caméras RGB-D

Lorsque tu travailles avec des images de profondeur, il est essentiel de s'assurer que les images RGB et de profondeur sont correctement alignées. Les caméras RGB-D, telles que la série Intel RealSense, fournissent des images RGB et de profondeur synchronisées, ce qui facilite la combinaison d'informations provenant des deux sources. Si tu utilises des caméras RGB et de profondeur séparées, il est crucial de les calibrer pour assurer un alignement précis.

Utilisation étape par étape de la profondeur

Dans cet exemple, nous utilisons YOLO pour segmenter une image et appliquons le masque extrait pour segmenter l'objet dans l'image de profondeur. Cela nous permet de déterminer la distance de chaque pixel de l'objet d'intérêt par rapport au centre focal de la caméra. En obtenant cette information de distance, nous pouvons calculer la distance entre la caméra et l'objet spécifique dans la scène. Commence par importer les bibliothèques nécessaires, créer un nœud ROS, et instancier un modèle de segmentation et un 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)

Ensuite, définis une fonction de rappel qui traite le message d'image de profondeur entrant. La fonction attend les messages d'image de profondeur et d'image RGB, les convertit en tableaux NumPy, et applique le modèle de segmentation à l'image RGB. Elle extrait ensuite le masque de segmentation pour chaque objet détecté et calcule la distance moyenne de l'objet par rapport à la caméra en utilisant l'image de profondeur. La plupart des capteurs ont une distance maximale, connue sous le nom de distance de clip, au-delà de laquelle les valeurs sont représentées par inf (np.inf). Avant le traitement, il est important de filtrer ces valeurs nulles et de leur assigner une valeur de 0. Enfin, elle publie les objets détectés ainsi que leurs distances moyennes sur le 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()
Code complet
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()

Utiliser Ultralytics avec sensor_msgs/PointCloud2 de 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.

Un nuage de points est une collection de points de données définis dans un système de coordonnées tridimensionnel. Ces points de données représentent la surface externe d'un objet ou d'une scène, capturée via des technologies de numérisation 3D. Chaque point dans le nuage a des coordonnées X, Y et Z, qui correspondent à sa position dans l'espace, et peut également inclure des informations supplémentaires telles que la couleur et l'intensité.

Cadre de référence

Lorsque tu travailles avec sensor_msgs/PointCloud2, il est essentiel de considérer le cadre de référence du capteur à partir duquel les données de nuage de points ont été acquises. Le nuage de points est initialement capturé dans le cadre de référence du capteur. Tu peux déterminer ce cadre de référence en écoutant le topic /tf_static. Cependant, selon les exigences spécifiques de ton application, tu pourrais avoir besoin de convertir le nuage de points dans un autre cadre de référence. Cette transformation peut être réalisée en utilisant le paquet tf2_ros, qui fournit des outils pour gérer les cadres de coordonnées et transformer les données entre eux.

Obtention de nuages de points

Les nuages de points peuvent être obtenus en utilisant divers capteurs :

  1. LIDAR (Light Detection and Ranging) : Utilise des impulsions laser pour mesurer les distances aux objets et créer des cartes 3D de haute précision.
  2. Caméras de profondeur : Capturent des informations de profondeur pour chaque pixel, permettant la reconstruction 3D de la scène.
  3. Caméras stéréo : Utilisent deux caméras ou plus pour obtenir des informations de profondeur par triangulation.
  4. Scanners à lumière structurée : Projettent un motif connu sur une surface et mesurent la déformation pour calculer la profondeur.

Utiliser YOLO avec des nuages de points

Pour intégrer YOLO avec les messages de type sensor_msgs/PointCloud2, nous pouvons employer une méthode similaire à celle utilisée pour les cartes de profondeur. En tirant parti des informations de couleur intégrées dans le nuage de points, nous pouvons extraire une image 2D, effectuer une segmentation sur cette image en utilisant YOLO, puis appliquer le masque résultant aux points tridimensionnels pour isoler l'objet 3D d'intérêt.

Pour la gestion des nuages de points, nous recommandons d'utiliser Open3D (pip install open3d), une bibliothèque Python conviviale. Open3D fournit des outils robustes pour gérer les structures de données de nuages de points, les visualiser et exécuter des opérations complexes de manière transparente. Cette bibliothèque peut simplifier considérablement le processus et améliorer notre capacité à manipuler et analyser les nuages de points en conjonction avec la segmentation basée sur YOLO.

Utilisation étape par étape des nuages de points

Importe les bibliothèques nécessaires et instancie le modèle YOLO pour la segmentation.

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.

La fonction renvoie les coordonnées xyz et les valeurs RGB au format de la résolution de la caméra d'origine (width x height). La plupart des capteurs ont une distance maximale, connue sous le nom de distance de clip, au-delà de laquelle les valeurs sont représentées par inf (np.inf). Avant le traitement, il est important de filtrer ces valeurs nulles et de leur assigner une valeur de 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

Ensuite, abonne-toi au topic /camera/depth/points pour recevoir le message de nuage de points et convertis le message sensor_msgs/PointCloud2 en tableaux NumPy contenant les coordonnées XYZ et les valeurs RGB (en utilisant la fonction pointcloud2_to_array). Traite l'image RGB en utilisant le modèle YOLO pour extraire les objets segmentés. Pour chaque objet détecté, extrais le masque de segmentation et applique-le à la fois à l'image RGB et aux coordonnées XYZ pour isoler l'objet dans l'espace 3D.

Le traitement du masque est simple puisqu'il consiste en des valeurs binaires, avec 1 indiquant la présence de l'objet et 0 indiquant l'absence. Pour appliquer le masque, multiplie simplement les canaux originaux par le masque. Cette opération isole efficacement l'objet d'intérêt au sein de l'image. Enfin, crée un objet de nuage de points Open3D et visualise l'objet segmenté dans l'espace 3D avec les couleurs associées.

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

FAQ

Qu'est-ce que le Robot Operating System (ROS) ?

Le Robot Operating System (ROS) est un framework open-source couramment utilisé en robotique pour aider les développeurs à créer des applications robotiques robustes. Il fournit une collection de bibliothèques et d'outils pour construire et s'interfacer avec des systèmes robotiques, permettant un développement plus facile d'applications complexes. ROS prend en charge la communication entre les nœuds en utilisant des messages via des topics ou des services.

Comment puis-je intégrer Ultralytics YOLO avec ROS pour la détection d'objets en temps réel ?

L'intégration d'Ultralytics YOLO avec ROS implique la configuration d'un environnement ROS et l'utilisation de YOLO pour traiter les données des capteurs. Commence par installer les dépendances requises comme ros_numpy et Ultralytics YOLO :

pip install ros_numpy ultralytics

Ensuite, crée un nœud ROS et abonne-toi à un topic d'image pour traiter les données entrantes. Voici un exemple minimal :

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

Qu'est-ce que les topics ROS et comment sont-ils utilisés dans Ultralytics YOLO ?

Les topics ROS facilitent la communication entre les nœuds dans un réseau ROS en utilisant un modèle de publication-abonnement. Un topic est un canal nommé que les nœuds utilisent pour envoyer et recevoir des messages de manière asynchrone. Dans le contexte d'Ultralytics YOLO, tu peux faire en sorte qu'un nœud s'abonne à un topic d'image, traite les images en utilisant YOLO pour des tâches comme la détection ou la segmentation, et publie les résultats sur de nouveaux topics.

Par exemple, abonne-toi à un topic de caméra et traite l'image entrante pour la détection :

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

Pourquoi utiliser des images de profondeur avec Ultralytics YOLO dans ROS ?

Les images de profondeur dans ROS, représentées par sensor_msgs/Image, fournissent la distance des objets par rapport à la caméra, ce qui est crucial pour des tâches comme l'évitement d'obstacles, la cartographie 3D et la localisation. En utilisant des informations de profondeur ainsi que des images RGB, les robots peuvent mieux comprendre leur environnement 3D.

Avec YOLO, tu peux extraire des masques de segmentation à partir d'images RGB et appliquer ces masques aux images de profondeur pour obtenir des informations précises sur les objets 3D, améliorant la capacité du robot à naviguer et à interagir avec son entourage.

Comment puis-je visualiser des nuages de points 3D avec YOLO dans ROS ?

Pour visualiser des nuages de points 3D dans ROS avec YOLO :

  1. Convertis les messages sensor_msgs/PointCloud2 en tableaux NumPy.
  2. Utilise YOLO pour segmenter les images RGB.
  3. Applique le masque de segmentation au nuage de points.

Voici un exemple utilisant Open3D pour la visualisation :

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

Cette approche fournit une visualisation 3D des objets segmentés, utile pour des tâches telles que la navigation et la manipulation dans des applications robotiques.

Commentaires