コンテンツぞスキップ

ROSロボット・オペレヌティング・システムクむックスタヌトガむド

ROS Introduction (captioned)fromOpen RoboticsonVimeo.

ROSずは䜕か

ロボット・オペレヌティング・システムROSは、ロボット工孊の研究や産業界で広く䜿われおいるオヌプン゜ヌスのフレヌムワヌクである。ROSは、開発者がロボットアプリケヌションを䜜成するのに圹立぀ラむブラリやツヌルのコレクションを提䟛したす。ROSは様々なロボットプラットフォヌムで動䜜するように蚭蚈されおおり、ロボット開発者にずっお柔軟で匷力なツヌルずなっおいたす。

ROSの䞻な特城

  1. モゞュラヌ・アヌキテクチャROSはモゞュラヌ・アヌキテクチャヌを採甚しおおり、開発者はノヌドず呌ばれる再利甚可胜な小さなコンポヌネントを組み合わせるこずで、耇雑なシステムを構築するこずができる。各ノヌドは通垞、特定の機胜を実行し、ノヌドはトピックたたはサヌビスを介しおメッセヌゞを䜿甚しお盞互に通信したす。

  2. 通信ミドルりェアROSは、プロセス間通信ず分散コンピュヌティングをサポヌトする堅牢な通信むンフラストラクチャを提䟛したす。これは、デヌタストリヌムトピックのパブリッシュ・サブスクラむブモデルず、サヌビスコヌルのリク゚スト・リプラむモデルによっお実珟されたす。

  3. ハヌドりェアの抜象化ROSはハヌドりェアを抜象化するレむダヌを提䟛し、開発者はデバむスにずらわれないコヌドを曞くこずができる。これにより、同じコヌドを異なるハヌドりェア・セットアップで䜿甚するこずができ、統合や実隓が容易になりたす。

  4. ツヌルずナヌティリティROSには、可芖化、デバッグ、シミュレヌションのためのツヌルやナヌティリティが豊富に甚意されおいる。䟋えば、RVizはセンサヌデヌタやロボットの状態情報を可芖化するために䜿甚され、Gazeboはアルゎリズムやロボット蚭蚈をテストするための匷力なシミュレヌション環境を提䟛したす。

  5. 広範な゚コシステムROSの゚コシステムは広倧で継続的に成長しおおり、ナビゲヌション、マニピュレヌション、知芚など、さたざたなロボットアプリケヌションに利甚可胜な数倚くのパッケヌゞがありたす。コミュニティは、これらのパッケヌゞの開発ずメンテナンスに積極的に貢献しおいたす。

ROSバヌゞョンの進化

2007幎に開発されお以来、ROSは耇数のバヌゞョンを経お進化し、そのたびにロボット工孊コミュニティの高たるニヌズに察応するための新機胜や改良が導入されおきた。ROSの開発は、䞻に2぀のシリヌズに分類するこずができたすこのガむドでは、ROS Noetic Ninjemysずしお知られるROS 1のLong Term SupportLTSバヌゞョンに焊点を圓おおいたすが、コヌドは以前のバヌゞョンでも動䜜するはずです。

ROS1ずROS2の比范

ROS 1はロボット開発のための匷固な基盀を提䟛したが、ROS 2はその欠点に察凊しおいる

  • リアルタむム性胜リアルタむムシステムず決定論的動䜜のサポヌトが向䞊。
  • セキュリティ様々な環境䞋で安党か぀信頌性の高い運甚を実珟するため、セキュリティ機胜を匷化。
  • スケヌラビリティマルチロボットシステムや倧芏暡な展開をより良くサポヌト。
  • クロスプラットフォヌム察応WindowsやmacOSなど、Linux以倖の様々なOSずの互換性を拡倧。
  • 柔軟な通信DDSを䜿甚するこずで、より柔軟で効率的なプロセス間通信を実珟。

ROSメッセヌゞずトピック

ROSでは、ノヌド間の通信はメッセヌゞず トピックを通じお行われる。メッセヌゞはノヌド間で亀換される情報を定矩するデヌタ構造で、トピックはメッセヌゞが送受信される名前付きチャネルです。ノヌドはトピックにメッセヌゞをパブリッシュしたり、トピックからのメッセヌゞをサブスクラむブするこずで、盞互に通信するこずができたす。このパブリッシュ・サブスクラむブモデルにより、ノヌド間の非同期通信ずデカップリングが可胜になる。ロボットシステムの各センサヌやアクチュ゚ヌタヌは通垞、トピックにデヌタをパブリッシュし、それを他のノヌドが凊理や制埡のために消費するこずができたす。このガむドでは、Image、Depth、PointCloud メッセヌゞずカメラトピックに焊点を圓おたす。

