콘텐츠로 건너뛰기

ROS(로봇 운영 체제) 빠른 시작 가이드

오픈 로보틱스의 ROS 소개(캡션) Vimeo의 동영상입니다.

ROS란 무엇인가요?

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 노에틱 닌제미로 알려진 ROS 1의 장기 지원(LTS) 버전에 초점을 맞추고 있으며, 이전 버전에서도 코드가 작동합니다.

ROS 1과 ROS 2

ROS 1이 로봇 개발을 위한 탄탄한 기반을 제공했다면, ROS 2는 다음과 같은 기능을 제공하여 단점을 보완합니다:

  • 실시간 퍼포먼스: 실시간 시스템 및 결정론적 동작에 대한 지원이 개선되었습니다.
  • 보안: 다양한 환경에서 안전하고 안정적으로 운영할 수 있도록 보안 기능이 강화되었습니다.
  • 확장성: 멀티 로봇 시스템 및 대규모 배포에 대한 지원 강화.
  • 크로스 플랫폼 지원: Linux를 넘어 Windows, macOS 등 다양한 운영 체제와의 호환성이 확대되었습니다.
  • 유연한 커뮤니케이션: 보다 유연하고 효율적인 프로세스 간 커뮤니케이션을 위해 DDS를 사용합니다.

ROS 메시지 및 주제

ROS에서 노드 간의 통신은 메시지와 토픽을 통해 이루어집니다. 메시지는 노드 간에 교환되는 정보를 정의하는 데이터 구조이며, 토픽은 메시지를 주고받는 명명된 채널입니다. 노드는 토픽에 메시지를 게시하거나 토픽의 메시지를 구독하여 서로 소통할 수 있습니다. 이 게시-구독 모델을 사용하면 노드 간에 비동기 통신 및 분리가 가능합니다. 로봇 시스템의 각 센서 또는 액추에이터는 일반적으로 데이터를 토픽에 게시하고, 다른 노드에서 처리 또는 제어를 위해 데이터를 사용할 수 있습니다. 이 가이드에서는 이미지, 깊이, 포인트 클라우드 메시지와 카메라 토픽에 초점을 맞추겠습니다.

ROS로 Ultralytics YOLO 설정하기

이 가이드는 ROSbot ROS 리포지토리의 포크인 이 ROS 환경을 사용하여 테스트되었습니다. 이 환경에는 Ultralytics YOLO 패키지, 손쉬운 설정을 위한 도커 컨테이너, 포괄적인 ROS 패키지, 빠른 테스트를 위한 가제보 월드가 포함되어 있습니다. 이 환경은 후사리온 ROSbot 2 PRO와 함께 작동하도록 설계되었습니다. 제공되는 코드 예제는 시뮬레이션과 실제 환경을 포함한 모든 ROS Noetic/Melodic 환경에서 작동합니다.

후사리온 ROS봇 2 PRO

종속성 설치

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

  • ROS 넘피 패키지: ROS 이미지 메시지와 숫자 배열 간의 빠른 변환을 위해 필요합니다.

    pip install ros_numpy
    
  • Ultralytics 패키지입니다:

    pip install ultralytics
    

ROS와 함께 Ultralytics 사용 sensor_msgs/Image

그리고 sensor_msgs/Image 메시지 유형 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.

ROS 가제보의 탐지 및 세분화

이미지 단계별 사용 방법

다음 코드 스니펫은 Ultralytics YOLO 패키지를 ROS와 함께 사용하는 방법을 보여줍니다. 이 예에서는 카메라 토픽을 구독하고 YOLO 을 사용하여 들어오는 이미지를 처리한 다음 감지된 객체를 새 토픽에 게시하여 감지분할을 수행합니다.

먼저 필요한 라이브러리를 가져와서 다음 두 가지 모델을 인스턴스화합니다. 세분화 그리고 하나는 탐지. ROS 노드를 초기화합니다( ultralytics)를 사용하여 ROS 마스터와 통신할 수 있습니다. 안정적인 연결을 보장하기 위해 잠시 일시 중지하여 노드가 연결을 설정할 수 있는 충분한 시간을 제공한 후 계속 진행합니다.

import time

import rospy

from ultralytics import YOLO

