TurtleBot3 Burger에서 Pi Camera 2와 LiDAR를 결합한 ROS 2 Humble 장애물 회피

1. 강의 목표

이번 강의의 목표는 TurtleBot3 Burger에 장착된 LiDAR와 Pi Camera 2 영상을 함께 사용하여 장애물을 회피하는 ROS 2 노드를 직접 만들어 보는 것입니다.

기본적인 TurtleBot3 예제에서는 주로 /scan 토픽의 LiDAR 거리값만 이용해 장애물을 회피합니다. 하지만 실제 로봇에서는 거리 정보만으로는 부족한 경우가 많습니다. 예를 들어 앞에 있는 물체가 벽인지, 사람인지, 색깔이 있는 표식인지, 지나갈 수 있는 공간인지 판단하려면 카메라 정보가 필요합니다.

이번 강의에서는 다음 구조를 사용합니다.

LiDAR는 로봇 주변의 거리 정보를 측정합니다.
Pi Camera 2는 로봇 전방의 영상 정보를 제공합니다.
ROS 2 노드는 /scan과 /camera/image_raw를 동시에 구독합니다.
노드는 LiDAR 거리값과 카메라 영상 분석 결과를 결합합니다.
최종적으로 /cmd_vel 토픽으로 TurtleBot3 Burger의 이동 명령을 발행합니다.

ROS 2에서 토픽은 노드 간 데이터를 주고받는 기본 통신 방식이며, 하나의 노드는 여러 토픽을 동시에 구독하거나 발행할 수 있습니다. 이 구조 덕분에 LiDAR, 카메라, 모터 제어 노드를 분리해서 설계할 수 있습니다.

2. 실습 환경

이번 강의의 기준 환경은 다음과 같습니다.

로봇: TurtleBot3 Burger
로봇 OS: Ubuntu 22.04 Server
ROS 버전: ROS 2 Humble
카메라: Raspberry Pi Camera 2
카메라 토픽: /camera/image_raw
LiDAR 토픽: /scan
속도 명령 토픽: /cmd_vel
원격 접속 방식: PC에서 SSH 접속
TurtleBot3 소스: 직접 다운로드 후 colcon build로 컴파일하여 사용
영상 확인 도구: 원격 PC의 rqt_image_view 또는 rqt

현재 원격 PC에서 로봇에 접속하여 bringup을 실행할 수 있고, 카메라 노드를 실행하면 원격 PC에서 image_raw를 구독하여 영상이 출력되는 상태입니다.

3. 카메라와 LiDAR를 같이 사용하는 이유

LiDAR만 사용하면 거리 판단은 안정적입니다. TurtleBot3 Burger에 사용되는 2D LiDAR는 로봇 주변 평면상의 거리값을 제공합니다. ROS 2에서 LiDAR 데이터는 일반적으로 sensor_msgs/msg/LaserScan 메시지로 전달되며, 이 메시지는 평면 레이저 스캐너의 한 번의 스캔 데이터를 표현합니다.

하지만 LiDAR만 사용할 경우 다음 한계가 있습니다.

물체의 종류를 알 수 없습니다.
색상이나 표식을 인식할 수 없습니다.
낮은 물체나 투명한 물체는 감지 성능이 떨어질 수 있습니다.
로봇 전방의 의미 있는 영역을 구분하기 어렵습니다.
사람, 박스, 벽, 문틈을 구분하기 어렵습니다.

카메라만 사용할 경우에도 문제가 있습니다.

단일 RGB 카메라만으로는 정확한 거리를 알기 어렵습니다.
조명 변화에 민감합니다.
바닥 무늬나 그림자를 장애물로 오인할 수 있습니다.
영상 처리 부하가 큽니다.
카메라가 보는 방향 밖의 장애물은 알 수 없습니다.

그래서 실전 로봇에서는 두 센서를 조합하는 방식이 안전합니다.

LiDAR는 거리 기반 안전 판단
카메라는 전방 영상 기반 상황 판단
두 센서가 모두 위험하다고 판단하면 정지 또는 회피
LiDAR는 위험하지 않지만 카메라가 위험하면 감속
카메라는 위험하지 않지만 LiDAR가 위험하면 즉시 회피

4. 전체 시스템 구조

전체 구조는 다음과 같습니다.

노드는 다음 역할을 합니다.

/camera/image_raw 구독
/scan 구독
OpenCV로 카메라 영상 분석
LiDAR 거리값을 전방, 좌측, 우측 영역으로 분리
카메라 분석 결과와 LiDAR 분석 결과를 결합
/cmd_vel로 직진, 감속, 좌회전, 우회전, 정지 명령 발행

5. 장애물 회피 전략

이번 예제는 너무 복잡한 AI 객체 인식부터 시작하지 않습니다. 확실히 동작하는 구조가 중요합니다.

따라서 다음과 같은 단순하면서 실전적인 전략을 사용합니다.