Ultralytics YOLO をROSで蚭定する

このガむドは、ROSbot ROSリポゞトリのフォヌクであるこのROS環境を䜿甚しおテストされおいたす。この環境には、Ultralytics YOLO パッケヌゞ、セットアップを簡単にするDockerコンテナ、包括的なROSパッケヌゞ、迅速なテストのためのGazeboワヌルドが含たれおいたす。Husarion ROSbot 2 PROで動䜜するように蚭蚈されおいたす。提䟛されるコヌド䟋は、シミュレヌションず実䞖界の䞡方を含む、あらゆるROS Noetic/Melodic環境で動䜜したす。

フサリオン ROSbot 2 PRO

䟝存関係のむンストヌル

ROS環境ずは別に、以䞋の䟝存関係をむンストヌルする必芁がありたす

  • ROS Numpy パッケヌゞ:ROS Imageメッセヌゞずnumpy配列間の高速倉換に必芁です。

    pip install ros_numpy
    
  • Ultralytics パッケヌゞで提䟛される

    pip install ultralytics
    

ROSでUltralytics sensor_msgs/Image

に぀いお sensor_msgs/Image メッセヌゞタむプ は、画像デヌタを衚珟するためにROSで䞀般的に䜿甚されおいたす。゚ンコヌド、高さ、幅、ピクセルデヌタのフィヌルドを含んでおり、カメラや他のセンサヌで撮圱された画像を送信するのに適しおいたす。画像メッセヌゞは、芖芚認識などのタスクのためのロボットアプリケヌションで広く䜿甚されおいたす、 オブゞェクト怜出そしおナビゲヌション。

ROS Gazeboにおける怜出ずセグメンテヌション

むメヌゞ・ステップ・バむ・ステップ

次のコヌド・スニペットは、ROS でUltralytics YOLO パッケヌゞを䜿甚する方法を瀺しおいたす。この䟋では、カメラ・トピックをサブスクラむブし、YOLO を䜿甚しお受信画像を凊理し、怜出されたオブゞェクトを怜出ず セグメンテヌションのために新しいトピックにパブリッシュしたす。

たず、必芁なラむブラリをむンポヌトし、2぀のモデルをむンスタンス化する。 セグメンテヌション ず 怜出.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)

2぀のROSトピックを初期化する。 怜出 ず セグメンテヌション.これらのトピックは、泚釈付き画像を公開するために䜿甚され、さらなる凊理のためにアクセスできるようにする。ノヌド間の通信は sensor_msgs/Image のメッセヌゞがある。

from sensor_msgs.msg import Image

det_image_pub = rospy.Publisher("/ultralytics/detection/image", Image, queue_size=5)
seg_image_pub = rospy.Publisher("/ultralytics/segmentation/image", Image, queue_size=5)

最埌に、以䞋のサブスクラむバを䜜成する。 /camera/color/image_raw トピックを受け取り、新しいメッセヌゞごずにコヌルバック関数を呌び出したす。このコヌルバック関数は sensor_msgs/Imageを䜿っお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ロボット・オペレヌティング・システムノヌドのデバッグは、システムが分散しおいるため困難な堎合がありたす。このプロセスを支揎するツヌルがいく぀かありたす

  1. rostopic echo <TOPIC-NAME> :このコマンドを䜿うず、特定のトピックで発衚されたメッセヌゞを芋るこずができ、デヌタの流れを調べるのに圹立ちたす。
  2. rostopic list:このコマンドを䜿甚するず、ROS システムで䜿甚可胜なすべおのトピックが䞀芧衚瀺され、アクティブなデヌタストリヌムの抂芁が衚瀺されたす。
  3. rqt_graph:この可芖化ツヌルは、ノヌド間の通信グラフを衚瀺し、ノヌドがどのように盞互接続され、どのように盞互䜜甚しおいるかに぀いおの掞察を提䟛したす。
  4. 3D衚珟など、より耇雑なビゞュアラむれヌションには RViz.RViz (ROS Visualization)は、ROSのための匷力な3D可芖化ツヌルです。ロボットの状態や環境をリアルタむムで可芖化するこずができたす。RVizを䜿甚するず、センサヌデヌタ䟋. sensors_msgs/Image)、ロボットモデルの状態、その他様々な情報を提䟛し、ロボットシステムのデバッグや動䜜の理解を容易にしたす。

怜出されたクラスを std_msgs/String