detection_model = YOLO("yolov8m.pt")
segmentation_model = YOLO("yolov8m-seg.pt")
rospy.init_node("ultralytics")
time.sleep(1)

두 개의 ROS 주제를 초기화합니다. 탐지 그리고 하나는 세분화. 이 주제는 주석이 달린 이미지를 게시하는 데 사용되어 추가 처리를 위해 액세스할 수 있도록 합니다. 노드 간의 통신은 다음을 사용하여 촉진됩니다. sensor_msgs/Image 메시지.

from sensor_msgs.msg import Image

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

마지막으로, 다음에서 메시지를 수신하는 구독자를 만듭니다. /camera/color/image_raw 토픽을 만들고 각각의 새 메시지에 대해 콜백 함수를 호출합니다. 이 콜백 함수는 다음과 같은 유형의 메시지를 수신합니다. sensor_msgs/Image를 사용하여 널 배열로 변환합니다. 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 시각화)는 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 토픽으로 이동합니다. 그러면 중앙 모니터링 시스템에서 이 정보를 사용하여 실시간으로 재고를 추적하고, 장애물을 피하도록 로봇의 경로 계획을 최적화하거나, 감지된 상자를 집어 올리는 등의 특정 작업을 트리거할 수 있습니다. 이 접근 방식은 통신에 필요한 대역폭을 줄이고 중요한 데이터 전송에 집중할 수 있습니다.

문자열 단계별 사용법

이 예는 Ultralytics YOLO 패키지를 ROS와 함께 사용하는 방법을 보여줍니다. 이 예에서는 카메라 토픽을 구독하고 YOLO 를 사용하여 들어오는 이미지를 처리한 다음 감지된 객체를 새 토픽에 게시합니다. /ultralytics/detection/classes 사용 std_msgs/String 메시지. 메시지 ros_numpy 패키지는 YOLO 로 처리할 수 있도록 ROS 이미지 메시지를 널 배열로 변환하는 데 사용됩니다.

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. ToF(비행 시간) 카메라: 빛이 물체에서 돌아오는 데 걸리는 시간을 측정합니다.
  3. 구조형 광 센서: 패턴을 투사하고 표면의 변형을 측정합니다.

깊이 이미지와 함께 YOLO 사용

ROS에서 깊이 이미지는 다음과 같이 표현됩니다. sensor_msgs/Image 메시지 유형에는 인코딩, 높이, 너비 및 픽셀 데이터 필드가 포함됩니다. 깊이 이미지의 인코딩 필드는 픽셀당 16비트 부호 없는 정수를 나타내는 "16UC1"과 같은 형식을 사용하는 경우가 많으며, 각 값은 물체까지의 거리를 나타냅니다. 깊이 이미지는 일반적으로 RGB 이미지와 함께 사용하여 환경을 보다 포괄적으로 파악할 수 있도록 합니다.

YOLO 을 사용하면 RGB 이미지와 심도 이미지 모두에서 정보를 추출하고 결합할 수 있습니다. 예를 들어 YOLO 은 RGB 이미지 내의 물체를 감지할 수 있으며, 이 감지를 통해 깊이 이미지에서 해당 영역을 정확히 찾아낼 수 있습니다. 이를 통해 감지된 물체에 대한 정확한 깊이 정보를 추출하여 로봇이 주변 환경을 3차원으로 이해하는 능력을 향상시킬 수 있습니다.

RGB-D 카메라

심도 이미지로 작업할 때는 RGB와 심도 이미지가 올바르게 정렬되어 있는지 확인하는 것이 중요합니다. RealSense 시리즈( Intel )와 같은 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 이미지 메시지를 기다렸다가 이를 널 배열로 변환한 다음 세분화 모델을 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 가제보의 탐지 및 세분화

그리고 sensor_msgs/PointCloud2 메시지 유형 는 ROS에서 3D 포인트 클라우드 데이터를 표현하는 데 사용되는 데이터 구조입니다. 이 메시지 유형은 로봇 애플리케이션에 필수적인 요소로 3D 매핑, 물체 인식, 로컬라이제이션과 같은 작업을 가능하게 합니다.

