[Research] Automated Driving with RGB Camera in Carla

 

Carla에서 RGB 카메라로 운전하기

이번 시간에는 Carla 시뮬레이터에서 Python 코드와 RGB 카메라를 이용하여 자율 주행을 구현하는 방법을 알아보겠다. 우선 "Driving straight with CV" 주제를 통해 간단한 RGB 카메라를 이용한 구현 예시를 살펴보고, 이후 "Steering along navigation route" 부분을 중심으로 코드를 단계별로 분석하며 RGB 카메라 센싱 및 경로 추종을 위한 조향 방법을 자세히 살펴보겠다.

1. Driving Straight with Computer Vision

1.1. 환경 설정 및 차량 생성

Python

import carla
import time
import cv2
import numpy as np
import math

# Carla client 생성
client = carla.Client('host ip', 2000) # host ip는 상황에 따라 설정할 것
world = client.get_world()
spawn_points = world.get_map().get_spawn_points()

# 차량 블루프린트 선택 및 생성
vehicle_bp = world.get_blueprint_library().filter('*mini*')
start_point = spawn_points[0]
vehicle = world.try_spawn_actor(vehicle_bp[0], start_point)

1.2. RGB 카메라 센서 설정 및 부착

Python

# 카메라 설정
CAMERA_ROZ_Z = 3  # 카메라 높이
CAMERA_ROZ_X = -5  # 카메라 위치 (차량 뒷쪽)
camera_bp = world.get_blueprint_library().find('sensor.camera.rgb')
camera_bp.set_attribute('image_size_x', '640')
camera_bp.set_attribute('image_size_y', '360')
camera_init_trans = carla.Transform(carla.Location(z=CAMERA_ROZ_Z, x=CAMERA_ROZ_X))

# 카메라 생성 및 차량에 부착
camera = world.spawn_actor(camera_bp, camera_init_trans, attach_to=vehicle)

1.3. 카메라 이미지 처리 및 속도 유지 함수 정의

Python

# 카메라 콜백 함수 정의 (이미지 데이터 처리)
def camera_callback(image, data_dict):
    data_dict['image'] = np.reshape(np.copy(image.raw_data), (image.height, image.width, 4))

image_w = camera_bp.get_attribute('image_size_x').as_int()
image_h = camera_bp.get_attribute('image_size_y').as_int()
camera_data = {'image': np.zeros((image_h, image_w, 4))}
camera.listen(lambda image: camera_callback(image, camera_data))

# 속도 유지 함수 정의
def maintain_speed(s):
    if s > PREFERRED_SPEED:
        return 0
    elif s < PREFERRED_SPEED - SPEED_THRESHOLD:
        return 0.8  # 가속
    else:
        return 0.4  # 유지

# 속도 관련 상수 정의
PREFERRED_SPEED = 20  # 목표 속도
SPEED_THRESHOLD = 2  # 속도 유지 범위

1.4. 주행 및 속도 제어

Python

# 주행 루프
while True:
    world.tick()
    if cv2.waitKey(1) == ord('q'):  # 'q' 키를 누르면 종료
        break

    image = camera_data['image']
    v = vehicle.get_velocity()
    speed = round(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2), 0)  # 속도 계산 (km/h)
    
    # 속도 유지 함수를 이용하여 throttle 값 계산
    estimated_throttle = maintain_speed(speed)
    
    # 차량 제어 (throttle, steer)
    vehicle.apply_control(carla.VehicleControl(throttle=estimated_throttle, steer=0))
    
    # 이미지 출력
    cv2.imshow('RGB Camera', image)

# 종료 시 센서 및 차량 제거
cv2.destroyAllWindows()
camera.stop()
for actor in world.get_actors().filter('*vehicle*'):
    actor.destroy()
for sensor in world.get_actors().filter('*sensor*'):
    sensor.destroy()

직진하는 모습

2. Steering Along Navigation Route

2.1. 경로 계획 및 생성

Python

