Carla Simulator 사용자 정의 경로 운행

Carla에서 waypoint는 차량이 이동 가능한 경로에 대한 데이터로 차량의 시작 영역과 목표 지점이 정해질 때 해당 waypoint를 따라간다. 하지만 이 값은 매우 촘촘하고 수가 많아 모든 waypoint를 지정하여 움직이기에는 복잡하기에 다른 방법으로 사용자 정의 경로를 정하였다.

사진 원본 블로그

해당 waypoint들은 번호를 매겨 찾아가기도 힘들 뿐더러 이동 시 waypoint간 직선을 경로로 잡기에 waypoint값을 떨어뜨려 설정할 시에 건물 사이를 지나가려 하는 등의 문제가 발생.

Carla Traffic manager 문서

해당 문서에서도 sqawn point를 waypoint값으로 지정.

따라서 sqawn point를 기반으로 경로를 지정하기로 하였다.

해당 이미지는 Town10의 sqawn point를 시각화한 코드로 해당 sqawn point의 순서와 위치는 고정되어있다.

import carla
import time

def draw_spawn_points():
    client = carla.Client('localhost', 2000)
    client.set_timeout(10.0)
    world = client.get_world()
    spawn_points = world.get_map().get_spawn_points()

    for i, transform in enumerate(spawn_points):
        world.debug.draw_string(
            transform.location,
            str(i), 
            draw_shadow=False,
            color=carla.Color(r=0, g=255, b=0),
            life_time=1500.0,
            persistent_lines=True
        )
        print(f"Spawn Point {i}: Location(x={transform.location.x}, y={transform.location.y}, z={transform.location.z})")

if __name__ == '__main__':
    try:
        draw_spawn_points()
        print("Press Ctrl+C to exit.")
        time.sleep(1500)
    except KeyboardInterrupt:
        print("Cancelled by user.")

다음 코드는 sqawn point를 waypoint로 설정하여 사용자 지정 경로를 운행하도록 만든 코드이다.

import carla
import random
import time
import pygame
import numpy as np

WINDOW_WIDTH = 800
WINDOW_HEIGHT = 600

def pygame_init():
    pygame.init()
    return pygame.display.set_mode((WINDOW_WIDTH, WINDOW_HEIGHT), pygame.HWSURFACE | pygame.DOUBLEBUF)

def process_image(image, display):
    array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8"))
    array = np.reshape(array, (image.height, image.width, 4))
    array = array[:, :, :3]
    array = array[:, :, ::-1]
    
    surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
    display.blit(surface, (0, 0))
    pygame.display.flip()

def main():
    client = carla.Client('localhost', 2000)
    client.set_timeout(10.0)
    world = client.get_world()

    ego_vehicle = None
    camera_sensor = None
    original_settings = None

    try:
        display = pygame_init()
        original_settings = world.get_settings()
        
        settings = world.get_settings()
        settings.synchronous_mode = True
        settings.fixed_delta_seconds = 0.05
        world.apply_settings(settings)

        traffic_manager = client.get_trafficmanager(8000)
        traffic_manager.set_synchronous_mode(True)

        blueprint_library = world.get_blueprint_library()
        ego_bp = blueprint_library.find('vehicle.lincoln.mkz')
        ego_bp.set_attribute('role_name', 'ego_vehicle')

        spawn_points = world.get_map().get_spawn_points()
        
        #경로
        route_indices = [121, 11, 85, 93, 57, 43, 118, 79, 104, 95]
        route_indices_0 = [121, 11, 85, 91, 111, 141]
        
        if not route_indices or route_indices[0] >= len(spawn_points):
            print("Invalid route indices. Exiting.")
            return

        start_transform = spawn_points[route_indices[0]]
        ego_vehicle = world.try_spawn_actor(ego_bp, start_transform)
        
        if ego_vehicle is None:
            print(f"Failed to spawn ego vehicle at spawn point {route_indices[0]}. Exiting.")
            return

        print(f"Ego vehicle spawned at spawn point {route_indices[0]}: {ego_vehicle.get_transform().location}")
        
        camera_bp = blueprint_library.find('sensor.camera.rgb')
        camera_bp.set_attribute('image_size_x', str(WINDOW_WIDTH))
        camera_bp.set_attribute('image_size_y', str(WINDOW_HEIGHT))
        camera_bp.set_attribute('fov', '100')
        camera_transform = carla.Transform(carla.Location(x=1.6, z=1.7), carla.Rotation(pitch=-15))
        camera_sensor = world.spawn_actor(camera_bp, camera_transform, attach_to=ego_vehicle)
        camera_sensor.listen(lambda image: process_image(image, display))
        
        route = []
        print("\nGenerated Route Waypoints:")
        for i, ind in enumerate(route_indices):
            if ind < len(spawn_points):
                waypoint_transform = spawn_points[ind]
                route.append(waypoint_transform.location)
                print(f"Waypoint {i+1}: Location(x={waypoint_transform.location.x}, y={waypoint_transform.location.y}, z={waypoint_transform.location.z})")
            else:
                print(f"Invalid spawn point index {ind}. Skipping.")
        
        traffic_manager.set_path(ego_vehicle, route)
        
        # 차량 속도 설정 (40km/h)
        target_speed = 40.0
        traffic_manager.vehicle_percentage_speed_difference(ego_vehicle, - (target_speed / 100.0))
        ego_vehicle.set_autopilot(True, traffic_manager.get_port())
        
        # 경로 시각화
        for i, loc in enumerate(route):
            world.debug.draw_string(loc, str(i + 1), life_time=999999.0, persistent_lines=True)
            
        print("Ego vehicle spawned and route set. Running...")
        
        last_waypoint_location = route[-1]
        
        while True:
            world.tick()
            
            distance_to_end = ego_vehicle.get_location().distance(last_waypoint_location)
            if distance_to_end < 2.0:
                traffic_manager.set_path(ego_vehicle, route)

            for event in pygame.event.get():
                if event.type == pygame.QUIT:
                    return

    finally:
        if original_settings is not None:
            world.apply_settings(original_settings)
        if camera_sensor is not None:
            camera_sensor.destroy()
        if ego_vehicle is not None:
            ego_vehicle.destroy()
        
        pygame.quit()
        print("Simulation finished.")

if __name__ == '__main__':
    try:
        main()
    except Exception as e:
        print(f"An error occurred: {e}")

해당 코드에서 waypoint 설정 시 유의할 점으로 Carla의 autopilot은 현재 위치한 차선에 대해 내부 알고리즘을 통하여 경로를 설정하므로 우회전 차선에서 좌회전을 요하는 waypoint 경로를 설정하게 될 경우 맵 전체를 한바퀴 돈 이후 원하는 경로로 이동하게 될 수 있다.

코드 작동 영상