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

Wego Limo는 조향 방식을 변경할 수 있는 교육용 모바일 로봇으로, 카메라와 LiDAR, IMU 등의 센서를 ROS 2 환경에서 활용할 수 있습니다.

이번 글에서는 Wego Limo를 Differential Mode로 설정한 상태에서 전용 트랙의 차선을 인식하고 자동으로 주행하는 ROS 2 애플리케이션을 살펴보겠습니다.

차선 주행 애플리케이션은 하나의 노드가 모든 기능을 처리하는 방식이 아니라, 차선 검출과 장애물 감지, 속도 제어 기능을 각각 별도의 노드로 분리한 구조입니다. 이를 통해 ROS 2의 토픽 통신, 파라미터 설정, Launch 파일 구성, 센서 데이터 처리 방법을 종합적으로 실습할 수 있습니다.

1. Wego Limo Track Application 개요

Wego Limo의 Track Application은 카메라로 차선을 검출하고, LiDAR로 전방 장애물을 감지한 뒤, 각 노드의 결과를 바탕으로 로봇의 선속도와 각속도를 계산하는 시스템입니다.

전체 동작은 다음 순서로 진행됩니다.

1. 카메라에서 전방 영상을 수신합니다.
2. 영상에서 지정된 색상 범위의 차선을 검출합니다.
3. 검출된 차선과 기준 위치 사이의 오차를 계산합니다.
4. LiDAR 데이터로 전방 장애물 유무를 확인합니다.
5. 과속방지턱 검출 기능을 사용하는 경우 감속 신호를 확인합니다.
6. 제어 노드에서 선속도와 각속도를 계산합니다.
7. 계산된 제어 명령을 /cmd_vel 토픽으로 발행합니다.
8. Limo가 차선을 따라 이동하거나 장애물 앞에서 정지합니다.

이 구조는 각 기능이 독립된 노드로 구성되어 있어 하나의 기능에 문제가 발생했을 때 원인을 비교적 쉽게 찾을 수 있다는 장점이 있습니다.

2. 실습 전 확인 사항

Track Application을 실행하기 전에 다음 내용을 확인해야 합니다.

1. Limo의 조향 모드가 Differential Mode인지 확인합니다.
2. 카메라 영상이 정상적으로 수신되는지 확인합니다.
3. LiDAR의 /scan 토픽이 발행되는지 확인합니다.
4. 트랙의 차선 색상이 검출 파라미터와 맞는지 확인합니다.
5. 조종기를 이용한 수동 모드와 자동 모드 전환이 정상적으로 동작하는지 확인합니다.
6. 비상 정지 방법을 미리 확인합니다.
7. 처음에는 낮은 속도로 테스트합니다.

카메라 토픽과 LiDAR 토픽은 다음 명령어로 확인할 수 있습니다.

ros2 topic list

카메라 이미지 토픽의 발행 주기는 다음과 같이 확인할 수 있습니다.

ros2 topic hz /camera/color/image_raw

LiDAR 데이터의 발행 주기는 다음 명령어로 확인합니다.

ros2 topic hz /scan

ros2 topic echo /scan

실제 토픽 이름은 카메라 드라이버와 LiDAR 설정에 따라 다를 수 있습니다.

scan 토픽에서 angle_min과 angle_increment 값을 확인합니다.

3. Track Application 실행 방법

첫 번째 터미널에서 Wego Limo 통합 드라이버를 실행합니다.

ros2 launch wego teleop_launch.py

두 번째 터미널에서 Track Application을 실행합니다.

ros2 launch limo_ros2_application track_application_launch.py

Launch 파일이 정상적으로 실행되면 차선 검출, 장애물 감지, 차량 제어와 관련된 노드가 시작됩니다.

모든 센서와 노드가 정상적으로 동작하는 것을 확인한 후 Limo를 자동 모드로 전환합니다.

실제 주행을 시작하기 전에는 Limo의 바퀴를 지면에서 약간 들어 올린 상태로 /cmd_vel 출력 방향이 올바른지 먼저 확인하는 것이 안전합니다.

4. Track Application의 주요 노드

Track Application은 기능별로 분리된 ROS 2 노드들로 구성됩니다.

핵심적으로 다음 노드가 사용됩니다.

1. limo_e_stop
2. detect_line
3. limo_control

프로젝트 구성에 따라 과속방지턱을 인식하는 detect_hump 노드가 함께 사용될 수 있습니다.

각 노드는 ROS 2 토픽을 통해 데이터를 주고받으며, 최종적으로 limo_control 노드가 이동 명령을 생성합니다.

5. limo_e_stop 노드

limo_e_stop은 LiDAR 데이터를 이용해 Limo 전방에 장애물이 있는지 판단하는 안전 노드입니다.

1) 코드 위치

~/wego_ws/src/limo_ros2_application/limo_ros2_application/limo_e_stop.py

import rclpy
from rclpy.node import Node

from std_msgs.msg import Bool 
from sensor_msgs.msg import LaserScan
from math import pi, cos, sin

class LimoEStop(Node):
    def __init__(self):
        super().__init__('limo_e_stop')
        self.subscription = self.create_subscription(
            LaserScan,
            'scan',
            self.laser_callback,
            rclpy.qos.qos_profile_sensor_data)
        self.subscription  # prevent unused variable warning

        self.publisher_ = self.create_publisher(Bool, 'e_stop', 10)
        
    def laser_callback(self, msg):
        estop = Bool()
        estop.data = False

        for i, data in enumerate(msg.ranges):
            current_angle = msg.angle_min + msg.angle_increment*i
            cx = data * cos(current_angle)
            cy = data * sin(current_angle)
            if 0.01 < cx <0.2 and -0.1 < cy < 0.1:
                estop.data = True
                break

        self.publisher_.publish(estop)

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

    limo_e_stop = LimoEStop()
    rclpy.spin(limo_e_stop)

    limo_e_stop.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

2) Bool 메시지 타입

다음 코드를 살펴보겠습니다.