import sys
sys.path.append('/opt/carla-simulator/PythonAPI/carla')
from agents.navigation.global_route_planner import GlobalRoutePlanner

# 경로 계획 도구 초기화
sampling_resolution = 1
grp = GlobalRoutePlanner(world.get_map(), sampling_resolution)

# 시작 지점 설정 (현재 차량 위치)
point_a = start_point.location

# 가장 긴 경로 찾기
distance = 0
for loc in spawn_points:
    cur_route = grp.trace_route(point_a, loc.location)
    if len(cur_route) > distance:
        distance = len(cur_route)
        route = cur_route

# 경로 시각화
for waypoint in route:
    world.debug.draw_string(waypoint[0].transform.location, '^', # '^' 모양 경로
                            draw_shadow=False, color=carla.Color(r=0, g=0, b=255),
                            life_time=600.0, persistent_lines=True)

2.2. 차량의 Forward Vector 및 Waypoint 정보 분석

Python

# 차량의 forward vector 정보 출력
v = vehicle.get_velocity()
speed = round(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2), 0)
fwd_vector = vehicle.get_transform().get_forward_vector()

# waypoint 25의 정보 출력 (예시)
wp_vector = route[25][0].transform.get_forward_vector()
car_x = vehicle.get_transform().location.x
car_y = vehicle.get_transform().location.y
wp_x = route[25][0].transform.location.x
wp_y = route[25][0].transform.location.y

# 차량과 waypoint 25 사이의 각도 계산
angle = math.degrees(math.atan((wp_y - car_y) / (wp_x - car_x))) # 약 39도

vector analysis

2.2.1 차량의 Forward Vector 분석

Carla에서 차량의 방향을 나타내는 forward vector는 get_forward_vector() 함수를 통해 얻을 수 있다. 이 벡터는 x, y, z 세 개의 값으로 구성되며, 차량의 앞쪽 방향을 나타내는 단위 벡터이다. 즉, 벡터의 크기는 항상 1이며, x, y 값은 각각 차량의 x축, y축 방향 성분을 나타낸다.

예를 들어, 차량이 북쪽을 향하고 있을 때 forward vector는 (0, 1, 0) 이 된다. 동쪽을 향하고 있을 때는 (1, 0, 0) 이 된다.

2.2.2 Waypoint 분석 및 각도 계산

Waypoint은 차량이 따라가야 할 경로 상의 지점을 나타낸다. 각 waypoint은 위치 정보와 forward vector를 가지고 있다. 차량의 현재 위치와 다음 waypoint의 위치를 이용하여 차량이 이동해야 할 방향을 계산할 수 있다.

차량의 현재 위치를 $(car\_x,car\_y)$, 다음 waypoint의 위치를 $(wp\_x,wp\_y)$ 라고 하면, 차량과 waypoint 사이의 각도는 다음과 같이 계산할 수 있다.

angle = math.degrees(math.atan((wp_y - car_y) / (wp_x - car_x)))

이 각도는 차량의 현재 방향과 waypoint을 잇는 직선 사이의 각도를 나타낸다.

2.3. 조향 각도 계산 및 차량 제어 함수 정의

Python

# 두 벡터 사이의 각도 계산 함수 정의
def angle_between(v1, v2):
    return math.degrees(np.arctan2(v1[1], v1[0]) - np.arctan2(v2[1], v2[0]))

# waypoint을 향하는 조향 각도 계산 함수 정의
def get_angle(car, wp):
    vehicle_pos = car.get_transform()
    car_x = vehicle_pos.location.x
    car_y = vehicle_pos.location.y
    wp_x = wp.transform.location.x
    wp_y = wp.transform.location.y

    # waypoint을 향하는 벡터 계산
    x = (wp_x - car_x) / ((wp_y - car_y)**2 + (wp_x - car_x)**2)**0.5
    y = (wp_y - car_y) / ((wp_y - car_y)**2 + (wp_x - car_x)**2)**0.5

    # 차량의 forward vector
    car_vector = vehicle_pos.get_forward_vector()

    # 두 벡터 사이의 각도 계산
    degrees = angle_between((x, y), (car_vector.x, car_vector.y))
    return degrees

