ROS (Robot Operating System) 퀵스타트 가이드

ROS Introduction (captioned) from Open Robotics on Vimeo.

ROS란 무엇입니까?

Robot Operating System (ROS)는 로봇 공학 연구 및 산업 분야에서 널리 사용되는 오픈 소스 프레임워크입니다. ROS는 개발자가 로봇 애플리케이션을 생성할 수 있도록 돕는 라이브러리 및 도구 모음을 제공합니다. ROS는 다양한 로봇 플랫폼과 연동되도록 설계되어 있어 로봇 공학자들에게 유연하고 강력한 도구를 제공합니다.

ROS의 주요 기능

  1. 모듈식 아키텍처: ROS는 모듈식 아키텍처를 갖추고 있어 개발자가 노드라고 불리는 재사용 가능한 작은 구성 요소를 조합하여 복잡한 시스템을 구축할 수 있습니다. 각 노드는 일반적으로 특정 기능을 수행하며, 노드 간에는 토픽이나 서비스를 통해 메시지를 주고받으며 통신합니다.

  2. 통신 미들웨어: ROS는 프로세스 간 통신 및 분산 컴퓨팅을 지원하는 강력한 통신 인프라를 제공합니다. 이는 데이터 스트림(토픽)을 위한 발행-구독 모델과 서비스 호출을 위한 요청-응답 모델을 통해 구현됩니다.

  3. 하드웨어 추상화: ROS는 하드웨어에 대한 추상화 계층을 제공하여 개발자가 특정 장치에 종속되지 않는 코드를 작성할 수 있게 합니다. 이를 통해 동일한 코드를 다양한 하드웨어 설정에서 사용할 수 있으며 통합 및 실험 과정을 간소화합니다.

  4. 도구 및 유틸리티: ROS는 시각화, 디버깅, 시뮬레이션을 위한 풍부한 도구 및 유틸리티 세트와 함께 제공됩니다. 예를 들어, RViz는 센서 데이터와 로봇 상태 정보를 시각화하는 데 사용되며, Gazebo는 알고리즘과 로봇 설계를 테스트하기 위한 강력한 시뮬레이션 환경을 제공합니다.

  5. 방대한 생태계: ROS 생태계는 방대하며 지속적으로 성장하고 있습니다. 내비게이션, 매니퓰레이션, 인식 등 다양한 로봇 애플리케이션을 위한 수많은 패키지를 사용할 수 있습니다. 커뮤니티는 이러한 패키지의 개발과 유지보수에 적극적으로 기여하고 있습니다.

ROS 버전의 발전

2007년 개발된 이후, ROS는 여러 버전을 거치며 발전해 왔으며 각 버전은 로봇 커뮤니티의 증가하는 요구를 충족하기 위해 새로운 기능과 개선 사항을 도입했습니다. ROS의 개발은 크게 ROS 1과 ROS 2라는 두 가지 시리즈로 분류할 수 있습니다. 이 가이드는 ROS Noetic Ninjemys로 알려진 ROS 1의 장기 지원(LTS) 버전에 초점을 맞추고 있으며, 코드는 이전 버전에서도 작동할 수 있습니다.

ROS 1 vs. ROS 2

ROS 1이 로봇 개발을 위한 견고한 기반을 제공했지만, ROS 2는 다음과 같은 기능을 제공하여 그 한계를 해결합니다:

  • 실시간 성능: 실시간 시스템 및 결정론적 동작에 대한 지원 개선.
  • 보안: 다양한 환경에서 안전하고 신뢰할 수 있는 작동을 위한 향상된 보안 기능.
  • 확장성: 다중 로봇 시스템 및 대규모 배포에 대한 더 나은 지원.
  • 크로스 플랫폼 지원: Linux를 넘어 Windows 및 macOS를 포함한 다양한 운영 체제와의 호환성 확대.
  • 유연한 통신: 보다 유연하고 효율적인 프로세스 간 통신을 위해 DDS 사용.

ROS 메시지 및 토픽