from std_msgs.msg import Bool

Bool은 ROS 2의 가장 기본적인 메시지 타입 중 하나입니다.

이번 코드에서는 긴급 정지 여부를 표현하는 데 사용합니다.

False → 장애물 없음
True → 장애물 감지

Publisher는 나중에 다음과 같이 만들어집니다.

self.publisher_ = self.create_publisher(Bool, 'e_stop', 10)

따라서 e_stop 토픽의 메시지 타입은 다음과 같습니다.

std_msgs/msg/Bool

3) LaserScan 메시지

다음 코드가 LiDAR 데이터와 관련된 부분입니다.

from sensor_msgs.msg import LaserScan

ROS 2에서 2D LiDAR 데이터를 전달할 때 가장 일반적으로 사용하는 메시지가 LaserScan입니다.

예를 들어 YDLIDAR, RPLIDAR, Hokuyo, Limo에 장착된 LiDAR 등 대부분의 2D LiDAR 드라이버가 이 형태의 데이터를 제공합니다.

LaserScan 메시지에는 대표적으로 다음 정보가 들어 있습니다.

angle_min
angle_max
angle_increment
range_min
range_max
ranges

그중 이번 코드에서 사용하는 값은 다음과 같습니다.

msg.angle_min
msg.angle_increment
msg.ranges

4) 삼각함수 사용

다음 코드에서는 수학 함수를 가져옵니다.

from math import pi, cos, sin

실제로 현재 소스에서는 다음 함수가 사용됩니다.

cos()
sin()

pi는 import되어 있지만 현재 코드에서는 사용하지 않습니다.

따라서 현재 코드만 기준으로 한다면 다음과 같이 작성해도 됩니다.

from math import cos, sin

cos()와 sin()은 LiDAR의 거리와 각도를 X, Y 좌표로 변환하는 데 사용됩니다.

5) LimoEStop 클래스

노드 클래스는 다음과 같이 정의됩니다.

class LimoEStop(Node):

그리고 생성자는 다음과 같습니다.

def __init__(self):

노드 객체가 생성되면 생성자 안에서 Subscriber와 Publisher를 초기화합니다.

6) LaserScan Subscriber 생성

LiDAR 데이터를 구독하는 Subscriber는 다음과 같습니다.

self.subscription = self.create_subscription(
    LaserScan,
    'scan',
    self.laser_callback,
    rclpy.qos.qos_profile_sensor_data)

즉 /scan 토픽으로 새로운 LiDAR 데이터가 들어올 때마다 laser_callback() 함수가 실행됩니다.

7) Sensor Data QoS 사용

Subscriber의 마지막 부분을 보면 다음 코드가 있습니다.

rclpy.qos.qos_profile_sensor_data

LiDAR, 카메라, IMU 같은 센서 데이터는 일반 데이터와 조금 다른 특성이 있습니다.

센서 값은 계속해서 새로운 값이 들어오기 때문에 오래된 데이터를 완벽하게 전달하는 것보다 최신 데이터를 빠르게 처리하는 것이 중요합니다.

따라서 ROS 2에서는 센서 데이터에 적합한 QoS 프로파일을 제공합니다.

qos_profile_sensor_data

카메라나 LiDAR Subscriber를 만들 때 자주 사용하는 설정입니다.

8) self.subscription 코드의 의미

다음 코드가 있습니다.

self.subscription

주석은 다음과 같습니다.

# prevent unused variable warning

ROS 2 Python 예제에서 종종 볼 수 있는 형태입니다.

Subscriber 객체를 self.subscription이라는 멤버 변수에 저장했기 때문에 실제 Python에서는 이 코드가 반드시 필요한 것은 아닙니다.

즉 다음 한 줄을 제거하더라도 일반적으로 Subscriber는 정상적으로 동작합니다.

self.subscription

따라서 기능적인 필수 코드라기보다는 예제 코드에서 명시적으로 Subscriber 객체를 유지한다는 의미에 가깝습니다.

9) E-Stop Publisher 생성

다음 코드는 긴급 정지 상태를 발행하는 Publisher를 생성합니다.

self.publisher_ = self.create_publisher(Bool, 'e_stop', 10)

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

std_msgs/msg/Bool

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

/e_stop

Queue Depth는 다음 값입니다.

10

이 Publisher가 최종적으로 다음 상태를 전달합니다.

장애물 없음 → False
장애물 있음 → True

10) laser_callback 함수

실제 장애물 판단은 다음 Callback 함수에서 수행됩니다.

def laser_callback(self, msg):

/scan 토픽으로 새로운 LaserScan 메시지가 들어올 때마다 실행됩니다.

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

LaserScan 수신
      ↓
E-Stop 기본값 False
      ↓
모든 LiDAR Point 검사
      ↓
각 Point의 각도 계산
      ↓
거리 + 각도 → X, Y 변환
      ↓
위험 영역 내부인지 확인
      ↓
장애물이 있으면 True
      ↓
/e_stop 발행

11) E-Stop 메시지 생성

Callback 함수가 시작되면 다음 객체를 생성합니다.

estop = Bool()

그리고 기본값을 다음과 같이 설정합니다.

estop.data = False

즉 처음에는 다음과 같이 가정합니다.

현재 장애물이 없다.

이후 LiDAR의 모든 데이터를 검사하면서 위험 영역에 장애물이 하나라도 발견되면 True로 변경합니다.

이 방식은 안전장치 프로그램에서 자주 사용되는 구조입니다.

12) LaserScan의 ranges 데이터

다음 코드가 핵심입니다.

for i, data in enumerate(msg.ranges):

msg.ranges에는 LiDAR가 측정한 거리값들이 배열 형태로 저장됩니다.

예를 들어 개념적으로 다음과 같은 데이터가 있다고 생각할 수 있습니다.

ranges[0] = 1.24
ranges[1] = 1.22
ranges[2] = 1.21
ranges[3] = 0.85
...

