diff --git a/docs/_posts/2024-10-15-Semana-5.md b/docs/_posts/2024-10-15-Semana-5.md new file mode 100644 index 0000000..8abdc8b --- /dev/null +++ b/docs/_posts/2024-10-15-Semana-5.md @@ -0,0 +1,168 @@ +--- +title: "Semana 5 - Visor 3D en tiempo real con Carla Y exploración de PCL y tipos de segmentación" +categories: + - Weblog +tags: + - Numpy + - Python + - LiDAR + - Visor-3D + - Open3D + - Carla + - PCL + - Segmentación +--- + +Esta semana me he dedicado a mejorar el visor para la visualización de muestras LiDAR en tiempo real provenientes de un sensor LiDAR simulado en Carla. Para ello he continuado desarrollando con **Open3D** para el manejo y visualización eficiente de las nubes de puntos. Tuve algunos problemas con la visualización ya que el renderizador colapsaba al pasarle las muestras directamente, pero conseguí implementar una solución sencilla para asegurar que los datos de entrada lleguen antes de la renderización. También realicé un buffer FIFO para para manejar correctamente la entrada de datos al renderizador, pero me pareció mas eficiente y sencilla la primera propuesta. + +Actualmente solo se trata de un script, la próxima semana me encargaré de empaquetar la primera versión del visor con todas las funcionalidades que he ido construyendo para las distintas fuentes de datos. + +## Visor 3D en tiempo real + +

+ +

