환경 인식과 장애물 탐지

장애물 탐지

장애물 탐지 기술은 자율운항 알고리즘의 주요 기술 중 하나로 카메라, 라이다 등 다양한 센서 데이터를 활용하여 주변 환경을 인식하고 장애물을 감지하여 충돌을 방지하는 역할을 합니다. 효율적인 동선으로 목적지에 도달하기 위해서는 정확하고 빠른 장애물 탐지 알고리즘이 필요합니다.

이번 실습에서는 장애물을 바로 회피하는 제어 코드까지 작성하기 전에, 먼저 센서 데이터에서 장애물 후보만 분리하는 노드를 만들어 볼 예정입니다. 카메라 노드는 특정 색상 영역을 추출하고, 라이다 노드는 전방 가까운 거리의 데이터만 추출합니다. 이렇게 분리한 데이터는 이후 판단 알고리즘에서 장애물이 있는가?, 어느 방향으로 피해야 하는가?를 결정하는 근거가 됩니다.

인지 단계에서 중요한 것은 주변 세계를 완벽하게 이해하는 것이 아니라, 판단에 필요한 정보를 적절한 형태로 정리하는 것입니다. 예를 들어 장애물 회피 알고리즘은 모든 픽셀의 색상이나 모든 라이다 거리값을 그대로 필요로 하지 않습니다. 대신 “전방에 가까운 장애물이 있는가”, “왼쪽과 오른쪽 중 어느 쪽이 더 비어 있는가”, “특정 색상의 부표가 보이는가”와 같은 요약된 정보가 필요합니다.

센서마다 얻을 수 있는 정보도 다릅니다.

센서	얻기 쉬운 정보	판단에 활용하는 예
카메라	색상, 형태, 표식	특정 색상의 부표 또는 표지판 인식
라이다	거리, 방향	전방 장애물 거리와 회피 방향 판단
GPS	전역 위치	목표 지점까지의 방향 계산
IMU	자세, 회전 변화	선박의 heading 변화 확인

장애물 탐지에 가장 널리 사용되고 있는 카메라와 라이다를 활용하여 간단한 장애물 탐지 알고리즘을 작성해 보겠습니다.

카메라 장애물 탐지

RGB

이미지 데이터를 활용하기 위해서는 이미지 데이터의 표현 방식을 이해할 필요가 있습니다. 컴퓨터는 사람의 눈처럼 사물이나 장면을 직관적으로 인식할 수 없기 때문에, 이미지 데이터를 색상과 밝기 같은 숫자로 표현하여 저장합니다. 가장 대표적인 표현 방식이 바로 빨간색(Red), 초록색(Green), 파란색(Blue)을 각각 숫자로 표현하는 RGB 방식입니다.

예를 들어, RGB 방식으로 흰색 이미지와 검은색 이미지를 컴퓨터에 저장한다면 아래와 같은 형태가 됩니다.

# 흰색 이미지
[[[255 255 255]
  [255 255 255]
  [255 255 255]
  ...
  [255 255 255]
  [255 255 255]
  [255 255 255]]

 ...

 [[255 255 255]
  [255 255 255]
  [255 255 255]
  ...
  [255 255 255]
  [255 255 255]
  [255 255 255]]]

# 검은색 이미지
[[[0 0 0]
  [0 0 0]
  [0 0 0]
  ...
  [0 0 0]
  [0 0 0]
  [0 0 0]]

 ...

 [[0 0 0]
  [0 0 0]
  [0 0 0]
  ...
  [0 0 0]
  [0 0 0]
  [0 0 0]]]

HSV

HSV는 색상(Hue), 채도(Saturation), 명도(Value)를 기준으로 색을 나타내는 방식입니다. HSV는 원기둥이나 원뿔과 같은 3차원의 도형으로 표현할 수 있어 색 공간이라고 표현하기도 합니다. HSV 색 공간은 인간의 색 인지 방식을 반영하며, 색상을 더 직관적으로 다룰 수 있게 해줍니다.

원기둥으로 표현한 HSV

원뿔로 표현한 HSV

• 색상(Hue): 색의 종류를 나타내며, 0도에서 360도까지의 각도로 표현됩니다. 예를 들어 빨강은 0도, 초록은 120도, 파랑은 240도에 해당합니다. 단, OpenCV에서는 0도에서 360도까지의 범위를 0도에서 180도까지의 범위로 변환하여 사용하니 주의해야 합니다. 예를 들어 OpenCV에서는 빨강은 0도, 초록은 60도, 파랑은 120도가 됩니다.
• 채도(Saturation): 색의 강도를 나타내며, 0%에서 100%까지의 비율로 표현됩니다. 채도가 0%면 회색, 100%면 가장 강한 색상입니다.
• 명도(Value): 색의 밝기를 나타내며, 0%에서 100%까지의 비율로 표현됩니다. 명도가 0%면 검정, 100%면 가장 밝은 상태입니다.