LiDAR 전방 거리가 가까우면 장애물로 판단합니다.
LiDAR 좌측과 우측 거리값을 비교합니다.
더 넓은 쪽으로 회전합니다.
카메라 영상 중앙 하단 영역을 관심 영역으로 설정합니다.
관심 영역 안에서 어두운 물체 또는 특정 색상 영역이 많이 보이면 장애물 가능성이 있다고 판단합니다.
LiDAR와 카메라 중 하나라도 위험하면 감속합니다.
LiDAR 전방 거리가 임계값보다 작으면 반드시 회피합니다.

핵심은 다음입니다.

LiDAR = 거리 안전장치
Camera = 전방 상황 보조 판단
/cmd_vel = 최종 이동 명령

6. ROS 2 메시지 이해

이번 예제에서 핵심적으로 사용하는 메시지는 세 가지입니다.

sensor_msgs/msg/Image
sensor_msgs/msg/LaserScan
geometry_msgs/msg/Twist

Image 메시지는 카메라 영상을 전달합니다. 이 메시지를 OpenCV에서 처리하려면 cv_bridge를 사용합니다. cv_bridge는 ROS 2 이미지 메시지와 OpenCV 이미지 표현 사이를 변환하는 역할을 합니다.

LaserScan 메시지는 LiDAR 거리 데이터를 전달합니다. 주요 필드는 다음과 같습니다.

angle_min
angle_max
angle_increment
range_min
range_max
ranges

이 중 가장 많이 사용하는 값은 ranges입니다. ranges는 각도별 거리값 배열입니다.

예를 들어 로봇 전방 거리만 보고 싶다면 ranges 배열의 중앙 부근 값을 사용합니다. 좌측은 배열의 뒤쪽, 우측은 배열의 앞쪽을 사용하는 방식으로 나눌 수 있습니다. 실제 방향 인덱스는 LiDAR 장착 방향과 드라이버 설정에 따라 확인이 필요합니다.

Twist 메시지는 로봇의 선속도와 각속도를 표현합니다. 구조는 크게 linear와 angular로 나뉘며, TurtleBot3 같은 차동구동 로봇에서는 보통 linear.x와 angular.z를 사용합니다.

예를 들어 다음 명령은 전진 명령입니다.

linear.x  > 0
angular.z = 0

다음 명령은 제자리 좌회전입니다.

linear.x  = 0
angular.z > 0

다음 명령은 제자리 우회전입니다.

linear.x  = 0
angular.z < 0

7. 패키지 생성

먼저 TurtleBot3 Burger에서 작업할 ROS 2 워크스페이스로 이동합니다.

cd ~/turtlebot3_ws/src

새 패키지를 생성합니다.

ros2 pkg create camera_lidar_avoidance \
  --build-type ament_python \
  --dependencies rclpy sensor_msgs geometry_msgs cv_bridge

패키지 구조는 다음과 같습니다.

camera_lidar_avoidance/
├── camera_lidar_avoidance
│   ├── __init__.py
│   └── camera_lidar_avoidance_node.py
├── package.xml
├── setup.py
├── setup.cfg
└── resource
    └── camera_lidar_avoidance

노드 파일을 생성합니다.

cd ~/turtlebot3_ws/src/camera_lidar_avoidance/camera_lidar_avoidance
touch camera_lidar_avoidance_node.py

8. 의존 패키지 설치

카메라 영상을 OpenCV로 처리하려면 OpenCV와 cv_bridge가 필요합니다. 이미 설치한 경우에는 아래의 과정을 생략합니다.

sudo apt update
sudo apt install -y \
  python3-opencv \
  ros-humble-cv-bridge

설치 후 다음 명령으로 확인합니다.

python3 -c "import cv2; print(cv2.__version__)"
python3 -c "from cv_bridge import CvBridge; print('cv_bridge ok')"

9. 소스

다음 코드는 LiDAR와 카메라를 동시에 사용하여 TurtleBot3 Burger를 회피 주행시키는 기본 예제입니다.

import math
import cv2
import numpy as np

import rclpy
from rclpy.node import Node

from sensor_msgs.msg import Image
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from cv_bridge import CvBridge
from rclpy.qos import qos_profile_sensor_data


