Wego Limo로 구현하는 ROS 2 카메라 기반 차선 주행 시스템 #2

7. detect_line

import rclpy
from rclpy.node import Node 

import cv2
import numpy as np
from cv_bridge import CvBridge

from sensor_msgs.msg import Image
from std_msgs.msg import Int32

class DetectLine(Node):
    def __init__(self):
        # CV bridge
        self.br = CvBridge()

        # Subscribe camera data
        super().__init__('detect_line')
        self.subscription = self.create_subscription(
                            Image,
                            '/camera/color/image_raw', 
                            self.image_callback, 
                            rclpy.qos.qos_profile_sensor_data)
        self.subscription # to prevent from warning

        # Publish result (offset between reference distance and real distance)
        self.dis_publisher = self.create_publisher(Int32, 'distance_y', 10)

        # Publish Image for debugging
        self.debug_publisher = self.create_publisher(Image, 'debug_image', 10)
        self.timer_ = self.create_timer(0.1, self.timer_callback)

        # Parameters (For Masking Lane, For the reference distance of lane, ROI)
        self.declare_parameter('roi_x_l', 0)
        self.declare_parameter('roi_x_h', 320) 
        self.declare_parameter('roi_y_l', 400)
        self.declare_parameter('roi_y_h', 480)
        
        self.roi_x_l=self.get_parameter('roi_x_l')
        self.roi_x_h=self.get_parameter('roi_x_h')
        self.roi_y_l=self.get_parameter('roi_y_l')
        self.roi_y_h=self.get_parameter('roi_y_h')

        self.declare_parameter('lane_h_l', 0)
        self.declare_parameter('lane_l_l', 90)
        self.declare_parameter('lane_s_l', 100)
        self.declare_parameter('lane_h_h', 60)
        self.declare_parameter('lane_l_h', 220)
        self.declare_parameter('lane_s_h', 255)

        lane_h_l=self.get_parameter('lane_h_l')
        lane_l_l=self.get_parameter('lane_l_l')
        lane_s_l=self.get_parameter('lane_s_l')
        lane_h_h=self.get_parameter('lane_h_h')
        lane_l_h=self.get_parameter('lane_l_h')
        lane_s_h=self.get_parameter('lane_s_h')
        
        self.yellow_lane_low = np.array([lane_h_l.value,
                                         lane_l_l.value,
                                         lane_s_l.value])
        self.yellow_lane_high = np.array([lane_h_h.value,
                                          lane_l_h.value, 
                                          lane_s_h.value])
        
        self.declare_parameter('reference_distance', 170)
        self.reference_distance = self.get_parameter('reference_distance')

        # Parameter For debugging
        # 0: ROI
        # 1: Masking
        # 2: Moment of lane
        self.declare_parameter('debug_image_num', 2) #default 2
        self.debug_sequence = self.get_parameter('debug_image_num')

        # to syncronize subscriber and publisher
        self.sub_flag = False
    
    def timer_callback(self):
        if self.sub_flag:     
            if self.debug_sequence.value == 0:
                self.debug_publisher.publish(self.br.cv2_to_imgmsg(self.roi_,'bgr8'))
            elif self.debug_sequence.value == 1:
                self.debug_publisher.publish(self.br.cv2_to_imgmsg(self.mask_yellow,'mono8'))
            else:    
                self.debug_publisher.publish(self.br.cv2_to_imgmsg(self.image_,'bgr8'))
          
    def image_callback(self, msg):
        # convert opencv Mat type to image msg type
        self.image_ = self.br.imgmsg_to_cv2(msg, 'bgr8')
        
        # Make Region of Interest
        self.roi_ = self.image_[self.roi_y_l.value:self.roi_y_h.value,
                                self.roi_x_l.value:self.roi_x_h.value]

        # Masking the lane
        hls = cv2.cvtColor(self.roi_, cv2.COLOR_BGR2HLS)
        self.mask_yellow = cv2.inRange(hls, self.yellow_lane_low,
                                            self.yellow_lane_high)
        
        # Calculating the Moment of the lane
        M = cv2.moments(self.mask_yellow)
        if M['m00'] > 0:
            cx = int(M['m10']/M['m00'])
            cy = int(M['m01']/M['m00'])
            cy = self.roi_y_l.value + cy
                        
            self.image_ = cv2.line(self.image_, 
                        (self.reference_distance.value, 0),
                        (self.reference_distance.value, 480),
                        (0, 255, 0), 
                        5)
            self.image_ = cv2.circle(self.image_, (cx, cy), 10,(255, 0, 0), -1)

            distance_to_ref = self.reference_distance.value -cx
        else: # When limo cannot find lane publish 0 data
            distance_to_ref = 0

        # Publishing the offset between current distance with lane and reference distance
        dis = Int32()
        dis.data = distance_to_ref
        self.dis_publisher.publish(dis)

        self.sub_flag = True

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

    detect_line = DetectLine()
    rclpy.spin(detect_line)

    detect_line.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

1) 전체 소스 코드의 역할

먼저 이 노드의 핵심 목적을 정리하면 다음과 같습니다.

카메라 영상에서 노란색 계열의 차선을 찾은 뒤, 해당 차선의 중심 좌표 cx를 계산합니다.

그리고 미리 설정된 기준 위치인 reference_distance와 비교하여 다음 값을 계산합니다.

distance_to_ref = self.reference_distance.value - cx

예를 들어 기준 위치가 170이고 실제 검출된 차선 중심이 150이라면 다음과 같습니다.

170 - 150 = 20