HSV 색 공간은 색상(Hue)이 분리되어 있기 때문에 특정 색상을 쉽게 추출하거나 강조할 수 있습니다. 예를 들어, 빨간색만 추출하고 싶다면 빨간색에 해당하는 Hue 값 범위를 지정하면 됩니다.

카메라를 이용한 장애물 탐지는 색상이나 형태가 명확할 때 효과적입니다. 예를 들어 경기장에 특정 색상의 부표가 놓여 있다면 HSV 범위를 이용해 해당 색상만 추출할 수 있습니다. 하지만 조명이 바뀌거나 그림자가 생기면 같은 물체도 다른 색처럼 보일 수 있습니다. 따라서 카메라 기반 인식은 HSV 범위를 상황에 맞게 조정하거나, 다른 센서와 함께 사용하는 것이 좋습니다.

이미지 색상 추출하기

팁

OpenCV(Open Source Computer Vision Library)는 실시간 컴퓨터 비전 애플리케이션을 개발하기 위한 오픈 소스 라이브러리입니다. OpenCV는 다양한 이미지 및 비디오 처리 기능을 제공하며, 컴퓨터 비전 관련 작업을 효율적으로 수행할 수 있게 도와줍니다.

OpenCV는 이미지를 읽어올 때 기본적으로 이미지를 BGR (Blue, Green, Red) 형식으로 읽어옵니다. 보편적인 RGB 방식에서 순서만 뒤집어졌습니다.

OpenCV의 함수를 사용해 색상 표현 방식을 쉽게 변경할 수 있습니다. 특정 색상을 추출하고자 하는 경우에는 편의와 정확도를 위해 BGR 형태의 이미지 데이터를 HSV 형태로 변환하여 색상을 추출하고 나타내는 방식을 자주 사용합니다.

새로운 패키지를 만들고 색상을 추출하는 노드를 작성해 보겠습니다. 터미널 창을 열고 아래 코드를 입력해 패키지를 생성합니다.

cd ~/ros2_ws/src
ros2 pkg create obstacle_package --build-type ament_python

obstacle_package 패키지에 camera_obstacle_node.py 파일을 생성하고 아래와 같이 코드를 작성합니다.

camera_obstacle_node.py

import cv2
import rclpy
from cv_bridge import CvBridge
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Image

class CameraObstacleNode(Node):
    def __init__(self):
        super().__init__("camera_obstacle_node")

        # 카메라 토픽 Subscription
        self.camera_subscription = self.create_subscription(
            Image, "image_raw", self.listener_callback, qos_profile_sensor_data
        )

        # 카메라 장애물 토픽 Publisher
        self.camera_obstacle_publisher = self.create_publisher(
            Image, "image_raw/obstacle", qos_profile_sensor_data
        )

        self.br = CvBridge()

        # 카메라 장애물 hsv 범위 지정 (Ex. 빨간색)
        self.lower_hsv1 = (0, 120, 70)
        self.upper_hsv1 = (10, 255, 255)
        self.lower_hsv2 = (160, 120, 70)
        self.upper_hsv2 = (180, 255, 255)

    def listener_callback(self, msg: Image):
        origin_image = self.br.imgmsg_to_cv2(msg, "bgr8")

        # 이미지 데이터가 존재할 경우에만 동작 (빈 값 제외하기)
        if len(origin_image):
            # hsv로 변환
            hsv_image = cv2.cvtColor(origin_image, cv2.COLOR_BGR2HSV)

            # 필터링 영역
            mask1 = cv2.inRange(hsv_image, self.lower_hsv1, self.upper_hsv1)
            mask2 = cv2.inRange(hsv_image, self.lower_hsv2, self.upper_hsv2)

            # 원본 이미지에서 필터링 영역만 가져오기
            result = cv2.bitwise_and(origin_image, origin_image, mask=mask1 + mask2)

            # 필터링된 데이터 Publish
            msg = self.br.cv2_to_imgmsg(result, "bgr8")
            msg.header.frame_id = "camera_link"
            self.camera_obstacle_publisher.publish(msg)

def main(args=None):
    rclpy.init(args=args)

    camera_obstacle_node = CameraObstacleNode()

    rclpy.spin(camera_obstacle_node)

    camera_obstacle_node.destroy_node()
    rclpy.shutdown()

if __name__ == "__main__":
    main()

팁

코드를 작성할 때 msg: Image와 같이 변수의 타입을 알려준다면 VS Code의 자동완성 기능을 사용할 수 있어서 편리합니다. 복잡한 코드를 작성할 때는 가능한 변수 타입을 지정하는 것이 좋습니다.