class CameraLidarAvoidanceNode(Node):
    def __init__(self):
        super().__init__('camera_lidar_avoidance_node')

        self.bridge = CvBridge()

        self.image_sub = self.create_subscription(
            Image,
            '/camera/image_raw',
            self.image_callback,
            10
        )

        self.scan_sub = self.create_subscription(
            LaserScan,
            '/scan',
            self.scan_callback,
            qos_profile_sensor_data
        )

        self.cmd_pub = self.create_publisher(
            Twist,
            '/cmd_vel',
            10
        )

        self.control_timer = self.create_timer(
            0.1,
            self.control_loop
        )

        self.front_distance = float('inf')
        self.left_distance = float('inf')
        self.right_distance = float('inf')

        self.camera_obstacle_detected = False
        self.last_image_time = None
        self.last_scan_time = None

        self.safe_distance = 0.35
        self.slow_distance = 0.55

        self.forward_speed = 0.08
        self.slow_speed = 0.04
        self.turn_speed = 0.45

        self.get_logger().info('Camera + LiDAR avoidance node started')

    def image_callback(self, msg):
        try:
            frame = self.bridge.imgmsg_to_cv2(
                msg,
                desired_encoding='bgr8'
            )
        except Exception as e:
            self.get_logger().error(f'Image conversion failed: {e}')
            return

        frame= cv2.flip(frame, -1)

        self.last_image_time = self.get_clock().now()

        height, width, _ = frame.shape

        roi_y_start = int(height * 0.55)
        roi_y_end = height
        roi_x_start = int(width * 0.25)
        roi_x_end = int(width * 0.75)

        roi = frame[roi_y_start:roi_y_end, roi_x_start:roi_x_end]

        gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)
        blurred = cv2.GaussianBlur(gray, (5, 5), 0)

        _, binary = cv2.threshold(
            blurred,
            80,
            255,
            cv2.THRESH_BINARY_INV
        )

        obstacle_pixels = cv2.countNonZero(binary)
        total_pixels = binary.shape[0] * binary.shape[1]
        obstacle_ratio = obstacle_pixels / total_pixels

        self.camera_obstacle_detected = obstacle_ratio > 0.25

        debug_frame = frame.copy()

        cv2.rectangle(
            debug_frame,
            (roi_x_start, roi_y_start),
            (roi_x_end, roi_y_end),
            (0, 255, 0),
            2
        )

        status_text = f'Camera obstacle: {self.camera_obstacle_detected}, ratio: {obstacle_ratio:.2f}'

        cv2.putText(
            debug_frame,
            status_text,
            (20, 40),
            cv2.FONT_HERSHEY_SIMPLEX,
            0.6,
            (0, 255, 255),
            2
        )

        cv2.imshow('camera_lidar_avoidance_debug', debug_frame)
        cv2.waitKey(1)

    def scan_callback(self, msg):
        self.last_scan_time = self.get_clock().now()

        ranges = np.array(msg.ranges)

        ranges = np.where(np.isinf(ranges), msg.range_max, ranges)
        ranges = np.where(np.isnan(ranges), msg.range_max, ranges)

        total_count = len(ranges)

        front_indices = list(range(0, 20)) + list(range(total_count - 20, total_count))

        left_indices = list(range(60, 120))
        right_indices = list(range(total_count - 120, total_count - 60))

        self.front_distance = self.get_min_distance(ranges, front_indices)
        self.left_distance = self.get_min_distance(ranges, left_indices)
        self.right_distance = self.get_min_distance(ranges, right_indices)

    def get_min_distance(self, ranges, indices):
        valid_values = []

        for i in indices:
            if 0 <= i < len(ranges):
                value = ranges[i]
                if math.isfinite(value) and value > 0.0:
                    valid_values.append(value)

        if len(valid_values) == 0:
            return float('inf')

        return float(np.min(valid_values))

    def control_loop(self):
        cmd = Twist()

        lidar_danger = self.front_distance < self.safe_distance
        lidar_slow = self.front_distance < self.slow_distance

        if lidar_danger:
            cmd.linear.x = 0.0

            if self.left_distance > self.right_distance:
                cmd.angular.z = self.turn_speed
                decision = 'turn left'
            else:
                cmd.angular.z = -self.turn_speed
                decision = 'turn right'

        elif self.camera_obstacle_detected or lidar_slow:
            cmd.linear.x = self.slow_speed

            if self.left_distance > self.right_distance:
                cmd.angular.z = 0.25
                decision = 'slow left'
            else:
                cmd.angular.z = -0.25
                decision = 'slow right'

        else:
            cmd.linear.x = self.forward_speed
            cmd.angular.z = 0.0
            decision = 'go forward'

        self.cmd_pub.publish(cmd)

        self.get_logger().info(
            f'front={self.front_distance:.2f}, '
            f'left={self.left_distance:.2f}, '
            f'right={self.right_distance:.2f}, '
            f'camera={self.camera_obstacle_detected}, '
            f'decision={decision}'
        )


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

    node = CameraLidarAvoidanceNode()

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    stop_cmd = Twist()
    node.cmd_pub.publish(stop_cmd)

    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

10. 소스 코드 상세 설명

1) 노드 클래스 선언

class CameraLidarAvoidanceNode(Node):

이 클래스가 ROS 2 노드입니다. 하나의 노드 안에서 카메라와 LiDAR를 동시에 구독하고, 속도 명령을 발행합니다.

super().__init__('camera_lidar_avoidance_node')

노드 이름을 camera_lidar_avoidance_node로 지정합니다. 실행 후 다음 명령으로 확인할 수 있습니다.

ros2 node list

2) CvBridge 생성

self.bridge = CvBridge()

ROS 2의 sensor_msgs/msg/Image는 OpenCV가 바로 처리할 수 있는 이미지 배열이 아닙니다. 그래서 CvBridge를 사용해 ROS 이미지 메시지를 OpenCV 이미지로 변환합니다.