반대로 실제 차선 중심이 190이라면 다음과 같습니다.

170 - 190 = -20

따라서 distance_y 토픽에는 양수, 음수 또는 0이 전달될 수 있습니다.

이 값을 이용하면 이후의 주행 제어 노드에서 조향 명령을 계산할 수 있습니다.

예를 들어 간단한 비례 제어는 다음과 같은 형태로 만들 수 있습니다.

steering = Kp × distance_y

즉, 이 노드는 직접 모터를 움직이는 노드라기보다는 차선 추종에 필요한 오차값을 계산해 주는 영상 인식 노드라고 보면 됩니다.

self.reference_distance 값을 150으로 설정할 경우 회전 시 차선 중앙에서 회전합니다.

2) 필요한 Python 모듈 불러오기

코드의 시작 부분은 다음과 같습니다.

from cv_bridge import CvBridge

from sensor_msgs.msg import Image
from std_msgs.msg import Int32

각 모듈의 역할을 살펴보겠습니다.

CvBridge

from cv_bridge import CvBridge

ROS 2의 sensor_msgs/Image 메시지와 OpenCV 이미지 사이를 변환하는 역할을 합니다.

ROS 2 카메라 토픽으로 들어오는 데이터는 일반적인 OpenCV 이미지가 아니라 sensor_msgs/msg/Image 형식입니다.

따라서 다음과 같은 변환 과정이 필요합니다.

ROS 2 Image → OpenCV 이미지

반대로 디버깅 이미지를 ROS 2 토픽으로 발행하려면 다음 변환이 필요합니다.

OpenCV 이미지 → ROS 2 Image

이 역할을 CvBridge가 담당합니다.

3) DetectLine 클래스 생성

클래스 선언부는 다음과 같습니다.

class DetectLine(Node):

ROS 2의 Node 클래스를 상속하고 있습니다.

생성자는 다음과 같이 시작합니다.

def __init__(self):

노드가 생성될 때 카메라 Subscriber, Publisher, Timer, Parameter 등이 모두 초기화됩니다.

4) CvBridge 객체 생성

self.br = CvBridge()

카메라 영상 메시지를 OpenCV 이미지로 변환하기 위한 CvBridge 객체를 생성합니다.

이 객체는 이후 다음과 같이 사용됩니다.

self.br.imgmsg_to_cv2()

그리고 반대 방향 변환은 다음과 같습니다.

self.br.cv2_to_imgmsg()

5) ROS 2 노드 초기화

다음 코드가 중요한 부분입니다.

super().__init__('detect_line')

부모 클래스인 ROS 2 Node의 생성자를 호출하면서 노드 이름을 detect_line으로 설정합니다.

6) 카메라 영상 Subscriber 생성

카메라 Subscriber는 다음과 같습니다.

self.subscription = self.create_subscription(
                    Image,
                    '/camera/color/image_raw', 
                    self.image_callback, 
                    rclpy.qos.qos_profile_sensor_data)

이 Subscriber는 다음 토픽을 구독합니다.

/camera/color/image_raw

메시지 타입은 다음과 같습니다.

sensor_msgs/msg/Image

새로운 영상이 들어오면 다음 함수가 호출됩니다.

self.image_callback

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

Camera
  ↓
/camera/color/image_raw
  ↓
DetectLine
  ↓
image_callback()

7) Sensor Data QoS를 사용하는 이유

Subscriber의 마지막 인자로 다음 설정이 들어갑니다.

rclpy.qos.qos_profile_sensor_data

ROS 2에서는 Publisher와 Subscriber 사이의 데이터 전달 정책을 QoS라고 합니다.

카메라 영상처럼 데이터가 빠르게 계속 들어오는 센서 데이터는 오래된 프레임을 반드시 모두 처리할 필요가 없습니다.

예를 들어 로봇이 현재 시점에 주행하고 있는데 1초 전 카메라 영상을 처리하는 것은 의미가 떨어질 수 있습니다.

따라서 센서 데이터에서는 낮은 지연시간을 우선하는 QoS 설정을 많이 사용합니다.

8) distance_y Publisher 생성

다음 코드는 차선 위치 오차를 발행합니다.

self.dis_publisher = self.create_publisher(Int32, 'distance_y', 10)

메시지 타입은 다음과 같습니다.

std_msgs/msg/Int32

토픽 이름은 다음과 같습니다.

/distance_y

이 토픽에는 최종적으로 계산된 차선 오차가 들어갑니다.

터미널에서는 다음과 같이 확인할 수 있습니다.

ros2 topic echo /distance_y

예를 들어 다음과 같은 값이 반복해서 출력될 수 있습니다.

data: 24

또는

data: -15

이 값은 차선 중심이 기준 위치의 어느 방향으로 얼마나 떨어져 있는지를 나타냅니다.

9) 디버깅 이미지 Publisher

self.debug_publisher = self.create_publisher(Image, 'debug_image', 10)

영상 처리 결과를 직접 확인하기 위한 Publisher입니다.

토픽 이름은 다음과 같습니다.

/debug_image

ROS 2에서 영상 처리 프로그램을 개발할 때 최종 숫자 결과만 확인해서는 문제를 찾기 어렵습니다.

따라서 다음과 같은 이미지를 직접 확인하는 것이 중요합니다.

ROI가 제대로 잘렸는지
색상 마스킹이 제대로 되었는지
차선 중심 좌표가 제대로 계산되었는지

이 코드에서는 하나의 디버그 토픽을 사용하면서 파라미터에 따라 서로 다른 영상을 출력하도록 구성되어 있습니다.