각 값은 특정 각도 방향에서 LiDAR 센서가 측정한 장애물까지의 거리입니다.

일반적으로 단위는 meter입니다.

따라서

0.2

라는 값이 있다면 약 20cm 거리의 장애물을 의미합니다.

13) enumerate를 사용하는 이유

다음 코드에서는 Python의 enumerate()를 사용합니다.

for i, data in enumerate(msg.ranges):

여기서 i는 배열의 Index입니다.

0
1
2
3
...

data는 해당 Index에서 측정된 거리값입니다.

예를 들어 다음과 같은 관계가 됩니다.

i = 0    data = ranges[0]
i = 1    data = ranges[1]
i = 2    data = ranges[2]

Index가 필요한 이유는 각 거리값이 어느 각도에서 측정된 값인지 계산하기 위해서입니다.

14) LiDAR의 현재 각도 계산

현재 측정 Point의 각도는 다음 코드로 계산합니다.

current_angle = msg.angle_min + msg.angle_increment*i

이 부분은 LaserScan을 이해할 때 매우 중요합니다.

LaserScan에는 시작 각도가 저장되어 있습니다.

msg.angle_min

그리고 다음 측정 Point까지의 각도 차이가 저장되어 있습니다.

msg.angle_increment

따라서 i번째 Point의 각도는 다음 공식으로 계산할 수 있습니다.

현재 각도
=
최소 각도
+
각도 증가량 × Index

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

θi = θmin + Δθ × i

코드에서는 정확히 다음과 같습니다.

current_angle = msg.angle_min + msg.angle_increment * i

15) LaserScan 각도의 단위

여기서 매우 중요한 부분이 있습니다.

ROS 2의 LaserScan 각도 단위는 일반적으로 Degree가 아니라 Radian입니다.

예를 들어 다음과 같습니다.

0 rad = 0°
π / 2 rad = 90°
π rad = 180°
-π / 2 rad = -90°

따라서 cos()와 sin()에 바로 사용할 수 있습니다.

현재 코드에서 math.cos()와 math.sin()이 정상적으로 동작하는 이유도 LaserScan 각도 값이 Radian 기준이기 때문입니다.

16) LiDAR 데이터는 극좌표 형태이다

LiDAR가 기본적으로 제공하는 정보는 다음 두 가지입니다.

거리 r
각도 θ

이러한 좌표 표현을 극좌표라고 합니다.

예를 들어 LiDAR 앞쪽 1m에 장애물이 있다면 개념적으로 다음과 같이 표현할 수 있습니다.

거리 = 1m
각도 = 0°

하지만 장애물이 로봇 기준으로 앞쪽으로 얼마나 떨어져 있는지, 좌우로 얼마나 떨어져 있는지를 검사하려면 XY 좌표가 더 편리합니다.

따라서 극좌표를 직교좌표로 변환합니다.

17) 극좌표를 XY 좌표로 변환

다음 코드가 좌표 변환 부분입니다.

cx = data * cos(current_angle)
cy = data * sin(current_angle)

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

x = r cos θ
y = r sin θ

여기서

r = LiDAR 거리
θ = LiDAR 각도
x = 전후방향 거리
y = 좌우방향 거리

입니다.

코드에서는 변수 이름을 다음과 같이 사용했습니다.

cx = x 방향 좌표
cy = y 방향 좌표

18) XY 좌표의 의미

일반적인 ROS 좌표계 기준에서는 모바일 로봇의 base_link를 기준으로 다음과 같이 생각할 수 있습니다.

+x → 로봇 전방
+y → 로봇 왼쪽
-y → 로봇 오른쪽

따라서 다음 조건은 로봇 앞쪽을 의미합니다.

cx > 0

다음 조건은 로봇 뒤쪽에 해당합니다.

cx < 0

이번 프로그램에서는 전방에 있는 장애물만 검사하므로 양수 X 영역을 사용합니다.

19) 장애물 감지 조건

이 프로그램에서 가장 중요한 부분은 다음 코드입니다.

if 0.01 < cx < 0.2 and -0.1 < cy < 0.1:

이 조건을 해석하면 다음과 같습니다.

X 방향은

0.01m < x < 0.2m

Y 방향은

-0.1m < y < 0.1m

입니다.

즉 LiDAR 기준으로 다음 영역입니다.

앞쪽 약 1cm ~ 20cm
좌우 약 -10cm ~ +10cm

이 사각형 영역 안에 LiDAR Point가 하나라도 존재하면 장애물이라고 판단합니다.

20) 위험 영역을 그림으로 이해하기

위에서 로봇을 내려다본다고 생각해 보겠습니다.

                로봇 전방
                   +X
                    ↑

        ┌───────┐
        │   위험 영역    │
        │                   │
        │ x: 0.01~0.2m  │
        │ y: ±0.1m       │
        └───────┘

                   LiDAR
                     ●

        +Y ←                    → -Y

정확한 좌우 방향은 LiDAR 및 TF 좌표계 설정에 따라 확인해야 하지만 핵심 개념은 동일합니다.

로봇 바로 앞의 작은 직사각형 영역을 검사하고 있다고 생각하면 됩니다.

21) 왜 단순 거리만 검사하지 않는가

다음과 같이 작성할 수도 있을 것 같습니다.

if data < 0.2:

하지만 이렇게 하면 문제가 발생합니다.

LiDAR는 로봇 앞쪽뿐만 아니라 측면과 뒤쪽까지 측정할 수 있기 때문입니다.

예를 들어 로봇 옆에 벽이 15cm 거리로 존재한다면 단순 거리 조건에서는 장애물로 판단합니다.

distance = 0.15m

하지만 실제로 로봇 진행방향을 막고 있지 않을 수도 있습니다.

현재 코드처럼 XY 좌표로 변환하면 다음과 같은 공간 조건을 설정할 수 있습니다.

앞으로 20cm
좌우 10cm

따라서 로봇 주행 경로에 실제로 영향을 주는 장애물만 선택적으로 감지할 수 있습니다.