변환은 다음 부분에서 수행됩니다.

frame = self.bridge.imgmsg_to_cv2(
    msg,
    desired_encoding='bgr8'
)

OpenCV는 일반적으로 BGR 색상 순서를 사용합니다. 그래서 desired_encoding='bgr8'로 지정했습니다.

3) 카메라 토픽 구독

self.image_sub = self.create_subscription(
    Image,
    '/camera/image_raw',
    self.image_callback,
    10
)

이 코드는 /camera/image_raw 토픽을 구독합니다.

구성은 다음과 같습니다.

Image: 구독할 메시지 타입
/camera/image_raw: 구독할 토픽 이름
self.image_callback: 이미지가 들어올 때 실행할 함수
10: QoS 큐 크기

카메라 프레임이 들어올 때마다 image_callback() 함수가 호출됩니다.

4) LiDAR 토픽 구독

self.scan_sub = self.create_subscription(
    LaserScan,
    '/scan',
    self.scan_callback,
    10
)

이 코드는 /scan 토픽을 구독합니다.

LiDAR 데이터가 들어올 때마다 scan_callback() 함수가 실행됩니다.

5) 속도 명령 발행자 생성

self.cmd_pub = self.create_publisher(
    Twist,
    '/cmd_vel',
    10
)

이 코드는 /cmd_vel 토픽으로 속도 명령을 발행하는 publisher를 만듭니다.

TurtleBot3 Burger는 /cmd_vel로 들어오는 geometry_msgs/msg/Twist 메시지를 받아 이동합니다. 핵심 값은 다음 두 개입니다.

cmd.linear.x
cmd.angular.z

cmd.linear.x는 전진 또는 후진 속도입니다.

cmd.angular.z는 좌회전 또는 우회전 각속도입니다.

6) 제어 타이머

self.control_timer = self.create_timer(
    0.1,
    self.control_loop
)

이 코드는 0.1초마다 control_loop() 함수를 실행합니다. 즉, 10Hz 주기로 로봇의 이동 명령을 계산합니다.

카메라 콜백과 LiDAR 콜백은 센서 데이터가 들어올 때마다 실행되고, 제어 루프는 일정 주기로 실행됩니다. 이렇게 분리하면 센서 주기가 서로 달라도 제어 구조가 안정적입니다.

7) 거리 변수 초기화

self.front_distance = float('inf')
self.left_distance = float('inf')
self.right_distance = float('inf')

초기값을 무한대로 설정했습니다. 아직 LiDAR 데이터가 들어오지 않았을 때 장애물이 없는 것처럼 처리하기 위한 기본값입니다.

실제 강의에서는 안전을 위해 초기값을 0으로 두고, LiDAR가 들어오기 전까지 정지시키는 방식도 좋습니다.

더 안전한 방식은 다음입니다.

self.front_distance = 0.0
self.left_distance = 0.0
self.right_distance = 0.0

그리고 LiDAR 수신 여부를 확인한 뒤에만 주행하도록 만드는 것입니다.

8) 임계값 설정

self.safe_distance = 0.35
self.slow_distance = 0.55

safe_distance는 반드시 회피해야 하는 거리입니다. 예제에서는 0.35m로 설정했습니다.

slow_distance는 감속을 시작하는 거리입니다. 예제에서는 0.55m로 설정했습니다.

TurtleBot3 Burger는 작지만 실내 환경에서는 너무 빠르게 움직이면 위험합니다. 실습 처음에 다음처럼 더 보수적으로 설정합니다.

self.safe_distance = 0.45
self.slow_distance = 0.70
self.forward_speed = 0.05
self.slow_speed = 0.03
self.turn_speed = 0.30

9) 카메라 영상 처리

카메라 콜백의 핵심은 다음입니다.

height, width, _ = frame.shape

프레임의 높이와 너비를 가져옵니다.

그다음 관심 영역을 설정합니다.

roi_y_start = int(height * 0.55)
roi_y_end = height
roi_x_start = int(width * 0.25)
roi_x_end = int(width * 0.75)

이 영역은 영상의 중앙 하단입니다.

로봇이 전진할 때 충돌 가능성이 높은 물체는 영상의 중앙 하단에 나타나는 경우가 많습니다. 그래서 전체 영상이 아니라 이 영역만 분석합니다. 이렇게 하면 처리 속도도 빨라집니다.

10) 흑백 변환

gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)

컬러 이미지를 흑백 이미지로 바꿉니다. 장애물 여부를 단순 판단할 때는 색상 전체가 필요하지 않을 수 있습니다.

11) 블러 처리

blurred = cv2.GaussianBlur(gray, (5, 5), 0)

카메라 영상에는 노이즈가 있습니다. 블러를 적용하면 작은 노이즈를 줄일 수 있습니다.

12) 이진화

_, binary = cv2.threshold(
    blurred,
    80,
    255,
    cv2.THRESH_BINARY_INV
)