10) Timer 생성

self.timer_ = self.create_timer(0.1, self.timer_callback)

0.1초마다 timer_callback() 함수가 실행됩니다.

즉 대략 10Hz 주기로 디버깅 영상을 발행합니다.

카메라 영상이 예를 들어 30fps로 들어오더라도 디버그 영상은 10Hz로 제한되는 구조입니다.

영상 처리 결과 확인용 토픽이기 때문에 반드시 카메라 입력 프레임과 동일한 속도로 발행할 필요는 없습니다.

11) ROI 파라미터 설정

ROI는 Region Of Interest의 약자로 관심 영역을 의미합니다.

다음 파라미터들이 사용됩니다.

self.declare_parameter('roi_x_l', 0)
self.declare_parameter('roi_x_h', 320) 
self.declare_parameter('roi_y_l', 400)
self.declare_parameter('roi_y_h', 480)

각 값은 다음 의미를 갖습니다.

roi_x_l = ROI의 왼쪽 X 좌표
roi_x_h = ROI의 오른쪽 X 좌표
roi_y_l = ROI의 위쪽 Y 좌표
roi_y_h = ROI의 아래쪽 Y 좌표

기본값을 기준으로 보면 다음 영역을 선택합니다.

X: 0 ~ 320
Y: 400 ~ 480

만약 카메라 영상이 640 × 480이라면 화면 왼쪽 아래쪽 일부 영역만 검사하는 것입니다.

ROI를 사용하는 이유는 매우 중요합니다.

전체 영상에 대해 색상 검출을 수행하면 차선과 관계없는 노란색 물체까지 검출될 수 있습니다.

하지만 실제 로봇의 차선은 일반적으로 카메라 영상의 아래쪽에 존재합니다.

따라서 필요한 영역만 처리하면 다음과 같은 장점이 있습니다.

연산량 감소
잘못된 차선 검출 감소
차선 검출 안정성 향상
처리 속도 향상

12) 파라미터 가져오기

다음 코드는 선언한 파라미터를 가져옵니다.

self.roi_x_l=self.get_parameter('roi_x_l')
self.roi_x_h=self.get_parameter('roi_x_h')
self.roi_y_l=self.get_parameter('roi_y_l')
self.roi_y_h=self.get_parameter('roi_y_h')

여기서 주의할 부분이 있습니다.

get_parameter()의 반환값 자체는 단순한 정수가 아닙니다.

ROS 2의 Parameter 객체입니다.

따라서 실제 값을 사용할 때는 다음처럼 .value를 붙입니다.

self.roi_y_l.value

예를 들어 다음 코드에서 확인할 수 있습니다.

self.image_[self.roi_y_l.value:self.roi_y_h.value,
            self.roi_x_l.value:self.roi_x_h.value]

13) 차선 색상 범위 파라미터

노란색 차선을 검출하기 위한 파라미터는 다음과 같습니다.

self.declare_parameter('lane_h_l', 0)
self.declare_parameter('lane_l_l', 90)
self.declare_parameter('lane_s_l', 100)

self.declare_parameter('lane_h_h', 60)
self.declare_parameter('lane_l_h', 220)
self.declare_parameter('lane_s_h', 255)

각 문자의 의미는 다음과 같습니다.

H = Hue
L = Lightness
S = Saturation

그리고

_l = Low
_h = High

라고 이해하면 됩니다.

예를 들어 다음 파라미터는 Hue의 하한입니다.

lane_h_l

다음 파라미터는 Hue의 상한입니다.

lane_h_h

결과적으로 차선 색상은 다음 범위로 설정됩니다.

self.yellow_lane_low = np.array([
    lane_h_l.value,
    lane_l_l.value,
    lane_s_l.value
])

self.yellow_lane_high = np.array([
    lane_h_h.value,
    lane_l_h.value,
    lane_s_h.value
])

즉 HLS 값이 해당 범위 안에 들어오는 픽셀만 차선으로 판단합니다.

14) 왜 RGB가 아니라 HLS를 사용하는가

카메라 영상은 기본적으로 BGR 형식으로 들어옵니다.

하지만 BGR 값만 가지고 특정 색상을 분리하면 조명 변화에 민감할 수 있습니다.

예를 들어 같은 노란색 차선도 햇빛의 세기나 그림자에 따라 BGR 값이 크게 변합니다.

HLS는 다음처럼 색상 정보를 분리해서 다룰 수 있습니다.

H : 실제 색상
L : 밝기
S : 색의 선명도

따라서 특정 색상의 차선을 검출할 때 BGR보다 튜닝하기 편리한 경우가 많습니다.

HSV를 사용하는 방법도 일반적이며, 실제 프로젝트에서는 카메라와 환경에 따라 HLS와 HSV를 비교해서 선택하면 됩니다.

15) 기준 위치 reference_distance

self.declare_parameter('reference_distance', 170)
self.reference_distance = self.get_parameter('reference_distance')

reference_distance는 차선이 위치하기를 원하는 기준 X 좌표입니다.

기본값은 다음과 같습니다.

이 값과 실제 검출된 차선의 중심 좌표 cx를 비교합니다.

distance_to_ref = self.reference_distance.value - cx

이 값이 사실상 차선 추종 제어기의 Error가 됩니다.

제어 시스템 관점에서는 다음과 같이 볼 수 있습니다.

Error = Reference - Current

즉 전형적인 피드백 제어 구조입니다.

16) debug_image_num 파라미터

