Adding LidarViz Carla and Test

carla-scripts/o3d-lidarViz.py

@@ -0,0 +1,114 @@
+import carla
+import numpy as np
+import open3d as o3d
+import time
+import queue
+import threading
+
+# Cola para almacenar los datos LiDAR
+lidar_queue = queue.Queue()
+
+# Función para procesar los datos LiDAR y almacenarlos en la cola
+def lidar_callback(lidar_data):
+    points = []
+    for point in lidar_data:
+        points.append([point.point.x, point.point.y, point.point.z])
+
+    if len(points) > 0:
+        print(f"Recibidos {len(points)} puntos LiDAR.")  # Imprime el número de puntos recibidos
+        points = np.array(points)
+        lidar_queue.put(points)  # Guardar los puntos en la cola
+    else:
+        print("Advertencia: No se han recibido puntos del LiDAR.")
+
+# Función para spawnear el vehículo y el sensor LiDAR
+def spawn_vehicle_and_lidar(world):
+    blueprint_library = world.get_blueprint_library()
+    vehicle_bp = blueprint_library.filter('vehicle.*')[0]
+    spawn_points = world.get_map().get_spawn_points()
+
+    vehicle = None
+    for spawn_point in spawn_points:
+        try:
+            vehicle = world.spawn_actor(vehicle_bp, spawn_point)
+            print(f"Vehículo creado en {spawn_point.location}")
+            vehicle.set_autopilot(True)  # Activar el piloto automático
+            break
+        except RuntimeError:
+            print(f"Fallo al crear vehículo en {spawn_point.location}, probando otro punto...")
+
+    if not vehicle:
+        raise RuntimeError("No se pudo spawnear el vehículo en ningún punto disponible")
+
+     # Configurar el sensor LiDAR
+    lidar_bp = blueprint_library.find('sensor.lidar.ray_cast')
+    lidar_bp.set_attribute('range', '100')
+    lidar_bp.set_attribute('lower_fov', '-20.0')
+    lidar_bp.set_attribute('channels', '64')
+    lidar_bp.set_attribute('rotation_frequency', str(1 / 0.05))  # 10 Hz
+    lidar_bp.set_attribute('points_per_second', '400000')  # 100,000 puntos por segundo
+
+    lidar_spawn_point = carla.Transform(carla.Location(x=0, z=1.8))  # Posición del LiDAR
+    lidar_sensor = world.spawn_actor(lidar_bp, lidar_spawn_point, attach_to=vehicle)
+    print("Sensor LiDAR creado y adjunto al vehículo")
+
+    return vehicle, lidar_sensor
+
+def main():
+    client = carla.Client('localhost', 2000)
+    client.set_timeout(10.0)
+    world = client.get_world()
+
+    # Crear el visualizador de Open3D
+    vis = o3d.visualization.Visualizer()
+    vis.create_window(window_name="Visualización de LiDAR", width=800, height=600)
+
+    # Crear una nube de puntos vacía para actualizarla con datos LiDAR
+    pcd = o3d.geometry.PointCloud()
+    vis.add_geometry(pcd)
+
+    render_options = vis.get_render_option()
+    render_options.point_size = 2  # Tamaño pequeño para los puntos
+
+    # Añadir ejes de referencia para visualizar el espacio
+    axes = o3d.geometry.TriangleMesh.create_coordinate_frame(size=3)
+    vis.add_geometry(axes)
+
+    # Configurar la cámara para vista de planta y zoom
+    ctr = vis.get_view_control()
+    ctr.set_zoom(15)  # Ajustar el zoom
+    ctr.set_front([0, 0, 1])  # Vista desde arriba (planta)
+    ctr.set_lookat([0, 0, 0])  # El punto de interés (centro de la escena)
+    ctr.set_up([0, -1, 0])  # Definir el eje "arriba" para la cámara
+
+    # Spawnear vehículo y añadir sensor LiDAR
+    vehicle, lidar_sensor = spawn_vehicle_and_lidar(world)
+
+    # Configurar el callback en un hilo separado para recibir datos LiDAR
+    lidar_thread = threading.Thread(target=lambda: lidar_sensor.listen(lidar_callback))
+    lidar_thread.start()
+
+    try:
+        print("Recolectando y visualizando datos LiDAR en vivo...")
+
+        while True:
+            # Procesar datos de la cola y actualizar la visualización
+            if not lidar_queue.empty():
+                points = lidar_queue.get()  # Obtener los datos LiDAR de la cola
+                pcd.points = o3d.utility.Vector3dVector(points)  # Actualizar la nube de puntos
+
+                # Forzar actualización de la geometría y visualización
+                vis.update_geometry(pcd)
+                vis.poll_events()
+                vis.update_renderer()
+
+            time.sleep(0.1)  # Pequeña pausa para evitar sobrecarga del bucle
+    except KeyboardInterrupt:
+        print("Interrumpido por el usuario. Deteniendo la simulación.")
+    finally:
+        lidar_sensor.stop()
+        vehicle.destroy()
+        vis.destroy_window()
+
+if __name__ == "__main__":
+    main()

carla-scripts/test.py