22) 장애물이 감지되었을 때

위험 영역 안에 Point가 하나라도 발견되면 다음 코드가 실행됩니다.

estop.data = True

즉 긴급 정지 상태로 변경합니다.

그리고 바로 다음 코드가 실행됩니다.

break

break는 현재 for 반복문을 즉시 종료합니다.

23) break를 사용하는 이유

예를 들어 LaserScan에 1000개의 거리 데이터가 있다고 가정하겠습니다.

초반 100번째 Point에서 이미 장애물이 발견되었다면 나머지 900개의 Point를 검사할 필요가 없습니다.

E-Stop 조건은 다음과 같습니다.

위험 영역에 장애물이 하나라도 존재하는가?

하나를 발견한 순간 결과는 이미 확정됩니다.

True

따라서 break를 사용하여 불필요한 연산을 줄입니다.

실시간 로봇 시스템에서는 이런 작은 최적화도 의미가 있습니다.

24) 장애물이 발견되지 않은 경우

위험 영역 안에 들어오는 Point가 하나도 없다면 처음 설정했던 값이 유지됩니다.

estop.data = False

즉 모든 데이터를 검사하고도 조건을 만족하는 Point가 없다면 다음 상태가 발행됩니다.

False

이는 정상 주행 가능 상태로 해석할 수 있습니다.

25) e_stop 토픽 발행

모든 판단이 끝난 후 다음 코드가 실행됩니다.

self.publisher_.publish(estop)

결과적으로 /e_stop 토픽에 다음 둘 중 하나가 발행됩니다.

data: false

또는

data: true

터미널에서는 다음 명령으로 확인할 수 있습니다.

ros2 topic echo /e_stop

장애물이 없다면 다음과 같이 출력될 수 있습니다.

data: false

전방에 장애물을 놓으면 다음과 같이 변경됩니다.

data: true

26) 전체 장애물 판단 과정

실제 Callback 함수의 핵심 동작을 간단하게 정리하면 다음과 같습니다.

for i, data in enumerate(msg.ranges):

모든 LiDAR 데이터를 순회합니다.

current_angle = msg.angle_min + msg.angle_increment * i

현재 Point의 측정 각도를 계산합니다.

cx = data * cos(current_angle)
cy = data * sin(current_angle)

극좌표를 XY 좌표로 변환합니다.

if 0.01 < cx < 0.2 and -0.1 < cy < 0.1:

위험 영역 내부에 있는지 검사합니다.

estop.data = True

장애물을 발견하면 E-Stop을 활성화합니다.

break

더 이상 검사하지 않고 반복문을 종료합니다.

마지막으로

self.publisher_.publish(estop)

결과를 ROS 2 토픽으로 발행합니다.

27) main 함수

ROS 2 프로그램의 실행 부분은 다음과 같습니다.

def main(args=None):

먼저 ROS 2를 초기화합니다.

rclpy.init(args=args)

그 다음 LimoEStop 노드 객체를 생성합니다.

limo_e_stop = LimoEStop()

이 순간 Subscriber와 Publisher도 함께 생성됩니다.

28) rclpy.spin의 역할

다음 코드는 ROS 2 노드를 계속 실행 상태로 유지합니다.

rclpy.spin(limo_e_stop)

spin()이 실행되는 동안 ROS 2는 /scan 데이터가 들어오는 것을 기다립니다.

새로운 데이터가 들어오면 다음 Callback을 실행합니다.

laser_callback()

그리고 다시 다음 데이터를 기다립니다.

29) 노드 종료 처리

프로그램 종료 시 다음 코드가 실행됩니다.

limo_e_stop.destroy_node()

ROS 2 노드 객체를 정리합니다.

그 다음 다음 코드로 ROS 2 통신 시스템을 종료합니다.

rclpy.shutdown()

ROS 2 Python 프로그램에서 일반적으로 사용하는 종료 구조입니다.

30) 이 노드의 ROS 2 통신 구조

이 프로그램에서 사용되는 토픽은 크게 입력과 출력 두 개입니다.

입력은 다음과 같습니다.

Topic
/scan

Message Type
sensor_msgs/msg/LaserScan

출력은 다음과 같습니다.

Topic
/e_stop

Message Type
std_msgs/msg/Bool

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

LiDAR Driver
      ↓
    /scan
      ↓
LimoEStop Node
      ↓
   /e_stop
      ↓
Drive Controller

주행 제어 프로그램에서는 /e_stop을 구독하여 긴급 정지 여부를 판단하면 됩니다.

31) 직사각형 영역을 사용하는 장점

현재 코드는 단순히 LiDAR 중심으로부터 반경 20cm를 검사하지 않습니다.

다음과 같은 직사각형 영역을 검사합니다.

0.01m < x < 0.2m
-0.1m < y < 0.1m

이 방식의 장점은 로봇의 실제 차체 폭과 진행 방향을 반영할 수 있다는 것입니다.

예를 들어 로봇 폭이 약 20cm라면

-0.1m < y < 0.1m

와 같이 설정하여 로봇 진행 경로와 겹치는 장애물만 확인할 수 있습니다.

이를 Collision Rectangle 또는 Safety Zone과 비슷한 개념으로 생각할 수 있습니다.

32) 거리 대신 XY 좌표를 사용하는 이유

LiDAR 데이터에는 이미 거리 data가 들어 있습니다.

하지만 거리만 가지고 다음과 같이 검사하면

if data < 0.2:

로봇의 모든 방향에서 20cm 안에 있는 장애물이 정지 조건이 됩니다.

현재 방식은

cx = data * cos(current_angle)
cy = data * sin(current_angle)

으로 좌표를 변환하므로 훨씬 세밀한 영역 설정이 가능합니다.

예를 들어 전방만 검사할 수도 있고

x > 0

오른쪽만 검사할 수도 있으며

y < 0

왼쪽만 검사할 수도 있습니다.

y > 0

로봇의 공간적인 위험 영역을 직접 설계할 수 있다는 것이 가장 큰 장점입니다.