self.declare_parameter('debug_image_num', 2)
self.debug_sequence = self.get_parameter('debug_image_num')

이 파라미터는 어떤 디버그 영상을 출력할지 결정합니다.

코드 주석을 기준으로 다음처럼 사용할 수 있습니다.

0 : ROI 영상
1 : Masking 영상
2 : 차선 중심이 표시된 최종 영상

따라서 영상 처리 개발 과정에서 매우 편리합니다.

17) sub_flag 변수

self.sub_flag = False

이 변수는 카메라 영상이 최소 한 번이라도 수신되었는지 확인하는 용도입니다.

카메라 영상이 들어오기 전에 timer_callback()이 먼저 실행될 수 있기 때문입니다.

그 상태에서 다음 변수에 접근하면 문제가 발생할 수 있습니다.

self.roi_
self.mask_yellow
self.image_

아직 생성되지 않은 변수이기 때문입니다.

따라서 카메라 콜백이 한 번 실행된 후 다음처럼 설정합니다.

self.sub_flag = True

그리고 Timer에서는 다음 조건을 확인합니다.

if self.sub_flag:

이렇게 하면 영상 데이터가 준비된 이후에만 디버그 이미지를 발행하게 됩니다.

18) timer_callback 함수

전체 코드는 다음과 같습니다.

def timer_callback(self):
    if self.sub_flag:     
        if self.debug_sequence.value == 0:
            self.debug_publisher.publish(
                self.br.cv2_to_imgmsg(self.roi_, 'bgr8')
            )
        elif self.debug_sequence.value == 1:
            self.debug_publisher.publish(
                self.br.cv2_to_imgmsg(self.mask_yellow, 'mono8')
            )
        else:    
            self.debug_publisher.publish(
                self.br.cv2_to_imgmsg(self.image_, 'bgr8')
            )

먼저 다음 조건을 검사합니다.

if self.sub_flag:

카메라 영상이 한 번 이상 처리된 경우에만 동작합니다.

debug_image_num = 0

self.br.cv2_to_imgmsg(self.roi_, 'bgr8')

ROI 이미지를 발행합니다.

ROI가 원하는 영역을 정확하게 포함하고 있는지 확인할 때 사용합니다.

debug_image_num = 1

self.br.cv2_to_imgmsg(self.mask_yellow, 'mono8')

색상 마스킹 결과를 출력합니다.

cv2.inRange() 결과는 흑백 영상이기 때문에 mono8 형식으로 전달합니다.

차선으로 인식된 영역은 흰색에 가깝게 나타나고 나머지 영역은 검은색으로 나타납니다.

debug_image_num = 2

self.br.cv2_to_imgmsg(self.image_, 'bgr8')

최종 결과 이미지를 출력합니다.

이 영상에는 기준선과 차선 중심 위치가 표시됩니다.

19) image_callback 함수

영상 처리의 핵심은 image_callback() 함수입니다.

def image_callback(self, msg):

카메라에서 새로운 영상이 들어올 때마다 실행됩니다.

전체 처리 과정은 다음과 같습니다.

Image 메시지 수신
        ↓
OpenCV 이미지 변환
        ↓
ROI 추출
        ↓
BGR → HLS 변환
        ↓
색상 Mask 생성
        ↓
Moment 계산
        ↓
차선 중심 계산
        ↓
기준 위치와 오차 계산
        ↓
distance_y 발행

20) ROS Image를 OpenCV 이미지로 변환

self.image_ = self.br.imgmsg_to_cv2(msg, 'bgr8')

카메라에서 받은 sensor_msgs/Image 메시지를 OpenCV 이미지로 변환합니다.

bgr8은 다음을 의미합니다.

Blue
Green
Red
각 채널 8bit

OpenCV의 일반적인 컬러 영상 형식입니다.

21) ROI 추출

다음 코드가 ROI를 생성합니다.

self.roi_ = self.image_[
    self.roi_y_l.value:self.roi_y_h.value,
    self.roi_x_l.value:self.roi_x_h.value
]

NumPy 배열에서 영상 좌표를 잘라내는 방식입니다.

중요한 점은 배열 접근 순서가 다음과 같다는 것입니다.

image[y, x]

따라서 코드에서도 Y 범위가 먼저 나오고 X 범위가 다음에 나옵니다.

일반적인 수학 좌표인 (x, y)와 순서가 반대이기 때문에 OpenCV를 처음 사용할 때 자주 실수하는 부분입니다.

22) BGR 영상을 HLS로 변환

hls = cv2.cvtColor(self.roi_, cv2.COLOR_BGR2HLS)

ROI 이미지를 BGR에서 HLS로 변환합니다.

원본 데이터는 다음 형식입니다.

BGR

변환된 데이터는 다음 형식입니다.

HLS

이후 색상 Threshold를 적용합니다.

23) 노란색 차선 Mask 생성

self.mask_yellow = cv2.inRange(
    hls,
    self.yellow_lane_low,
    self.yellow_lane_high
)

cv2.inRange()는 각 픽셀의 값이 지정한 범위 안에 포함되는지 검사합니다.

범위 안이면 보통 다음 값이 됩니다.

범위 밖이면 다음 값이 됩니다.

따라서 결과 영상은 흑백 Binary Image가 됩니다.

쉽게 표현하면 다음과 같습니다.

검출된 차선 → 흰색
그 외 영역 → 검은색

24) 이미지 모멘트 계산

다음 코드가 핵심 중 하나입니다.

M = cv2.moments(self.mask_yellow)

