이번 글에서는 WEGO LIMO에서 딥러닝으로 검출한 트랙 위치와 YOLO로 검출한 객체 정보를 결합하여 실제 로봇의 속도와 조향을 제어하는 dl_control.py 프로그램을 분석합니다.
앞에서 살펴본 트랙 인식 프로그램은 카메라 영상으로부터 Normal Mode와 Turn Mode의 트랙 중심 오차를 계산하여 /control 토픽으로 전달했습니다.
YOLO 객체 인식 프로그램은 카메라 영상에서 객체를 검출한 뒤 객체의 클래스 이름, 신뢰도, Bounding Box 좌표를 /yolo_bounding_boxes 토픽으로 전달했습니다.
이번 dl_control.py는 이 두 결과를 최종적으로 결합합니다.
전체적인 역할은 다음과 같습니다.
딥러닝 트랙 인식
↓
/control
↓
┐
│
├── dl_control_node
│ ↓
│ /cmd_vel
│ ↓
│ WEGO LIMO
│
┘
YOLO 객체 인식
↓
/yolo_bounding_boxes
즉 이전까지의 프로그램이 Perception 영역이었다면 이번 프로그램은 Perception 결과를 이용해 실제 행동을 결정하는 Control 영역이라고 볼 수 있습니다.
1. 프로그램의 전체 동작 구조
dl_control.py의 전체 흐름부터 정리하면 다음과 같습니다.
1. 딥러닝 트랙 오차 수신
/control
↓
Normal Mode
Turn Mode
2. YOLO 객체 정보 수신
/yolo_bounding_boxes
↓
slow_down
speed_up
turn_right
red_light
yellow_light
green_light
pedestrian
3. Emergency Stop 수신
/emergency_stop
4. 트랙 오차 필터링
5. 현재 객체 상태 판단
6. 객체의 Bounding Box 크기 판단
7. 객체별 활성 상태 유지 시간 계산
8. Normal Mode 또는 Turn Mode 선택
9. 선속도 계산
10. 각속도 계산
11. 교통 표지와 신호에 따른 속도 보정
12. 속도와 조향 제한
13. Twist 메시지 생성
14. /cmd_vel Publish
15. 검출 결과와 트랙 위치를 디버그 영상으로 표시
이 프로그램에서는 트랙 인식 결과가 주행 방향을 결정하고 YOLO 객체 검출 결과가 주행 상태를 변경합니다.
다시 말하면 다음과 같습니다.
Track Detection
→ 어디로 갈 것인가?
YOLO Detection
→ 어떻게 갈 것인가?
예를 들어 트랙이 오른쪽으로 휘어 있으면 딥러닝 트랙 인식 결과가 조향량을 만들어냅니다.
그 상태에서 slow_down 표지가 검출되면 속도를 낮춥니다.
red_light가 검출되면 완전히 정지합니다.
turn_right가 검출되면 Normal Mode 대신 Turn Mode 트랙 결과를 사용합니다.
따라서 단순한 차선 추종을 넘어 상황에 따라 주행 동작을 변경하는 구조입니다.
2. 프로그램에서 사용하는 라이브러리
먼저 필요한 라이브러리를 살펴보겠습니다.
import rclpy
from rclpy.node import Node
from dl_ros2_msgs.msg import Control, BoundingBoxes
from geometry_msgs.msg import Twist
from std_msgs.msg import Bool
from cv_bridge import CvBridge
from sensor_msgs.msg import CompressedImage, Image
import cv2
import time
import numpy as np
각 라이브러리의 역할은 명확합니다.
rclpy는 ROS 2 Python Node를 생성하기 위해 사용합니다.
from dl_ros2_msgs.msg import Control, BoundingBoxes
Control은 딥러닝 트랙 인식 결과를 받습니다.
BoundingBoxes는 YOLO 객체 검출 결과를 받습니다.
from geometry_msgs.msg import Twist
Twist는 WEGO LIMO에 최종 속도 명령을 전달하기 위해 사용합니다.
일반적으로 다음 두 값이 핵심입니다.
linear.x
→ 전진 속도
angular.z
→ 회전 속도
from std_msgs.msg import Bool
Bool은 Emergency Stop 상태를 받기 위해 사용합니다.
OpenCV와 CvBridge는 디버그 영상을 생성하는 데 사용합니다.
3. sigmoid 함수
프로그램 시작 부분에 다음 함수가 있습니다.
def sigmoid(x, gain):
return gain / (1 + np.exp(-x))
Sigmoid 형태의 비선형 함수를 사용합니다.
수식으로 표현하면 다음과 같습니다.
gain
sigmoid(x) = ─────────────
1 + e^(-x)
하지만 이 프로그램에서는 이 값을 단독으로 사용하지 않습니다.
뒤에서 다음과 같이 사용합니다.
sigmoid_1 = self.control_1 * sigmoid(
self.control_1,
self.max_steering
)
따라서 실제 조향 계산은 개념적으로 다음과 같습니다.
max_steering
steering = control × ───────────────
1 + e^-control
트랙 오차를 그대로 angular.z로 사용하는 것이 아니라 비선형 변환을 한번 적용하는 구조입니다.
4. DlControlNode 클래스
실제 ROS 2 Node는 다음 클래스입니다.
class DlControlNode(Node):
생성자는 다음과 같이 시작합니다.
def __init__(self):
super().__init__('dl_control_node')
따라서 Node 이름은
dl_control_node
입니다.
5. CvBridge 생성
self.br = CvBridge()
디버그 영상을 OpenCV 이미지에서 ROS 2 Image 또는 CompressedImage 메시지로 변환하기 위해 사용합니다.
6. Emergency Stop 상태
self.stop_flag = False
Emergency Stop 여부를 저장합니다.
True가 되면 로봇은 즉시 정지 명령을 Publish합니다.
7. 주행 시작 상태
self.start_flag = False
프로그램을 실행했다고 바로 LIMO가 움직이지 않도록 하는 역할을 합니다.
딥러닝 트랙 인식 결과인 /control 메시지가 실제로 들어온 뒤에
self.start_flag = True
가 됩니다.
따라서 트랙 인식 결과가 하나도 들어오지 않은 상태에서는 /cmd_vel로 정지 명령을 보냅니다.
이 부분은 안전 측면에서도 중요합니다.
8. 두 종류의 트랙 제어값
self.control_1 = 0.0
self.control_2 = 0.0
트랙 인식 프로그램에서 전달된 두 종류의 제어값을 저장합니다.
구조는 다음과 같습니다.
control_1
→ Normal Mode
control_2
→ Turn Mode
앞서 트랙 인식 프로그램에서는 다음 두 값을 전달했습니다.
normal_mode
turn_mode
이번 프로그램은 이 값을 실제 조향 계산에 사용합니다.
9. 이전 제어값 저장 변수
self.prev_control_1 = 0.0
self.prev_control_2 = 0.0
이전 제어값과 현재 제어값 차이를 이용하여 갑작스러운 조향 변화를 줄이기 위한 목적으로 만들어진 변수입니다.
뒤에서
apply_derivative_control()
함수에서 사용됩니다.
다만 현재 소스에서 중요한 점이 하나 있습니다.
prev_control_1, prev_control_2는 생성자에서 0.0으로 초기화된 이후 새로운 제어값으로 갱신되는 코드가 없습니다.
따라서 현재 소스 그대로라면 이전 프레임 값과 비교하는 것이 아니라 계속 초기값 0.0과 비교하게 됩니다.
이 부분은 뒤에서 다시 살펴보겠습니다.
10. YOLO 객체별 상태 관리
다음 flags 구조가 이 프로그램의 핵심 중 하나입니다.
self.flags = {
'slow_down': {...},
'speed_up': {...},
'turn_right': {...},
'red_light': {...},
'yellow_light': {...},
'green_light': {...},
'pedestrian': {...}
}
YOLO가 검출할 객체의 Class 이름과 직접 연결되어 있습니다.
따라서 YOLO 모델의 Class 이름이 정확히 다음과 같아야 합니다.
slow_down
speed_up
turn_right
red_light
yellow_light
green_light
pedestrian
예를 들어 모델 Class 이름이 redlight인데 제어 프로그램에서는 red_light를 찾는다면 해당 기능은 동작하지 않습니다.
Class 문자열이 정확하게 일치해야 합니다.
11. 각 YOLO Flag가 의미하는 동작
각 객체의 역할은 이름을 통해 알 수 있습니다.
slow_down
→ 감속
speed_up
→ 가속
turn_right
→ 우회전 모드
red_light
→ 정지
yellow_light
→ 황색 신호 감속
green_light
→ 녹색 신호 속도 조정
pedestrian
→ 보행자 정지
이 객체들이 검출되었다고 바로 활성화되는 것은 아닙니다.
Bounding Box 크기와 유지 시간을 함께 사용합니다.
12. Flag 내부 데이터
일반적인 Flag 구조는 다음과 같습니다.
{
'active': False,
'past_time': time.time(),
'duration_sec': 0.0,
'size_threshold': 0.0
}
각 값의 의미를 살펴보겠습니다.
active
→ 현재 해당 동작이 활성화되어 있는가?
past_time
→ 마지막 검출 시간을 저장
duration_sec
→ 객체가 사라진 뒤에도 상태를 유지할 시간
size_threshold
→ 객체를 유효하게 판단할 Bounding Box 최소 크기
13. pedestrian의 추가 상태
보행자만 하나의 설정이 더 있습니다.
'pass_by_duration_sec'
보행자 처리는 다른 객체와 조금 다른 시간 로직을 사용합니다.
pedestrian
├─ duration_sec
├─ pass_by_duration_sec
└─ size_threshold
14. 현재 카메라 영상
self.current_image = None
카메라에서 가장 최근에 받은 영상을 저장합니다.
이 영상은 차량 제어에 직접 사용되는 것이 아니라 디버그 영상 생성에 사용됩니다.
15. 현재 Bounding Box
self.current_boxes = []
YOLO에서 가장 최근에 받은 Bounding Box 목록을 저장합니다.
디버그 영상과 객체 상태 판단에서 모두 사용합니다.
16. ROS 2 Parameter 선언
다음 함수를 호출합니다.
self._declare_parameters()
self._get_parameters()
첫 번째 함수는 사용할 Parameter를 선언하고 두 번째 함수는 Parameter 값을 읽습니다.
이 프로그램의 주행 특성 대부분은 Parameter로 조정할 수 있도록 만들어져 있습니다.
17. 기본 주행 관련 Parameter
self.declare_parameter('max_steering', 0.0)
self.declare_parameter('linear_x_base_speed', 0.0)
self.declare_parameter('max_speed', 0.0)
self.declare_parameter('min_speed', 0.0)
각각의 의미는 다음과 같습니다.
max_steering
→ 최대 조향값
linear_x_base_speed
→ 기본 전진 속도
max_speed
→ 최대 속도
min_speed
→ 최소 속도
현재 소스에서는 기본값이 모두 0.0입니다.
따라서 실제 주행에서는 Launch 파일이나 YAML Parameter 파일 등을 통해 적절한 값을 전달해야 합니다.
18. 트랙 제어 Gain
self.declare_parameter('control_1_gain', 0.0)
self.declare_parameter('control_2_gain', 0.0)
트랙 인식 프로그램에서 전달된 오차를 실제 제어량으로 변환할 때 사용하는 Gain입니다.
normal_mode
× control_1_gain
→ raw_control_1
turn_mode
× control_2_gain
→ raw_control_2
19. 조향 안정화 관련 Parameter
self.declare_parameter('prev_gain', 0.0)
self.declare_parameter('current_gain', 0.0)
self.declare_parameter('threshold_max', 0.0)
self.declare_parameter('threshold_min', 0.0)
self.declare_parameter('deceleration_ratio', 0.0)
각각 다음 역할을 담당합니다.
prev_gain
→ 이전 제어값 가중치
current_gain
→ 현재 제어값 가중치
threshold_min
→ 작은 변화 무시 기준
threshold_max
→ 보간 제어 적용 기준
deceleration_ratio
→ 조향량 증가에 따른 감속 정도
20. YOLO 이벤트별 속도 Factor
self.declare_parameter('speed_up_factor', 0.0)
self.declare_parameter('slow_down_factor', 0.0)
self.declare_parameter('turn_right_factor', 0.0)
self.declare_parameter('yellow_light_factor', 0.0)
self.declare_parameter('green_light_factor', 0.0)
객체 인식 상태에 따라 기존 속도나 조향값에 곱하는 비율입니다.
예를 들어
기본 속도 × slow_down_factor
형태로 감속합니다.
정확한 Factor 값은 현재 소스에 포함되어 있지 않으며 외부 Parameter 설정에 의해 결정됩니다.
21. 객체 상태 유지 시간 Parameter
각 객체마다 유지 시간이 존재합니다.
self.declare_parameter('slow_down_duration_sec', 0.0)
self.declare_parameter('speed_up_duration_sec', 0.0)
self.declare_parameter('turn_right_duration_sec', 0.0)
self.declare_parameter('red_light_duration_sec', 0.0)
self.declare_parameter('yellow_light_duration_sec', 0.0)
self.declare_parameter('green_light_duration_sec', 0.0)
self.declare_parameter('pedestrian_duration_sec', 0.0)
YOLO는 매 프레임 정확히 같은 객체를 검출한다고 보장할 수 없습니다.
한 프레임에서 객체를 놓쳤다고 바로 상태를 해제하면 차량 동작이 불안정해질 수 있습니다.
따라서 마지막 검출 이후 일정 시간 동안 상태를 유지하도록 설계되어 있습니다.
22. Bounding Box 크기 Threshold
self.declare_parameter('slow_down_size_threshold', 0.0)
...
self.declare_parameter('pedestrian_size_threshold', 0.0)
객체별로 Bounding Box 면적 기준값을 따로 설정할 수 있습니다.
YOLO가 어떤 객체를 화면 아주 멀리에서 검출했다고 바로 행동을 변경하지 않고, 일정 크기 이상 가까워졌을 때 제어 동작을 실행하기 위한 방법입니다.
23. ROS 2 Subscriber 구성
이 Node는 총 네 종류의 데이터를 Subscribe합니다.
self.stop_sub_ = self.create_subscription(
Bool,
'/emergency_stop',
self.stop_callback,
10
)
Emergency Stop입니다.
self.control_sub_ = self.create_subscription(
Control,
'/control',
self.control_callback,
10
)
딥러닝 트랙 인식 결과입니다.
self.box_sub_ = self.create_subscription(
BoundingBoxes,
'/yolo_bounding_boxes',
self.yolo_callback,
10
)
YOLO 객체 검출 결과입니다.
self.image_sub_ = self.create_subscription(
CompressedImage,
'/camera/color/image_raw/compressed',
self.image_callback,
10
)
디버그 영상 생성용 카메라 영상입니다.
24. ROS 2 Publisher 구성
가장 중요한 Publisher는 다음과 같습니다.
self.cmd_pub_ = self.create_publisher(
Twist,
'/cmd_vel',
10
)
최종 차량 제어 명령입니다.
추가로 디버그 영상도 Publish합니다.
self.dl_image_pub = self.create_publisher(
Image,
'/dl_image',
10
)
self.dl_image_compressed_pub = self.create_publisher(
CompressedImage,
'/dl_image/compressed',
10
)
따라서 출력은 다음과 같습니다.
/cmd_vel
/dl_image
/dl_image/compressed
25. 제어 Timer
self.create_timer(
0.1,
self.timer_callback
)
주기는 0.1초입니다.
따라서 이론적으로 다음 주기로 실행됩니다.
1 / 0.1
= 10 Hz
Timer에서는 두 가지 작업을 실행합니다.
def timer_callback(self):
self.publish_cmd_vel()
self.publish_debug_image()
즉 약 10 Hz 주기로 차량 명령과 디버그 영상을 생성합니다.
26. Emergency Stop Callback
def stop_callback(self, msg: Bool):
self.stop_flag = msg.data
매우 단순합니다.
/emergency_stop에서 True가 들어오면
stop_flag = True
가 됩니다.
이 상태는 최종 publish_cmd_vel()에서 가장 먼저 검사됩니다.
27. 카메라 Callback
def image_callback(self, msg: CompressedImage):
디버그 영상 생성에 사용할 카메라 이미지를 처리합니다.
압축된 데이터를 NumPy 배열로 변환합니다.
np_arr = np.frombuffer(
msg.data,
np.uint8
)
이후 OpenCV 영상으로 디코딩합니다.
self.current_image = cv2.imdecode(
np_arr,
cv2.IMREAD_COLOR
)
따라서
CompressedImage
→ NumPy
→ OpenCV BGR Image
과정을 거칩니다.
28. apply_derivative_control 함수
트랙 인식 결과를 그대로 사용하지 않고 변화량에 따라 필터링합니다.
def apply_derivative_control(
self,
current_val,
prev_val
):
먼저 이전 값과 현재 값의 차이를 계산합니다.
diff = abs(
current_val - prev_val
)
29. 아주 작은 변화 무시
if diff < self.threshold_min:
return prev_val
변화량이 threshold_min보다 작으면 새로운 값을 사용하지 않습니다.
현재값 변화가 매우 작다
↓
Noise 또는 미세 변화로 판단
↓
이전값 유지
조향 명령이 작은 노이즈 때문에 계속 흔들리는 것을 줄이기 위한 구조로 볼 수 있습니다.
30. 제어값이 감소하는 경우의 보간
다음 조건도 있습니다.
if abs(current_val) < abs(prev_val) \
and diff < self.threshold_max:
현재 제어값의 절대값이 이전보다 작고 변화량이 일정 범위 안이라면 두 값을 섞습니다.
return (
prev_val *
self.get_parameter('prev_gain').value
+
current_val *
self.get_parameter('current_gain').value
)
즉
filtered_control
= previous × prev_gain
+ current × current_gain
입니다.
갑자기 조향량을 줄이지 않고 부드럽게 변화시키려는 구조입니다.
31. 변화가 큰 경우
앞의 두 조건에 해당하지 않으면 현재 값을 그대로 사용합니다.
return current_val
즉 개념적인 흐름은 다음과 같습니다.
현재값
↓
이전값과 차이 계산
↓
차이가 매우 작음?
├─ Yes → 이전값 유지
│
└─ No
↓
제어량이 줄고 변화량이 제한 범위?
├─ Yes → 이전값과 현재값 혼합
│
└─ No → 현재값 사용
32. 현재 소스의 prev_control 관련 주의점
여기에서 반드시 확인할 부분이 있습니다.
생성자에는 다음 변수가 있습니다.
self.prev_control_1 = 0.0
self.prev_control_2 = 0.0
그리고 control_callback()에서 다음처럼 사용합니다.
self.control_1 = self.apply_derivative_control(
raw_control_1,
self.prev_control_1
)
self.control_2 = self.apply_derivative_control(
raw_control_2,
self.prev_control_2
)
하지만 현재 제공된 소스에는 이후 다음과 같은 갱신 코드가 없습니다.
self.prev_control_1 = self.control_1
self.prev_control_2 = self.control_2
따라서 현재 소스만 보면 prev_control_1과 prev_control_2는 계속 0.0입니다.
즉 이름은 Previous Control이지만 실제 이전 Callback의 제어값을 계속 추적하는 구조는 아닙니다.
33. 딥러닝 트랙 제어 Callback
트랙 인식 결과는 다음 함수로 들어옵니다.
def control_callback(self, msg: Control):
Normal Mode 결과에 Gain을 적용합니다.
raw_control_1 = (
msg.normal_mode *
self.get_parameter(
'control_1_gain'
).value
)
Turn Mode 역시 동일합니다.
raw_control_2 = (
msg.turn_mode *
self.get_parameter(
'control_2_gain'
).value
)
따라서 관계는 다음과 같습니다.
normal_mode
↓
control_1_gain
↓
raw_control_1
turn_mode
↓
control_2_gain
↓
raw_control_2
34. Track Control Filtering
Gain을 적용한 후 바로 사용하지 않고 앞에서 살펴본 필터를 적용합니다.
self.control_1 = self.apply_derivative_control(
raw_control_1,
self.prev_control_1
)
self.control_2 = self.apply_derivative_control(
raw_control_2,
self.prev_control_2
)
결과적으로 실제 차량 제어에서 사용하는 값은
control_1
control_2
입니다.
35. 주행 시작 Flag
트랙 결과를 한 번이라도 정상적으로 받으면 다음 코드가 실행됩니다.
self.start_flag = True
이후부터 /cmd_vel 주행 명령이 만들어질 수 있습니다.
따라서 전체 시스템에서 딥러닝 트랙 인식 Node가 실행되지 않으면 LIMO는 움직이지 않는 구조입니다.
36. YOLO Callback
YOLO 객체 결과는 다음 함수에서 처리합니다.
def yolo_callback(
self,
msg: BoundingBoxes
):
먼저 현재 Bounding Box들을 저장합니다.
self.current_boxes = msg.bounding_boxes
현재 시간을 가져옵니다.
current_time = time.time()
37. 임시 Flag 생성
매 YOLO 메시지마다 다음 Dictionary를 생성합니다.
temp_flags = {
key: False
for key in self.flags
}
처음에는 모든 객체가 검출되지 않았다고 가정합니다.
slow_down = False
speed_up = False
turn_right = False
red_light = False
yellow_light = False
green_light = False
pedestrian = False
이후 Bounding Box를 검사하면서 필요한 Flag만 True로 변경합니다.
38. YOLO Class 확인
for box in self.current_boxes:
if box.class_name in self.flags:
현재 검출된 객체의 class_name이 제어 대상 Class인지 확인합니다.
예를 들어 일반적인 객체가 검출되어도 self.flags에 등록되어 있지 않다면 차량 제어에는 사용하지 않습니다.
즉 YOLO가 어떤 객체를 많이 검출하더라도 실제 차량 행동에 영향을 주는 것은 다음 Class뿐입니다.
slow_down
speed_up
turn_right
red_light
yellow_light
green_light
pedestrian
39. Bounding Box 면적 계산
유효한 Class라면 Bounding Box의 면적을 계산합니다.
area = (
box.xmax - box.xmin
) * (
box.ymax - box.ymin
)
수식으로 표현하면
Area
= Bounding Box Width
× Bounding Box Height
입니다.
40. Bounding Box 크기로 객체 거리 간접 판단
계산한 면적을 Class별 Threshold와 비교합니다.
if area > \
self.flags[
box.class_name
]['size_threshold']:
Threshold보다 크다면
temp_flags[
box.class_name
] = True
가 됩니다.
즉 단순히 YOLO에서 객체가 검출되었다고 바로 행동하지 않습니다.
객체 검출
↓
Bounding Box 크기 계산
↓
Threshold보다 큼?
↓
Yes
↓
실제 제어 이벤트 발생
카메라에서 가까운 물체일수록 일반적으로 영상에서 Bounding Box가 커지는 특성을 이용하는 방식입니다.
물론 정확한 거리를 계산하는 방식은 아닙니다.
41. 객체별 Size Threshold의 장점
객체마다 Threshold를 따로 설정할 수 있습니다.
예를 들어
red_light_size_threshold
pedestrian_size_threshold
turn_right_size_threshold
를 서로 다르게 설정할 수 있습니다.
따라서 어떤 객체는 멀리서부터 반응하게 하고 어떤 객체는 충분히 가까워졌을 때만 반응하도록 조절할 수 있습니다.
42. 일반 객체의 상태 유지
pedestrian을 제외한 대부분의 객체는 다음 방식으로 동작합니다.
if temp_flags[flag_key]:
flag_data['past_time'] = current_time
flag_data['active'] = True
객체가 현재 검출되었다면
마지막 검출 시간 = 현재 시간
active = True
가 됩니다.
43. 객체가 사라져도 바로 비활성화하지 않음
객체가 현재 프레임에서 검출되지 않았을 때는 다음과 같습니다.
flag_data['active'] = (
current_time -
flag_data['past_time']
<=
flag_data['duration_sec']
)
즉 마지막 검출 후 설정한 시간이 지나지 않았다면 계속 활성 상태를 유지합니다.
예를 들어 개념적으로
red_light_duration_sec = 2초
라고 설정되어 있다고 가정하겠습니다.
빨간불을 마지막으로 본 뒤 YOLO가 한두 프레임 놓치더라도 바로 차량이 출발하지 않고 설정 시간 동안 red_light 상태를 유지할 수 있습니다.
실제 Parameter 값은 외부 설정에 따라 결정됩니다.
44. Duration을 사용하는 이유
YOLO 기반 실시간 검출에서는 다음 현상이 발생할 수 있습니다.
Frame 1
검출
Frame 2
검출
Frame 3
미검출
Frame 4
검출
Frame 5
검출
한 프레임의 미검출 때문에 상태가 계속
ON
OFF
ON
OFF
로 반복되면 차량 제어가 불안정해질 수 있습니다.
Duration은 이러한 문제를 줄이기 위한 일종의 상태 유지 기능입니다.
45. pedestrian 처리
보행자는 다른 객체와 별도의 로직을 사용합니다.
if flag_key == 'pedestrian':
먼저 마지막 기준 시점으로부터 경과 시간을 계산합니다.
time_elapsed = (
current_time -
flag_data['past_time']
)
46. 보행자가 검출된 경우
if temp_flags['pedestrian']:
그리고
if time_elapsed > \
flag_data['pass_by_duration_sec']:
조건을 검사합니다.
조건을 만족하면
flag_data['past_time'] = current_time
flag_data['active'] = True
가 됩니다.
즉 보행자는 일반 이벤트와 달리 pass_by_duration_sec이라는 별도의 시간 조건을 사용합니다.
47. 보행자 재활성화 로직
보행자가 검출되었지만 아직 pass_by_duration_sec이 지나지 않은 경우에는
flag_data['active'] = (
time_elapsed
<=
flag_data['duration_sec']
)
로 판단합니다.
보행자가 검출되지 않은 경우도 동일하게
flag_data['active'] = (
time_elapsed
<=
flag_data['duration_sec']
)
를 사용합니다.
따라서 보행자 처리는
pedestrian_duration_sec
pedestrian_pass_by_duration_sec
두 종류의 시간을 함께 사용하는 특별한 상태 관리 방식입니다.
실제 두 Parameter의 값에 따라 정지 유지 시간과 재활성화 시점이 달라집니다.
48. timer_callback
10 Hz Timer에서는 다음 두 함수를 호출합니다.
def timer_callback(self):
self.publish_cmd_vel()
self.publish_debug_image()
즉 차량 제어와 영상 출력을 같은 Timer에서 처리합니다.
49. publish_cmd_vel 함수
이제 프로그램에서 가장 중요한 차량 제어 부분입니다.
def publish_cmd_vel(self):
먼저 빈 Twist 메시지를 생성합니다.
cmd = Twist()
초기값은 모든 속도가 0입니다.
50. Emergency Stop과 Start 조건
가장 먼저 다음 조건을 확인합니다.
if self.stop_flag \
or not self.start_flag:
self.cmd_pub_.publish(cmd)
return
다음 두 경우에는 무조건 정지합니다.
1. emergency_stop = True
2. 아직 트랙 인식 결과를 받지 않음
이 경우 빈 Twist 메시지를 Publish하므로 결과는
linear.x = 0
angular.z = 0
입니다.
51. 빨간불과 보행자의 우선순위
정상적으로 주행 가능한 상태가 되더라도 다음 조건을 먼저 검사합니다.
if self.flags['red_light']['active'] \
or \
self.flags['pedestrian']['active']:
둘 중 하나라도 활성화되면
cmd.linear.x = 0.0
cmd.angular.z = 0.0
가 됩니다.
즉
red_light
또는
pedestrian
→ 완전 정지
입니다.
가속 표지나 감속 표지보다 높은 우선순위를 가지고 있습니다.
52. 트랙 오차의 비선형 변환
정지 조건이 아니라면 트랙 제어값을 변환합니다.
sigmoid_1 = (
self.control_1 *
sigmoid(
self.control_1,
self.max_steering
)
)
Turn Mode도 동일합니다.
sigmoid_2 = (
self.control_2 *
sigmoid(
self.control_2,
self.max_steering
)
)
따라서
control_1
↓
비선형 변환
↓
sigmoid_1
control_2
↓
비선형 변환
↓
sigmoid_2
가 됩니다.
53. Normal Mode와 Turn Mode 선택
YOLO가 turn_right 객체를 검출했는지를 확인합니다.
if self.flags[
'turn_right'
]['active']:
활성 상태이면 Turn Mode를 사용합니다.
그렇지 않으면 Normal Mode를 사용합니다.
이 부분이 딥러닝 트랙 인식과 YOLO 객체 인식이 직접 연결되는 핵심입니다.
YOLO turn_right
False
↓
Normal Mode Track 사용
True
↓
Turn Mode Track 사용
54. Turn Right Mode의 속도
우회전 상태에서는 다음과 같이 선속도를 계산합니다.
cmd.linear.x = (
self.linear_x_base_speed
-
(
abs(sigmoid_2)
*
self.get_parameter(
'deceleration_ratio'
).value
)
)
수식으로 정리하면
linear.x
= Base Speed
- |Steering|
× Deceleration Ratio
입니다.
조향량이 클수록 속도를 낮추는 구조입니다.
55. 회전하면서 감속하는 이유
급한 곡선에서 직선과 동일한 속도로 주행하면 차량이 트랙을 벗어날 가능성이 높아집니다.
이 프로그램은 조향량의 절대값이 커질수록 자동으로 전진 속도를 낮춥니다.
직선
↓
Steering 작음
↓
속도 높음
급회전
↓
Steering 큼
↓
속도 감소
56. Turn Mode의 각속도
우회전 상태에서는
cmd.angular.z = (
sigmoid_2 *
self.turn_right_factor
)
를 사용합니다.
즉 Turn Mode 트랙 인식 결과에 turn_right_factor를 추가 적용합니다.
Turn Mode Track Error
↓
control_2
↓
sigmoid_2
↓
turn_right_factor
↓
angular.z
57. Normal Mode 주행
turn_right가 활성 상태가 아니면 일반 트랙을 사용합니다.
cmd.linear.x = (
self.linear_x_base_speed
-
(
abs(sigmoid_1)
*
self.get_parameter(
'deceleration_ratio'
).value
)
)
그리고
cmd.angular.z = sigmoid_1
입니다.
즉 Normal Mode에서는 control_1이 직접 조향의 기반이 됩니다.
58. Normal Mode와 Turn Mode 비교
전체 관계를 정리하면 다음과 같습니다.
Normal Driving
/control.normal_mode
↓
control_1
↓
sigmoid_1
↓
angular.z
우회전 표지가 활성화되면
Turn Right
/control.turn_mode
↓
control_2
↓
sigmoid_2
↓
turn_right_factor
↓
angular.z
이 구조 때문에 앞의 트랙 인식 모델이 Normal Mode와 Turn Mode 두 종류의 출력을 생성했던 것입니다.
59. speed_up 처리
YOLO에서 speed_up이 활성 상태라면
cmd.linear.x *= self.speed_up_factor
cmd.angular.z *= self.speed_up_factor
를 실행합니다.
따라서 전진 속도와 조향값 모두 Factor가 적용됩니다.
60. slow_down 처리
speed_up이 아니라 slow_down이 활성 상태이면
cmd.linear.x *= self.slow_down_factor
로 선속도를 줄입니다.
조향은 다음과 같습니다.
cmd.angular.z *= (
self.slow_down_factor / 2
)
즉 감속 시 각속도에는 slow_down_factor / 2가 적용됩니다.
61. speed_up과 slow_down의 우선순위
소스는 다음 구조입니다.
if speed_up:
...
elif slow_down:
...
따라서 두 Flag가 동시에 True인 경우에는 speed_up이 먼저 적용됩니다.
slow_down은 실행되지 않습니다.
이것은 코드상의 명확한 우선순위입니다.
62. yellow_light 처리
황색 신호가 활성화되면
cmd.linear.x *= \
self.yellow_light_factor
각속도는
cmd.angular.z *= (
self.yellow_light_factor / 2
)
가 됩니다.
즉 황색 신호에 해당하는 Factor를 이용해 속도와 조향을 조정합니다.
63. green_light 처리
황색 신호가 활성화되어 있지 않고 녹색 신호가 활성화되어 있으면
cmd.linear.x *= \
self.green_light_factor
cmd.angular.z *= \
self.green_light_factor
를 적용합니다.
64. 신호등 우선순위
빨간불은 앞에서 이미 처리했습니다.
red_light
→ 완전 정지
나머지 두 신호는 다음 구조입니다.
if yellow_light:
...
elif green_light:
...
따라서 황색과 녹색 Flag가 동시에 활성화되어 있다면 황색 신호 처리가 우선합니다.
전체 우선순위를 단순화하면 다음과 같습니다.
Red
→ 정지
Yellow
→ yellow_light_factor
Green
→ green_light_factor
65. 최종 선속도 제한
모든 계산을 마친 후 다음 코드를 실행합니다.
cmd.linear.x = np.clip(
cmd.linear.x,
self.min_speed,
self.max_speed
)
따라서 선속도는 지정된 범위를 벗어날 수 없습니다.
min_speed
≤ linear.x
≤ max_speed
66. 최종 조향 제한
조향도 제한합니다.
cmd.angular.z = np.clip(
cmd.angular.z,
-self.max_steering,
self.max_steering
)
따라서
-max_steering
≤ angular.z
≤ max_steering
범위 안에서만 움직입니다.
이는 비정상적으로 큰 트랙 오차가 들어왔을 때 과도한 회전 명령이 발생하는 것을 막는 역할을 합니다.
67. 정지 상태와 min_speed의 관계
한 가지 중요한 점이 있습니다.
Emergency Stop이나 빨간불, 보행자 정지에서는 속도를 명시적으로 0.0으로 설정합니다.
그리고 빨간불과 보행자 정지는 일반 주행 계산을 수행하지 않기 때문에 min_speed 제한을 적용받지 않습니다.
따라서 min_speed가 양수로 설정되어 있더라도
Emergency Stop
Red Light
Pedestrian
상태에서는 실제로 0을 보낼 수 있습니다.
68. 최종 Twist Publish
모든 계산이 끝나면
self.cmd_pub_.publish(cmd)
를 실행합니다.
토픽은
/cmd_vel
입니다.
최종적으로 WEGO LIMO가 사용하는 핵심 값은
cmd.linear.x
cmd.angular.z
입니다.
69. 최종 제어 흐름
차량 제어 부분을 한 번에 표현하면 다음과 같습니다.
/control
↓
Track Error
↓
Gain
↓
Filter
↓
Nonlinear Steering
↓
Normal / Turn 선택
↓
조향에 따른 감속
↓
YOLO Speed Factor
↓
Traffic Light Factor
↓
Speed Limit
↓
Steering Limit
↓
Twist
↓
/cmd_vel
70. 객체 인식 제어 우선순위
YOLO 객체를 기준으로 제어 구조를 정리하면 다음과 같이 이해할 수 있습니다.
Emergency Stop
↓
무조건 정지
Red Light / Pedestrian
↓
무조건 정지
Turn Right
↓
Turn Mode Track 사용
Speed Up / Slow Down
↓
속도 조정
Yellow / Green Light
↓
추가 속도 조정
최종 속도 제한
↓
/cmd_vel
71. publish_debug_image 함수
이 프로그램은 최종 주행 상태를 영상에서도 확인할 수 있도록 만들어져 있습니다.
def publish_debug_image(self):
먼저 카메라 영상이 없으면 종료합니다.
if self.current_image is None:
return
72. 디버그 영상 복사
debug_image = \
self.current_image.copy()
원본 이미지를 직접 수정하지 않고 복사본을 사용합니다.
영상 크기도 가져옵니다.
h, w, _ = debug_image.shape
73. YOLO Bounding Box 다시 표시
현재 저장되어 있는 모든 객체에 대해 반복합니다.
for box in self.current_boxes:
Bounding Box 좌표를 가져옵니다.
xmin = box.xmin
ymin = box.ymin
xmax = box.xmax
ymax = box.ymax
Label은 다음과 같이 만듭니다.
label = (
f"{box.class_name}: "
f"{box.probability:.2f}"
)
예를 들어
red_light: 0.93
형태로 표시됩니다.
74. 객체 Bounding Box 면적 다시 계산
area = (
xmax - xmin
) * (
ymax - ymin
)
YOLO Callback에서 사용했던 것과 동일하게 Bounding Box 면적을 계산합니다.
75. Bounding Box 색상
기본 색상은 빨간색입니다.
box_color = (0, 0, 255)
OpenCV는 BGR 순서이므로 빨간색입니다.
76. 제어 Threshold를 넘으면 녹색
현재 객체가 제어 대상 Class이고
if box.class_name in self.flags:
Bounding Box 면적이 Threshold보다 크다면
box_color = (0, 255, 0)
으로 변경합니다.
즉 화면을 보면 바로 알 수 있습니다.
빨간 Bounding Box
→ 객체는 검출되었지만 크기 조건 미충족
녹색 Bounding Box
→ 제어 조건을 만족할 정도로 충분히 큰 객체
디버깅할 때 매우 유용한 기능입니다.
77. Bounding Box 출력
cv2.rectangle(
debug_image,
(xmin, ymin),
(xmax, ymax),
box_color,
2
)
객체 이름과 Confidence도 출력합니다.
cv2.putText(
debug_image,
label,
(xmin, ymin - 10),
cv2.FONT_HERSHEY_SIMPLEX,
0.5,
box_color,
2
)
78. 트랙 기준 중심 계산
영상 중앙을 계산합니다.
ref_x = w // 2
트랙 인식 프로그램은 300 Pixel Width를 기준으로 동작했습니다.
따라서 현재 영상 크기에 맞추기 위한 Scale Factor도 계산합니다.
scale_factor = w / 300.0
79. 트랙 위치를 원래 좌표로 복원하는 이유
트랙 인식 프로그램에서는 다음 관계를 사용했습니다.
Control Error
= 150 - Track X
그리고 dl_control.py에서는 이 결과에 Gain을 곱했습니다.
디버그 영상에서는 반대로 계산하여 트랙 위치를 다시 영상 위에 표시합니다.
80. Turn Mode 표시
현재 turn_right가 활성 상태라면
if self.flags[
'turn_right'
]['active']:
Turn Mode를 화면에 표시합니다.
먼저 Gain을 제거합니다.
turn_mode_scaled = (
self.control_2
/
self.get_parameter(
'control_2_gain'
).value
)
원래 300 Pixel 기준 좌표를 계산합니다.
cx_2_300 = \
150 - turn_mode_scaled
81. 현재 카메라 Width에 맞게 좌표 변환
cx_2_scaled = int(
cx_2_300 *
scale_factor
)
트랙 인식은 300 Pixel 기준이지만 현재 디버그 영상 Width는 다를 수 있습니다.
따라서
300 Pixel 좌표
↓
현재 영상 Width 비율 적용
↓
Debug Image 좌표
방식으로 변환합니다.
82. Turn Mode 시각화
화면에
Turn Mode
문자를 표시합니다.
기준점과 검출된 트랙 위치 사이에 빨간 선을 그립니다.
중앙 기준점은 녹색으로 표시합니다.
Turn Mode 트랙 위치는 빨간색으로 표시합니다.
결과적으로 현재 우회전용 트랙 위치가 화면 중심에서 어느 정도 떨어져 있는지 한눈에 확인할 수 있습니다.
83. Normal Mode 표시
turn_right가 활성 상태가 아니라면 Normal Mode를 표시합니다.
normal_mode_scaled = (
self.control_1
/
self.get_parameter(
'control_1_gain'
).value
)
300 Pixel 기준의 원래 트랙 위치는
cx_1_300 = \
150 - normal_mode_scaled
입니다.
현재 영상 Width에 맞게 변환합니다.
cx_1_scaled = int(
cx_1_300 *
scale_factor
)
84. Normal Mode 시각화
Normal Mode는 파란색 계열로 표시합니다.
녹색 점
→ 영상 중심 기준
파란 점
→ Normal Mode Track 위치
파란 선
→ 두 위치의 오차
따라서 실제 차량이 어떤 트랙 위치를 기준으로 조향하고 있는지 확인할 수 있습니다.
85. Debug 좌표와 필터의 관계
현재 디버그 좌표를 계산할 때 사용하는 값은
self.control_1
self.control_2
입니다.
즉 딥러닝 모델이 처음 출력한 원본 값 자체라기보다는 Gain과 apply_derivative_control()을 거친 제어값을 이용합니다.
따라서 화면에 표시되는 위치는 제어 프로그램 내부에서 사용 중인 값을 역산해 표현한 결과라고 이해하는 것이 정확합니다.
86. control gain이 0이면 발생하는 문제
디버그 영상에서는 다음 연산을 수행합니다.
self.control_1 /
control_1_gain
또는
self.control_2 /
control_2_gain
따라서 실제 실행 Parameter에서
control_1_gain = 0
control_2_gain = 0
으로 설정하면 0으로 나누는 문제가 발생할 수 있습니다.
소스의 Parameter 기본값은 0.0이므로 실제 Launch나 YAML 설정에서는 이 Gain들을 정상적인 값으로 반드시 설정해야 합니다.
87. Debug Image Publish
완성된 OpenCV 이미지를 ROS 2 Image로 변환합니다.
out_msg = self.br.cv2_to_imgmsg(
debug_image,
encoding='bgr8'
)
그리고
self.dl_image_pub.publish(
out_msg
)
를 통해
/dl_image
토픽으로 Publish합니다.
88. 압축 Debug Image Publish
같은 영상을 압축 이미지로도 변환합니다.
debug_compressed_msg = \
self.br.cv2_to_compressed_imgmsg(
debug_image
)
그리고
self.dl_image_compressed_pub.publish(
debug_compressed_msg
)
를 실행합니다.
토픽은
/dl_image/compressed
입니다.
89. 이 Node의 전체 ROS 2 Topic 구조
입력 Topic을 정리하면 다음과 같습니다.
/emergency_stop
→ 긴급 정지
/control
→ 딥러닝 트랙 인식 결과
/yolo_bounding_boxes
→ YOLO 객체 검출 결과
/camera/color/image_raw/compressed
→ 디버그용 카메라 영상
출력 Topic은 다음과 같습니다.
/cmd_vel
→ 실제 차량 제어
/dl_image
→ 일반 디버그 영상
/dl_image/compressed
→ 압축 디버그 영상
전체 관계는 다음과 같습니다.
Track Detection Node
↓
/control
↓
│
YOLO │
Detection Node
↓
/yolo_bounding_boxes
↓
│
├────→ dl_control_node
│ ↓
Emergency Stop ↓
│ /cmd_vel
└────→ ↓
LIMO
90. 딥러닝 트랙 인식과 YOLO의 역할 분담
이번 프로그램을 이해하는 가장 좋은 방법은 두 딥러닝 결과의 역할을 분리해서 보는 것입니다.
트랙 인식은 다음을 결정합니다.
현재 트랙 중심은 어디인가?
얼마나 좌우로 벗어났는가?
Normal Mode에서 어디로 가야 하는가?
Turn Mode에서 어디로 가야 하는가?
YOLO는 다음을 결정합니다.
감속해야 하는가?
가속해야 하는가?
우회전해야 하는가?
정지 신호인가?
황색 신호인가?
녹색 신호인가?
보행자가 있는가?
91. turn_right 객체가 중요한 이유
특히 turn_right는 다른 YOLO Class와 역할이 다릅니다.
slow_down이나 speed_up은 기존 주행 명령에 Factor를 적용합니다.
하지만 turn_right는 사용해야 할 딥러닝 트랙 자체를 변경합니다.
turn_right OFF
Normal Track
→ control_1
→ Steering
turn_right ON
Turn Track
→ control_2
→ Steering
따라서 YOLO가 단순히 속도를 조절하는 것뿐만 아니라 트랙 인식 결과 선택에도 관여하고 있습니다.
92. red_light와 pedestrian이 가장 높은 이유
빨간불과 보행자는 다음 코드에 의해 일반 주행 계산보다 먼저 처리됩니다.
if red_light or pedestrian:
linear.x = 0
angular.z = 0
따라서 다음 상태보다 우선합니다.
speed_up
slow_down
turn_right
yellow_light
green_light
예를 들어 speed_up과 red_light가 동시에 활성화되어 있어도 차량은 정지합니다.
93. Bounding Box 크기와 Duration을 함께 사용하는 이유
YOLO 결과를 차량 제어에 직접 연결하면 여러 문제가 발생할 수 있습니다.
멀리 있는 표지판에 너무 빨리 반응할 수 있습니다.
한두 프레임 검출이 실패하면 동작이 갑자기 해제될 수 있습니다.
이번 프로그램은 이 두 문제를 다음 기능으로 해결하려고 합니다.
Size Threshold
→ 가까워졌을 때 반응
Duration
→ 잠시 검출을 놓쳐도 상태 유지
따라서 객체 인식 결과를 실제 로봇 제어에 연결할 때 필요한 추가적인 상태 처리 구조를 확인할 수 있습니다.
94. 조향과 속도를 함께 계산하는 이유
이 프로그램에서 속도는 일정하지 않습니다.
기본 속도를
linear_x_base_speed
로 설정한 뒤 조향량만큼 속도를 감소시킵니다.
Speed
= Base Speed
- Steering Magnitude
× Deceleration Ratio
따라서 직선 구간에서는 상대적으로 빠르게 주행하고 급한 곡선에서는 자연스럽게 느려지는 형태를 만들 수 있습니다.