Brain y visualización de nubes de puntos LiDAR

Construcción del brain y visualización del nubes de puntos LiDAR

Esta semana hemos logrado arreglar los problemas de visualización del sensor LiDAR. Ya se puede visualizar exitosamente la nube de puntos a medida que avanza la simulación y se teleopera un vehículo. Una vez arreglados los problemas de visualización y estructurado el código, ya tenemos creado el cerebro.

Nuestro cerebro tiene las siguientes funciones:

• Control del vehículo.
• Generar y visualizar datos de un sensor LiDAR.
• Visualizar vista frontal del vehículo.

Control del vehículo

Esta función es la que establece el control manual del vehículo para poder teleoperarlo mediante teclado (teclas W,A,S,D):

def control(vehicle):

    control = carla.VehicleControl()

    # Leer teclas
    keys = pygame.key.get_pressed()

    # valores iniciales
    control.throttle = 0.0
    control.steer = 0.0
    control.brake = 0.0
    control.hand_brake = False
    control.reverse = False

    # W acelerar
    if keys[pygame.K_w]:
        control.throttle = 1.0

    # S retroceso/stop
    if keys[pygame.K_s]:
        if vehicle.get_velocity().length() < 0.1:
            # Si está casi parado: poner marcha atrás
            control.reverse = True
            control.throttle = 1.0
        else:
            # Si va hacia adelante: frenar
            control.brake = 1.0
    
    # A girar a la izquierda
    if keys[pygame.K_a]:
        control.steer = -0.5  # negativo = izquierda

    # D : girar a la derecha
    if keys[pygame.K_d]:
        control.steer = 0.5   # positivo = derecha

    # Aplicar al vehículo
    vehicle.apply_control(control)

Sensor LiDAR

Para la parte del sensor LiDAR, se aplican las funciones set_lidar, que añade el LiDAR semántico (etiqueta los puntos con la clase del objeto) al vehículo y lo ‘spawnea’ en el escenario del simulador, y lidar_callback, encargada de estructurar los datos y las coordenadas del sensor para poder visualizarlos. Además, se ha utilizado esta vez el sensor sensor.lidar.ray_cast_semantic, que además de devolver una nube de puntos, cada uno de estos tiene una etiqueta semántica (clase de objeto detectado).

A continuación, se muestran las funciones del sensor LiDAR:

def set_lidar(world, ego_vehicle, delta):

    # Crear LiDAR
    lidar_transform = carla.Transform(carla.Location(z=2.5))
    lidar_bp = world.get_blueprint_library().find('sensor.lidar.ray_cast_semantic')
    lidar_bp.set_attribute('channels', '128')        # Número de rayos
    lidar_bp.set_attribute('range', '100')           # Alcance en metros
    lidar_bp.set_attribute('rotation_frequency', str(1 / delta))  # Hz
    lidar_bp.set_attribute('points_per_second', '1200000') 

    # Spawnear sensor y aplicarlo al vehículo
    sensor_lidar = world.spawn_actor(lidar_bp, lidar_transform, attach_to=ego_vehicle)
    return sensor_lidar

# Función de callback del sensor LiDAR
def lidar_callback(lidar_data, point_cloud, vehicle_transform, absolute_view):

    # Definir el formato del paquete de datos del LiDAR semántico
    dtype = np.dtype([
        ('x', np.float32),
        ('y', np.float32),
        ('z', np.float32),
        ('cos_inc_angle', np.float32),
        ('object_idx', np.uint32),
        ('object_tag', np.uint32)
    ])

    # Convertir los datos binarios a un array de numpy con el dtype estructurado
    data = np.frombuffer(lidar_data.raw_data, dtype=dtype)

    # Extraer coordenadas de los puntos
    points = np.vstack((data['x'], data['y'], data['z'])).T

    # Reflejar los datos en el eje X (para ajustarlos al sistema CARLA)
    points[:, 0] = -points[:, 0]

    # Obtener etiquetas semánticas (object_tag)
    semantic_tags = data['object_tag']

    # Asignar color según la etiqueta semántica
    # Por ejemplo, con el colormap inferno o viridis
    normalized_tags = (semantic_tags - semantic_tags.min()) / (np.ptp(semantic_tags) + 1e-6)
    colors = VIRIDIS[(normalized_tags * (VIRIDIS.shape[0]-1)).astype(np.int32)]

    if absolute_view:
        vehicle_location = np.array([
            -vehicle_transform.location.x,
            vehicle_transform.location.y,
            vehicle_transform.location.z
        ])

        pitch = vehicle_transform.rotation.pitch
        yaw = vehicle_transform.rotation.yaw
        roll = vehicle_transform.rotation.roll

        rotation_matrix = euler_to_rotation_matrix(pitch, yaw, roll).T

        # Transformar los puntos al sistema global
        points = (rotation_matrix @ points.T).T + vehicle_location

    # Actualizar el point cloud en Open3D
    point_cloud.points = o3d.utility.Vector3dVector(points)
    point_cloud.colors = o3d.utility.Vector3dVector(colors)