픽셀 밝기가 80보다 낮은 부분을 흰색으로 바꿉니다. THRESH_BINARY_INV를 사용했기 때문에 어두운 영역이 흰색으로 표시됩니다.

이 예제에서는 어두운 물체가 전방에 많이 나타나면 장애물 가능성이 있다고 판단합니다.

실제 환경에서는 바닥 색상, 조명, 그림자에 영향을 많이 받습니다. 실전에서는 HSV 색상 필터, 객체 인식, 세그멘테이션, Depth 카메라, 또는 LiDAR 기반 costmap을 함께 사용하는 것이 더 좋습니다.

13) 장애물 픽셀 비율 계산

obstacle_pixels = cv2.countNonZero(binary)
total_pixels = binary.shape[0] * binary.shape[1]
obstacle_ratio = obstacle_pixels / total_pixels

장애물 비율 = 장애물로 판단된 픽셀 수 / 전체 검사 영역 픽셀 수

즉, 카메라 영상 중에서 관심 영역, 즉 ROI 안에 장애물 후보가 얼마나 많이 차지하는지를 계산합니다.

예를 들어 ROI 안에 전체 픽셀이 10,000개 있고, 그중 3,000개가 장애물 후보로 판단되었다면 다음과 같습니다.

obstacle_ratio = 3000 / 10000
obstacle_ratio = 0.3

즉, ROI의 30%가 장애물 후보라는 뜻입니다.

cv2.countNonZero()는 이미지 배열 안에서 0이 아닌 픽셀의 개수를 셉니다.

total_pixels = binary.shape[0] * binary.shape[1]

이 줄은 binary 이미지의 전체 픽셀 개수를 계산합니다.

OpenCV 이미지에서 shape는 이미지 크기 정보를 가지고 있습니다.

binary.shape[0]은 이미지의 높이입니다. binary.shape[1]은 이미지의 너비입니다.

이 코드는 관심 영역 안에서 장애물로 추정되는 픽셀 비율을 계산합니다.

self.camera_obstacle_detected = obstacle_ratio > 0.25

관심 영역의 25% 이상이 어두운 장애물 후보로 판단되면 카메라 장애물 감지 상태를 True로 설정합니다.

이 값은 환경에 맞게 조정해야 합니다.

obstacle_ratio > 0.15

로 설정하면 더 민감해집니다.

obstacle_ratio > 0.35

로 설정하면 덜 민감해집니다.

14) 디버그 화면 출력

debug_frame = frame.copy()

원본 이미지를 복사합니다.

cv2.rectangle(
    debug_frame,
    (roi_x_start, roi_y_start),
    (roi_x_end, roi_y_end),
    (0, 255, 0),
    2
)

관심 영역을 초록색 사각형으로 표시합니다.

cv2.putText(
    debug_frame,
    status_text,
    (20, 40),
    cv2.FONT_HERSHEY_SIMPLEX,
    0.6,
    (0, 255, 255),
    2
)

현재 장애물 판단 상태를 영상 위에 표시합니다.

cv2.imshow('camera_lidar_avoidance_debug', debug_frame)
cv2.waitKey(1)

OpenCV 창으로 디버그 영상을 출력합니다.

주의할 점이 있습니다. TurtleBot3 Burger를 Ubuntu Server로 사용하고 SSH로 접속하는 경우, 로봇 자체에는 GUI가 없을 수 있습니다. 이 경우 cv2.imshow()가 동작하지 않을 수 있습니다.

15) LiDAR 데이터 처리 설명

LiDAR 콜백의 핵심은 다음입니다.

ranges = np.array(msg.ranges)

msg.ranges는 거리값 배열입니다.

그런데 LiDAR 값에는 inf 또는 nan이 들어올 수 있습니다.

ranges = np.where(np.isinf(ranges), msg.range_max, ranges)
ranges = np.where(np.isnan(ranges), msg.range_max, ranges)

inf는 측정 범위 안에 물체가 없다는 의미로 자주 나타납니다. 예제에서는 이 값을 최대 거리값으로 바꿨습니다. nan은 잘못된 측정값입니다. 이것도 최대 거리값으로 처리했습니다.

np.isinf(ranges)는 각 값이 inf인지 검사합니다. ranges 배열 안에서 inf인 값은 msg.range_max로 바꾸고, inf가 아닌 값은 원래 값을 그대로 사용합니다.

np.where()가 조건에 따라 값을 바꿉니다.

np.where(조건, 조건이 True일 때 값, 조건이 False일 때 값)

따라서:

ranges = np.where(np.isinf(ranges), msg.range_max, ranges)

는 다음 구조입니다.

조건: ranges 값이 inf인가?
True이면: msg.range_max 사용
False이면: 기존 ranges 값 사용

a. 전방 영역 인덱스

front_indices = list(range(0, 20)) + list(range(total_count - 20, total_count))

일반적으로 TurtleBot3의 /scan 데이터에서 전방은 배열의 처음과 끝 부분에 걸쳐 있는 경우가 많습니다. 그래서 앞쪽 20개와 뒤쪽 20개 인덱스를 합쳐 전방 영역으로 사용했습니다.

