37
38 route_file_folder = LaunchConfiguration('route_file_folder')
39 vehicle_calibration_dir = LaunchConfiguration('vehicle_calibration_dir')
40 vehicle_characteristics_param_file = LaunchConfiguration('vehicle_characteristics_param_file')
41 enable_guidance_plugin_validator = LaunchConfiguration('enable_guidance_plugin_validator')
42 strategic_plugins_to_validate = LaunchConfiguration('strategic_plugins_to_validate')
43 tactical_plugins_to_validate = LaunchConfiguration('tactical_plugins_to_validate')
44 control_plugins_to_validate = LaunchConfiguration('control_plugins_to_validate')
45
46 vehicle_config_param_file = LaunchConfiguration('vehicle_config_param_file')
47
48 vehicle_config_dir = LaunchConfiguration('vehicle_config_dir')
49 declare_vehicle_config_dir_arg = DeclareLaunchArgument(
50 name = 'vehicle_config_dir',
51 default_value = "/opt/carma/vehicle/config",
52 description = "Path to vehicle configuration directory populated by carma-config"
53 )
54
55
56
57 global_params_override_file = LaunchConfiguration('global_params_override_file')
58 declare_global_params_override_file_arg = DeclareLaunchArgument(
59 name = 'global_params_override_file',
60 default_value = [vehicle_config_dir, "/GlobalParamsOverride.yaml"],
61 description = "Path to global file containing the parameters overwrite"
62 )
63
64 inlanecruising_plugin_file_path = os.path.join(
65 get_package_share_directory('inlanecruising_plugin'), 'config/parameters.yaml')
66
67 route_following_plugin_file_path = os.path.join(
68 get_package_share_directory('route_following_plugin'), 'config/parameters.yaml')
69
70 stop_and_wait_plugin_param_file = os.path.join(
71 get_package_share_directory('stop_and_wait_plugin'), 'config/parameters.yaml')
72
73 light_controlled_intersection_tactical_plugin_param_file = os.path.join(
74 get_package_share_directory('light_controlled_intersection_tactical_plugin'), 'config/parameters.yaml')
75
76 cooperative_lanechange_param_file = os.path.join(
77 get_package_share_directory('cooperative_lanechange'), 'config/parameters.yaml')
78
79 platooning_strategic_ihp_param_file = os.path.join(
80 get_package_share_directory('platooning_strategic_ihp'), 'config/parameters.yaml')
81
82 sci_strategic_plugin_file_path = os.path.join(
83 get_package_share_directory('sci_strategic_plugin'), 'config/parameters.yaml')
84
85 lci_strategic_plugin_file_path = os.path.join(
86 get_package_share_directory('lci_strategic_plugin'), 'config/parameters.yaml')
87
88 stop_and_dwell_strategic_plugin_container_file_path = os.path.join(
89 get_package_share_directory('stop_and_dwell_strategic_plugin'), 'config/parameters.yaml')
90
91 yield_plugin_file_path = os.path.join(
92 get_package_share_directory('yield_plugin'), 'config/parameters.yaml')
93
94 platoon_tactical_ihp_param_file = os.path.join(
95 get_package_share_directory('platooning_tactical_plugin'), 'config/parameters.yaml')
96
97 approaching_emergency_vehicle_plugin_param_file = os.path.join(
98 get_package_share_directory('approaching_emergency_vehicle_plugin'), 'config/parameters.yaml')
99
100 stop_controlled_intersection_tactical_plugin_file_path = os.path.join(
101 get_package_share_directory('stop_controlled_intersection_tactical_plugin'), 'config/parameters.yaml')
102
103 trajectory_follower_wrapper_param_file = os.path.join(
104 get_package_share_directory('trajectory_follower_wrapper'), 'config/parameters.yaml')
105
106 env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }')
107
108 pure_pursuit_tuning_parameters = [vehicle_calibration_dir, "/pure_pursuit/calibration.yaml"]
109
110 unique_vehicle_calibration_params = [vehicle_calibration_dir, "/identifiers/UniqueVehicleParams.yaml"]
111
112 platooning_control_param_file = os.path.join(
113 get_package_share_directory('platooning_control'), 'config/parameters.yaml')
114
115 carma_inlanecruising_plugin_container = ComposableNodeContainer(
116 package='carma_ros2_utils',
117 name='carma_lainlanecruising_plugin_container',
118 executable='carma_component_container_mt',
119 namespace=GetCurrentNamespace(),
120 composable_node_descriptions=[
121 ComposableNode(
122 package='inlanecruising_plugin',
123 plugin='inlanecruising_plugin::InLaneCruisingPluginNode',
124 name='inlanecruising_plugin',
125 extra_arguments=[
126 {'use_intra_process_comms': True},
127 {'--log-level' : GetLogLevel('inlanecruising_plugin', env_log_levels) }
128 ],
129 remappings = [
130 ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ),
131 ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ),
132 ("roadway_objects", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/roadway_objects" ] ),
133 ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ),
134 ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ),
135 ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] ),
136 ],
137 parameters=[
138 inlanecruising_plugin_file_path,
139 vehicle_config_param_file,
140 global_params_override_file
141 ]
142 ),
143 ]
144 )
145
146 carma_route_following_plugin_container = ComposableNodeContainer(
147 package='carma_ros2_utils',
148 name='carma_route_following_plugin_container',
149 executable='carma_component_container_mt',
150 namespace=GetCurrentNamespace(),
151 composable_node_descriptions=[
152
153 ComposableNode(
154 package='route_following_plugin',
155 plugin='route_following_plugin::RouteFollowingPlugin',
156 name='route_following_plugin',
157 extra_arguments=[
158 {'use_intra_process_comms': True},
159 {'--log-level' : GetLogLevel('route_following_plugin', env_log_levels) }
160 ],
161 remappings = [
162 ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ),
163 ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ),
164 ("roadway_objects", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/roadway_objects" ] ),
165 ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ),
166 ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ),
167 ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] ),
168 ("current_velocity", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ),
169 ("maneuver_plan", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/final_maneuver_plan" ] ),
170 ],
171 parameters=[
172 route_following_plugin_file_path,
173 vehicle_config_param_file,
174 global_params_override_file
175 ]
176 ),
177 ]
178 )
179
180 carma_approaching_emergency_vehicle_plugin_container = ComposableNodeContainer(
181 package='carma_ros2_utils',
182 name='carma_approaching_emergency_vehicle_plugin_container',
183 executable='carma_component_container_mt',
184 namespace=GetCurrentNamespace(),
185 composable_node_descriptions=[
186
187 ComposableNode(
188 package='approaching_emergency_vehicle_plugin',
189 plugin='approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin',
190 name='approaching_emergency_vehicle_plugin',
191 extra_arguments=[
192 {'use_intra_process_comms': True},
193 {'--log-level' : GetLogLevel('approaching_emergency_vehicle_plugin', env_log_levels) }
194 ],
195 remappings = [
196 ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ),
197 ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ),
198 ("roadway_objects", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/roadway_objects" ] ),
199 ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ),
200 ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ),
201 ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] ),
202 ("state", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/state" ] ),
203 ("approaching_erv_status", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/approaching_erv_status" ] ),
204 ("hazard_light_status", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/hazard_light_status" ] ),
205 ("current_velocity", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ),
206 ("incoming_bsm", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_bsm" ] ),
207 ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ),
208 ("route_state", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route_state" ] ),
209 ("outgoing_emergency_vehicle_response", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_emergency_vehicle_response" ] ),
210 ("incoming_emergency_vehicle_ack", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_emergency_vehicle_ack" ])
211 ],
212 parameters=[
213 approaching_emergency_vehicle_plugin_param_file,
214 vehicle_characteristics_param_file,
215 vehicle_config_param_file,
216 global_params_override_file
217 ]
218 ),
219 ]
220 )
221
222 carma_stop_and_wait_plugin_container = ComposableNodeContainer(
223 package='carma_ros2_utils',
224 name='carma_stop_and_wait_plugin_container',
225 executable='carma_component_container_mt',
226 namespace=GetCurrentNamespace(),
227 composable_node_descriptions=[
228
229 ComposableNode(
230 package='stop_and_wait_plugin',
231 plugin='stop_and_wait_plugin::StopandWaitNode',
232 name='stop_and_wait_plugin',
233 extra_arguments=[
234 {'use_intra_process_comms': True},
235 {'--log-level' : GetLogLevel('stop_and_wait_plugin', env_log_levels) }
236 ],
237 remappings = [
238 ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ),
239 ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ),
240 ("roadway_objects", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/roadway_objects" ] ),
241 ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ),
242 ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ),
243 ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] ),
244 ],
245 parameters=[
246 stop_and_wait_plugin_param_file,
247 vehicle_config_param_file,
248 global_params_override_file
249 ]
250 ),
251 ]
252 )
253
254 carma_sci_strategic_plugin_container = ComposableNodeContainer(
255 package='carma_ros2_utils',
256 name='carma_sci_strategic_plugin_container',
257 executable='carma_component_container_mt',
258 namespace=GetCurrentNamespace(),
259 composable_node_descriptions=[
260 ComposableNode(
261 package='sci_strategic_plugin',
262 plugin='sci_strategic_plugin::SCIStrategicPlugin',
263 name='sci_strategic_plugin',
264 extra_arguments=[
265 {'use_intra_process_comms': True},
266 {'--log-level' : GetLogLevel('sci_strategic_plugin', env_log_levels) }
267 ],
268 remappings = [
269 ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ),
270 ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ),
271 ("roadway_objects", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/roadway_objects" ] ),
272 ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ),
273 ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ),
274 ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] ),
275 ("maneuver_plan", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/final_maneuver_plan" ] ),
276 ("outgoing_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_operation" ] ),
277 ("incoming_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_operation" ] ),
278 ("bsm_outbound", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/bsm_outbound" ] ),
279 ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
280 ],
281 parameters=[
282 sci_strategic_plugin_file_path,
283 vehicle_config_param_file,
284 global_params_override_file
285 ]
286 ),
287 ]
288 )
289
290 carma_lci_strategic_plugin_container = ComposableNodeContainer(
291 package='carma_ros2_utils',
292 name='carma_lci_strategic_plugin_container',
293 executable='carma_component_container_mt',
294 namespace=GetCurrentNamespace(),
295 composable_node_descriptions=[
296 ComposableNode(
297 package='lci_strategic_plugin',
298 plugin='lci_strategic_plugin::LCIStrategicPlugin',
299 name='lci_strategic_plugin',
300 extra_arguments=[
301 {'use_intra_process_comms': True},
302 {'--log-level' : GetLogLevel('lci_strategic_plugin', env_log_levels) }
303 ],
304 remappings = [
305 ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ),
306 ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ),
307 ("roadway_objects", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/roadway_objects" ] ),
308 ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ),
309 ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ),
310 ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] ),
311 ("maneuver_plan", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/final_maneuver_plan" ] ),
312 ("outgoing_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_operation" ] ),
313 ("incoming_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_operation" ] ),
314 ("bsm_outbound", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/bsm_outbound" ] ),
315 ],
316 parameters=[
317 lci_strategic_plugin_file_path,
318 vehicle_config_param_file,
319 unique_vehicle_calibration_params,
320 global_params_override_file
321 ]
322 ),
323 ]
324 )
325
326 carma_stop_controlled_intersection_tactical_plugin_container = ComposableNodeContainer(
327 package='carma_ros2_utils',
328 name='carma_stop_controlled_intersection_tactical_plugin_container',
329 executable='carma_component_container_mt',
330 namespace=GetCurrentNamespace(),
331 composable_node_descriptions=[
332 ComposableNode(
333 package='stop_controlled_intersection_tactical_plugin',
334 plugin='stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin',
335 name='stop_controlled_intersection_tactical_plugin',
336 extra_arguments=[
337 {'use_intra_process_comms': True},
338 {'--log-level' : GetLogLevel('stop_controlled_intersection_tactical_plugin', env_log_levels) }
339 ],
340 remappings = [
341 ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ),
342 ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ),
343 ("roadway_objects", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/roadway_objects" ] ),
344 ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ),
345 ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ),
346 ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] )
347 ],
348 parameters=[
349 stop_controlled_intersection_tactical_plugin_file_path,
350 vehicle_config_param_file,
351 global_params_override_file
352 ]
353 ),
354 ]
355 )
356
357 carma_cooperative_lanechange_plugins_container = ComposableNodeContainer(
358 package='carma_ros2_utils',
359 name='carma_cooperative_lanechange_plugins_container',
360 executable='carma_component_container_mt',
361 namespace=GetCurrentNamespace(),
362 composable_node_descriptions=[
363 ComposableNode(
364 package='cooperative_lanechange',
365 plugin='cooperative_lanechange::CooperativeLaneChangePlugin',
366 name='cooperative_lanechange',
367 extra_arguments=[
368 {'use_intra_process_comms': True},
369 {'--log-level' : GetLogLevel('cooperative_lanechange', env_log_levels) }
370 ],
371 remappings = [
372 ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ),
373 ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ),
374 ("roadway_objects", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/roadway_objects" ] ),
375 ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ),
376 ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ),
377 ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] ),
378 ("current_velocity", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ),
379 ("cooperative_lane_change_status", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/cooperative_lane_change_status" ] ),
380 ("bsm_outbound", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/bsm_outbound" ] ),
381 ("outgoing_mobility_request", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_request" ] ),
382 ("incoming_mobility_response", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_response" ] ),
383 ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ),
384 ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] )
385 ],
386 parameters=[
387 cooperative_lanechange_param_file,
388 vehicle_characteristics_param_file,
389 vehicle_config_param_file,
390 global_params_override_file
391 ]
392 ),
393 ]
394 )
395
396 carma_yield_plugin_container = ComposableNodeContainer(
397 package='carma_ros2_utils',
398 name='carma_yield_plugin_container',
399 executable='carma_component_container_mt',
400 namespace=GetCurrentNamespace(),
401 composable_node_descriptions=[
402 ComposableNode(
403 package='yield_plugin',
404 plugin='yield_plugin::YieldPluginNode',
405 name='yield_plugin',
406 extra_arguments=[
407 {'use_intra_process_comms': True},
408 {'--log-level' : GetLogLevel('yield_plugin', env_log_levels) }
409 ],
410 remappings = [
411 ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ),
412 ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ),
413 ("external_object_predictions", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/external_object_predictions" ] ),
414 ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ),
415 ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ),
416 ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] ),
417 ("outgoing_mobility_response", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_response" ] ),
418 ("incoming_mobility_request", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_request" ] ),
419 ("cooperative_lane_change_status", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/cooperative_lane_change_status" ] ),
420 ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference"]),
421 ("bsm_outbound", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/bsm_outbound" ] ),
422 ],
423 parameters=[
424 yield_plugin_file_path,
425 vehicle_config_param_file,
426 global_params_override_file
427 ]
428 ),
429 ]
430 )
431
432 carma_light_controlled_intersection_plugins_container = ComposableNodeContainer(
433 package='carma_ros2_utils',
434 name='carma_light_controlled_intersection_plugins_container',
435 executable='carma_component_container_mt',
436 namespace=GetCurrentNamespace(),
437 composable_node_descriptions=[
438 ComposableNode(
439 package='light_controlled_intersection_tactical_plugin',
440 plugin='light_controlled_intersection_tactical_plugin::LightControlledIntersectionTransitPluginNode',
441 name='light_controlled_intersection_tactical_plugin',
442 extra_arguments=[
443 {'use_intra_process_comms': True},
444 {'--log-level' : GetLogLevel('light_controlled_intersection_tactical_plugin', env_log_levels) }
445 ],
446 remappings = [
447 ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ),
448 ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ),
449 ("roadway_objects", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/roadway_objects" ] ),
450 ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ),
451 ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ),
452 ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] )
453 ],
454 parameters=[
455 vehicle_config_param_file,
456 vehicle_characteristics_param_file,
457 light_controlled_intersection_tactical_plugin_param_file,
458 global_params_override_file
459 ]
460 ),
461 ]
462 )
463
464 carma_pure_pursuit_wrapper_container = ComposableNodeContainer(
465 package='carma_ros2_utils',
466 name='carma_pure_pursuit_wrapper_container',
467 executable='carma_component_container_mt',
468 namespace=GetCurrentNamespace(),
469 composable_node_descriptions=[
470 ComposableNode(
471 package='pure_pursuit_wrapper',
472 plugin='pure_pursuit_wrapper::PurePursuitWrapperNode',
473 name='pure_pursuit_wrapper',
474 extra_arguments=[
475 {'use_intra_process_comms': True},
476 {'--log-level' : GetLogLevel('pure_pursuit_wrapper', env_log_levels) }
477 ],
478 remappings = [
479 ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ),
480 ("ctrl_raw", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/ctrl_raw" ] ),
481 ("pure_pursuit_wrapper/plan_trajectory", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugins/pure_pursuit/plan_trajectory" ] ),
482 ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
483 ("vehicle/twist", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ),
484 ],
485 parameters=[
486 vehicle_characteristics_param_file,
487 vehicle_config_param_file,
488 pure_pursuit_tuning_parameters,
489 global_params_override_file
490 ]
491 ),
492 ]
493 )
494
495 trajectory_follower_container = ComposableNodeContainer(
496 package='carma_ros2_utils',
497 name='trajectory_follower_container',
498 executable='carma_component_container_mt',
499 namespace=GetCurrentNamespace(),
500 composable_node_descriptions=[
501 ComposableNode(
502 package='trajectory_follower_nodes',
503 plugin='autoware::motion::control::trajectory_follower_nodes::LatLonMuxer',
504 name='latlon_muxer_node',
505 extra_arguments=[
506 {'use_intra_process_comms': False},
507 {'--log-level' : GetLogLevel('latlon_muxer', env_log_levels) }
508 ],
509 remappings = [
510 ("input/lateral/control_cmd", "trajectory_follower/lateral/control_cmd"),
511 ("input/longitudinal/control_cmd", "trajectory_follower/longitudinal/control_cmd"),
512 ("output/control_cmd", "trajectory_follower/control_cmd")
513 ],
514 parameters=[
515 {'timeout_thr_sec':0.5},
516 global_params_override_file
517 ]
518 ),
519 ComposableNode(
520 package='trajectory_follower_nodes',
521 plugin='autoware::motion::control::trajectory_follower_nodes::LateralController',
522 name='lateral_controller_node',
523 extra_arguments=[
524 {'use_intra_process_comms': True},
525 {'--log-level' : GetLogLevel('lateral_controller', env_log_levels) }
526 ],
527 remappings = [
528 ("output/lateral/control_cmd", "trajectory_follower/lateral/control_cmd"),
529 ("input/current_kinematic_state", "trajectory_follower/current_kinematic_state"),
530 ("input/reference_trajectory","trajectory_follower/reference_trajectory" )
531 ],
532 parameters = [
533 [vehicle_calibration_dir,
534 "/trajectory_follower/lateral_controller_defaults.yaml"],
535 global_params_override_file
536 ]
537 ),
538 ComposableNode(
539 package='trajectory_follower_nodes',
540 plugin='autoware::motion::control::trajectory_follower_nodes::LongitudinalController',
541 name='longitudinal_controller_node',
542 extra_arguments=[
543 {'use_intra_process_comms': False},
544 {'--log-level' : GetLogLevel('longitudinal_controller', env_log_levels) }
545 ],
546 remappings = [
547 ("output/longitudinal/control_cmd", "trajectory_follower/longitudinal/control_cmd"),
548 ("input/current_trajectory", "trajectory_follower/reference_trajectory"),
549 ("input/current_state", "trajectory_follower/current_kinematic_state")
550 ],
551 parameters = [
552 [vehicle_calibration_dir,
553 "/trajectory_follower/longitudinal_controller_defaults.yaml"],
554 global_params_override_file
555 ]
556 )
557 ]
558 )
559 carma_trajectory_follower_wrapper_container = ComposableNodeContainer(
560 package='carma_ros2_utils',
561 name='carma_trajectory_follower_wrapper_container',
562 executable='carma_component_container_mt',
563 namespace=GetCurrentNamespace(),
564 composable_node_descriptions=[
565 ComposableNode(
566 package='trajectory_follower_wrapper',
567 plugin='trajectory_follower_wrapper::TrajectoryFollowerWrapperNode',
568 name='trajectory_follower_wrapper',
569 extra_arguments=[
570 {'use_intra_process_comms': True},
571 {'--log-level' : GetLogLevel('trajectory_follower_wrapper', env_log_levels) }
572 ],
573 remappings = [
574 ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ),
575 ("ctrl_raw", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/ctrl_raw" ] ),
576 ("trajectory_follower_wrapper/plan_trajectory", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugins/trajectory_follower_wrapper/plan_trajectory" ] ),
577 ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
578 ("vehicle/twist", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ),
579 ],
580 parameters=[
581 vehicle_characteristics_param_file,
582 trajectory_follower_wrapper_param_file,
583 global_params_override_file
584 ]
585 ),
586 ]
587 )
588
589 platooning_strategic_plugin_container = ComposableNodeContainer(
590 package='carma_ros2_utils',
591 name='platooning_strategic_plugin_container',
592 executable='carma_component_container_mt',
593 namespace=GetCurrentNamespace(),
594 composable_node_descriptions=[
595 ComposableNode(
596 package='platooning_strategic_ihp',
597 plugin='platooning_strategic_ihp::Node',
598 name='platooning_strategic_ihp_node',
599 extra_arguments=[
600 {'use_intra_process_comms': True},
601 {'--log-level' : GetLogLevel('platooning_strategic_ihp', env_log_levels) }
602 ],
603 remappings = [
604 ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ),
605 ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ),
606 ("roadway_objects", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/roadway_objects" ] ),
607 ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ),
608 ("outgoing_mobility_response", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_response" ] ),
609 ("outgoing_mobility_request", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_request" ] ),
610 ("outgoing_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_operation" ] ),
611 ("incoming_mobility_request", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_request" ] ),
612 ("incoming_mobility_response", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_response" ] ),
613 ("incoming_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_operation" ] ),
614 ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ),
615 ("twist_raw", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/twist_raw" ] ),
616 ("platoon_info", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/platoon_info" ] ),
617 ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ),
618 ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] ),
619 ("current_velocity", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ),
620 ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
621 ],
622 parameters=[
623 platooning_strategic_ihp_param_file,
624 vehicle_config_param_file,
625 global_params_override_file
626 ]
627 ),
628 ]
629 )
630
631 platooning_tactical_plugin_container = ComposableNodeContainer(
632 package='carma_ros2_utils',
633 name='platooning_tactical_plugin_container',
634 executable='carma_component_container_mt',
635 namespace=GetCurrentNamespace(),
636 composable_node_descriptions=[
637 ComposableNode(
638 package='platooning_tactical_plugin',
639 plugin='platooning_tactical_plugin::Node',
640 name='platooning_tactical_plugin_node',
641 extra_arguments=[
642 {'use_intra_process_comms': True},
643 {'--log-level' : GetLogLevel('platooning_tactical_plugin', env_log_levels) }
644 ],
645 remappings = [
646 ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ),
647 ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ),
648 ("roadway_objects", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/roadway_objects" ] ),
649 ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ),
650 ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ),
651 ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ),
652 ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] ),
653 ],
654 parameters=[platoon_tactical_ihp_param_file,
655 vehicle_config_param_file,
656 global_params_override_file]
657 ),
658 ]
659 )
660
661 platooning_control_plugin_container = ComposableNodeContainer(
662 package='carma_ros2_utils',
663 name='platooning_control_container',
664 executable='carma_component_container_mt',
665 namespace=GetCurrentNamespace(),
666 composable_node_descriptions=[
667 ComposableNode(
668 package='platooning_control',
669 plugin='platooning_control::PlatooningControlPlugin',
670 name='platooning_control',
671 extra_arguments=[
672 {'use_intra_process_comms': True},
673 {'--log-level' : GetLogLevel('platooning_control_plugin', env_log_levels) }
674 ],
675 remappings = [
676 ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ),
677 ("ctrl_raw", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/ctrl_raw" ] ),
678 ("twist_raw", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/twist_raw" ] ),
679 ("platooning_control/plan_trajectory", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugins/platooning_control/plan_trajectory" ] ),
680 ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
681 ("vehicle/twist", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ),
682 ],
683 parameters=[ platooning_control_param_file,
684 vehicle_config_param_file,
685 unique_vehicle_calibration_params,
686 global_params_override_file]
687 )
688 ]
689 )
690
691 carma_stop_and_dwell_strategic_plugin_container = ComposableNodeContainer(
692 package='carma_ros2_utils',
693 name='carma_stop_and_dwell_strategic_plugin_container',
694 executable='carma_component_container_mt',
695 namespace=GetCurrentNamespace(),
696 composable_node_descriptions=[
697 ComposableNode(
698 package='stop_and_dwell_strategic_plugin',
699 plugin='stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin',
700 name='stop_and_dwell_strategic_plugin',
701 extra_arguments=[
702 {'use_intra_process_comms': True},
703 {'--log-level' : GetLogLevel('stop_and_dwell_strategic_plugin', env_log_levels) }
704 ],
705 remappings = [
706 ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ),
707 ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ),
708 ("roadway_objects", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/roadway_objects" ] ),
709 ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ),
710 ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ),
711 ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] ),
712 ("maneuver_plan", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/final_maneuver_plan" ] ),
713 ("state", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/state" ] ),
714 ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
715 ],
716 parameters=[
717 stop_and_dwell_strategic_plugin_container_file_path,
718 vehicle_config_param_file,
719 global_params_override_file
720 ]
721 ),
722 ]
723 )
724
725 intersection_transit_maneuvering_container = ComposableNodeContainer(
726 package='carma_ros2_utils',
727 name='intersection_transit_maneuvering_container',
728 executable='carma_component_container_mt',
729 namespace=GetCurrentNamespace(),
730 composable_node_descriptions=[
731 ComposableNode(
732 package='intersection_transit_maneuvering',
733 plugin='intersection_transit_maneuvering::IntersectionTransitManeuveringNode',
734 name='intersection_transit_maneuvering',
735 extra_arguments=[
736 {'use_intra_process_comms': True},
737 {'--log-level' : GetLogLevel('intersection_transit_maneuvering', env_log_levels) }
738 ],
739 remappings = [],
740 parameters=[
741 vehicle_config_param_file,
742 global_params_override_file
743 ]
744 ),
745 ]
746 )
747
748 return LaunchDescription([
749 declare_vehicle_config_dir_arg,
750 declare_global_params_override_file_arg,
751 carma_inlanecruising_plugin_container,
752 carma_route_following_plugin_container,
753 carma_approaching_emergency_vehicle_plugin_container,
754 carma_stop_and_wait_plugin_container,
755 carma_sci_strategic_plugin_container,
756 carma_stop_and_dwell_strategic_plugin_container,
757 carma_lci_strategic_plugin_container,
758 carma_stop_controlled_intersection_tactical_plugin_container,
759 carma_cooperative_lanechange_plugins_container,
760 carma_yield_plugin_container,
761 carma_light_controlled_intersection_plugins_container,
762 carma_pure_pursuit_wrapper_container,
763 carma_trajectory_follower_wrapper_container,
764
765 platooning_tactical_plugin_container,
766 platooning_control_plugin_container,
767 intersection_transit_maneuvering_container,
768 trajectory_follower_container
769
770 ])
def generate_launch_description()