コンテンツにスキップ

ROS(Robot Operating System)クイックスタートガイド

ROS入門(字幕付き)Open RoboticsVimeoより)。

ROSとは何ですか?

<a href="https://www.ros.org/">Robot Operating System (ROS)は、ロボット工学の研究および産業で広く使用されているオープンソースフレームワークです。ROSは、開発者がロボットアプリケーションを作成するのに役立つ<a href="https://www.ros.org/blog/ecosystem/">ライブラリとツールのコレクションを提供します。ROSは、さまざまな<a href="https://robots.ros.org/">ロボットプラットフォームで動作するように設計されており、ロボット研究者にとって柔軟で強力なツールとなっています。

ROSの主な機能

  1. モジュール式アーキテクチャ:ROSはモジュール式アーキテクチャを採用しており、開発者はノードと呼ばれる再利用可能な小さなコンポーネントを組み合わせることで、複雑なシステムを構築できます。各ノードは通常、特定の機能を実行し、ノードはトピックまたはサービスを介してメッセージを使用して相互に通信します。

  2. 通信ミドルウェア: ROSは、プロセス間通信と分散コンピューティングをサポートする堅牢な通信インフラストラクチャを提供します。これは、データストリーム(トピック)のパブリッシュ-サブスクライブモデルと、サービスコールのリクエスト-リプライモデルを通じて実現されます。

  3. ハードウェアの抽象化: ROSはハードウェア上に抽象化レイヤーを提供し、開発者はデバイスに依存しないコードを作成できます。これにより、同じコードを異なるハードウェア構成で使用でき、統合と実験が容易になります。

  4. ツールとユーティリティ:ROSには、視覚化、デバッグ、シミュレーションのための豊富なツールとユーティリティが付属しています。たとえば、RVizはセンサーデータとロボットの状態情報を視覚化するために使用され、Gazeboはアルゴリズムとロボット設計をテストするための強力なシミュレーション環境を提供します。

  5. 広範なエコシステム: ROSエコシステムは広大で、ナビゲーション、操作、知覚など、さまざまなロボットアプリケーションで利用できる多数のパッケージが継続的に成長しています。コミュニティは、これらのパッケージの開発とメンテナンスに積極的に貢献しています。

ROSバージョンの進化

2007年の開発以来、ROSは複数のバージョンを経て進化し、ロボット工学コミュニティの増大するニーズを満たすために、それぞれが新機能と改善を導入しています。ROSの開発は、ROS 1とROS 2の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 のセットアップ

このガイドは、ROSbot ROSリポジトリのフォークであるこの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を使用して受信画像を処理し、検出されたオブジェクトを検出セグメンテーションのために新しいトピックに公開します。

まず、必要なライブラリをインポートし、2つのモデルをインスタンス化します。1つは セグメンテーション と1つは 検出。(名前を付けて)ROSノードを初期化します ultralytics)を使用してROSマスターとの通信を確立します。安定した接続を確保するために、ノードが接続を確立するのに十分な時間を確保するための短い一時停止を設けています。

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)

