|
| 1 | +#!/usr/bin/env python3 |
| 2 | +from launch import LaunchDescription |
| 3 | +from launch.actions import DeclareLaunchArgument |
| 4 | +from launch.substitutions import LaunchConfiguration |
| 5 | +from launch_ros.actions import Node |
| 6 | + |
| 7 | +def generate_launch_description(): |
| 8 | + # Declarar argumentos equivalentes a los <arg> del XML |
| 9 | + host_arg = DeclareLaunchArgument( |
| 10 | + 'host', |
| 11 | + default_value='localhost', |
| 12 | + description='IP o nombre de host de CARLA' |
| 13 | + ) |
| 14 | + port_arg = DeclareLaunchArgument( |
| 15 | + 'port', |
| 16 | + default_value='2000', |
| 17 | + description='Puerto de CARLA' |
| 18 | + ) |
| 19 | + timeout_arg = DeclareLaunchArgument( |
| 20 | + 'timeout', |
| 21 | + default_value='10', |
| 22 | + description='Timeout (segundos) de conexión' |
| 23 | + ) |
| 24 | + town_arg = DeclareLaunchArgument( |
| 25 | + 'town', |
| 26 | + default_value='Town01', # o puedes dejarlo vacío si quieres |
| 27 | + description='Mapa/town que va a cargar CARLA' |
| 28 | + ) |
| 29 | + spawn_point_arg = DeclareLaunchArgument( |
| 30 | + 'spawn_point', |
| 31 | + default_value='0', |
| 32 | + description='Spawn point para ego_vehicle' |
| 33 | + ) |
| 34 | + passive_arg = DeclareLaunchArgument( |
| 35 | + 'passive', |
| 36 | + default_value='', |
| 37 | + description='Activar modo pasivo (true/false). Por defecto vacío' |
| 38 | + ) |
| 39 | + synchronous_mode_arg = DeclareLaunchArgument( |
| 40 | + 'synchronous_mode', |
| 41 | + default_value='True', |
| 42 | + description='Habilitar modo síncrono en CARLA' |
| 43 | + ) |
| 44 | + synchronous_wait_arg = DeclareLaunchArgument( |
| 45 | + 'synchronous_mode_wait_for_vehicle_control_command', |
| 46 | + default_value='False', |
| 47 | + description='Esperar comandos de control de vehículo en modo síncrono' |
| 48 | + ) |
| 49 | + fixed_delta_arg = DeclareLaunchArgument( |
| 50 | + 'fixed_delta_seconds', |
| 51 | + default_value='0.05', |
| 52 | + description='Delta de tiempo fijo en modo síncrono' |
| 53 | + ) |
| 54 | + objects_def_file_arg = DeclareLaunchArgument( |
| 55 | + 'objects_definition_file', |
| 56 | + default_value='$(env OBJECT_PATH)/main_car_custom_camera.json', |
| 57 | + description='JSON con la definición de objetos (vehículos, sensores, etc.)' |
| 58 | + ) |
| 59 | + ego_vehicle_role_arg = DeclareLaunchArgument( |
| 60 | + 'ego_vehicle_role_name', |
| 61 | + default_value='ego_vehicle', |
| 62 | + description='Rol del vehículo ego' |
| 63 | + ) |
| 64 | + |
| 65 | + # Nodo para el bridge de ROS con CARLA |
| 66 | + carla_ros_bridge_node = Node( |
| 67 | + package='carla_ros_bridge', |
| 68 | + executable='bridge.py', |
| 69 | + name='carla_ros_bridge', |
| 70 | + output='screen', |
| 71 | + parameters=[{ |
| 72 | + 'host': LaunchConfiguration('host'), |
| 73 | + 'port': LaunchConfiguration('port'), |
| 74 | + 'timeout': LaunchConfiguration('timeout'), |
| 75 | + 'passive': LaunchConfiguration('passive'), |
| 76 | + 'synchronous_mode': LaunchConfiguration('synchronous_mode'), |
| 77 | + 'synchronous_mode_wait_for_vehicle_control_command': LaunchConfiguration('synchronous_mode_wait_for_vehicle_control_command'), |
| 78 | + 'fixed_delta_seconds': LaunchConfiguration('fixed_delta_seconds'), |
| 79 | + 'register_all_sensors': True, |
| 80 | + 'town': LaunchConfiguration('town'), |
| 81 | + 'ego_vehicle_role_name': LaunchConfiguration('ego_vehicle_role_name') |
| 82 | + }] |
| 83 | + ) |
| 84 | + |
| 85 | + # Nodo para spawnear los objetos (vehículos, sensores, etc.) |
| 86 | + spawn_objects_node = Node( |
| 87 | + package='carla_spawn_objects', |
| 88 | + executable='carla_spawn_objects.py', |
| 89 | + name='carla_spawn_objects', |
| 90 | + output='screen', |
| 91 | + parameters=[{ |
| 92 | + 'objects_definition_file': LaunchConfiguration('objects_definition_file'), |
| 93 | + # Observa cómo el "spawn_point_$(arg ego_vehicle_role_name)" del XML |
| 94 | + # se “mapea” ahora a un diccionario en Python. Normalmente, si |
| 95 | + # carla_spawn_objects.py procesa un parámetro llamado |
| 96 | + # "spawn_point_ego_vehicle", lo indicamos así: |
| 97 | + 'spawn_point_ego_vehicle': LaunchConfiguration('spawn_point'), |
| 98 | + 'spawn_sensors_only': False |
| 99 | + }] |
| 100 | + ) |
| 101 | + |
| 102 | + # Nodo para el control manual (carla_manual_control.launch se traduce a un Node) |
| 103 | + # Si el package carla_manual_control trae un launch.py, puedes hacer un IncludeLaunchDescription. |
| 104 | + # Si es un ejecutable Python, lo llamas con Node. Aquí asumimos que es un script: |
| 105 | + manual_control_node = Node( |
| 106 | + package='carla_manual_control', |
| 107 | + executable='carla_manual_control', |
| 108 | + name='carla_manual_control', |
| 109 | + output='screen', |
| 110 | + # El script seguramente necesitará un argumento 'role_name' (equivalente a la arg del XML). |
| 111 | + # Muchos scripts en carla_manual_control usan un param o un arg 'role_name' o 'rolename': |
| 112 | + parameters=[{ |
| 113 | + 'role_name': LaunchConfiguration('ego_vehicle_role_name') |
| 114 | + }] |
| 115 | + ) |
| 116 | + |
| 117 | + # Juntamos todo en el LaunchDescription |
| 118 | + return LaunchDescription([ |
| 119 | + # Primero declaramos todos los argumentos |
| 120 | + host_arg, |
| 121 | + port_arg, |
| 122 | + timeout_arg, |
| 123 | + town_arg, |
| 124 | + spawn_point_arg, |
| 125 | + passive_arg, |
| 126 | + synchronous_mode_arg, |
| 127 | + synchronous_wait_arg, |
| 128 | + fixed_delta_arg, |
| 129 | + objects_def_file_arg, |
| 130 | + ego_vehicle_role_arg, |
| 131 | + # Luego añadimos los nodos |
| 132 | + carla_ros_bridge_node, |
| 133 | + spawn_objects_node, |
| 134 | + manual_control_node |
| 135 | + ]) |
0 commit comments