이미지 모멘트는 물체의 면적, 중심, 방향 등의 정보를 계산하는 데 사용됩니다.

이 코드에서 특히 중요한 값은 다음과 같습니다.

M['m00']
M['m10']
M['m01']

m00은 검출 영역의 면적과 관련된 값입니다.

그리고 중심 좌표는 다음과 같이 계산할 수 있습니다.

cx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00'])

즉 차선으로 검출된 모든 픽셀들의 무게중심을 계산한다고 이해하면 됩니다.

25) M[‘m00’]을 먼저 확인하는 이유

다음 조건이 있습니다.

if M['m00'] > 0:

왜 필요한지 이해하는 것이 중요합니다.

차선이 전혀 검출되지 않았다면 M['m00'] 값이 0이 될 수 있습니다.

이때 다음 연산을 수행하면 문제가 발생합니다.

M['m10'] / M['m00']

즉 0으로 나누기가 발생합니다.

따라서 반드시 먼저 다음 조건을 확인해야 합니다.

M['m00'] > 0

차선 픽셀이 존재할 때만 중심 좌표를 계산하는 것입니다.

26) 차선 중심 좌표 cx와 cy

다음 코드로 중심을 계산합니다.

cx = int(M['m10']/M['m00'])
cy = int(M['m01']/M['m00'])

여기서 cx와 cy는 ROI 내부에서의 좌표입니다.

예를 들어 ROI의 Y 범위가 다음과 같다고 해보겠습니다.

400 ~ 480

ROI 내부의 cy가 30이라면 실제 전체 영상 기준 Y 좌표는 다음과 같습니다.

400 + 30 = 430

그래서 코드에서는 다음 연산을 수행합니다.

cy = self.roi_y_l.value + cy

27) X 좌표는 왜 보정하지 않는가

현재 코드는 cy만 전체 영상 좌표로 변환하고 cx는 그대로 사용합니다.

cy = self.roi_y_l.value + cy

하지만 엄밀히 말하면 ROI의 시작 X 좌표가 0이 아닌 경우에는 cx도 보정해야 합니다.

현재 기본 설정은 다음과 같습니다.

roi_x_l = 0

따라서 문제가 드러나지 않습니다.

하지만 나중에 ROI를 다음과 같이 변경한다면

roi_x_l = 100

실제 전체 영상의 X 좌표를 구하려면 다음과 같은 처리가 필요합니다.

cx_global = self.roi_x_l.value + cx

현재 코드에서 reference_distance가 전체 영상 기준 좌표라면 이 부분은 수정할 필요가 있습니다.

반대로 reference_distance 자체도 ROI 내부 좌표 기준으로 설정하는 구조라면 현재 방식도 사용할 수 있습니다.

좌표 기준을 명확히 통일하는 것이 중요합니다.

28) 기준선을 영상에 표시

다음 코드는 초록색 기준선을 그립니다.

self.image_ = cv2.line(
    self.image_, 
    (self.reference_distance.value, 0),
    (self.reference_distance.value, 480),
    (0, 255, 0), 
    5
)

기준선의 X 좌표는 다음입니다.

self.reference_distance.value

Y 좌표는 다음 범위입니다.

0 ~ 480

색상은 BGR 기준으로 다음입니다.

(0, 255, 0)

즉 초록색입니다.

선 두께는 다음과 같습니다.

5 pixel

29) 검출된 차선 중심 표시

다음 코드는 차선 중심에 원을 그립니다.

self.image_ = cv2.circle(
    self.image_,
    (cx, cy),
    10,
    (255, 0, 0),
    -1
)

원의 중심은 다음과 같습니다.

(cx, cy)

반지름은 다음과 같습니다.

10 pixel

색상은 BGR 기준으로 다음입니다.

(255, 0, 0)

즉 파란색입니다.

마지막 인자가 -1이므로 내부가 채워진 원을 그립니다.

30) 기준 위치와 차선 사이의 오차 계산

가장 중요한 제어 데이터는 다음 코드에서 계산됩니다.

distance_to_ref = self.reference_distance.value - cx

수식으로 표현하면 다음과 같습니다.

Distance Error = Reference X - Lane Center X

예를 들어 기준값이 170일 때 차선 중심이 130이면 다음과 같습니다.

170 - 130 = 40

차선 중심이 200이면 다음과 같습니다.

170 - 200 = -30

이 값의 부호를 이용하면 차선이 기준 위치의 어느 방향에 있는지 판단할 수 있습니다.

다만 실제 차량에서 양수가 좌회전을 의미하는지 우회전을 의미하는지는 카메라 방향, 모터 배치, 조향 명령의 부호 체계에 따라 달라집니다.

따라서 실제 주행에서는 반드시 방향을 확인해야 합니다.

31) 차선을 찾지 못했을 때

차선이 검출되지 않으면 다음 코드가 실행됩니다.

else:
    distance_to_ref = 0

즉 차선을 찾지 못하면 0을 발행합니다.

이 방식은 실습 단계에서는 간단하지만 실제 주행 시스템에서는 약간 위험할 수 있습니다.

왜냐하면 0이라는 값이 다음 두 가지 의미를 동시에 가질 수 있기 때문입니다.

정상적으로 차선 중앙에 위치함
차선을 전혀 찾지 못함

두 상황을 구분할 수 없습니다.

실제 시스템에서는 별도의 차선 검출 상태 토픽을 사용하는 것이 더 안전합니다.

예를 들어 다음과 같이 구성할 수 있습니다.

/distance_y
/lane_detected