6. limo_control 모드

limo_control은 차선 오차와 장애물 감지 결과, 감속 신호를 이용해 최종 이동 명령을 생성하는 제어 노드입니다.

1) 코드 위치

~/wego_ws/src/limo_ros2_application/limo_ros2_application/limo_control.py

import rclpy
from rclpy.node import Node

from geometry_msgs.msg import Twist
from std_msgs.msg import Int32, Bool

class LimoControl(Node):
    def __init__(self):
        super().__init__('limo_control')
        
        # Setting for publisher of cmd velocity
        self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
        self.timer_period = 0.1  # seconds
        self.timer = self.create_timer(self.timer_period, self.timer_callback)
        
        # Setting for subscriber of e_stop and lane_detection
        self.e_stop_subscription = self.create_subscription(
            Bool,
            'e_stop',
            self.e_stop_callback,
            10)
        self.distance_subscription = self.create_subscription(
            Int32,
            'distance_y',
            self.distance_callback,
            10)
        
        # prevent from warning
        self.e_stop_subscription
        self.distance_subscription
        
        # flag and input value of twisting
        self.e_stop_flag = True
        self.gap = 0

        # parameter for default speed, p_gain for twist
        self.declare_parameter('default_speed', 0.2)
        self.declare_parameter('p_gain', 0.01)

        self.default_speed =self.get_parameter('default_speed')
        self.p_gain =self.get_parameter('p_gain')

    def e_stop_callback(self, msg):
        self.e_stop_flag = msg.data
    
    def distance_callback(self, msg):
        self.gap = msg.data

    def timer_callback(self):
        # set the limo speed
        msg = Twist()
        msg.linear.x = self.default_speed.value
        msg.angular.z = self.gap * self.p_gain.value

        # if e_stop called
        if self.e_stop_flag :
            msg.linear.x = 0.0
            msg.angular.z = 0.0
                
        self.publisher_.publish(msg)
        
def main(args=None):
    rclpy.init(args=args)
    limo_control = LimoControl()

    rclpy.spin(limo_control)

    limo_control.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

2) Twist 메시지

다음 코드는 로봇의 이동 명령을 보내기 위해 Twist 메시지를 불러옵니다.

from geometry_msgs.msg import Twist

Twist 메시지는 ROS와 ROS 2에서 모바일 로봇 제어에 매우 자주 사용됩니다.

구조는 다음과 같습니다.

Twist
 ├── linear
 │    ├── x
 │    ├── y
 │    └── z
 │
 └── angular
      ├── x
      ├── y
      └── z

일반적인 2차원 이동 로봇에서는 주로 다음 두 값을 사용합니다.

msg.linear.x
msg.angular.z

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

linear.x > 0  → 전진
linear.x = 0  → 정지
linear.x < 0  → 후진

angular.z는 로봇의 회전 명령입니다.

일반적인 ROS 좌표계에서는 다음과 같이 해석합니다.

angular.z > 0  → 좌회전 방향
angular.z = 0  → 직진
angular.z < 0  → 우회전 방향

LIMO가 Ackermann Steering 모드인지 Differential Drive 모드인지에 따라 내부적인 차량 구동 방식은 달라질 수 있지만, 상위 ROS 2 노드에서는 일반적으로 cmd_vel의 Twist 메시지를 이용해 제어할 수 있습니다.

3) Int32와 Bool 메시지

다음 두 메시지도 사용합니다.

from std_msgs.msg import Int32, Bool

Int32는 정수 데이터를 전달합니다.

이 프로그램에서는 차선 중심과 차량 중심 사이의 오차를 전달하는 데 사용합니다.

distance_y

예를 들어 카메라 영상의 중심이 320이고 차선 중심이 350이라면 어떤 기준으로 오차를 정의했는지에 따라 다음과 같은 값이 만들어질 수 있습니다.

gap = 30

또는 반대로 계산했다면

gap = -30

이 값의 부호는 차선 인식 노드에서 어떤 식으로 오차를 정의했는지에 따라 달라집니다.

Bool은 참과 거짓을 전달합니다.

Bool

이 프로그램에서는 비상 정지 상태를 전달합니다.

True  → 정지
False → 주행

4) LimoControl 클래스

노드 클래스는 다음과 같이 정의됩니다.

class LimoControl(Node):

그리고 생성자에서 부모 클래스인 Node를 초기화합니다.

def __init__(self):
    super().__init__('limo_control')

여기서

limo_control

은 ROS 2에서 사용하는 노드 이름입니다.

정상적으로 실행되었다면 다음과 같은 노드가 나타납니다.

5) cmd_vel Publisher 생성

다음 부분은 LIMO로 속도 명령을 전송하기 위한 Publisher를 생성합니다.

self.publisher_ = self.create_publisher(
    Twist,
    'cmd_vel',
    10
)

구성 요소는 다음과 같습니다.

메시지 타입 : Twist
토픽 이름   : cmd_vel
Queue 크기  : 10

6) 0.1초 주기의 Timer 생성

다음 코드는 제어 명령을 주기적으로 실행하기 위한 Timer입니다.

self.timer_period = 0.1

주기는 0.1초입니다.

따라서 이 프로그램은 초당 10번씩 LIMO에 속도 명령을 전송합니다.

Timer는 다음과 같이 생성합니다.

self.timer = self.create_timer(
    self.timer_period,
    self.timer_callback
)

즉 매 0.1초마다 다음 함수가 호출됩니다.

timer_callback()

따라서 지속적으로 최신 주행 명령이 cmd_vel로 전송됩니다.

7) e_stop Subscriber

다음 부분은 비상 정지 신호를 받는 Subscriber입니다.

self.e_stop_subscription = self.create_subscription(
    Bool,
    'e_stop',
    self.e_stop_callback,
    10
)

토픽 이름은

/e_stop

이고 메시지 타입은

std_msgs/msg/Bool

입니다.

데이터가 들어오면 다음 callback 함수가 실행됩니다.

