CARLA - waypoint 생성하고 추종하기

자율주행 시 차량이 추종하는 경로점을 waypoint라고 부른다.

이번에는 자율주행 시뮬레이터 CARLA에서 wapoint를 생성하는 법을 알아보겠다.

 

맵에 waypoint를 생성한 모습

 

구현하고자 하는 것은 사진처럼 맵 전체에 차량이 추종할 waypoint를 생성하는 것이다.

CARLA에서는 waypoint를 생성하는 기능을 보유하고 있다.

 

def visualize_waypoints(world, map):
    waypoints = map.generate_waypoints(distance=1.0)
    for waypoint in waypoints:
        world.debug.draw_string(waypoint.transform.location, 'O', draw_shadow=False,
                                color=carla.Color(r=255, g=0, b=0), life_time=120.0,
                                persistent_lines=True)
                                
                                
# 메인 코드들 ....

visualize_waypoints(world, world.get_map())

 

이런 식으로 generate_waypoint를 이용하여 원하는 간격으로 waypoint를 생성할 수 있다. 

이후 메인 함수에서 visualize_waypoint 함수를 불러 사용하면 된다.

 

차량이 waypoint를 추종하는 모습

 

이제 차량이 waypoint를 따라 주행하도록 해보자.

차량 주행을 위해서는 차량의 제어 코드를 작성해야 한다.

 

먼저 스티어링 휠을 조정할 수 있도록 해야 한다. 

그러려면 차량의 위치와 추종할 waypoint를 알아야 한다.

이를 이용하여 다음 waypoint로 이동하려면 스티어링 휠을 얼마만큼 조정해야 하는지 계산할 수 있다.

아래 코드는 언급한 사항들을 구현한 예시이다.

def compute_steer(next_waypoint, vehicle):
    vehicle_transform = vehicle.get_transform()
    vehicle_location = vehicle_transform.location
    waypoint_location = next_waypoint.transform.location
    

    direction_vector = np.array([waypoint_location.x - vehicle_location.x, waypoint_location.y - vehicle_location.y])
    direction_vector /= np.linalg.norm(direction_vector)
    
    forward_vector = np.array([np.cos(np.radians(vehicle_transform.rotation.yaw)), np.sin(np.radians(vehicle_transform.rotation.yaw))])

    dot_product = np.dot(forward_vector, direction_vector)
    steer_angle = np.arccos(dot_product) 
    

    steer = steer_angle / np.pi
    steer *= np.sign(direction_vector[1] * forward_vector[0] - direction_vector[0] * forward_vector[1]) 
    
    return steer

 

스티어링 휠을 조정할 수 있는 코드를 짰으니 이를 이용하여 차량을 제어하는 코드를 작성해야 한다.

현재 waypoint를 알고, 다음으로 추종할 wapoint를 선택한 다음에 속도를 제어한다.

이를 위해서는 적절한 속도 유지를 위한 코드가 있어야 한다.

쓰로틀과 브레이크값을 적절히 할당하도록 하자.

아래 코드는 앞서 작성한 compute_steer 함수를 이용하였다.

def control_vehicle(vehicle, map, target_speed):
    current_waypoint = map.get_waypoint(vehicle.get_location())
    next_waypoint = random.choice(current_waypoint.next(2.0))

    
    current_velocity = vehicle.get_velocity()
    current_speed = 3.6 * np.sqrt(current_velocity.x**2 + current_velocity.y**2 + current_velocity.z**2)  # km/h로 변환

    
    if current_speed < target_speed:
        throttle = 1.5
        brake = 0.0
    else:
        throttle = 0.0
        brake = 0.1 

    
    control = carla.VehicleControl(throttle=throttle, steer=compute_steer(next_waypoint, vehicle), brake=brake)
    vehicle.apply_control(control)

 

메인 코드에서의 차량 제어 사용법은 다음과 같다.

# 메인 코드들 ...

while True:
            world.tick()
            if not vehicle_stop:  
                control_vehicle(vehicle, world.get_map(), target_speed=20.0)  # 차량 제어 로직 호출

 

아래 영상은 녹화를 위해 차량의 속도를 줄였다.

차량이 waypoint를 따라 주행하는 모습을 확인할 수 있다.