이전 시간에는 publisher에서는 토픽을 보내주고 subscriber에서는 토픽을 받는 동작만을 구현했습니다. 이번에 작성할 camera_obstacle_node는 토픽을 받고 색상을 추출한 후 다시 토픽을 보내주는 중간 다리 역할을 하게 됩니다. 즉, publisher 노드와 subscriber 노드를 융합하는 형태로 코드를 작성하면 쉽게 작성할 수 있습니다.

노드를 새로 생성한 뒤에는 반드시 잊지 말고 setup.py 파일에 추가합니다.

setup.py

from setuptools import find_packages, setup

package_name = "obstacle_package"

setup(
    name=package_name,
    version="0.0.0",
    packages=find_packages(exclude=["test"]),
    data_files=[
        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
        ("share/" + package_name, ["package.xml"]),
    ],
    install_requires=["setuptools"],
    zip_safe=True,
    maintainer="ubuntu",
    maintainer_email="ubuntu@todo.todo",
    description="TODO: Package description",
    license="TODO: License declaration",
    tests_require=["pytest"],
    entry_points={
        "console_scripts": [
            "camera_obstacle = obstacle_package.camera_obstacle_node:main",
        ],
    },
)

이제 코드를 빌드하고 노드를 실행해 RViz를 통해 토픽을 확인해 보겠습니다.

cb
ros2 launch mechaship_bringup mechaship_bringup.launch.py
# 새 터미널 창을 열고 실행합니다.
ros2 run obstacle_package camera_obstacle
# 새 터미널 창을 열고 실행합니다.
rviz2

수신할 토픽 이름을 image_raw와 image_raw/obstacle로 변경해 보면서 색상이 잘 추출되었는지 확인할 수 있습니다.

팁

원하는 대로 색상이 잘 추출되지 않았다면, 추출하는 HSV 범위를 조절합니다.

라이다 장애물 탐지

LaserScan Message

라이다 데이터는 sensor_msgs의 LaserScan 타입의 토픽으로 발행됩니다. 라이다 데이터를 활용하기 위해 메세지 타입을 보다 자세히 살펴보겠습니다.

📌 sensor_msgs/LaserScan

# Single scan from a planar laser range-finder
#
# If you have another ranging device with different behavior (e.g. a sonar
# array), please find or create a different message, since applications
# will make fairly laser-specific assumptions about this data

Header header            # timestamp in the header is the acquisition time of
                         # the first ray in the scan.
                         #
                         # in frame frame_id, angles are measured around
                         # the positive Z axis (counterclockwise, if Z is up)
                         # with zero angle being forward along the x axis

float32 angle_min        # start angle of the scan [rad]
float32 angle_max        # end angle of the scan [rad]
float32 angle_increment  # angular distance between measurements [rad]

float32 time_increment   # time between measurements [seconds] - if your scanner
                         # is moving, this will be used in interpolating position
                         # of 3d points
float32 scan_time        # time between scans [seconds]

float32 range_min        # minimum range value [m]
float32 range_max        # maximum range value [m]

float32[] ranges         # range data [m] (Note: values < range_min or > range_max should be discarded)
float32[] intensities    # intensity data [device-specific units].  If your
                         # device does not provide intensities, please leave
                         # the array empty.

주석을 통해 각 필드에 어떤 값이 어떤 단위와 형태로 들어가야 하는지 설명되어 있습니다. 특히, 시간과 관련된 필드는 초(seconds), 거리와 관련된 필드는 미터(m), 각도와 관련된 필드는 라디안(rad) 단위를 기본적으로 사용하니 주의해야 합니다.

LaserScan 메시지에서 가장 많이 사용하는 필드는 ranges입니다. ranges는 라이다가 각 방향으로 측정한 거리값의 배열입니다. 배열의 첫 번째 값이 어느 각도에 해당하는지는 angle_min으로 알 수 있고, 다음 값과의 각도 차이는 angle_increment로 알 수 있습니다. 따라서 특정 방향의 거리값을 사용하려면 원하는 각도를 배열 인덱스로 변환해야 합니다.

예를 들어, 각도와 관련된 필드 2개를 살펴보겠습니다.

• angle_min: -3.14 (-180도)
• angle_max: 3.14 (180도)

라이다 자체는 360도로 회전하는 라이다이지만, 라이다의 앞면을 0도로 고정하기 위해 각도 범위를 -180도부터 180도까지로 표현하고 있습니다. 또, 디그리로 표현된 각도를 라디안 단위로 변환해야 합니다. 디그리와 라디안 각도는 각각 아래의 공식으로 변환할 수 있습니다.

• 1 rad = 180 ° / π
• 1 ° = π / 180 rad

전방 1m 이내 거리 데이터 추출하기