Cámara frontal

Para establecer una cámara RGB en el vehículo y poder así visualizar el recorrido de este mientras se teleopera, se han añadido las siguiente funciones:

# Función para establecer cámara en el vehículo
def set_camera(world, vehicle):
    
    camera_bp = world.get_blueprint_library().find('sensor.camera.rgb')
    camera_bp.set_attribute('image_size_x', '800')
    camera_bp.set_attribute('image_size_y', '600')
    camera_bp.set_attribute('fov', '90')
    camera_transform = carla.Transform(
        carla.Location(x=1.5, z=1.2),       # 1.5 m adelante, 1.2 m de altura
        carla.Rotation(pitch=0.0, yaw=0.0, roll=0.0))  # mirando hacia adelante
    
    camera = world.spawn_actor(camera_bp, camera_transform, attach_to=vehicle)
    return camera

Nota: la visión que muestra la cámara es la frontal, es decir, la que se vería si se colocara la cámara en el capó del coche:

# Función para procesar imágenes de la cámara
def camera_callback(image, display):
    array = np.frombuffer(image.raw_data, dtype=np.uint8)
    array = np.reshape(array, (image.height, image.width, 4))
    array = array[:, :, :3]  # Quitar el canal alfa
    array = array[:, :, ::-1]  # Convertir de BGRA a RGB
    surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
    display.blit(surface, (0, 0))
    # Actualizar solo esta superficie en vez de toda la pantalla
    pygame.display.update(display.get_rect())

Función principal: brain()

La función brain() es la que se encarga de conectar con el servidor, configurar y añadir todos los agente al escenario del simulador (‘Town05’, en este caso), procesa eventos del simulador, actualiza la nube de puntos y, además, maneja las distintas vistas en la visualización del sensor LiDAR. A continuación, se muestra el código del cerebro:

# Cerebro / Función principal
def brain():

    pygame.init()

    # Configurar Pygame sin usar OpenGL explícitamente
    pygame.display.gl_set_attribute(pygame.GL_ACCELERATED_VISUAL, 0)
    width, height = 800, 600
    screen = pygame.display.set_mode((width, height), pygame.SRCALPHA)
    pygame.display.set_caption("CARLA Vehículo Control")

    # Conexión con servidor de Carla
    client = carla.Client('localhost', 2000)
    client.set_timeout(10.0)
    client.load_world('Town05') #Establecer escenario determinado

    # Crear escenario
    world = client.get_world()

    # Librería del simulador
    blueprint_library = world.get_blueprint_library()
    traffic_manager = client.get_trafficmanager(8000)               # Control del tráfico de vehículos automáticos
    traffic_manager.set_synchronous_mode(True)                      # Modo síncrono: avanza en cada "world.tick()"
    traffic_manager.set_global_distance_to_leading_vehicle(2.5)     # Distancia de seguridad entre vehículos

    # Configuración del simulador
    settings = world.get_settings()                # Obtener configuración actual              
    delta = 0.033                                   # 0.05 s por cada tick: 20 FPS
    settings.fixed_delta_seconds = delta           # la simulación avanza cada 0.05 s
    settings.synchronous_mode = True               # Activa el modo síncrono
    world.apply_settings(settings)                 # Aplica los ajustes modificados

    
    # Librería de vehículos
    vehicle_blueprints = blueprint_library.filter('*vehicle*')

    # Cogemos todos los puntos de aparición predefinidos del escenario
    spawn_points = world.get_map().get_spawn_points()

    # Seleccionamos un vehículo determinado de la librería
    vehicle_bp = vehicle_blueprints.find('vehicle.audi.tt')

    # Generar vehículo con el que se va a trabajar
    ego_vehicle = world.spawn_actor(vehicle_bp, spawn_points[0])
    if ego_vehicle is not None:
        print('Vehículo spawneado correctamente')

    # Establecer sensor LiDAR al vehículo
    lidar = set_lidar(world, ego_vehicle, delta)

    # Establecer cámara rgb al vehículo
    camera = set_camera(world, ego_vehicle)

    global actor_list, current_view_index, origin_sphere_added, absolute_view

    actor_list.append(ego_vehicle)
    actor_list.append(camera)
    actor_list.append(lidar)

    camera.listen(lambda image: camera_callback(image, screen))

    # Crear nube de puntos y llamar al callback
    point_cloud = o3d.geometry.PointCloud()
    lidar.listen(lambda data: lidar_callback(data, point_cloud, lidar.get_transform(), absolute_view))

    # Visualizador de Open3D y configuraciones iniciales
    viz = o3d.visualization.VisualizerWithKeyCallback()
    viz.create_window(window_name='Sensor LiDAR', width=960, height=540, left=480, top=270)
    viz.get_render_option().background_color = [0.05, 0.05, 0.05]
    viz.get_render_option().point_size = 1.35
    viz.get_render_option().show_coordinate_frame = True

    # Configurar vista
    set_camera_view(viz,current_view_index)

    #Inicializar efera en el origen
    origin_sphere = create_origin_sphere()
    origin_sphere_added = False

    def toggle_camera_view(_):
        global current_view_index, absolute_view, origin_sphere_added
        # Alternar a la siguiente vista en el array de cámaras
        current_view_index = (current_view_index + 1) % len(camera_views)
        set_camera_view(viz, current_view_index)
        
        # Activar o desactivar vista absoluta y la esfera en el origen
        absolute_view = (current_view_index == len(camera_views) - 1)  # Vista cenital como absoluta
        if absolute_view and not origin_sphere_added:
            viz.add_geometry(origin_sphere)
            origin_sphere_added = True
        elif not absolute_view and origin_sphere_added:
            viz.remove_geometry(origin_sphere)
            origin_sphere_added = False
        
        print(f"Cambiando a vista {current_view_index + 1}")
        return True

    # Callback para alternar vistas
    viz.register_key_callback(ord("V"), toggle_camera_view)

    # Ciclo principal de visualización
    lidar_data_received = False
    dt0 = datetime.now()
    frame = 0
    try:
        while True:

            # Procesar control del vehículo
            control(ego_vehicle)

            # Procesar eventos de Pygame
            for event in pygame.event.get():
                if event.type == pygame.QUIT:
                    print("\n Cierre de ventana detectado.")
                    raise KeyboardInterrupt

            # Añadir nube de puntos al visualizador una sola vez
            if frame == 5 and not lidar_data_received:
                viz.add_geometry(point_cloud)
                lidar_data_received = True
                print("Nube de puntos añadida a Open3D")

            # Actualizar visualización
            viz.update_geometry(point_cloud)    # actualizar nube de puntos
            viz.poll_events()                   # procesar eventos del sistema de ventanas
            viz.update_renderer()               # actualizar el contenido de la ventana
            time.sleep(0.03)
            world.tick()                        # avanzar la simulación


            # Mostrar FPS
            process_time = datetime.now() - dt0
            if process_time.total_seconds() > 0:
                fps = 1.0 / process_time.total_seconds()
                sys.stdout.write(f'\rFPS: {fps:.2f} ')
                sys.stdout.flush()
            dt0 = datetime.now()
            frame += 1

            # Salir si Open3D se cierra
            if not viz.poll_events():
                break

    except KeyboardInterrupt:
        pass
    finally:
        cleanup()

Resultado de la visualización del LiDAR

Al ejecutar el código anterior junto con el simulador, aparecen las siguiente ventanas de simulación: cámara frontal del vehículo generado (izquierda) y la visualización del sensor LiDAR.

[Futuros avances]

Tras haber creado ya brain(), el siguiente paso será el de procesar los datos del LiDAR para entrenar un modelo que en función de estos datos, sea capaz de darnos una salida que sea el comportamiento que deben de seguir los actuadores del vehículo (throttle, steer, brake).

El modelo que prevemos utilizar, será de Imitation Learning (aprendizaje por imitación). A priori, tendremos que utilizar un dataset con suficientes muestras de conducción experta, para que el modelo las aprenda, o bien generaremos muestras estableciendo el autopilot al vehículo en Carla. Un posible modelo a implementar sería PilotNet (de NVIDIA), que es muy utilizado para conducción autónoma a partir de imágenes (desconozco por el momento si también se ha entrenado con nubes de puntos). Las siguientes semanas seguiremos avanzando por esta línea.