2.3.1. 조향 각도 계산

차량의 forward vector와 waypoint을 향하는 벡터 사이의 각도를 계산하여 조향 각도를 결정한다. 두 벡터 사이의 각도는 다음과 같이 계산할 수 있다.

Python

def angle_between(v1, v2):
    return math.degrees(np.arctan2(v1[1], v1[0]) - np.arctan2(v2[1], v2[0]))

v1은 차량의 forward vector, v2는 waypoint을 향하는 벡터이다. np.arctan2() 함수는 두 점의 좌표를 이용하여 각도를 계산하는 함수이다.

2.4. 주행 및 조향 제어

Python

# 주행 루프
curr_wp = 5  # 현재 waypoint 인덱스
predicted_angle = 0

while curr_wp < len(route) - 1:
    world.tick()
    if cv2.waitKey(1) == ord('q'):  # 'q' 키를 누르면 종료
        break

    image = camera_data['image']

    # 현재 waypoint에 가까워지면 다음 waypoint으로 이동
    while curr_wp < len(route) and vehicle.get_transform().location.distance(route[curr_wp][0].transform.location) < 5:
        curr_wp += 1

    # 다음 waypoint을 향하는 조향 각도 계산
    predicted_angle = get_angle(vehicle, route[curr_wp][0])

    # 각도 범위 조정 (-360 ~ 360)
    if predicted_angle < -300:
        predicted_angle += 360
    elif predicted_angle > 300:
        predicted_angle -= 360

    # 조향 각도 제한 (-40 ~ 40) 및 스케일링 (-1 ~ 1)
    steer_input = predicted_angle
    if predicted_angle < -40:
        steer_input = -40
    elif predicted_angle > 40:
        steer_input = 40
    steer_input = steer_input / 75  # 최대 조향 각도 75도로 가정

    # 속도 유지 함수를 이용하여 throttle 값 계산
    estimated_throttle = maintain_speed(speed)

    # 차량 제어 (throttle, steer)
    vehicle.apply_control(carla.VehicleControl(throttle=estimated_throttle, steer=steer_input))

    # 이미지 출력
    cv2.imshow('RGB Camera', image)

# 종료 시 센서 및 차량 제거
cv2.destroyAllWindows()
camera.stop()
for sensor in world.get_actors().filter('*sensor*'):
    sensor.destroy()
vehicle.apply_control(carla.VehicleControl(throttle=0, steer=0, brake=1))

result

2.4.1. 차량 제어

계산된 조향 각도를 이용하여 차량을 제어한다. Carla에서는 apply_control() 함수를 통해 차량의 throttle(가속), steer(조향), brake(감속) 값을 설정할 수 있다. 조향 각도는 steer 값으로 설정하며, -1.0에서 1.0 사이의 값을 가진다. -1.0은 최대 좌회전, 1.0은 최대 우회전을 의미한다.

steer = -1

결론

이번 포스팅에서는 Carla 시뮬레이터에서 Python 코드와 RGB 카메라를 이용하여 자율 주행을 구현하는 방법을 살펴보았다. carla.Client를 통해 시뮬레이터에 연결하고, sensor.camera.rgb를 사용하여 RGB 카메라 센서를 생성하여 차량에 부착했다. GlobalRoutePlanner를 이용하여 경로를 생성하고, 차량의 forward vector와 waypoint 정보를 분석하여 조향 각도를 계산하는 방법을 알아보았다. 이를 통해 Carla 시뮬레이터에서 RGB 카메라를 이용한 자율 주행 시뮬레이션을 개발할 수 있다.

추천글:

[Research] Carla 환경 설정 (server: window + client: ubuntu) | be별하

[Research] Carla Client and Vehicle spawn | be별하