포인트 클라우드는 3차원 좌표계 내에 정의된 데이터 포인트의 모음입니다. 이러한 데이터 포인트는 3D 스캐닝 기술을 통해 캡처한 물체 또는 장면의 외부 표면을 나타냅니다. 클라우드의 각 포인트에는 다음이 포함됩니다. X, YZ 좌표는 공간에서의 위치에 해당하며 색상 및 강도와 같은 추가 정보를 포함할 수도 있습니다.

참조 프레임

다음과 함께 작업할 때 sensor_msgs/PointCloud2를 사용하려면 포인트 클라우드 데이터를 수집한 센서의 기준 프레임을 고려하는 것이 중요합니다. 포인트 클라우드는 처음에 센서의 기준 프레임에서 캡처됩니다. 이 기준 프레임을 결정하려면 센서의 /tf_static 주제를 참조하세요. 그러나 특정 애플리케이션 요구 사항에 따라 포인트 클라우드를 다른 참조 프레임으로 변환해야 할 수도 있습니다. 이 변환은 tf2_ros 좌표 프레임을 관리하고 프레임 간 데이터를 변환하는 도구를 제공하는 패키지입니다.

포인트 클라우드 가져오기

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

  1. LIDAR (Light Detection and Ranging): Uses laser pulses to measure distances to objects and create high-precision 3D maps.
  2. 뎁스 카메라: 각 픽셀에 대한 깊이 정보를 캡처하여 장면을 3D로 재구성할 수 있습니다.
  3. 스테레오 카메라: 두 대 이상의 카메라를 활용하여 삼각 측량을 통해 깊이 정보를 얻습니다.
  4. 구조광 스캐너: 알려진 패턴을 표면에 투사하고 변형을 측정하여 깊이를 계산합니다.

포인트 클라우드와 함께 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("yolov8m-seg.pt")

함수 만들기 pointcloud2_to_array를 변환하는 sensor_msgs/PointCloud2 메시지를 두 개의 널 배열로 나눕니다. 두 개의 sensor_msgs/PointCloud2 메시지에는 다음이 포함됩니다. n 포인트에 따라 width 그리고 height 로 설정할 수 있습니다. 예를 들어 480 x 640 이미지에는 307,200 점입니다. 각 점에는 세 개의 공간 좌표(xyz)의 해당 색상과 RGB 형식입니다. 이는 두 개의 개별 정보 채널로 간주할 수 있습니다.

이 함수는 xyz 좌표와 RGB 값을 원래 카메라 해상도 형식(width x height). 대부분의 센서에는 클립 거리라고 하는 최대 거리가 있으며, 그 이상의 값은 inf(np.inf). 처리하기 전에 이러한 null 값을 필터링하고 값을 0.

import numpy as np
import ros_numpy


def pointcloud2_to_array(pointcloud2: PointCloud2) -> tuple:
    """
    Convert a ROS PointCloud2 message to a numpy array.

    Args:
        pointcloud2 (PointCloud2): the PointCloud2 message

    Returns:
        (tuple): tuple containing (xyz, rgb)
    """
    pc_array = ros_numpy.point_cloud2.pointcloud2_to_array(pointcloud2)
    split = ros_numpy.point_cloud2.split_rgb_field(pc_array)
    rgb = np.stack([split["b"], split["g"], split["r"]], axis=2)
    xyz = ros_numpy.point_cloud2.get_xyz_points(pc_array, remove_nans=False)
    xyz = np.array(xyz).reshape((pointcloud2.height, pointcloud2.width, 3))
    nan_rows = np.isnan(xyz).all(axis=2)
    xyz[nan_rows] = [0, 0, 0]
    rgb[nan_rows] = [0, 0, 0]
    return xyz, rgb

다음으로 /camera/depth/points 토픽을 사용하여 포인트 클라우드 메시지를 수신하고 sensor_msgs/PointCloud2 메시지를 XYZ 좌표와 RGB 값을 포함하는 널 배열로 변환합니다( 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는 주제 또는 서비스에 대한 메시지를 사용하여 노드 간의 통신을 지원합니다.

실시간 객체 감지를 위해 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 메시지를 배열에 전달합니다.
  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 시각화하여 탐색 및 조작과 같은 작업에 유용합니다.

📅 Created 4 months ago ✏️ Updated 18 days ago

댓글