self.e_stop_callback

8) distance_y Subscriber

차선 인식 결과를 받는 Subscriber입니다.

self.distance_subscription = self.create_subscription(
    Int32,
    'distance_y',
    self.distance_callback,
    10
)

토픽은 다음과 같습니다.

/distance_y

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

std_msgs/msg/Int32

이 값은 차선 중심과 차량 또는 화면 중심의 차이를 나타내는 값으로 사용할 수 있습니다.

예를 들어 영상이 다음과 같다고 가정해 보겠습니다.

카메라 영상 폭 = 640 pixel
화면 중심      = 320 pixel
차선 중심      = 350 pixel

오차를 다음과 같이 계산한다면

gap = 350 - 320

결과는

gap = 30

이 됩니다.

이 값이 distance_y 토픽으로 전달됩니다.

9) Subscriber 객체 유지

다음 코드는 얼핏 보면 아무 동작도 하지 않는 것처럼 보입니다.

self.e_stop_subscription
self.distance_subscription

ROS 2 Python 예제에서 Subscriber 객체가 생성되었음을 명확하게 유지하기 위해 이런 형태가 사용되기도 합니다.

현재처럼 이미 다음 변수에 객체를 저장하고 있기 때문에

self.e_stop_subscription
self.distance_subscription

실질적으로 위 두 줄은 반드시 필요한 코드는 아닙니다.

즉 다음과 같이 작성해도 됩니다.

self.e_stop_subscription = self.create_subscription(...)
self.distance_subscription = self.create_subscription(...)

객체가 멤버 변수로 유지되기 때문에 정상적으로 동작합니다.

10) 비상 정지 초기값

다음 코드는 비상 정지 상태의 초기값입니다.

self.e_stop_flag = True

매우 중요한 부분입니다.

프로그램을 처음 실행하면 True이기 때문에 로봇은 즉시 움직이지 않습니다.

즉 기본 상태가

STOP

입니다.

e_stop 토픽으로 다음 데이터가 들어와야 합니다.

False

그때부터 주행할 수 있습니다.

이 방식은 안전 관점에서 의미가 있습니다.

만약 초기값이 다음과 같다면

self.e_stop_flag = False

노드 실행 직후 cmd_vel이 발생하면서 로봇이 바로 움직일 가능성이 있습니다.

반면 현재 코드는

self.e_stop_flag = True

이므로 처음에는 정지 상태에서 시작합니다.

11) 차선 오차 초기값

다음 변수는 차선 중심 오차입니다.

self.gap = 0

초기에는 차선 오차가 없는 것으로 가정합니다.

따라서 P 제어 계산은

angular.z = gap × p_gain

이고 초기값에서는

angular.z = 0 × p_gain
          = 0

이 됩니다.

즉 조향 명령은 없는 상태입니다.

12) ROS 2 Parameter 선언

이 프로그램은 주행 속도와 P Gain을 코드에 완전히 고정하지 않고 ROS 2 Parameter로 관리합니다.

self.declare_parameter('default_speed', 0.2)
self.declare_parameter('p_gain', 0.01)

2개의 Parameter가 선언됩니다.

default_speed = 0.2
p_gain        = 0.01

default_speed는 기본 전진 속도입니다.

default_speed = 0.2

따라서 기본적으로

linear.x = 0.2

가 됩니다.

p_gain은 차선 오차에 따른 조향 반응의 강도를 결정합니다.

p_gain = 0.01

13) Parameter 값을 가져오는 부분

선언한 Parameter를 다음과 같이 가져옵니다.

self.default_speed = self.get_parameter('default_speed')
self.p_gain = self.get_parameter('p_gain')

여기서 주의해야 할 점이 있습니다.

get_parameter()가 반환하는 것은 단순한 float 값이 아니라 ROS 2 Parameter 객체입니다.

따라서 실제 값을 사용할 때는

self.default_speed.value

처럼 .value를 사용합니다.

예를 들어

self.default_speed.value

의 값은 기본 설정에서는

0.2

입니다.

그리고

self.p_gain.value

의 값은

0.01

입니다.

14) e_stop_callback 함수

비상 정지 메시지가 들어오면 다음 함수가 실행됩니다.

def e_stop_callback(self, msg):
    self.e_stop_flag = msg.data

예를 들어 다음 메시지가 들어왔다고 가정해 보겠습니다.

data: true

그러면

self.e_stop_flag = True

가 됩니다.

반대로

data: false

가 들어오면

self.e_stop_flag = False

가 됩니다.

즉 다음과 같은 구조입니다.

/e_stop = True
       │
       ▼
e_stop_flag = True
       │
       ▼
로봇 정지

그리고

/e_stop = False
       │
       ▼
e_stop_flag = False
       │
       ▼
정상 주행

15) distance_callback 함수

차선 오차가 전달되면 다음 callback이 호출됩니다.

def distance_callback(self, msg):
    self.gap = msg.data

예를 들어 /distance_y로

data: 25

가 들어오면

self.gap = 25

가 됩니다.

이 값은 이후 timer_callback()에서 조향값을 계산하는 데 사용됩니다.

16) timer_callback 함수

실제 차량 제어의 핵심 부분입니다.

def timer_callback(self):

이 함수는 0.1초마다 실행됩니다.

따라서 10Hz 주기로 차량의 속도와 조향값을 계산하고 cmd_vel을 발행합니다.

17) Twist 메시지 생성

먼저 새로운 Twist 메시지를 만듭니다.

msg = Twist()

초기 상태에서는 대부분의 값이 0입니다.

이후 필요한 값만 설정합니다.

msg.linear.x
msg.angular.z

18) 전진 속도 설정

다음 코드에서 전진 속도를 지정합니다.

msg.linear.x = self.default_speed.value

기본 Parameter 값은

0.2

이므로 결과적으로 다음과 같은 명령이 됩니다.

linear.x = 0.2

즉 로봇은 일정한 전진 속도를 유지합니다.