ROS에서 노드 간 통신은 메시지토픽을 통해 이루어집니다. 메시지는 노드 간에 교환되는 정보를 정의하는 데이터 구조이며, 토픽은 메시지가 송수신되는 이름이 지정된 채널입니다. 노드는 토픽에 메시지를 발행하거나 토픽에서 메시지를 구독함으로써 서로 통신할 수 있습니다. 이러한 발행-구독 모델은 노드 간의 비동기 통신과 디커플링을 가능하게 합니다. 로봇 시스템의 각 센서나 액추에이터는 일반적으로 토픽에 데이터를 발행하며, 이는 이후 다른 노드에서 처리나 제어를 위해 사용됩니다. 이 가이드에서는 Image, Depth, PointCloud 메시지와 카메라 토픽에 집중할 것입니다.

Ultralytics YOLO와 ROS 설정하기

This guide has been tested using this ROS environment, which is a fork of the ROSbot ROS repository. This environment includes the Ultralytics YOLO package, a Docker container for easy setup, comprehensive ROS packages, and Gazebo worlds for rapid testing. It is designed to work with the Husarion ROSbot 2 PRO. The code examples provided will work in any ROS Noetic/Melodic environment, including both simulation and real-world.

Husarion ROSbot 2 PRO autonomous robot platform

종속성 설치

ROS 환경 외에도 다음 종속성을 설치해야 합니다:

  • ROS NumPy 패키지: ROS Image 메시지와 NumPy 배열 간의 빠른 변환을 위해 필요합니다.

    pip install ros_numpy
  • Ultralytics 패키지:

    pip install ultralytics

ROS sensor_msgs/Image와 함께 Ultralytics 사용하기

sensor_msgs/Image 메시지 유형은 ROS에서 이미지 데이터를 표현하는 데 일반적으로 사용됩니다. 인코딩, 높이, 너비 및 픽셀 데이터 필드를 포함하고 있어 카메라나 기타 센서로 캡처한 이미지를 전송하는 데 적합합니다. 이미지 메시지는 시각적 인식, 객체 탐지, 내비게이션과 같은 로봇 애플리케이션 작업에 널리 사용됩니다.

Detection and Segmentation in ROS Gazebo

이미지 단계별 사용 방법

다음 코드 스니펫은 ROS와 함께 Ultralytics YOLO 패키지를 사용하는 방법을 보여줍니다. 이 예제에서는 카메라 토픽을 구독하고, YOLO를 사용하여 수신된 이미지를 처리하며, 탐지세그멘테이션을 위해 새로운 토픽으로 탐지된 객체를 발행합니다.

먼저 필요한 라이브러리를 가져오고 두 개의 모델을 인스턴스화합니다. 하나는 세그멘테이션용, 다른 하나는 탐지용입니다. ROS 마스터와 통신을 활성화하기 위해 (ultralytics라는 이름으로) ROS 노드를 초기화합니다. 안정적인 연결을 보장하기 위해 짧은 일시 중지를 포함하여 노드가 진행하기 전에 연결을 설정할 수 있는 충분한 시간을 제공합니다.

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)