이 코드는 전방 영역을 나타내는 인덱스를 만듭니다.

ranges 배열
[전방 일부][우측][후방][좌측][전방 일부]

단, LiDAR 드라이버 설정이나 센서 장착 방향에 따라 이 인덱스는 달라질 수 있습니다. 반드시 다음 명령으로 실제 값을 확인해야 합니다.

ros2 topic echo /scan

또는 RViz2에서 LaserScan을 표시해 방향을 확인합니다.

b. 좌측 영역 인덱스

left_indices = list(range(60, 120))

좌측 방향 거리값을 가져오기 위한 인덱스입니다.

c. 우측 영역 인덱스

right_indices = list(range(total_count - 120, total_count - 60))

우측 방향 거리값을 가져오기 위한 인덱스입니다.

만약 라이다 데이터의 방향이 반대로 적용된다면 아래와 같이 수정해야 합니다.

right_indices = list(range(60, 120))
left_indices = list(range(total_count - 120, total_count - 60))

d. 최소 거리 계산

self.front_distance = self.get_min_distance(ranges, front_indices)
self.left_distance = self.get_min_distance(ranges, left_indices)
self.right_distance = self.get_min_distance(ranges, right_indices)

각 영역에서 가장 가까운 거리값을 계산합니다.

장애물 회피에서는 평균 거리보다 최소 거리가 더 중요합니다. 평균값은 좁은 장애물을 놓칠 수 있기 때문입니다.

예를 들어 전방 40개 거리값 중 39개는 2m이고 1개만 0.2m라면 평균은 안전해 보일 수 있습니다. 하지만 실제로는 충돌 위험이 있습니다. 그래서 최소값을 사용합니다.

16) 제어 로직 설명

제어 루프의 핵심은 다음입니다.

lidar_danger = self.front_distance < self.safe_distance
lidar_slow = self.front_distance < self.slow_distance

LiDAR 전방 거리가 safe_distance보다 작으면 위험 상태입니다.

LiDAR 전방 거리가 slow_distance보다 작으면 감속 상태입니다.

a. 가장 위험한 경우

if lidar_danger:
    cmd.linear.x = 0.0

    if self.left_distance > self.right_distance:
        cmd.angular.z = self.turn_speed
        decision = 'turn left'
    else:
        cmd.angular.z = -self.turn_speed
        decision = 'turn right'

전방 장애물이 너무 가까우면 전진을 멈춥니다.

그다음 좌측과 우측 거리값을 비교합니다.

좌측이 더 넓으면 좌회전합니다.

cmd.angular.z = self.turn_speed

우측이 더 넓으면 우회전합니다.

cmd.angular.z = -self.turn_speed

b. 감속 또는 카메라 위험 감지

elif self.camera_obstacle_detected or lidar_slow:
    cmd.linear.x = self.slow_speed

카메라가 장애물 가능성을 감지했거나 LiDAR 전방 거리가 애매하게 가까우면 감속합니다.

이때 바로 멈추지 않고 천천히 움직이면서 더 넓은 방향으로 약간 회전합니다.

if self.left_distance > self.right_distance:
    cmd.angular.z = 0.25
else:
    cmd.angular.z = -0.25

실전에서는 카메라 감지 결과만으로 회전 방향을 결정하기보다, LiDAR의 좌우 거리 차이를 함께 사용하는 방식이 안정적입니다.

c. 안전한 경우

else:
    cmd.linear.x = self.forward_speed
    cmd.angular.z = 0.0

전방이 안전하고 카메라에서도 위험이 없으면 직진합니다.

11. setup.py 수정

setup.py 파일을 다음과 같이 수정합니다.

from setuptools import setup

package_name = 'camera_lidar_avoidance'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        (
            'share/ament_index/resource_index/packages',
            ['resource/' + package_name]
        ),
        (
            'share/' + package_name,
            ['package.xml']
        ),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='turtlebot3_user',
    maintainer_email='user@example.com',
    description='Camera and LiDAR based obstacle avoidance for TurtleBot3 Burger',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'camera_lidar_avoidance_node = camera_lidar_avoidance.camera_lidar_avoidance_node:main',
        ],
    },
)

11. package.xml 확인

package.xml에는 다음 의존성이 있어야 합니다.

<?xml version="1.0"?>
<package format="3">
  <name>camera_lidar_avoidance</name>
  <version>0.0.0</version>
  <description>Camera and LiDAR based obstacle avoidance for TurtleBot3 Burger</description>
  <maintainer email="user@example.com">turtlebot3_user</maintainer>
  <license>Apache-2.0</license>

  <depend>rclpy</depend>
  <depend>sensor_msgs</depend>
  <depend>geometry_msgs</depend>
  <depend>cv_bridge</depend>

  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>
  <test_depend>python3-pytest</test_depend>

  <export>
    <build_type>ament_python</build_type>
  </export>