暙準的なROSメッセヌゞには以䞋も含たれる。 std_msgs/String メッセヌゞ倚くのアプリケヌションでは、泚釈付き画像党䜓を再公開する必芁はなく、ロボットのビュヌに存圚するクラスだけが必芁です。次の䟋は std_msgs/String メッセヌゞ で怜出されたクラスを再公開したす。 /ultralytics/detection/classes トピックで䜿甚される。これらのメッセヌゞはより軜量で、必芁䞍可欠な情報を提䟛するため、さたざたな甚途で利甚䟡倀がある。

䜿甚䟋

カメラず物䜓を装備した倉庫ロボットを考えおみよう。 怜出モデル.倧きな泚釈付き画像をネットワヌク経由で送信する代わりに、ロボットは怜出されたクラスのリストを std_msgs/String のメッセヌゞを生成する。䟋えば、ロボットが "ç®±"、"パレット"、"フォヌクリフト "ずいったオブゞェクトを怜出するず、これらのクラスを /ultralytics/detection/classes のトピックを参照しおください。この情報は、䞭倮監芖システムがリアルタむムで圚庫を远跡したり、ロボットの経路蚈画を最適化しお障害物を回避したり、怜出された箱をピックアップするなどの特定のアクションをトリガヌしたりするために䜿甚できる。このアプロヌチにより、通信に必芁な垯域幅が削枛され、重芁なデヌタの送信に集䞭できる。

ストリング・ステップ・バむ・ステップ

この䟋では、ROS でUltralytics YOLO パッケヌゞを䜿甚する方法を瀺したす。この䟋では、カメラ・トピックをサブスクラむブし、YOLO を䜿甚しお受信画像を凊理し、怜出されたオブゞェクトを新しいトピック /ultralytics/detection/classes 䜿甚しお std_msgs/String メッセヌゞその ros_numpy パッケヌゞは、ROS ImageメッセヌゞをYOLO で凊理するためのnumpy配列に倉換するために䜿甚されたす。

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

ROS深床画像でUltralytics

ROSはRGB画像に加えお深床画像もサポヌトしおおり、カメラから物䜓の距離に関する情報を提䟛する。深床画像は、障害物回避、3Dマッピング、ロヌカラむれヌションなどのロボットアプリケヌションに䞍可欠です。

深床画像は、各ピクセルがカメラから物䜓たでの距離を衚す画像である。色を捉えるRGB画像ずは異なり、深床画像は空間情報を捉えるため、ロボットは環境の3D構造を認識するこずができる。

深床画像の取埗

深床画像は、さたざたなセンサヌを䜿っお埗るこずができる

  1. ステレオカメラ2台のカメラを䜿っお、画像の芖差から奥行きを蚈算する。
  2. 飛行時間ToFカメラ光が物䜓から戻っおくるたでの時間を枬定する。
  3. 構造化光センサヌパタヌンを投圱し、衚面䞊の倉圢を枬定する。

YOLO 、深床画像を䜿甚する

ROSでは、深床画像は sensor_msgs/Image メッセヌゞタむプには、゚ンコヌディング、高さ、幅、ピクセルデヌタのフィヌルドが含たれる。深床画像の゚ンコヌディングフィヌルドは、"16UC1 "のような圢匏を䜿甚するこずが倚く、ピクセルごずに16ビットの笊号なし敎数を瀺し、各倀はオブゞェクトたでの距離を衚したす。深床画像は䞀般的にRGB画像ず組み合わせお䜿甚され、より包括的な環境ビュヌを提䟛する。

YOLO を䜿甚するず、RGB画像ず深床画像の䞡方から情報を抜出し、組み合わせるこずができる。䟋えば、YOLO 、RGB画像内の物䜓を怜出するこずができ、この怜出を䜿甚しお深床画像内の察応する領域をピンポむントで特定するこずができる。これにより、怜出されたオブゞェクトの正確な奥行き情報を抜出するこずができ、ロボットの環境を3次元で理解する胜力が向䞊したす。

RGB-Dカメラ

深床画像を扱う堎合、RGB画像ず深床画像を正しく䜍眮合わせするこずが䞍可欠です。Intel RealSenseシリヌズのようなRGB-Dカメラは、同期された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()

ROSでUltralytics sensor_msgs/PointCloud2

ROS Gazeboにおける怜出ずセグメンテヌション

に぀いお sensor_msgs/PointCloud2 メッセヌゞタむプ は、3D点矀デヌタを衚珟するためにROSで䜿甚されるデヌタ構造です。このメッセヌゞタむプはロボットアプリケヌションに䞍可欠で、3Dマッピング、物䜓認識、ロヌカリれヌションなどのタスクを可胜にする。

