Carla Simulator 사용자 정의 경로 운행(2)

차량 BBox추가와 주행한 영역 표시.

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))

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
    npc_vehicles = []

    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]
        
        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. Exiting.")
            return

        print(f"Ego vehicle spawned at: {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 = []
        for ind in route_indices:
            if ind < len(spawn_points):
                route.append(spawn_points[ind].location)
        
        traffic_manager.set_path(ego_vehicle, route)
        
        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())
        
        print("\nSpawning 10 NPC vehicles...")
        for i in range(10):
            npc_bp = random.choice(blueprint_library.filter('vehicle'))
            npc_bp.set_attribute('role_name', f'npc_vehicle_{i}')
            
            spawn_point = random.choice(spawn_points)
            npc_vehicle = world.try_spawn_actor(npc_bp, spawn_point)
            
            if npc_vehicle is not None:
                npc_vehicles.append(npc_vehicle)
                print(f"NPC vehicle {i+1} spawned.")
        
        for npc in npc_vehicles:
            npc.set_autopilot(True, traffic_manager.get_port())
            
        print("Ego vehicle and NPC vehicles spawned. Running...")
        
        last_waypoint_location = route[-1]
        prev_location = ego_vehicle.get_location()
        
        while True:
            world.tick()
            
            current_location = ego_vehicle.get_location()
            if prev_location:
                world.debug.draw_line(
                    prev_location,
                    current_location,
                    thickness=1.0,
                    color=carla.Color(r=0, g=0, b=255, a=255),
                    life_time=9999999.0
                )
            prev_location = current_location

            for npc in npc_vehicles:
                bbox = npc.bounding_box
                transform = npc.get_transform()
                
                world.debug.draw_box(
                    carla.BoundingBox(transform.location + bbox.location, bbox.extent),
                    transform.rotation,
                    0.1,
                    carla.Color(255, 0, 0, 255),
                    life_time=world.get_settings().fixed_delta_seconds * 1.2 
                )
            
            pygame.display.flip()
            
            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:
        print("\nCleaning up actors...")
        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()
        
        client.apply_batch([carla.command.DestroyActor(x) for x in npc_vehicles])
        
        pygame.quit()
        print("Simulation finished.")

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

작동 링크

current_location과 prev_location을 반복하며 초기화하여 주행한 영역 표시.
waypoint간 경로로 미리 경로를 지정하는 코드는 진행 중.

ego차량이 아닌 일반 actor들에 대해서는 bbox를 호출하여 그리는 형식으로 진행.

Carla 최단 경로 주행 관련 모듈 위치

Carla 제어기 관련 모듈 위치

GlobalRoutePlanner 클래스에서 주관하는 것으로 확인.

PID 제어 및 Simulator에서의 Actor 경로 설정

지정경로가 순환경로이긴 하나 한번의 waypoint 리스트를 경유하고 난 이후 리스트의 처음 번호를 시작 waypoint로 재설정하여 경로를 초기화하는 결과를 가지게 되어 문제가 되지 않는다.

distance_to_end = ego_vehicle.get_location().distance(last_waypoint_location)
if distance_to_end < 2.0:
    traffic_manager.set_path(ego_vehicle, route)