From f28529960821f9ff707dbe6a5dd9a845762c2253 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?F=C3=A9lix?= Date: Tue, 15 Oct 2024 02:43:02 +0200 Subject: [PATCH] Adding LidarViz Carla and Test --- carla-scripts/o3d-lidarViz.py | 114 ++++++++++++++++++++++++ carla-scripts/test.py | 159 ++++++++++++++++++++++++++++++++++ 2 files changed, 273 insertions(+) create mode 100644 carla-scripts/o3d-lidarViz.py create mode 100644 carla-scripts/test.py diff --git a/carla-scripts/o3d-lidarViz.py b/carla-scripts/o3d-lidarViz.py new file mode 100644 index 0000000..bc2d41d --- /dev/null +++ b/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() diff --git a/carla-scripts/test.py b/carla-scripts/test.py new file mode 100644 index 0000000..a61e12e --- /dev/null +++ b/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()