</package>

여기서 중요한 부분은 다음입니다.

<export>
  <build_type>ament_python</build_type>
</export>

이 부분이 빠지면 ROS 2 Python 패키지가 정상적으로 인식되지 않는 경우가 있습니다. 특히 직접 패키지를 만들고 colcon build할 때 자주 실수하는 부분입니다.

12. 빌드

워크스페이스 루트로 이동합니다.

cd ~/turtlebot3_ws

빌드합니다.

colcon build --packages-select camera_lidar_avoidance

환경 설정을 적용합니다.

source install/setup.bash

13. 실행

실습 시에는 터미널을 여러 개 사용하는 것이 편합니다. SSH 접속 환경이라면 tmux를 사용하는 것을 추천합니다.

1) TurtleBot3 Bringup 실행

ros2 launch turtlebot3_bringup robot.launch.py

2) 카메라 실행

ros2 launch turtlebot3_bringup camera.launch.py format:=BGR888

3) 토픽 확인

ros2 topic list

다음 토픽이 있는지 확인합니다.

/scan
/camera/image_raw
/cmd_vel

4) 영상 확인

원격 PC에서 ROS_DOMAIN_ID와 네트워크 설정이 맞다면 다음 명령으로 영상을 볼 수 있습니다.

rqt_image_view

/camera/image_raw를 선택합니다.

5) 장애물 회피 노드 실행

ros2 run camera_lidar_avoidance camera_lidar_avoidance_node

17. 실행 중 확인해야 할 값

노드가 실행되면 다음과 같은 로그가 출력됩니다.

front=0.82, left=1.24, right=0.75, camera=False, decision=go forward
front=0.42, left=0.90, right=0.30, camera=True, decision=slow left
front=0.25, left=0.40, right=1.20, camera=True, decision=turn right

각 값의 의미는 다음과 같습니다.

front: 전방 최소 거리
left: 좌측 최소 거리
right: 우측 최소 거리
camera: 카메라 기반 장애물 감지 여부
decision: 최종 이동 판단

실행 결과를 보고 다음 질문에 답을 생각해보세요..

왜 로봇이 좌회전했는가?
왜 로봇이 감속했는가?
카메라가 True인데 LiDAR는 안전하면 어떻게 동작하는가?
LiDAR가 위험한데 카메라가 False이면 어떻게 동작하는가?
조명이 바뀌면 카메라 판단이 어떻게 변하는가?

18. 개선 방법 고려

기본 예제에서는 어두운 영역 비율로 장애물을 판단했습니다. 하지만 이 방식은 너무 단순합니다.

다음 방식으로 개선할 수 있습니다.

1) HSV 색상 필터 사용

특정 색상의 장애물만 감지하고 싶다면 HSV 색상 공간을 사용합니다.

예를 들어 빨간색 물체를 감지하려면 다음 구조를 사용할 수 있습니다.

hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)

lower_red1 = np.array([0, 100, 100])
upper_red1 = np.array([10, 255, 255])

lower_red2 = np.array([160, 100, 100])
upper_red2 = np.array([179, 255, 255])

mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
mask2 = cv2.inRange(hsv, lower_red2, upper_red2)

mask = mask1 + mask2

red_pixels = cv2.countNonZero(mask)
red_ratio = red_pixels / (mask.shape[0] * mask.shape[1])

이 방식은 “카메라로 특정 색상 표식을 감지하고 LiDAR로 거리 안전 판단을 하는 예제”로 확장하기 좋습니다.

2) 윤곽선 검출

장애물 후보의 크기와 위치를 알고 싶다면 contour를 사용할 수 있습니다.

contours, _ = cv2.findContours(
    binary,
    cv2.RETR_EXTERNAL,
    cv2.CHAIN_APPROX_SIMPLE
)

for contour in contours:
    area = cv2.contourArea(contour)

    if area > 500:
        x, y, w, h = cv2.boundingRect(contour)
        cv2.rectangle(roi, (x, y), (x + w, y + h), (255, 0, 0), 2)

이렇게 하면 단순 픽셀 비율보다 더 의미 있는 장애물 후보를 찾을 수 있습니다.

3) 카메라 방향 기반 회피

카메라 영상의 왼쪽에 장애물이 많으면 오른쪽으로 회피하고, 오른쪽에 장애물이 많으면 왼쪽으로 회피하도록 만들 수 있습니다.