+ + +## Código: Funciones Principales + +### 1. Función `lidar_callback()` + +Esta función procesa los datos LiDAR obtenidos desde el simulador Carla y los convierte en un objeto PointCloud de la libreria Open3D. Se procesan los datos de Carla con Numpy y se ordenan en un array bidimensional. Se le aplica un mapa de color de Matplotly. Es la funcion que se ejecutará asíncronamente cada vez que el LiDAR tome una muestra, modificando el valor de la nube de puntos a renderizar. + +```python +def lidar_callback(lidar_data, point_cloud): + ''' + Procesa los datos de Carla + cada vez que se toma una muestra con .listen (callback) + y actualiza la nube de puntos en el objeto PointCloud de open3D + ''' + data = np.copy(np.frombuffer(lidar_data.raw_data, dtype=np.dtype('f4'))) + data = np.reshape(data, (int(data.shape[0] / 4), 4)) + + intensity = data[:, -1] + int_color = np.c_[ + np.interp(intensity, VID_RANGE, VIRIDIS[:, 0]), + np.interp(intensity, VID_RANGE, VIRIDIS[:, 1]), + np.interp(intensity, VID_RANGE, VIRIDIS[:, 2])] + + # Convertir la nube de puntos al formato de Open3D + point_cloud.points = o3d.utility.Vector3dVector(data[:, :-1]) + point_cloud.colors = o3d.utility.Vector3dVector(int_color) +``` + +### 2. Función `spawn_vehicle_lidar()` + +Esta función recibe como parámetros el mundo importado del servidor de Carla, la `blueprint_library` para importar los actores disponibles, el `traffic_manager` para gestionar las opciones de tráfico y el parámetro temporal `delta` para sincronizar el visualizador con el simulador + +Se escoge un punto de spawn aleatorio dentro de los posibles, se crea un vehículo y se le adjunta un sensor LiDAR. El sensor LiDAR queda ajustado con los parametros adecuados para que la visualización sea precisa y coordinada. Para ello se ajusta el parámetro `rotation_frequency` en función de `delta`. De esta manera se consigue que la rotación esté sincronizada al tiempo de de simulación y se obtiene una visualización mucho más limpia. Finalmente se estable el modo de conducción automática al vehiculo + +```python +def spawn_vehicle_lidar(world, bp, traffic_manager, delta): + vehicle_bp = bp.filter('vehicle.*')[0] + spawn_points = world.get_map().get_spawn_points() + + # Elegir un punto de spawn aleatorio + spawn_point = random.choice(spawn_points) + + # Crear el vehículo en el punto aleatorio + vehicle = world.spawn_actor(vehicle_bp, spawn_point) + + lidar_bp = bp.find('sensor.lidar.ray_cast') + + lidar_bp.set_attribute('range', '100') + lidar_bp.set_attribute('rotation_frequency', str(1 / delta)) + lidar_bp.set_attribute('channels', '64') + lidar_bp.set_attribute('points_per_second', '500000') + + lidar_position = carla.Transform(carla.Location(x=-0.5, z=1.8)) + lidar = world.spawn_actor(lidar_bp, lidar_position, attach_to=vehicle) + + # Conectar el vehículo al Traffic Manager y activar el autopiloto + vehicle.set_autopilot(True, traffic_manager.get_port()) + + return vehicle, lidar +``` + +### 3. Programa principal `main()` + +- **Conexión a CARLA**: Establece la conexión con el servidor de CARLA y recupera el mundo y los blueprints +- **Configuración del Traffic Manager**: Configura el Traffic Manager para manejar el tráfico en modo sincrónico y establece una distancia de seguridad entre los vehículos +- **Configuración del mundo**: Establece el mundo en modo sincrónico y configura un valor de `delta` de 0.05 segundos para los pasos de simulación `ticks` +- **Creación de actores**: Llama a la función `spawn_vehicle_lidar()` para generar un vehículo y un sensor LiDAR, los cuales se añaden a la lista de actores. +- **Visualización de nube de puntos**: Inicializa una nube de puntos en Open3D y utiliza una función de retrollamada (callback) a través del método `lidar.listen()` para recibir y procesar continuamente los datos de LiDAR en tiempo real +- **Bucle de renderizado**: Entra en un bucle que actualiza la geometría de la nube de puntos en el visualizador y renderiza los datos hasta que se cierre la ventana. Espera 5 frames a comenzar a renderizar para que el renderizador no colapse +- **Finalización**: Al salir del bucle, limpia los actores creados para liberar los recursos + +```python +def main(): + # Conectarse al servidor de CARLA + client = carla.Client('localhost', 2000) + client.set_timeout(10.0) + + # Obtener el mundo y la biblioteca de planos + world = client.get_world() + blueprint_library = world.get_blueprint_library() + + # Configurar el Traffic Manager + traffic_manager = client.get_trafficmanager(8000) + traffic_manager.set_synchronous_mode(True) + traffic_manager.set_global_distance_to_leading_vehicle(2.5) + + # Configuración del mundo (modo sincrónico y delta fijo) + settings = world.get_settings() + delta = 0.05 + settings.fixed_delta_seconds = delta + settings.synchronous_mode = True + world.apply_settings(settings) + + # Crear el vehículo y LiDAR + global actor_list + vehicle, lidar = spawn_vehicle_lidar(world, blueprint_library, traffic_manager, delta) + actor_list.append(vehicle) + actor_list.append(lidar) + + # Inicializar la nube de puntos y escuchar datos LiDAR + point_cloud = o3d.geometry.PointCloud() + lidar.listen(lambda data: lidar_callback(data, point_cloud)) + + # Configurar el visualizador de Open3D + viz = o3d.visualization.Visualizer() + viz.create_window(window_name='Lidar simulado en Carla', 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 + + add_open3d_axis(viz) + + # Bucle principal: actualizar la geometría y renderizar la nube de puntos + frame = 0 + dt0 = datetime.now() + lidar_data_received = False + + while True: + if frame == 5 and not lidar_data_received: + viz.add_geometry(point_cloud) + lidar_data_received = True + + viz.update_geometry(point_cloud) + viz.poll_events() + viz.update_renderer() + time.sleep(0.005) + world.tick() + + # Calcular 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 no hay eventos + if not viz.poll_events(): + break + + cleanup() # Limpiar los actores al final +``` \ No newline at end of file