lane_detected가 False라면 차량을 정지하거나 이전 조향 값을 제한적으로 유지하도록 만들 수 있습니다.

32) Int32 메시지 생성

다음 코드에서는 ROS 2 메시지를 생성합니다.

dis = Int32()

그리고 계산 결과를 저장합니다.

dis.data = distance_to_ref

마지막으로 발행합니다.

self.dis_publisher.publish(dis)

결과적으로 다른 ROS 2 노드에서는 /distance_y 토픽만 구독하면 영상 처리 내부 구조를 몰라도 차선 위치 오차를 사용할 수 있습니다.

이것이 ROS 2의 큰 장점 중 하나입니다.

영상 처리와 모터 제어를 서로 독립적인 노드로 분리할 수 있습니다.

33) sub_flag 활성화

영상 처리가 완료된 후 다음 코드가 실행됩니다.

self.sub_flag = True

이 순간부터 Timer에서 디버그 영상을 발행할 수 있습니다.

즉 카메라 영상이 준비되지 않은 상태에서 발생할 수 있는 변수 접근 오류를 방지하는 장치입니다.

34) main 함수

ROS 2 Python 프로그램의 진입점은 다음과 같습니다.

def main(args=None):

ROS 2 초기화는 다음과 같이 수행합니다.

rclpy.init(args=args)

그 다음 노드를 생성합니다.

detect_line = DetectLine()

노드를 계속 실행시키기 위해 다음 코드를 사용합니다.

rclpy.spin(detect_line)

spin()이 실행되는 동안 ROS 2는 Subscriber Callback과 Timer Callback을 계속 처리합니다.

프로그램이 종료되면 다음 코드를 통해 노드를 제거합니다.

detect_line.destroy_node()

그리고 ROS 2를 종료합니다.

rclpy.shutdown()

8. 디버그 이미지 확인 방법

디버그 이미지는 rqt_image_view를 이용해 확인할 수 있습니다.

ros2 run rqt_image_view rqt_image_view

실행 후 이미지 토픽 목록에서 다음 토픽을 선택합니다.

/debug_image

1) debug_image_num 0

ROI로 지정된 영역을 확인합니다.

이 단계에서는 차선이 ROI 내부에 충분히 포함되어 있는지 확인해야 합니다.

차선이 ROI 밖에 있다면 다음 값을 수정합니다.

roi_x_l
roi_x_h
roi_y_l
roi_y_h

2) debug_image_num 1

HLS 마스크 결과를 확인합니다.

차선 영역은 흰색으로, 검출되지 않은 영역은 검은색으로 나타나는 것이 일반적입니다.

차선 이외의 바닥이나 물체가 함께 흰색으로 검출된다면 HLS 범위를 좁혀야 합니다.

차선이 거의 검출되지 않는다면 범위를 넓히거나 조명을 조정해야 합니다.

3) debug_image_num 2

최종적으로 검출된 차선의 위치를 확인합니다.

차선 중심점이나 검출 기준선이 실제 차선 위에 표시되는지 확인해야 합니다.

검출 위치가 계속 흔들린다면 다음 방법을 적용할 수 있습니다.

모폴로지 연산으로 작은 노이즈 제거
가장 큰 Contour만 선택
이전 프레임과 현재 프레임 위치를 평균 처리
이동 평균 필터 적용
검출 실패 시 이전 유효값 유지
최소 검출 면적 설정

12. HLS 파라미터 조정 방법

카메라 기반 차선 인식은 조명에 큰 영향을 받습니다.

동일한 트랙이라도 낮과 밤, 실내 조명, 햇빛 반사, 카메라 노출값에 따라 검출 결과가 달라질 수 있습니다.

파라미터는 다음 순서로 조정하는 것이 좋습니다.

카메라 영상을 확인합니다.
ROI를 차선이 보이는 영역으로 제한합니다.
debug_image_num을 1로 설정합니다.
H 값으로 차선 색상을 대략적으로 선택합니다.
L 값으로 너무 어둡거나 밝은 영역을 제거합니다.
S 값으로 채도가 낮은 바닥 영역을 제거합니다.
마스크에서 차선만 남는지 확인합니다.
debug_image_num을 2로 변경합니다.
검출 중심이 실제 차선에 위치하는지 확인합니다.
저속으로 주행하면서 값을 미세 조정합니다.

파라미터 변경 후 노드를 다시 실행해야 하는 구조라면 Launch 파일을 재실행합니다.

ROS 2 파라미터를 동적으로 변경할 수 있도록 구현되어 있다면 다음 명령어를 사용할 수 있습니다.

ros2 param list /detect_line

현재 값을 확인합니다.

ros2 param get /detect_line reference_distance

값을 변경합니다.

ros2 param set /detect_line reference_distance 220

실제 노드 이름은 Launch 파일의 설정에 따라 다를 수 있습니다.

13. P 제어를 이용한 차선 추종

차선 제어에서 사용하는 오차는 다음과 같이 정의할 수 있습니다.

오차 = 기준 차선 위치 - 실제 차선 위치

P 제어에서는 오차에 비례해 회전 명령을 생성합니다.

회전 속도 = P Gain × 오차

오차가 작으면 조금 회전하고, 오차가 크면 더 빠르게 회전합니다.

장점은 구현이 간단하고 계산량이 적다는 것입니다.

단점은 카메라 영상이 흔들리거나 차선 위치가 급격히 변하면 로봇이 좌우로 진동할 수 있다는 점입니다.

필요한 경우 PD 제어로 확장할 수 있습니다.