현재 프로그램은 차선 오차와 관계없이 전진 속도가 일정합니다.

gap 작음 → speed 0.2
gap 큼   → speed 0.2

따라서 급격한 커브에서도 기본적으로 같은 속도를 사용합니다.

실제 자율주행에서는 이후 다음과 같이 확장할 수도 있습니다.

직선 구간 → 빠르게
곡선 구간 → 느리게

19) P 제어를 이용한 조향값 계산

가장 중요한 제어식입니다.

msg.angular.z = self.gap * self.p_gain.value

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

ω = e × Kp

여기서

ω  = angular.z
e  = gap
Kp = p_gain

입니다.

즉 차선 중심 오차가 클수록 더 강하게 회전합니다.

이것이 가장 기본적인 P 제어입니다.

20) P 제어 계산 예제

현재

p_gain = 0.01

이라고 가정하겠습니다.

gap이 10이라면

angular.z
= 10 × 0.01
= 0.1

입니다.

gap이 30이라면

angular.z
= 30 × 0.01
= 0.3

입니다.

gap이 50이라면

angular.z
= 50 × 0.01
= 0.5

입니다.

반대로 gap이 -30이라면

angular.z
= -30 × 0.01
= -0.3

이 됩니다.

따라서 오차의 크기뿐만 아니라 부호에 따라 회전 방향도 결정됩니다.

21) gap과 angular.z 관계

현재 Gain이 0.01인 경우 다음과 같은 관계가 만들어집니다.

gap	angular.z
-50	-0.50
-30	-0.30
-10	-0.10
0	0.00
10	0.10
30	0.30
50	0.50

즉 gap이 커질수록 조향값도 선형적으로 증가합니다.

그래프로 생각하면 다음과 같습니다.

angular.z
   ^
0.5|            /
   |           /
   |         /
0.0|------/----------> gap
   |      /
   |    /
-0.5| /

이러한 형태가 P 제어의 기본적인 특징입니다.

22) P Gain이 너무 작은 경우

p_gain이 지나치게 작으면 차량의 조향 반응이 느려집니다.

예를 들어

p_gain = 0.001
gap = 30

이라면

angular.z = 30 × 0.001
          = 0.03

입니다.

차량이 차선에서 크게 벗어나더라도 조향량이 충분하지 않을 수 있습니다.

결과적으로 다음과 같은 현상이 발생할 수 있습니다.

차선 오차 발생
   ↓
조향 반응 부족
   ↓
차선 중심 복귀 지연
   ↓
차선 이탈 가능성 증가

23) P Gain이 너무 큰 경우

반대로 Gain이 너무 크면 차량이 과도하게 반응합니다.

예를 들어

p_gain = 0.05
gap = 30

이면

angular.z = 1.5

가 됩니다.

차량이 차선 중심을 지나쳐 반대 방향으로 크게 움직일 수 있습니다.

이 과정이 반복되면 다음과 같이 주행할 수 있습니다.

왼쪽으로 이탈
   ↓
강한 오른쪽 조향
   ↓
오른쪽으로 이탈
   ↓
강한 왼쪽 조향
   ↓
반복

즉 차량이 차선 중앙을 기준으로 좌우로 흔들리는 Oscillation 현상이 발생할 수 있습니다.

따라서 p_gain 튜닝이 중요합니다.

24) 비상 정지 처리

다음 코드가 안전 정지를 담당합니다.

if self.e_stop_flag:
    msg.linear.x = 0.0
    msg.angular.z = 0.0

e_stop_flag가 True이면 기존에 계산한 모든 주행 명령을 무시합니다.

즉 원래 다음 값이 계산되었더라도

linear.x  = 0.2
angular.z = 0.3

비상 정지 상태라면

linear.x  = 0.0
angular.z = 0.0

로 변경됩니다.

따라서 최종적으로 로봇에는 정지 명령이 전달됩니다.

25) 비상 정지가 우선되는 이유

코드 순서를 보면 먼저 정상 주행 명령을 계산합니다.

msg.linear.x = self.default_speed.value
msg.angular.z = self.gap * self.p_gain.value

그 다음 비상 정지 상태를 검사합니다.

if self.e_stop_flag:

그리고 값을 0으로 덮어씁니다.

msg.linear.x = 0.0
msg.angular.z = 0.0

따라서 제어 우선순위는 다음과 같습니다.

1순위 : e_stop
2순위 : 주행 제어

이 구조는 매우 중요합니다.

차선 인식 노드가 계속해서 조향 명령을 계산하고 있어도 e_stop이 활성화되면 최종 출력은 무조건 정지가 됩니다.

26) cmd_vel 발행

마지막으로 완성된 Twist 메시지를 발행합니다.

self.publisher_.publish(msg)

토픽은

/cmd_vel

입니다.

예를 들어 정상 주행 중이고

default_speed = 0.2
gap = 20
p_gain = 0.01

이라면 다음과 같은 메시지가 전송됩니다.

linear:
  x: 0.2
  y: 0.0
  z: 0.0

angular:
  x: 0.0
  y: 0.0
  z: 0.2

27) main 함수

ROS 2 Python 프로그램의 시작 부분입니다.

def main(args=None):

먼저 ROS 2 통신 시스템을 초기화합니다.

rclpy.init(args=args)

그리고 LimoControl 객체를 생성합니다.

limo_control = LimoControl()

이 순간 다음 요소들이 생성됩니다.

Node
Publisher
Timer
e_stop Subscriber
distance_y Subscriber
ROS 2 Parameters

28) rclpy.spin()

다음 코드가 매우 중요합니다.

rclpy.spin(limo_control)

spin()은 노드를 계속 실행하면서 ROS 2 이벤트를 처리합니다.

예를 들어 다음 이벤트를 처리합니다.

Timer 발생
Subscriber 메시지 수신
Callback 실행
Publisher 처리

프로그램 흐름은 다음과 같습니다.