roi_left = binary[:, :binary.shape[1] // 2]
roi_right = binary[:, binary.shape[1] // 2:]

left_ratio = cv2.countNonZero(roi_left) / roi_left.size
right_ratio = cv2.countNonZero(roi_right) / roi_right.size

판단은 다음처럼 할 수 있습니다.

if left_ratio > right_ratio:
    camera_turn_direction = 'right'
else:
    camera_turn_direction = 'left'

다만 최종 회피 방향은 LiDAR 거리값과 함께 결정해야 합니다.

카메라: 왼쪽 장애물 많음 → 오른쪽 선호
LiDAR: 오른쪽이 좁음 → 오른쪽 회피 금지
최종 판단: 정지 또는 좌회전

이런 식으로 센서 융합의 필요성을 설명할 수 있습니다.

4) 센서 데이터 타임아웃

카메라나 LiDAR 데이터가 일정 시간 이상 들어오지 않으면 정지해야 합니다.

def is_sensor_timeout(self):
    now = self.get_clock().now()

    if self.last_scan_time is None:
        return True

    scan_dt = (now - self.last_scan_time).nanoseconds / 1e9

    if scan_dt > 1.0:
        return True

    return False

제어 루프에서 다음처럼 사용합니다.

if self.is_sensor_timeout():
    cmd.linear.x = 0.0
    cmd.angular.z = 0.0
    self.cmd_pub.publish(cmd)
    return

이 로직은 꼭 넣는 것이 좋습니다. 센서가 멈췄는데 로봇이 계속 전진하면 위험합니다.

5) 최대 속도 제한

강의장에서는 속도를 낮게 제한해야 합니다.

self.forward_speed = 0.05
self.slow_speed = 0.03
self.turn_speed = 0.25

처음부터 빠르게 움직이면 디버깅이 어렵고 충돌 위험이 커집니다.

6) 비상 정지 명령

테스트 중에는 별도 터미널에서 다음 명령으로 정지할 수 있어야 합니다.

ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist \
"{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"

하지만 장애물 회피 노드가 계속 /cmd_vel을 발행하고 있으면 다시 움직일 수 있습니다. 그래서 급할 때는 노드를 종료하는 것이 확실합니다.

Ctrl + C

또는 TurtleBot3 bringup을 종료합니다.

19. launch 파일 추가

실습을 편하게 하려면 launch 파일을 추가할 수 있습니다.

패키지 안에 launch 디렉터리를 만듭니다.

cd ~/turtlebot3_ws/src/camera_lidar_avoidance
mkdir launch
touch launch/camera_lidar_avoidance.launch.py

camera_lidar_avoidance.launch.py를 다음과 같이 작성합니다.

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    return LaunchDescription([
        Node(
            package='camera_lidar_avoidance',
            executable='camera_lidar_avoidance_node',
            name='camera_lidar_avoidance_node',
            output='screen'
        )
    ])

그리고 setup.py의 data_files에 launch 파일 설치 설정을 추가합니다.

import os
from glob import glob
from setuptools import setup

package_name = 'camera_lidar_avoidance'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        (
            'share/ament_index/resource_index/packages',
            ['resource/' + package_name]
        ),
        (
            'share/' + package_name,
            ['package.xml']
        ),
        (
            os.path.join('share', package_name, 'launch'),
            glob('launch/*.launch.py')
        ),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='turtlebot3_user',
    maintainer_email='user@example.com',
    description='Camera and LiDAR based obstacle avoidance for TurtleBot3 Burger',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'camera_lidar_avoidance_node = camera_lidar_avoidance.camera_lidar_avoidance_node:main',
        ],
    },
)

다시 빌드합니다.

cd ~/turtlebot3_ws
colcon build --symlink-install
source install/setup.bash

실행합니다.

ros2 launch camera_lidar_avoidance camera_lidar_avoidance.launch.py

20. 도전 과제

1) 과제 1: 안전 거리 변경

safe_distance와 slow_distance를 변경해 로봇의 반응 차이를 확인합니다.

self.safe_distance = 0.45
self.slow_distance = 0.70

다음의 질문에 답해보세요.

안전 거리를 크게 하면 어떤 변화가 있는가?
안전 거리를 작게 하면 어떤 위험이 있는가?
강의장 환경에서는 어떤 값이 적절한가?

2) 과제 2: 카메라 ROI 변경

ROI를 중앙 하단에서 전체 하단으로 바꿔봅니다.

roi_x_start = 0
roi_x_end = width

비교 질문은 다음과 같습니다.

ROI가 넓어지면 오검출이 늘어나는가?
ROI가 좁아지면 장애물을 놓치는가?
TurtleBot3 Burger 전방 주행에는 어떤 ROI가 적절한가?

3) 과제 3: 색상 기반 장애물 감지

빨간색, 파란색, 노란색 중 하나를 선택해 특정 색상 장애물을 감지하게 만듭니다.

핵심 코드는 HSV 변환입니다.

hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, lower_color, upper_color)

4) 과제 4: 카메라 좌우 판단 추가

카메라 영상의 왼쪽과 오른쪽 장애물 비율을 계산해 회피 방향 결정에 반영합니다.

left_ratio = cv2.countNonZero(roi_left) / roi_left.size
right_ratio = cv2.countNonZero(roi_right) / roi_right.size

5) 과제 5: 센서 타임아웃 추가

LiDAR 데이터가 1초 이상 들어오지 않으면 정지하도록 만듭니다.

실전 로봇에서는 이 과제가 가장 중요합니다. 센서가 멈췄는데 로봇이 계속 움직이면 안 됩니다.