2つのROSトピックを初期化: 1つは 検出 と1つは セグメンテーション。これらのトピックは、アノテーション付き画像を公開するために使用され、それらをさらに処理するためにアクセス可能にします。ノード間の通信は、以下を使用して容易になります。 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("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)は、ROS用の強力な3D可視化ツールです。これにより、ロボットの状態とその環境をリアルタイムで可視化できます。RVizを使用すると、センサーデータ(例: sensors_msgs/Image)、ロボットモデルの状態、およびその他のさまざまな種類の情報を提供し、ロボットシステムの動作をデバッグして理解しやすくします。

検出されたクラスを公開(Publish) std_msgs/String

標準ROSメッセージにも以下が含まれます std_msgs/String メッセージに公開します。多くの場合、注釈付き画像全体を再公開する必要はなく、ロボットの視野に存在するクラスのみが必要です。次の例では、 std_msgs/String メッセージ 検出されたクラスを再公開するには、 /ultralytics/detection/classes トピックです。これらのメッセージはより軽量で、重要な情報を提供するため、さまざまなアプリケーションに役立ちます。

ユースケースの例

カメラとオブジェクトを搭載した倉庫ロボットを検討してください 検出モデル。ロボットは、大きなアノテーション付き画像をネットワーク経由で送信する代わりに、検出されたクラスのリストを公開できます。 std_msgs/String メッセージ。たとえば、ロボットが「box」、「pallet」、「forklift」のようなオブジェクトを検出すると、これらのクラスを /ultralytics/detection/classes トピックをサブスクライブします。この情報は、中央監視システムがリアルタイムで在庫を追跡したり、ロボットの経路計画を最適化して障害物を回避したり、検出されたボックスを拾い上げたりするなどの特定のアクションをトリガーするために使用できます。このアプローチにより、通信に必要な帯域幅が削減され、重要なデータの送信に集中できます。

文字列によるステップバイステップの使用法

この例では、Ultralytics YOLOパッケージをROSで使用する方法を示します。この例では、カメラトピックをサブスクライブし、YOLOを使用して受信画像を処理し、検出されたオブジェクトを新しいトピックに公開します。 /ultralytics/detection/classes 使用 std_msgs/String メッセージを使用する方法を示します。この ros_numpy packageは、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 Depth Imagesで使用する

ROSは、RGB画像に加えて、カメラからのオブジェクトの距離に関する情報を提供する深度画像をサポートしています。深度画像は、障害物回避、3Dマッピング、ローカリゼーションなどのロボットアプリケーションにとって非常に重要です。

深度画像は、各ピクセルがカメラからオブジェクトまでの距離を表す画像です。色をキャプチャするRGB画像とは異なり、深度画像は空間情報をキャプチャし、ロボットが環境の3D構造を認識できるようにします。

深度画像の取得

深度画像は、さまざまなセンサーを使用して取得できます。

  1. ステレオカメラ:2台のカメラを使用して、画像のずれに基づいて奥行きを計算します。
  2. Time-of-Flight (ToF) カメラ: 対象物から光が戻るまでの時間を測定します。
  3. 構造化光センサー:パターンを投影し、表面上の変形を測定します。

Depth Images を用いた 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("yolo11m-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)

    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での検出とセグメンテーション

The sensor_msgs/PointCloud2 メッセージタイプ は、3D点群データを表現するためにROSで使用されるデータ構造です。このメッセージタイプは、3Dマッピング、物体認識、ローカリゼーションなどのタスクを可能にする、ロボットアプリケーションに不可欠なものです。

点群とは、3次元座標系内で定義されたデータ点の集合です。これらのデータ点は、3Dスキャン技術を介してキャプチャされた、オブジェクトまたはシーンの外部表面を表します。クラウド内の各ポイントには、 X, Y、および Z 座標。これは空間内の位置に対応し、色や強度などの追加情報も含まれる場合があります。

基準フレーム

作業時に sensor_msgs/PointCloud2、ポイントクラウドデータが取得されたセンサーの参照フレームを考慮することが不可欠です。ポイントクラウドは最初にセンサーの参照フレームでキャプチャされます。この参照フレームは、次のものをリッスンすることで判断できます。 /tf_static トピックです。ただし、特定のアプリケーション要件によっては、点群を別の参照フレームに変換する必要がある場合があります。この変換は、 tf2_ros packageは、座標フレームの管理と、それらの間でのデータ変換のためのツールを提供します。

点群の取得

点群は、さまざまなセンサーを使用して取得できます。

  1. LIDAR (Light Detection and Ranging): レーザーパルスを使用してオブジェクトまでの距離を測定し、高精度の3Dマップを作成します。
  2. 深度カメラ: 各ピクセルの深度情報をキャプチャし、シーンの3D再構築を可能にします。
  3. ステレオカメラ: 2つ以上のカメラを利用して、三角測量によって深度情報を取得します。
  4. 構造化ライトスキャナー: 既知のパターンを表面に投影し、その変形を測定して深度を計算します。

Point Clouds を用いた YOLO の利用

YOLO を統合するには sensor_msgs/PointCloud2 メッセージを入力すると、深度マップに使用されるものと同様の方法を使用できます。ポイントクラウドに埋め込まれたカラー情報を活用することで、2D画像を抽出し、YOLOを使用してこの画像に対してセグメンテーションを実行し、結果として得られるマスクを3次元ポイントに適用して、目的の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("yolo11m-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 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)とは何ですか?

<a href="https://www.ros.org/">Robot Operating System (ROS)は、開発者が堅牢なロボットアプリケーションを作成するのに役立つ、ロボット工学で一般的に使用されるオープンソースフレームワークです。ロボットシステムの構築とインターフェースのための<a href="https://www.ros.org/blog/ecosystem/">ライブラリとツールのコレクションを提供し、複雑なアプリケーションの開発を容易にします。ROSは、<a href="https://wiki.ros.org/ROS/Tutorials/UnderstandingTopics">トピックまたは<a href="https://wiki.ros.org/ROS/Tutorials/UnderstandingServicesParams">サービスを介したメッセージを使用したノード間の通信をサポートしています。

リアルタイムオブジェクト検出のためにUltralytics YOLOをROSと統合するにはどうすればよいですか?

Ultralytics YOLO を ROS と統合するには、ROS 環境をセットアップし、YOLO を使用してセンサーデータを処理します。まず、必要な依存関係をインストールします。 ros_numpy そして、Ultralytics YOLO:

pip install ros_numpy ultralytics

次に、ROSノードを作成し、受信データを処理するためにimage topicをサブスクライブします。最小限の例を次に示します。

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)

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

このアプローチは、セグメント化されたオブジェクトの3D視覚化を提供し、ロボット工学アプリケーションにおけるナビゲーションや操作などのタスクに役立ちます。



📅 1年前に作成 ✏️ 6日前に更新

コメント