error = reference_distance - detected_lane_x
derivative = error - previous_error

angular_z = kp * error + kd * derivative

previous_error = error

D 항은 오차가 급격하게 변하는 정도를 반영해 조향 진동을 줄이는 데 도움이 될 수 있습니다.

14. 제어 우선순위

차선 추종 시스템에서는 여러 입력이 동시에 들어올 수 있습니다.

예를 들어 차선을 따라가는 중에 전방 장애물과 감속 구간이 동시에 감지될 수 있습니다.

일반적으로 다음과 같은 우선순위를 사용하는 것이 안전합니다.

E-stop
센서 데이터 오류
차선 검출 실패
감속 구간
정상 차선 주행

개념적인 제어 코드는 다음과 같습니다.

if e_stop:
    linear_x = 0.0
    angular_z = 0.0

elif not lane_detected:
    linear_x = 0.0
    angular_z = 0.0

elif slow_down:
    linear_x = slow_speed
    angular_z = steering_command

else:
    linear_x = default_speed
    angular_z = steering_command

차선 검출에 실패했는데 이전 조향값을 계속 사용하는 경우 트랙 밖으로 이탈할 수 있습니다. 일정 시간 이상 차선이 검출되지 않으면 정지하도록 구성하는 것이 안전합니다.

15. rqt_graph로 노드 연결 구조 확인하기

ROS 2 노드와 토픽의 연결 구조는 rqt_graph를 이용해 확인할 수 있습니다.

ros2 run rqt_graph rqt_graph

정상적인 시스템에서는 대략 다음과 같은 데이터 흐름을 확인할 수 있습니다.

Camera
  ↓
detect_line
  ↓
distance_y
  ↓
limo_control
  ↓
cmd_vel
  ↓
Limo Base Driver

LiDAR 기반 장애물 감지 경로는 다음과 같습니다.

LiDAR
  ↓
scan
  ↓
limo_e_stop
  ↓
e_stop
  ↓
limo_control

과속방지턱 감지 기능을 사용할 경우 다음 흐름이 추가됩니다.

Camera 또는 센서
  ↓
detect_hump
  ↓
slow_down
  ↓
limo_control

rqt_graph에서 노드가 보이지 않는다면 다음 사항을 확인합니다.

노드가 실제로 실행 중인지 확인합니다.
토픽 이름이 서로 같은지 확인합니다.
메시지 타입이 일치하는지 확인합니다.
Namespace가 추가되어 있지 않은지 확인합니다.
QoS 설정이 호환되는지 확인합니다.

16. rqt_plot으로 제어값 확인하기

차선 오차와 회전 속도를 그래프로 확인하면 Gain 설정을 조정하는 데 도움이 됩니다.

ros2 run rqt_plot rqt_plot

다음 값을 추가해 확인할 수 있습니다.

/distance_y/data
/cmd_vel/linear/x
/cmd_vel/angular/z

ROS 2 배포판과 rqt_plot 버전에 따라 필드 입력 방식이 다음과 같을 수도 있습니다.

/distance_y
/cmd_vel/linear/x
/cmd_vel/angular/z

그래프에서 차선 오차가 좌우로 크게 반복되면 p_gain이 너무 크거나 영상 검출값이 불안정한지 확인해야 합니다.

17. 토픽을 직접 확인하는 방법

차선 오차 확인:

ros2 topic echo /distance_y

E-stop 상태 확인:

ros2 topic echo /e_stop

감속 상태 확인:

ros2 topic echo /slow_down

최종 속도 명령 확인:

ros2 topic echo /cmd_vel

토픽 발행 주기 확인:

ros2 topic hz /distance_y

ros2 topic hz /cmd_vel

카메라는 정상적으로 동작하지만 /distance_y가 발행되지 않는다면 detect_line 노드에서 이미지 변환 오류나 차선 검출 실패가 발생했는지 로그를 확인해야 합니다.

18. Launch 파일에서 확인할 부분

Track Application의 Launch 파일에서는 일반적으로 다음 내용을 설정합니다.

실행할 패키지 이름
실행 파일 이름
노드 이름
파라미터 YAML 경로
토픽 Remapping
출력 로그 설정
Namespace 설정
조건부 노드 실행

예시는 다음과 같은 구조입니다.

Node(
    package='limo_ros2_application',
    executable='detect_line',
    name='detect_line',
    output='screen',
    parameters=[detect_line_param_file]
)

파라미터 파일 경로는 다음과 같이 구성할 수 있습니다.

detect_line_param_file = os.path.join(
    get_package_share_directory('limo_ros2_application'),
    'params',
    'detect_line.yaml'
)

코드나 파라미터 파일을 수정한 후에는 패키지를 다시 빌드합니다.

cd ~/wego_ws

colcon build --packages-select limo_ros2_application

source install/setup.bash

19. 차선이 검출되지 않을 때 확인할 사항

차선이 검출되지 않는 경우 다음 순서로 점검합니다.

카메라 토픽이 정상적으로 발행되는지 확인합니다.
detect_line 노드가 올바른 토픽을 구독하는지 확인합니다.
rqt_image_view에서 원본 영상을 확인합니다.
ROI 안에 차선이 포함되는지 확인합니다.
HLS 마스크에서 차선이 흰색으로 표시되는지 확인합니다.
HLS 최소값과 최대값을 조정합니다.
카메라 노출과 화이트밸런스를 확인합니다.
차선 검출 면적이 최소 기준보다 큰지 확인합니다.
검출한 차선 위치가 이미지 범위를 벗어나지 않는지 확인합니다.
OpenCV와 cv_bridge 오류 로그를 확인합니다.

