15from ament_index_python
import get_package_share_directory
17from launch.actions
import Shutdown
18from launch
import LaunchDescription
19from launch_ros.actions
import Node
20from launch.actions
import IncludeLaunchDescription
21from launch.launch_description_sources
import PythonLaunchDescriptionSource
22from launch.substitutions
import ThisLaunchFileDir
23from launch.substitutions
import EnvironmentVariable
24from launch.actions
import GroupAction
25from launch.conditions
import IfCondition
26from launch_ros.actions
import PushRosNamespace
27from carma_ros2_utils.launch.get_log_level
import GetLogLevel
28from launch.substitutions
import LaunchConfiguration, PythonExpression
29from launch.actions
import DeclareLaunchArgument
30from launch.actions
import OpaqueFunction
32from tracetools_launch.action
import Trace
34from datetime
import datetime
38from launch
import LaunchDescription, LaunchContext
42 Opaque Function for generating a
'Trace' ROS 2 launch action, which
is dependent on the
43 'ROS_LOG_DIR' EnvironmentVariable
and the
'is_ros2_tracing_enabled' LaunchConfiguration.
45 NOTE: This Opaque Function
is required
in order to evaluate the
'ROS_LOG_DIR' environment
48 log_directory_string = launch.substitutions.EnvironmentVariable('ROS_LOG_DIR').perform(context)
51 condition=IfCondition(LaunchConfiguration(
'is_ros2_tracing_enabled')),
54 base_path = log_directory_string,
55 session_name=
'my-tracing-session-' +
str(datetime.now().strftime(
'%Y-%m-%d_%H%M%S')),
68 system_controller_param_file = os.path.join(
69 get_package_share_directory('system_controller'),
'config/config.yaml')
71 env_log_levels = EnvironmentVariable(
'CARMA_ROS_LOGGING_CONFIG', default_value=
'{ "default_level" : "WARN" }')
74 vehicle_calibration_dir = LaunchConfiguration(
'vehicle_calibration_dir')
75 declare_vehicle_calibration_dir_arg = DeclareLaunchArgument(
76 name =
'vehicle_calibration_dir',
77 default_value =
"/opt/carma/vehicle/calibration",
78 description =
"Path to folder containing vehicle calibration directories"
81 vehicle_config_dir = LaunchConfiguration(
'vehicle_config_dir')
82 declare_vehicle_config_dir_arg = DeclareLaunchArgument(
83 name =
'vehicle_config_dir',
84 default_value =
"/opt/carma/vehicle/config",
85 description =
"Path to vehicle configuration directory populated by carma-config"
88 vehicle_characteristics_param_file = LaunchConfiguration(
'vehicle_characteristics_param_file')
89 declare_vehicle_characteristics_param_file_arg = DeclareLaunchArgument(
90 name =
'vehicle_characteristics_param_file',
91 default_value = [vehicle_calibration_dir,
"/identifiers/UniqueVehicleParams.yaml"],
92 description =
"Path to file containing unique vehicle characteristics"
96 vehicle_config_param_file = LaunchConfiguration(
'vehicle_config_param_file')
97 declare_vehicle_config_param_file_arg = DeclareLaunchArgument(
98 name =
'vehicle_config_param_file',
99 default_value = [vehicle_config_dir,
"/VehicleConfigParams.yaml"],
100 description =
"Path to file contain vehicle configuration parameters"
105 global_params_override_file = LaunchConfiguration(
'global_params_override_file')
106 declare_global_params_override_file_arg = DeclareLaunchArgument(
107 name =
'global_params_override_file',
108 default_value = [vehicle_config_dir,
"/GlobalParamsOverride.yaml"],
109 description =
"Path to global file containing the parameters overwrite"
112 use_sim_time = LaunchConfiguration(
'use_sim_time')
113 declare_use_sim_time_arg = DeclareLaunchArgument(
114 name =
'use_sim_time',
115 default_value =
'False',
116 description =
"True of simulation mode is on"
119 use_real_time_spat_in_sim = LaunchConfiguration(
'use_real_time_spat_in_sim')
120 declare_use_real_time_spat_in_sim_arg = DeclareLaunchArgument(
121 name =
'use_real_time_spat_in_sim',
122 default_value =
'False',
123 description =
"True if SPaT is being published based on wall clock"
127 route_file_folder = LaunchConfiguration(
'route_file_folder')
128 declare_route_file_folder = DeclareLaunchArgument(
129 name =
'route_file_folder',
130 default_value=
'/opt/carma/routes/',
131 description =
'Path of folder containing routes to load'
135 enable_guidance_plugin_validator = LaunchConfiguration(
'enable_guidance_plugin_validator')
136 declare_enable_guidance_plugin_validator = DeclareLaunchArgument(
137 name =
'enable_guidance_plugin_validator',
138 default_value=
'false',
139 description=
'Flag indicating whether the Guidance Plugin Validator node will actively validate guidance strategic, tactical, and control plugins'
143 strategic_plugins_to_validate = LaunchConfiguration(
'strategic_plugins_to_validate')
144 declare_strategic_plugins_to_validate = DeclareLaunchArgument(
145 name =
'strategic_plugins_to_validate',
146 default_value =
'[]',
147 description =
'List of String: Guidance Strategic Plugins that will be validated by the Guidance Plugin Validator Node if enabled'
151 tactical_plugins_to_validate = LaunchConfiguration(
'tactical_plugins_to_validate')
152 declare_tactical_plugins_to_validate = DeclareLaunchArgument(
153 name =
'tactical_plugins_to_validate',
155 description=
'List of String: Guidance Tactical Plugins that will be validated by the Guidance Plugin Validator Node if enabled'
159 control_plugins_to_validate = LaunchConfiguration(
'control_plugins_to_validate')
160 declare_control_plugins_to_validate = DeclareLaunchArgument(
161 name =
'control_plugins_to_validate',
163 description=
'List of String: Guidance Control Plugins that will be validated by the Guidance Plugin Validator Node if enabled'
167 enable_opening_tunnels = LaunchConfiguration(
'enable_opening_tunnels')
168 declare_enable_opening_tunnels = DeclareLaunchArgument(
169 name =
'enable_opening_tunnels',
170 default_value=
'False',
171 description=
'Flag to enable opening http tunnesl to CARMA Cloud'
175 port = LaunchConfiguration(
'port')
176 declare_port = DeclareLaunchArgument(
178 default_value=
"9090",
179 description=
'The default port for rosbridge is 909'
183 load_type = LaunchConfiguration(
'load_type')
184 declare_load_type= DeclareLaunchArgument(name =
'load_type', default_value =
"noupdate")
186 single_pcd_path = LaunchConfiguration(
'single_pcd_path')
187 declare_single_pcd_path = DeclareLaunchArgument(name=
'single_pcd_path', default_value=
"['/opt/carma/maps/pcd_map.pcd']")
189 area = LaunchConfiguration(
'area')
190 declare_area = DeclareLaunchArgument(name=
'area', default_value=
"1x1")
192 arealist_path = LaunchConfiguration(
'arealist_path')
193 declare_arealist_path = DeclareLaunchArgument(name=
'arealist_path', default_value=
"/opt/carma/maps/arealist.txt")
195 vector_map_file = LaunchConfiguration(
'vector_map_file')
196 declare_vector_map_file = DeclareLaunchArgument(name=
'vector_map_file', default_value=
'/opt/carma/maps/vector_map.osm')
198 is_ros2_tracing_enabled = LaunchConfiguration(
'is_ros2_tracing_enabled')
199 declare_is_ros2_tracing_enabled = DeclareLaunchArgument(
200 name=
'is_ros2_tracing_enabled',
201 default_value =
'False',
202 description =
'True if user wants ROS 2 Tracing logs to be generated from CARMA Platform.')
206 is_cp_mot_enabled = LaunchConfiguration(
'is_cp_mot_enabled')
207 declare_is_cp_mot_enabled = DeclareLaunchArgument(
208 name=
'is_cp_mot_enabled',
209 default_value =
'False',
210 description =
'True if user wants Cooperative Perception capability using Multiple Object Tracking to be enabled'
215 is_autoware_lidar_obj_detection_enabled = LaunchConfiguration(
'is_autoware_lidar_obj_detection_enabled')
216 declare_is_autoware_lidar_obj_detection_enabled = DeclareLaunchArgument(
217 name=
'is_autoware_lidar_obj_detection_enabled',
218 default_value =
'False',
219 description =
'True if user wants Autoware Lidar Object Detection to be enabled'
223 ros2_rosbag_launch = GroupAction(
225 IncludeLaunchDescription(
226 PythonLaunchDescriptionSource([ThisLaunchFileDir(),
'/ros2_rosbag.launch.py']),
228 'vehicle_config_dir' : vehicle_config_dir,
229 'vehicle_config_param_file' : vehicle_config_param_file
237 transform_group = GroupAction(
239 PushRosNamespace(EnvironmentVariable(
'CARMA_TF_NS', default_value=
'/')),
240 IncludeLaunchDescription(
241 PythonLaunchDescriptionSource([ThisLaunchFileDir(),
'/transforms.launch.py']),
243 'vehicle_config_param_file' : vehicle_config_param_file,
244 'use_sim_time' : use_sim_time
250 environment_group = GroupAction(
252 PushRosNamespace(EnvironmentVariable(
'CARMA_ENV_NS', default_value=
'environment')),
253 IncludeLaunchDescription(
254 PythonLaunchDescriptionSource([ThisLaunchFileDir(),
'/environment.launch.py']),
256 'subsystem_controller_param_file' : [vehicle_config_dir,
'/SubsystemControllerParams.yaml'],
257 'global_params_override_file' : global_params_override_file,
258 'vehicle_config_param_file' : vehicle_config_param_file,
259 'vehicle_calibration_dir': vehicle_calibration_dir,
260 'vehicle_characteristics_param_file' : vehicle_characteristics_param_file,
261 'vector_map_file' : vector_map_file,
262 'use_sim_time' : use_sim_time,
263 'is_cp_mot_enabled' : is_cp_mot_enabled,
264 'is_autoware_lidar_obj_detection_enabled' : is_autoware_lidar_obj_detection_enabled
270 localization_group = GroupAction(
272 PushRosNamespace(EnvironmentVariable(
'CARMA_LOCZ_NS', default_value=
'localization')),
273 IncludeLaunchDescription(
274 PythonLaunchDescriptionSource([ThisLaunchFileDir(),
'/localization.launch.py']),
276 'vehicle_config_param_file' : vehicle_config_param_file,
277 'global_params_override_file' : global_params_override_file,
278 'subsystem_controller_param_file' : [vehicle_config_dir,
'/SubsystemControllerParams.yaml'],
279 'load_type' : load_type,
280 'single_pcd_path' : single_pcd_path,
282 'arealist_path' : arealist_path,
283 'vector_map_file' : vector_map_file,
284 'vehicle_calibration_dir': vehicle_calibration_dir,
285 'use_sim_time' : use_sim_time
291 v2x_group = GroupAction(
293 PushRosNamespace(EnvironmentVariable(
'CARMA_MSG_NS', default_value=
'message')),
294 IncludeLaunchDescription(
295 PythonLaunchDescriptionSource([ThisLaunchFileDir(),
'/message.launch.py']),
297 'vehicle_characteristics_param_file' : vehicle_characteristics_param_file,
298 'global_params_override_file' : global_params_override_file,
299 'vehicle_config_param_file' : vehicle_config_param_file,
300 'enable_opening_tunnels' : enable_opening_tunnels,
301 'subsystem_controller_param_file' : [vehicle_config_dir,
'/SubsystemControllerParams.yaml'],
302 'use_sim_time' : use_sim_time
308 guidance_group = GroupAction(
310 PushRosNamespace(EnvironmentVariable(
'CARMA_GUIDE_NS', default_value=
'guidance')),
312 IncludeLaunchDescription(
313 PythonLaunchDescriptionSource([ThisLaunchFileDir(),
'/guidance.launch.py']),
315 'route_file_folder' : route_file_folder,
316 'global_params_override_file' : global_params_override_file,
317 'vehicle_characteristics_param_file' : vehicle_characteristics_param_file,
318 'vehicle_config_param_file' : vehicle_config_param_file,
319 'enable_guidance_plugin_validator' : enable_guidance_plugin_validator,
320 'strategic_plugins_to_validate' : strategic_plugins_to_validate,
321 'tactical_plugins_to_validate' : tactical_plugins_to_validate,
322 'control_plugins_to_validate' : control_plugins_to_validate,
323 'subsystem_controller_param_file' : [vehicle_config_dir,
'/SubsystemControllerParams.yaml'],
324 'use_sim_time' : use_sim_time,
325 'use_real_time_spat_in_sim' : use_real_time_spat_in_sim
331 drivers_group = GroupAction(
333 PushRosNamespace(EnvironmentVariable(
'CARMA_INTR_NS', default_value=
'hardware_interface')),
334 IncludeLaunchDescription(
335 PythonLaunchDescriptionSource([ThisLaunchFileDir(),
'/drivers.launch.py']),
337 'subsystem_controller_param_file' : [vehicle_config_dir,
'/SubsystemControllerParams.yaml'],
338 'vehicle_config_param_file' : vehicle_config_param_file,
339 'global_params_override_file' : global_params_override_file,
340 'use_sim_time' : use_sim_time
346 system_controller = Node(
347 package=
'system_controller',
348 name=
'system_controller',
349 executable=
'system_controller',
351 system_controller_param_file,
352 {
"use_sim_time" : use_sim_time},
353 global_params_override_file],
354 on_exit = Shutdown(),
355 arguments=[
'--ros-args',
'--log-level', GetLogLevel(
'system_controller', env_log_levels)]
358 ui_group = GroupAction(
360 PushRosNamespace(EnvironmentVariable(
'CARMA_UI_NS', default_value=
'ui')),
362 IncludeLaunchDescription(
363 PythonLaunchDescriptionSource([ThisLaunchFileDir(),
'/ui.launch.py']),
371 return LaunchDescription([
372 declare_vehicle_calibration_dir_arg,
373 declare_vehicle_config_dir_arg,
374 declare_vehicle_characteristics_param_file_arg,
375 declare_vehicle_config_param_file_arg,
376 declare_use_sim_time_arg,
377 declare_use_real_time_spat_in_sim_arg,
378 declare_route_file_folder,
379 declare_enable_guidance_plugin_validator,
380 declare_strategic_plugins_to_validate,
381 declare_tactical_plugins_to_validate,
382 declare_control_plugins_to_validate,
383 declare_enable_opening_tunnels,
386 declare_single_pcd_path,
388 declare_arealist_path,
389 declare_vector_map_file,
390 declare_is_ros2_tracing_enabled,
391 declare_is_cp_mot_enabled,
392 declare_is_autoware_lidar_obj_detection_enabled,
393 declare_global_params_override_file_arg,
395 OpaqueFunction(function=create_ros2_tracing_action),
def create_ros2_tracing_action(context, *args, **kwargs)
def generate_launch_description()