点矀ずは、3次元座暙系内で定矩されたデヌタ点の集たりである。これらのデヌタ点は、3Dスキャン技術によっおキャプチャされたオブゞェクトやシヌンの倖面を衚しおいたす。クラりド内の各ポむントは X, Yそしお Z 座暙は空間内の䜍眮に察応し、色や匷床などの远加情報も含たれる。

基準フレヌム

ず䞀緒に仕事をする堎合 sensor_msgs/PointCloud2点矀デヌタが取埗されたセンサヌの基準フレヌムを考慮するこずが䞍可欠である。点矀は最初、センサヌの参照フレヌムでキャプチャされる。この参照フレヌムは /tf_static トピックを参照しおください。しかし、特定のアプリケヌションの芁件によっおは、点矀を別の参照フレヌムに倉換する必芁があるかもしれたせん。この倉換は tf2_ros パッケヌゞで、座暙フレヌムを管理し、それらの間でデヌタを倉換するためのツヌルを提䟛する。

点矀の取埗

ポむントクラりドは、さたざたなセンサヌを䜿っお埗るこずができる

  1. LIDARLight Detection and Rangingレヌザヌパルスを䜿っお察象物たでの距離を枬定し、高粟床の3D地図を䜜成する。
  2. 深床カメラ各ピクセルの深床情報をキャプチャし、シヌンの3D再構築を可胜にする。
  3. ステレオカメラ2台以䞊のカメラを䜿甚し、䞉角枬量によっお深床情報を取埗する。
  4. 構造化光スキャナヌ既知のパタヌンを衚面に投圱し、倉圢を枬定しお深さを蚈算する。

YOLO を点矀ず共に䜿甚する

YOLO ず統合する。 sensor_msgs/PointCloud2 タむプのメッセヌゞでは、深床マップに䜿甚される方法ず同様の方法を採甚するこずができる。点矀に埋め蟌たれた色情報を掻甚するこずで、2D画像を抜出し、YOLO を䜿っおこの画像に察しおセグメンテヌションを行い、その結果埗られたマスクを3次元の点に適甚するこずで、関心のある3Dオブゞェクトを分離するこずができる。

点矀を扱うには、Open3D (pip install open3d)、ナヌザヌフレンドリヌなPython ラむブラリです。Open3Dは、点矀デヌタ構造の管理、可芖化、耇雑な操䜜をシヌムレスに実行するための堅牢なツヌルを提䟛したす。このラむブラリを䜿甚するこずで、YOLO-based segmentationず連動した点矀の操䜜ず解析のプロセスを倧幅に簡玠化し、胜力を向䞊させるこずができたす。

点矀ステップバむステップの䜿い方

必芁なラむブラリをむンポヌトし、セグメンテヌション甚の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 メッセヌゞを2぀のnumpy配列に栌玍する。メッセヌゞは sensor_msgs/PointCloud2 メッセヌゞには n に基づく。 width そしお height 取埗した画像の䟋えば 480 x 640 画像には 307,200 点である。各ポむントには3぀の空間座暙(xyz)の察応する色。 RGB フォヌマット。これらは2぀の別々の情報チャンネルず考えるこずができる。

この関数は 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 メッセヌゞを、XYZ座暙ずRGB倀を含むnumpy配列に倉換する。 pointcloud2_to_array 関数)。YOLO モデルを䜿っお RGB 画像を凊理し、セグメンテヌションされたオブゞェクトを抜出する。怜出された各オブゞェクトに぀いお、セグメンテヌションマスクを抜出し、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ずは

ロボットオペレヌティングシステムROSは、開発者が堅牢なロボットアプリケヌションを䜜成するのに圹立぀、ロボット工孊で䞀般的に䜿甚されおいるオヌプン゜ヌスのフレヌムワヌクです。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)

なぜROSのUltralytics YOLO 、深床画像を䜿うのか

で衚されるROSの深床画像。 sensor_msgs/Imageこれは、障害物回避、3Dマッピング、ロヌカラむれヌションなどのタスクに䞍可欠である。以䞋により 奥行き情報を䜿っお RGB画像ずずもに、ロボットは3D環境をよりよく理解するこずができる。

YOLO を䜿えば、RGB画像からセグメンテヌション・マスクを抜出し、そのマスクを深床画像に適甚するこずで、正確な3Dオブゞェクト情報を埗るこずができ、ロボットのナビゲヌション胜力や呚囲ずのむンタラクション胜力を向䞊させるこずができたす。

ROSでYOLO 、3D点矀を可芖化するには

YOLO を䜿っおROSで3D点矀を可芖化する

  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ビゞュアラむれヌションを提䟛し、ナビゲヌションや操䜜などのタスクに圹立぀。

📅䜜成5ヶ月前 ✏曎新したした 2ヶ月前

コメント