탐지용과 세그멘테이션용으로 각각 하나씩 두 개의 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 유형의 메시지를 수신하여 ros_numpy를 사용하여 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("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()
디버깅

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를 사용하면 센서 데이터(예: sensor_msgs/Image), 로봇 모델 상태 및 다양한 유형의 정보를 볼 수 있어 로봇 시스템의 동작을 디버깅하고 이해하기가 더 쉬워집니다.

std_msgs/String을 사용하여 탐지된 클래스 발행하기

표준 ROS 메시지에는 std_msgs/String 메시지도 포함되어 있습니다. 많은 애플리케이션에서 전체 주석 이미지를 다시 게시할 필요는 없으며, 대신 로봇의 시야에 있는 클래스만 필요합니다. 다음 예제는 std_msgs/String 메시지를 사용하여 /ultralytics/detection/classes 토픽에서 탐지된 클래스를 다시 게시하는 방법을 보여줍니다. 이 메시지들은 더 가볍고 필수 정보를 제공하므로 다양한 애플리케이션에서 유용합니다.

사용 사례 예시

카메라와 객체 탐지 모델이 장착된 창고 로봇을 생각해 보십시오. 네트워크를 통해 큰 주석 이미지를 보내는 대신, 로봇은 탐지된 클래스 목록을 std_msgs/String 메시지로 발행할 수 있습니다. 예를 들어, 로봇이 "box", "pallet", "forklift"와 같은 객체를 탐지하면 이 클래스들을 /ultralytics/detection/classes 토픽에 발행합니다. 이 정보는 중앙 모니터링 시스템에서 재고를 실시간으로 추적하거나, 장애물을 피하기 위해 로봇의 경로 계획을 최적화하거나, 탐지된 상자를 집어 올리는 것과 같은 특정 작업을 수행하도록 트리거하는 데 사용될 수 있습니다. 이 방식은 통신에 필요한 대역폭을 줄이고 중요한 데이터를 전송하는 데 집중하게 합니다.

String 단계별 사용 방법

This example demonstrates how to use the Ultralytics YOLO package with ROS. In this example, we subscribe to a camera topic, process the incoming image using YOLO, and publish the detected objects to new topic /ultralytics/detection/classes using std_msgs/String messages. The ros_numpy package is used to convert the ROS Image message to a NumPy array for processing with 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()

ROS Depth 이미지와 함께 Ultralytics 사용하기

RGB 이미지 외에도 ROS는 카메라로부터의 객체 거리 정보를 제공하는 Depth 이미지를 지원합니다. Depth 이미지는 장애물 회피, 3D 매핑 및 위치 추정과 같은 로봇 애플리케이션에 매우 중요합니다.

Depth 이미지는 각 픽셀이 카메라에서 객체까지의 거리를 나타내는 이미지입니다. 색상을 캡처하는 RGB 이미지와 달리 Depth 이미지는 공간 정보를 캡처하여 로봇이 환경의 3D 구조를 인식할 수 있게 합니다.

Depth 이미지 획득

Depth 이미지는 다양한 센서를 사용하여 획득할 수 있습니다:

  1. 스테레오 카메라: 두 개의 카메라를 사용하여 이미지 시차를 기반으로 거리를 계산합니다.
  2. Time-of-Flight (ToF) 카메라: 빛이 객체에서 반사되어 돌아오는 시간을 측정합니다.
  3. 구조광 센서: 패턴을 투사하고 표면에서의 변형을 측정합니다.

Depth 이미지와 함께 YOLO 사용하기

ROS에서 Depth 이미지는 sensor_msgs/Image 메시지 유형으로 표현되며, 여기에는 인코딩, 높이, 너비 및 픽셀 데이터 필드가 포함됩니다. Depth 이미지의 인코딩 필드는 종종 "16UC1"과 같은 형식을 사용하며, 이는 픽셀당 16비트 부호 없는 정수를 나타내고 각 값은 객체까지의 거리를 나타냅니다. Depth 이미지는 일반적으로 환경에 대한 보다 포괄적인 시야를 제공하기 위해 RGB 이미지와 함께 사용됩니다.

YOLO를 사용하면 RGB와 Depth 이미지 모두에서 정보를 추출하고 결합할 수 있습니다. 예를 들어, YOLO는 RGB 이미지 내의 객체를 탐지할 수 있으며, 이 탐지 결과는 Depth 이미지 내의 해당 영역을 정확히 찾아내는 데 사용될 수 있습니다. 이를 통해 탐지된 객체에 대한 정확한 거리 정보를 추출하여 3D 환경을 이해하는 로봇의 능력을 향상시킬 수 있습니다.

RGB-D 카메라

Depth 이미지를 작업할 때는 RGB와 Depth 이미지가 올바르게 정렬되었는지 확인하는 것이 필수적입니다. Intel RealSense 시리즈와 같은 RGB-D 카메라는 동기화된 RGB 및 Depth 이미지를 제공하므로 두 소스의 정보를 결합하기가 더 쉽습니다. 별도의 RGB 및 Depth 카메라를 사용하는 경우 정확한 정렬을 보장하기 위해 이들을 보정하는 것이 매우 중요합니다.

Depth 단계별 사용 방법

이 예제에서는 YOLO를 사용하여 이미지를 세그멘테이션하고 추출된 마스크를 Depth 이미지의 객체 세그멘테이션에 적용합니다. 이를 통해 관심 객체의 각 픽셀이 카메라 초점 중심에서 얼마나 떨어져 있는지 결정할 수 있습니다. 이 거리 정보를 얻음으로써 장면 내 카메라와 특정 객체 사이의 거리를 계산할 수 있습니다. 필요한 라이브러리를 가져오고, 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("yolo26m-seg.pt")

classes_pub = rospy.Publisher("/ultralytics/detection/distance", String, queue_size=5)

다음으로, 들어오는 Depth 이미지 메시지를 처리하는 콜백 함수를 정의합니다. 이 함수는 Depth 이미지와 RGB 이미지 메시지를 기다리고, 이를 NumPy 배열로 변환하며, RGB 이미지에 세그멘테이션 모델을 적용합니다. 그런 다음 각 탐지된 객체에 대한 세그멘테이션 마스크를 추출하고 Depth 이미지를 사용하여 카메라로부터의 객체 평균 거리를 계산합니다. 대부분의 센서에는 클립 거리(clip distance)라고 불리는 최대 거리가 있으며, 이 거리를 넘는 값은 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("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()

ROS sensor_msgs/PointCloud2와 함께 Ultralytics 사용하기

Detection and Segmentation in ROS Gazebo

sensor_msgs/PointCloud2 메시지 유형은 ROS에서 3D 포인트 클라우드 데이터를 나타내는 데 사용되는 데이터 구조입니다. 이 메시지 유형은 3D 매핑, 객체 인식 및 위치 추정과 같은 작업을 가능하게 하여 로봇 애플리케이션에 필수적입니다.

포인트 클라우드는 3차원 좌표계 내에 정의된 데이터 포인트의 모음입니다. 이 데이터 포인트들은 3D 스캔 기술을 통해 캡처된 객체나 장면의 외부 표면을 나타냅니다. 클라우드의 각 포인트에는 공간 내 위치에 해당하는 X, Y, Z 좌표가 있으며 색상 및 강도와 같은 추가 정보도 포함될 수 있습니다.

참조 프레임

sensor_msgs/PointCloud2를 작업할 때는 포인트 클라우드 데이터가 획득된 센서의 참조 프레임을 고려하는 것이 필수적입니다. 포인트 클라우드는 처음에 센서의 참조 프레임에서 캡처됩니다. /tf_static 토픽을 수신하여 이 참조 프레임을 확인할 수 있습니다. 그러나 특정 애플리케이션 요구 사항에 따라 포인트 클라우드를 다른 참조 프레임으로 변환해야 할 수도 있습니다. 이 변환은 좌표 프레임을 관리하고 데이터 간의 변환을 위한 도구를 제공하는 tf2_ros 패키지를 사용하여 수행할 수 있습니다.

포인트 클라우드 획득

포인트 클라우드는 다양한 센서를 사용하여 획득할 수 있습니다:

  1. LIDAR (Light Detection and Ranging): 레이저 펄스를 사용하여 객체까지의 거리를 측정하고 고정밀 3D 지도를 생성합니다.
  2. Depth 카메라: 각 픽셀에 대한 거리 정보를 캡처하여 장면의 3D 재구성을 가능하게 합니다.
  3. 스테레오 카메라: 둘 이상의 카메라를 활용하여 삼각 측량을 통해 거리 정보를 얻습니다.
  4. 구조광 스캐너: 알려진 패턴을 표면에 투사하고 변형을 측정하여 거리를 계산합니다.

포인트 클라우드와 함께 YOLO 사용하기

YOLO를 sensor_msgs/PointCloud2 유형 메시지와 통합하기 위해 Depth 맵에 사용되는 것과 유사한 방법을 사용할 수 있습니다. 포인트 클라우드에 내장된 색상 정보를 활용하여 2D 이미지를 추출하고, YOLO를 사용하여 이 이미지에 세그멘테이션을 수행한 다음, 결과 마스크를 3차원 포인트에 적용하여 관심 있는 3D 객체를 분리할 수 있습니다.

포인트 클라우드 처리를 위해서는 사용자 친화적인 Python 라이브러리인 Open3D(pip install open3d)를 사용하는 것을 권장합니다. Open3D는 포인트 클라우드 데이터 구조 관리, 시각화, 복잡한 작업 수행을 위한 강력한 도구를 제공합니다. 이 라이브러리는 프로세스를 크게 간소화하고 YOLO 기반 세그멘테이션과 함께 포인트 클라우드를 조작 및 분석하는 능력을 향상시킬 수 있습니다.

포인트 클라우드 단계별 사용 방법

필요한 라이브러리를 가져오고 세그멘테이션을 위한 YOLO 모델을 인스턴스화합니다.

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.

The function returns the xyz coordinates and RGB values in the format of the original camera resolution (width x height). Most sensors have a maximum distance, known as the clip distance, beyond which values are represented as inf (np.inf). Before processing, it is important to filter out these null values and assign them a value of 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

Next, subscribe to the /camera/depth/points topic to receive the point cloud message and convert the sensor_msgs/PointCloud2 message into NumPy arrays containing the XYZ coordinates and RGB values (using the pointcloud2_to_array function). Process the RGB image using the YOLO model to extract segmented objects. For each detected object, extract the segmentation mask and apply it to both the RGB image and the XYZ coordinates to isolate the object in 3D space.

마스크 처리는 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("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

Robot Operating System (ROS)이란 무엇입니까?

Robot Operating System (ROS)는 로봇 공학에서 개발자가 강력한 로봇 애플리케이션을 만들 수 있도록 일반적으로 사용되는 오픈 소스 프레임워크입니다. 이는 로봇 시스템을 구축하고 연동하기 위한 라이브러리 및 도구 모음을 제공하여 복잡한 애플리케이션의 개발을 더 쉽게 만듭니다. ROS는 토픽이나 서비스를 통해 메시지를 주고받으며 노드 간의 통신을 지원합니다.

실시간 객체 탐지를 위해 Ultralytics YOLO를 ROS와 어떻게 통합합니까?

Ultralytics YOLO를 ROS와 통합하려면 ROS 환경을 설정하고 센서 데이터 처리를 위해 YOLO를 사용해야 합니다. 먼저 ros_numpy 및 Ultralytics YOLO와 같은 필수 종속성을 설치하여 시작하십시오:

pip install ros_numpy ultralytics

다음으로 ROS 노드를 생성하고 들어오는 데이터를 처리하기 위해 이미지 토픽을 구독합니다. 최소한의 예제는 다음과 같습니다:

import ros_numpy
import rospy
from sensor_msgs.msg import Image

from ultralytics import YOLO

detection_model = YOLO("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()

ROS 토픽이란 무엇이며 Ultralytics YOLO에서는 어떻게 사용됩니까?

ROS 토픽은 발행-구독 모델을 사용하여 ROS 네트워크 내의 노드 간 통신을 촉진합니다. 토픽은 노드가 메시지를 비동기적으로 송수신하는 데 사용하는 이름이 지정된 채널입니다. Ultralytics YOLO의 맥락에서 노드가 이미지 토픽을 구독하게 하고, 탐지세그멘테이션과 같은 작업을 위해 YOLO를 사용하여 이미지를 처리한 다음, 그 결과를 새로운 토픽으로 발행할 수 있습니다.

예를 들어, 카메라 토픽을 구독하고 탐지를 위해 들어오는 이미지를 처리합니다:

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

ROS에서 Ultralytics YOLO와 함께 Depth 이미지를 사용하는 이유는 무엇입니까?

sensor_msgs/Image로 표현되는 ROS의 Depth 이미지는 장애물 회피, 3D 매핑 및 위치 추정과 같은 작업에 필수적인 카메라로부터의 객체 거리 정보를 제공합니다. RGB 이미지와 함께 Depth 정보 사용을 통해 로봇은 3D 환경을 더 잘 이해할 수 있습니다.

YOLO를 사용하면 RGB 이미지에서 세그멘테이션 마스크를 추출하고 이 마스크를 Depth 이미지에 적용하여 정확한 3D 객체 정보를 얻을 수 있으며, 이는 로봇이 주변 환경을 탐색하고 상호 작용하는 능력을 향상시킵니다.

ROS에서 YOLO를 사용하여 3D 포인트 클라우드를 어떻게 시각화할 수 있습니까?

ROS에서 YOLO와 함께 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("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])

이 접근 방식은 세그멘테이션된 객체의 3D 시각화를 제공하며, 로봇 애플리케이션에서의 내비게이션 및 매니퓰레이션과 같은 작업에 유용합니다.

댓글