@@ -0,0 +1,159 @@
+import carla
+import numpy as np
+import open3d as o3d
+import time
+from datetime import datetime
+import sys
+import signal
+
+def lidar_callback(lidar_data, point_cloud):
+    '''
+    Procesa los datos brutos de carla 
+    cada vez que se toma una muestra (callback)
+    y actualiza la nube de puntos en el objeto PointCloud
+    '''
+    data = np.copy(np.frombuffer(lidar_data.raw_data, dtype=np.dtype('f4')))
+    data = np.reshape(data, (int(data.shape[0] / 4), 4))
+
+    # Convertir la nube de puntos al formato de Open3D
+    point_cloud.points = o3d.utility.Vector3dVector(data[:, :-1])
+
+    # Asegurar que todos los puntos sean negros
+    num_points = data.shape[0]
+    black_color = np.tile([0, 0, 0], (num_points, 1))  # Crear color negro para todos los puntos
+
+    # Asignar los colores negros a la nube de puntos
+    point_cloud.colors = o3d.utility.Vector3dVector(black_color)
+
+def spawn_vehicle_lidar(world, bp):
+    vehicle_00 = bp.filter('vehicle.*')[0]
+    spawn_point = world.get_map().get_spawn_points()[0]
+    vehicle = world.spawn_actor(vehicle_00, spawn_point)
+    vehicle.set_autopilot(True)
+
+    lidar_bp = bp.find('sensor.lidar.ray_cast')
+
+    lidar_bp.set_attribute('range', '100')
+    lidar_bp.set_attribute('rotation_frequency', '90')
+    lidar_bp.set_attribute('channels', '128')
+    lidar_bp.set_attribute('points_per_second', '1500000')
+
+    lidar_position = carla.Transform(carla.Location(x=0, z=1.8))
+    lidar = world.spawn_actor(lidar_bp, lidar_position, attach_to=vehicle)
+
+    return vehicle, lidar
+
+def add_open3d_axis(vis):
+    """
+    Añade un pequeño 3D axis en el Open3D Visualizer
+    """
+    axis = o3d.geometry.LineSet()
+    axis.points = o3d.utility.Vector3dVector(np.array([
+        [0.0, 0.0, 0.0],
+        [1.0, 0.0, 0.0],
+        [0.0, 1.0, 0.0],
+        [0.0, 0.0, 1.0]]))
+    axis.lines = o3d.utility.Vector2iVector(np.array([
+        [0, 1],
+        [0, 2],
+        [0, 3]]))
+    axis.colors = o3d.utility.Vector3dVector(np.array([
+        [1.0, 0.0, 0.0],
+        [0.0, 1.0, 0.0],
+        [0.0, 0.0, 1.0]]))
+    vis.add_geometry(axis)
+
+# Variables globales para almacenar actores
+actor_list = []
+
+def cleanup():
+    """
+    Elimina todos los actores de Carla (vehículos, sensores, etc.)
+    """
+    global actor_list
+    print("\nLimpiando actores...")
+    for actor in actor_list:
+        if actor is not None:
+            actor.destroy()
+    actor_list = []
+    print("Actores eliminados.")
+
+def signal_handler(sig, frame):
+    """
+    Captura la señal de interrupción (Ctrl+C) y limpia los actores antes de salir.
+    """
+    print("\nInterrupción recibida. Finalizando...")
+    cleanup()
+    sys.exit(0)
+
+def main():
+    '''
+    Funcion Main del programa
+    '''
+    client = carla.Client('localhost', 2000)
+    client.set_timeout(10.0)  # Tiempo límite para conectarse
+
+    world = client.get_world()
+    blueprint_library = world.get_blueprint_library()
+
+    # Global para evitar que los actores se eliminen automáticamente
+    global actor_list
+    vehicle, lidar = spawn_vehicle_lidar(world, blueprint_library)
+    actor_list.append(vehicle)
+    actor_list.append(lidar)
+
+    point_cloud = o3d.geometry.PointCloud()
+
+    lidar.listen(lambda data: lidar_callback(data, point_cloud))
+
+    viz = o3d.visualization.Visualizer()
+    viz.create_window(
+        window_name='Lidar simulado desde Carla'
+    )
+    viz.get_render_option().point_size = 1.3
+    viz.get_render_option().show_coordinate_frame = True
+
+    add_open3d_axis(viz)
+
+    frame = 0
+    dt0 = datetime.now()
+    lidar_data_received = False  # Verificar si se recibe data de LiDAR
+
+    while True:
+        if frame == 2 and not lidar_data_received:
+            # Añadir la nube de puntos solo después de recibir los datos
+            viz.add_geometry(point_cloud)
+            lidar_data_received = True  # Marca que hemos recibido datos
+            print("Geometry added to the visualizer")
+
+        # Actualizamos la geometría y nos aseguramos de que los puntos sigan siendo negros
+        viz.update_geometry(point_cloud)
+
+        viz.poll_events()
+        viz.update_renderer()
+
+        time.sleep(0.005)
+        world.tick()
+
+        # Calcular el tiempo de procesamiento para determinar los FPS
+        process_time = datetime.now() - dt0
+        if process_time.total_seconds() > 0:  # Evitar divisiones por cero
+            fps = 1.0 / process_time.total_seconds()
+            # Actualizar los FPS en la misma línea
+            sys.stdout.write(f'\rFPS: {fps:.2f} ')
+            sys.stdout.flush()
+        dt0 = datetime.now()
+        frame += 1
+
+        # Condición de salida para terminar de forma segura
+        if not viz.poll_events():
+            print("Exiting visualization")
+            break
+
+    cleanup()  # Asegurarse de limpiar los actores al salir del ciclo principal
+
+if __name__ == "__main__":
+    # Capturar señales de interrupción (Ctrl+C)
+    signal.signal(signal.SIGINT, signal_handler)
+
+    main()