로봇 동작을 위해 특정 각도 또는 특정 거리 내의 데이터를 따로 분리해야 하는 경우가 있습니다. 특히, 전진만 가능하고 후진이 불가능한 로봇은 후방의 장애물 정보는 필요하지 않을 수 있습니다. 또, 너무 먼 거리에 있는 장애물 정보는 목적이나 상황에 따라 오히려 계산을 복잡하게 만들어 운항에 어려움을 줄 수도 있습니다.

이번에는 전방 1m 이내의 거리 데이터만 추출해서 publish 하는 노드를 작성해 보겠습니다.

전방 데이터만 분리하는 이유는 선박이 주로 앞으로 이동하기 때문입니다. 후방이나 측면의 정보도 중요할 수 있지만, 가장 먼저 충돌 위험을 판단해야 하는 영역은 진행 방향입니다. 처음에는 전방 180도 또는 전방 90도처럼 넓은 영역을 사용하고, 이후 선박의 속도와 회전 반경에 맞춰 영역을 조정할 수 있습니다.

camera_obstacle_node와 동작하는 형태가 유사하니 참고해서 작성하면 쉽게 작성할 수 있습니다. 동일하게 obstacle_package 패키지에 lidar_obstacle_node 파일을 생성하고 아래와 같이 코드를 작성합니다.

lidar_obstacle_node.py

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import LaserScan

class LidarObstacleNode(Node):
    def __init__(self):
        super().__init__("lidar_obstacle_node")

        # 라이다 토픽 Subscription
        self.lidar_subscription = self.create_subscription(
            LaserScan, "scan", self.listener_callback, qos_profile_sensor_data
        )

        # 라이다 장애물 토픽 Publisher
        self.lidar_obstacle_publisher = self.create_publisher(
            LaserScan, "scan/obstacle", qos_profile_sensor_data
        )

    def listener_callback(self, msg: LaserScan):
        obstacle_scan = LaserScan()
        obstacle_scan.header = msg.header

        # 전방 180도 데이터만 추출
        obstacle_scan.angle_min = 90 / 180 * -3.14  # 라디안
        obstacle_scan.angle_max = 90 / 180 * 3.14  # 라디안

        obstacle_scan.angle_increment = msg.angle_increment
        obstacle_scan.time_increment = msg.time_increment
        obstacle_scan.scan_time = msg.scan_time
        obstacle_scan.range_min = msg.range_min
        obstacle_scan.range_max = msg.range_max

        # 전방 데이터만 추출
        min_index = int((obstacle_scan.angle_min - msg.angle_min) / msg.angle_increment)
        max_index = int((obstacle_scan.angle_max - msg.angle_min) / msg.angle_increment)

        front_ranges = msg.ranges[min_index:max_index]
        if len(msg.intensities):
            front_intensities = msg.intensities[min_index:max_index]
        else:
            front_intensities = []

        # 1미터 이내의 범위 필터링
        obstacle_scan.ranges = []
        for range_value in front_ranges:
            if msg.range_min <= range_value <= 1.0:
                obstacle_scan.ranges.append(range_value)
            else:
                obstacle_scan.ranges.append(float("inf"))
        obstacle_scan.intensities = front_intensities  # intensities는 그대로 사용

        # 필터링된 데이터 Publish
        self.lidar_obstacle_publisher.publish(obstacle_scan)

def main(args=None):
    rclpy.init(args=args)

    lidar_obstacle_node = LidarObstacleNode()

    rclpy.spin(lidar_obstacle_node)

    lidar_obstacle_node.destroy_node()
    rclpy.shutdown()

if __name__ == "__main__":
    main()

노드를 새로 생성한 뒤에는 이번에도 잊지 말고 setup.py 파일에 추가합니다.

setup.py

from setuptools import find_packages, setup

package_name = "obstacle_package"

setup(
    name=package_name,
    version="0.0.0",
    packages=find_packages(exclude=["test"]),
    data_files=[
        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
        ("share/" + package_name, ["package.xml"]),
    ],
    install_requires=["setuptools"],
    zip_safe=True,
    maintainer="ubuntu",
    maintainer_email="ubuntu@todo.todo",
    description="TODO: Package description",
    license="TODO: License declaration",
    tests_require=["pytest"],
    entry_points={
        "console_scripts": [
            "camera_obstacle = obstacle_package.camera_obstacle_node:main",
            "lidar_obstacle = obstacle_package.lidar_obstacle_node:main",
        ],
    },
)

이제 코드를 빌드하고 노드를 실행해 RViz를 통해 토픽을 확인해 보겠습니다.

cb
ros2 launch mechaship_bringup mechaship_bringup.launch.py
# 새 터미널 창을 열고 실행합니다.
ros2 run obstacle_package lidar_obstacle
# 새 터미널 창을 열고 실행합니다.
rviz2

이제 RViz로 scan/obstacle 토픽을 받아오면 라이다의 전방 1m 이내의 거리 데이터만 출력되고 있는 것을 확인할 수 있습니다.