20. Limo가 반대 방향으로 조향할 때

차선 오차값은 정상적으로 변하지만 Limo가 반대 방향으로 회전한다면 회전 명령의 부호가 반대로 적용된 것입니다.

예를 들어 기존 코드가 다음과 같다면

angular_z = p_gain * error

다음과 같이 부호를 반대로 변경해야 할 수 있습니다.

angular_z = -p_gain * error

오차값 계산 순서를 반대로 변경하는 방법도 있습니다.

error = detected_lane_x - reference_distance

또는

error = reference_distance - detected_lane_x

둘 중 하나만 변경해야 합니다. 오차 계산과 회전 명령 부호를 동시에 변경하면 결과가 다시 같아질 수 있습니다.

21. Limo가 좌우로 심하게 흔들릴 때

다음 원인을 확인합니다.

p_gain이 너무 큰 경우
기본 속도가 너무 빠른 경우
카메라 영상의 차선 위치가 불안정한 경우
ROI가 너무 좁은 경우
차선 이외의 영역이 함께 검출되는 경우
차선 중심값에 필터가 없는 경우
제어 주기가 일정하지 않은 경우

우선 default_speed를 낮춘 후 p_gain을 조금씩 줄이는 것이 좋습니다.

예시는 다음과 같습니다.

default_speed: 0.1
p_gain: 0.004

검출값에 이동 평균 필터를 적용하는 방법도 있습니다.

filtered_error = sum(error_history) / len(error_history)

다만 필터 구간을 지나치게 길게 설정하면 반응이 늦어져 곡선에서 차선을 이탈할 수 있습니다.

22. 장애물이 있어도 정지하지 않을 때

다음 항목을 확인합니다.

/scan 토픽이 정상적으로 발행되는지 확인합니다.
LiDAR의 전방 방향이 코드의 전방 인덱스와 일치하는지 확인합니다.
거리값에 inf 또는 nan이 포함되어 있는지 확인합니다.
정지 거리 파라미터가 너무 작지 않은지 확인합니다.
/e_stop 값이 True로 변경되는지 확인합니다.
limo_control이 /e_stop 토픽을 구독하는지 확인합니다.
다른 노드가 /cmd_vel을 동시에 발행하는지 확인합니다.

/cmd_vel 발행 노드는 다음 명령어로 확인할 수 있습니다.

ros2 topic info /cmd_vel --verbose

여러 노드가 동시에 /cmd_vel을 발행하면 명령이 충돌할 수 있습니다. 실제 시스템에서는 Twist Multiplexer를 사용해 Teleop, Navigation, Track, E-stop 명령의 우선순위를 관리하는 방법도 고려할 수 있습니다.

23. QoS 설정 확인하기

카메라와 LiDAR 토픽은 센서 데이터용 QoS를 사용하는 경우가 많습니다.

Publisher와 Subscriber의 QoS가 호환되지 않으면 토픽 이름과 메시지 타입이 같아도 데이터를 수신하지 못할 수 있습니다.

Python 노드에서는 다음과 같이 Sensor Data QoS를 사용할 수 있습니다.

from rclpy.qos import qos_profile_sensor_data

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

LiDAR 구독에서도 동일한 방식을 사용할 수 있습니다.

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

토픽의 QoS 정보는 다음 명령어로 확인합니다.

ros2 topic info /scan --verbose

24. 제어 명령 제한 추가하기

차선 오차가 순간적으로 크게 발생하면 과도한 회전 명령이 출력될 수 있습니다.

안전을 위해 최대 각속도를 제한하는 것이 좋습니다.

max_angular_speed = 0.8

angular_z = max(
    -max_angular_speed,
    min(max_angular_speed, angular_z)
)

전진 속도도 범위를 제한할 수 있습니다.

max_linear_speed = 0.2

linear_x = max(
    0.0,
    min(max_linear_speed, linear_x)
)

곡선 구간에서 회전값이 커질수록 전진 속도를 낮추는 방식도 사용할 수 있습니다.

speed_ratio = 1.0 - min(
    abs(angular_z) / max_angular_speed,
    1.0
)

linear_x = minimum_speed + \
    (default_speed - minimum_speed) * speed_ratio

이를 적용하면 직선에서는 빠르게 주행하고 곡선에서는 자동으로 감속할 수 있습니다.

25. Watchdog 기능 추가하기

카메라 노드나 차선 검출 노드가 중단되었는데 마지막 속도 명령이 계속 유지되면 위험할 수 있습니다.

따라서 일정 시간 동안 /distance_y 또는 센서 데이터가 들어오지 않으면 자동 정지하는 Watchdog 기능을 추가하는 것이 좋습니다.

개념적인 코드는 다음과 같습니다.

current_time = self.get_clock().now()

if current_time - self.last_lane_msg_time > timeout:
    cmd_vel.linear.x = 0.0
    cmd_vel.angular.z = 0.0

다음 상황에서 자동 정지하도록 구성할 수 있습니다.

카메라 데이터 수신 중단
LiDAR 데이터 수신 중단
차선 검출 결과 수신 중단
E-stop 노드 통신 중단
제어 노드 내부 예외 발생

26. 차선 검출 실패 상태 추가하기

기본 구조에서는 /distance_y만 발행하지만, 검출 성공 여부를 별도의 Boolean 토픽으로 발행하면 시스템을 더 안전하게 만들 수 있습니다.