rclpy.spin()
     │
     ├── /e_stop 수신
     │      ↓
     │  e_stop_callback()
     │
     ├── /distance_y 수신
     │      ↓
     │  distance_callback()
     │
     └── 0.1초 경과
            ↓
       timer_callback()

따라서 spin()이 실행되고 있는 동안 ROS 2 노드가 지속적으로 동작합니다.

29) 노드 종료 처리

노드 실행이 종료되면 다음 코드를 실행합니다.

limo_control.destroy_node()

ROS 2 노드를 정상적으로 제거합니다.

그 다음

rclpy.shutdown()

을 호출하여 ROS 2 시스템을 종료합니다.

30) 전체 제어 흐름 정리

프로그램 전체를 순서대로 보면 다음과 같습니다.

ROS 2 실행
   │
   ▼
LimoControl 생성
   │
   ├── cmd_vel Publisher 생성
   ├── e_stop Subscriber 생성
   ├── distance_y Subscriber 생성
   ├── 0.1초 Timer 생성
   └── Parameter 설정
   │
   ▼
rclpy.spin()
   │
   ├── distance_y 수신
   │       │
   │       ▼
   │     gap 저장
   │
   ├── e_stop 수신
   │       │
   │       ▼
   │  e_stop_flag 저장
   │
   └── 0.1초마다 timer_callback()
           │
           ▼
       linear.x 계산
           │
           ▼
       angular.z 계산
           │
           ▼
       e_stop 확인
           │
       ┌───┴────┐
       │                       │
     True                     False
       │                       │
       ▼                       ▼
     정지                  정상주행
       │                       │
       └───┬────┘
                  ▼
           cmd_vel 발행

31) ROS 2 토픽 연결 구조

이 프로그램에 관련된 토픽 구조는 다음과 같이 정리할 수 있습니다.

Lane Detection Node
           │
           │ /distance_y
           │ std_msgs/Int32
           ▼
+--------------------+
|                         |
|    limo_control     |
|                         |
+--------------------+
           ▲
           │ /e_stop
           │ std_msgs/Bool
           │
Emergency Stop Node

           │
           │ /cmd_vel
           │ geometry_msgs/Twist
           ▼

      LIMO Robot

32) 터미널에서 e_stop 테스트하기

ROS 2 CLI를 이용하면 별도의 노드를 작성하지 않고도 테스트할 수 있습니다.

비상 정지를 활성화하려면 다음과 같이 실행합니다.

ros2 topic pub /e_stop std_msgs/msg/Bool "{data: true}"

그러면 차량은 정지합니다.

비상 정지를 해제하려면

ros2 topic pub /e_stop std_msgs/msg/Bool "{data: false}"

를 사용할 수 있습니다.

실제 차량에서 테스트할 때는 반드시 안전을 확보한 상태에서 진행해야 합니다.

33) distance_y 테스트하기

다음 명령을 이용하면 가상의 차선 오차를 입력할 수 있습니다.

ros2 topic pub /distance_y std_msgs/msg/Int32 "{data: 20}"

현재

p_gain = 0.01

이라면

angular.z = 20 × 0.01
          = 0.2

가 됩니다.

반대 방향을 테스트하려면

ros2 topic pub /distance_y std_msgs/msg/Int32 "{data: -20}"

을 사용할 수 있습니다.

이 경우

angular.z = -0.2

가 됩니다.

34) cmd_vel 확인하기

실제 출력되고 있는 제어 명령은 다음 명령으로 확인할 수 있습니다.

ros2 topic echo /cmd_vel

예를 들어 다음과 같이 출력될 수 있습니다.

linear:
  x: 0.2
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.2

비상 정지를 활성화하면 다음과 같이 변경됩니다.

linear:
  x: 0.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.0

35) Parameter 확인하기

현재 노드에 등록된 Parameter는 다음 명령으로 확인할 수 있습니다.

ros2 param list /limo_control

default_speed 값을 확인하려면

ros2 param get /limo_control default_speed

를 사용합니다.

p_gain은 다음과 같이 확인합니다.

ros2 param get /limo_control p_gain

36) 실행 시 Parameter 변경하기

ROS 2 Parameter를 사용한 가장 큰 장점 중 하나는 소스 코드를 수정하지 않고 값을 변경할 수 있다는 것입니다.

예를 들어 기본 속도를 0.1로 실행하고 싶다면 다음과 같이 사용할 수 있습니다.

ros2 run 패키지이름 limo_control --ros-args -p default_speed:=0.1

P Gain을 0.005로 사용하고 싶다면

ros2 run 패키지이름 limo_control --ros-args -p p_gain:=0.005

처럼 실행할 수 있습니다.

두 값을 동시에 설정할 수도 있습니다.

ros2 run 패키지이름 limo_control \
--ros-args \
-p default_speed:=0.15 \
-p p_gain:=0.008

실제 차선 추종 테스트에서는 이 기능을 이용하면 소스 코드를 계속 수정하지 않고 Gain을 조정할 수 있습니다.

37) 파라미터 파일 위치

~/wego_ws/src/limo_ros2_application/params/limo_control.yaml

default_speed

기본값: 0.2
단위: m/s

정상 주행 상태에서 사용할 기본 전진 속도입니다.

처음 실습할 때는 안전을 위해 다음과 같이 낮은 값부터 시작하는 것이 좋습니다.

default_speed: 0.1

차선 검출과 조향이 안정적으로 동작하면 조금씩 속도를 높일 수 있습니다.

p_gain

기본값: 0.0065

차선 오차를 회전 속도로 변환하는 비례 제어 Gain입니다.

기본적인 제어식은 다음과 같이 표현할 수 있습니다.

angular_z = p_gain * distance_y

코드의 좌표계와 회전 방향에 따라 음수 부호가 추가될 수 있습니다.

angular_z = -p_gain * distance_y

p_gain이 너무 작으면 차선을 벗어나도 천천히 방향을 수정합니다.

p_gain이 너무 크면 좌우로 빠르게 흔들리면서 지그재그 주행을 할 수 있습니다.