33 """
34 Launch perception nodes.
35 """
36 vehicle_calibration_dir = LaunchConfiguration('vehicle_calibration_dir')
37
38 vehicle_config_param_file = LaunchConfiguration('vehicle_config_param_file')
39 declare_vehicle_config_param_file_arg = DeclareLaunchArgument(
40 name = 'vehicle_config_param_file',
41 default_value = "/opt/carma/vehicle/config/VehicleConfigParams.yaml",
42 description = "Path to file contain vehicle configuration parameters"
43 )
44
45 use_sim_time = LaunchConfiguration('use_sim_time')
46 declare_use_sim_time_arg = DeclareLaunchArgument(
47 name = 'use_sim_time',
48 default_value = "False",
49 description = "True if simulation mode is on"
50 )
51
52 vehicle_characteristics_param_file = LaunchConfiguration('vehicle_characteristics_param_file')
53 declare_vehicle_characteristics_param_file_arg = DeclareLaunchArgument(
54 name = 'vehicle_characteristics_param_file',
55 default_value = "/opt/carma/vehicle/calibration/identifiers/UniqueVehicleParams.yaml",
56 description = "Path to file containing unique vehicle calibrations"
57 )
58
59 vehicle_config_dir = LaunchConfiguration('vehicle_config_dir')
60 declare_vehicle_config_dir_arg = DeclareLaunchArgument(
61 name = 'vehicle_config_dir',
62 default_value = "/opt/carma/vehicle/config",
63 description = "Path to vehicle configuration directory populated by carma-config"
64 )
65
66
67
68 global_params_override_file = LaunchConfiguration('global_params_override_file')
69 declare_global_params_override_file_arg = DeclareLaunchArgument(
70 name = 'global_params_override_file',
71 default_value = [vehicle_config_dir, "/GlobalParamsOverride.yaml"],
72 description = "Path to global file containing the parameters overwrite"
73 )
74
75 vector_map_file = LaunchConfiguration('vector_map_file')
76 declare_vector_map_file = DeclareLaunchArgument(name='vector_map_file', default_value = 'vector_map.osm', description = "Path to the map osm file if using the noupdate load type")
77
78
79
80 is_cp_mot_enabled = LaunchConfiguration('is_cp_mot_enabled')
81 declare_is_cp_mot_enabled = DeclareLaunchArgument(
82 name='is_cp_mot_enabled',
83 default_value = 'False',
84 description = 'True if user wants Cooperative Perception capability using Multiple Object Tracking to be enabled'
85 )
86
87
88
89 is_autoware_lidar_obj_detection_enabled = LaunchConfiguration('is_autoware_lidar_obj_detection_enabled')
90 declare_is_autoware_lidar_obj_detection_enabled = DeclareLaunchArgument(
91 name='is_autoware_lidar_obj_detection_enabled',
92 default_value = 'False',
93 description = 'True if user wants Autoware Lidar Object Detection to be enabled'
94 )
95
96 autoware_auto_launch_pkg_prefix = get_package_share_directory(
97 'autoware_auto_launch')
98
99 euclidean_cluster_param_file = os.path.join(
100 autoware_auto_launch_pkg_prefix, 'param/component_style/euclidean_cluster.param.yaml')
101
102 ray_ground_classifier_param_file = os.path.join(
103 autoware_auto_launch_pkg_prefix, 'param/component_style/ray_ground_classifier.param.yaml')
104
105 tracking_nodes_param_file = os.path.join(
106 autoware_auto_launch_pkg_prefix, 'param/component_style/tracking_nodes.param.yaml')
107
108 object_detection_tracking_param_file = os.path.join(
109 get_package_share_directory('object_detection_tracking'), 'config/parameters.yaml')
110
111 subsystem_controller_default_param_file = os.path.join(
112 get_package_share_directory('subsystem_controllers'), 'config/environment_perception_controller_config.yaml')
113
114 subsystem_controller_param_file = LaunchConfiguration('subsystem_controller_param_file')
115 declare_subsystem_controller_param_file_arg = DeclareLaunchArgument(
116 name = 'subsystem_controller_param_file',
117 default_value = subsystem_controller_default_param_file,
118 description = "Path to file containing override parameters for the subsystem controller"
119 )
120
121 frame_transformer_param_file = os.path.join(
122 get_package_share_directory('frame_transformer'), 'config/parameters.yaml')
123
124 object_visualizer_param_file = os.path.join(
125 get_package_share_directory('object_visualizer'), 'config/parameters.yaml')
126
127 points_map_filter_param_file = os.path.join(
128 get_package_share_directory('points_map_filter'), 'config/parameters.yaml')
129
130 motion_computation_param_file = os.path.join(
131 get_package_share_directory('motion_computation'), 'config/parameters.yaml')
132
133 env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }')
134
135 carma_wm_ctrl_param_file = os.path.join(
136 get_package_share_directory('carma_wm_ctrl'), 'config/parameters.yaml')
137
138 cp_multiple_object_tracker_node_file =
str(
139 PurePath(get_package_share_directory("carma_cooperative_perception"),
140 "config/cp_multiple_object_tracker_node.yaml"))
141 cp_host_vehicle_filter_node_file =
str(
142 PurePath(get_package_share_directory("carma_cooperative_perception"),
143 "config/cp_host_vehicle_filter_node.yaml"))
144 cp_sdsm_to_detection_list_node_file =
str(
145 PurePath(get_package_share_directory("carma_cooperative_perception"),
146 "config/cp_sdsm_to_detection_list_node.yaml"))
147
148
149
150
151
152 lidar_perception_container = ComposableNodeContainer(
153 condition=IfCondition(is_autoware_lidar_obj_detection_enabled),
154 package='carma_ros2_utils',
155 name='perception_points_filter_container',
156 executable='lifecycle_component_wrapper_mt',
157 namespace=GetCurrentNamespace(),
158 composable_node_descriptions=[
159 ComposableNode(
160 package='frame_transformer',
161 plugin='frame_transformer::Node',
162 name='lidar_to_map_frame_transformer',
163 extra_arguments=[
164 {'use_intra_process_comms': True},
165 {'--log-level' : GetLogLevel('frame_transformer', env_log_levels) },
166 {'is_lifecycle_node': True}
167 ],
168 remappings=[
169 ("input", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/lidar/points_raw" ] ),
170 ("output", "points_in_map"),
171 ("change_state", "disabled_change_state"),
172 ("get_state", "disabled_get_state")
173 ],
174 parameters=[
175 { "target_frame" : "map"},
176 { "message_type" : "sensor_msgs/PointCloud2"},
177 { "queue_size" : 1},
178 { "timeout" : 50 },
179 vehicle_config_param_file,
180 global_params_override_file
181 ]
182 ),
183 ComposableNode(
184 package='points_map_filter',
185 plugin='points_map_filter::Node',
186 name='points_map_filter',
187 extra_arguments=[
188 {'use_intra_process_comms': True},
189 {'--log-level' : GetLogLevel('points_map_filter', env_log_levels) },
190 {'is_lifecycle_node': True}
191 ],
192 remappings=[
193 ("points_raw", "points_in_map" ),
194 ("filtered_points", "map_filtered_points"),
195 ("lanelet2_map", "semantic_map"),
196 ("change_state", "disabled_change_state"),
197 ("get_state", "disabled_get_state")
198 ],
199 parameters=[ points_map_filter_param_file,
200 vehicle_config_param_file,
201 global_params_override_file]
202 ),
203 ComposableNode(
204 package='frame_transformer',
205 plugin='frame_transformer::Node',
206 name='lidar_frame_transformer',
207 extra_arguments=[
208 {'use_intra_process_comms': True},
209 {'--log-level' : GetLogLevel('frame_transformer', env_log_levels) },
210 {'is_lifecycle_node': True}
211 ],
212 remappings=[
213 ("input", "map_filtered_points" ),
214 ("output", "points_in_base_link"),
215 ("change_state", "disabled_change_state"),
216 ("get_state", "disabled_get_state")
217 ],
218 parameters=[frame_transformer_param_file,
219 vehicle_config_param_file,
220 global_params_override_file]
221 ),
222 ComposableNode(
223 package='ray_ground_classifier_nodes',
224 name='ray_ground_filter',
225 plugin='autoware::perception::filters::ray_ground_classifier_nodes::RayGroundClassifierCloudNode',
226 extra_arguments=[
227 {'use_intra_process_comms': True},
228 {'--log-level' : GetLogLevel('ray_ground_classifier_nodes', env_log_levels) }
229 ],
230 remappings=[
231 ("points_in", "points_in_base_link"),
232 ("points_nonground", "points_no_ground")
233 ],
234 parameters=[ray_ground_classifier_param_file,
235 vehicle_config_param_file,
236 global_params_override_file]
237 ),
238 ComposableNode(
239 package='euclidean_cluster_nodes',
240 name='euclidean_cluster',
241 plugin='autoware::perception::segmentation::euclidean_cluster_nodes::EuclideanClusterNode',
242 extra_arguments=[
243 {'use_intra_process_comms': True},
244 {'--log-level' : GetLogLevel('euclidean_cluster_nodes', env_log_levels) }
245 ],
246 remappings=[
247 ("points_in", "points_no_ground")
248 ],
249 parameters=[euclidean_cluster_param_file,
250 vehicle_config_param_file,
251 global_params_override_file]
252 ),
253 ComposableNode(
254 package='object_detection_tracking',
255 plugin='bounding_box_to_detected_object::Node',
256 name='bounding_box_converter',
257 extra_arguments=[
258 {'use_intra_process_comms': True},
259 {'--log-level' : GetLogLevel('object_detection_tracking', env_log_levels) },
260 {'is_lifecycle_node': True}
261 ],
262 remappings=[
263 ("bounding_boxes", "lidar_bounding_boxes"),
264 ("lidar_detected_objects", "detected_objects"),
265 ],
266 parameters=[vehicle_config_param_file, global_params_override_file]
267 ),
268 ComposableNode(
269 package='tracking_nodes',
270 plugin='autoware::tracking_nodes::MultiObjectTrackerNode',
271 name='tracking_nodes_node',
272 extra_arguments=[
273 {'use_intra_process_comms': True},
274 {'--log-level' : GetLogLevel('tracking_nodes', env_log_levels) }
275 ],
276 remappings=[
277 ("ego_state", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose_with_covariance" ] ),
278
279
280 ],
281 parameters=[tracking_nodes_param_file,
282 vehicle_config_param_file,
283 global_params_override_file]
284 )
285 ]
286 )
287
288
289
290
291
292 carma_external_objects_container = ComposableNodeContainer(
293 package='carma_ros2_utils',
294 name='external_objects_container',
295 executable='carma_component_container_mt',
296 namespace=GetCurrentNamespace(),
297 composable_node_descriptions=[
298 ComposableNode(
299 package='carma_wm_ctrl',
300 plugin='carma_wm_ctrl::WMBroadcasterNode',
301 name='carma_wm_broadcaster',
302 extra_arguments=[
303 {'use_intra_process_comms': True},
304 {'--log-level' : GetLogLevel('carma_wm_ctrl', env_log_levels) }
305 ],
306 remappings=[
307 ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ),
308 ("geofence", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_geofence_control" ] ),
309 ("incoming_map", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_map" ] ),
310 ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
311 ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] ),
312 ("outgoing_geofence_ack", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_operation" ] ),
313 ("outgoing_geofence_request", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_geofence_request" ] )
314 ],
315 parameters=[carma_wm_ctrl_param_file,
316 vehicle_config_param_file,
317 vehicle_characteristics_param_file,
318 global_params_override_file]
319 ),
320 ComposableNode(
321 package='object_detection_tracking',
322 plugin='object::ObjectDetectionTrackingNode',
323 name='external_object',
324 extra_arguments=[
325 {'use_intra_process_comms': True},
326 {'--log-level' : GetLogLevel('object_detection_tracking', env_log_levels) }
327 ],
328 remappings=[
329 ("detected_objects", "tracked_objects"),
330 ],
331 parameters=[object_detection_tracking_param_file,
332 vehicle_config_param_file,
333 global_params_override_file]
334 ),
335 ComposableNode(
336 package='object_visualizer',
337 plugin='object_visualizer::Node',
338 name='object_visualizer_node',
339 extra_arguments=[
340 {'use_intra_process_comms': True},
341 {'--log-level' : GetLogLevel('object_visualizer', env_log_levels) }
342 ],
343 remappings=[
344 ("external_objects", "external_object_predictions"),
345 ("external_objects_viz", "fused_external_objects_viz")
346 ],
347 parameters=[object_visualizer_param_file, vehicle_config_param_file,
348 {'pedestrian_icon_path': [
349 'file:///',
350 vehicle_calibration_dir,
351 '/visualization_meshes/pedestrian.stl']},
352 global_params_override_file
353 ]
354 ),
355 ComposableNode(
356 package='motion_computation',
357 plugin='motion_computation::MotionComputationNode',
358 name='motion_computation_node',
359 extra_arguments=[
360 {'use_intra_process_comms': True},
361 {'--log-level' : GetLogLevel('motion_computation', env_log_levels) }
362 ],
363 remappings=[
364 ("incoming_mobility_path", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_path" ] ),
365 ("incoming_psm", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_psm" ] ),
366 ("incoming_bsm", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_bsm" ] ),
367 ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ),
368
369 ("external_objects", PythonExpression(['"fused_external_objects" if "', is_cp_mot_enabled, '" == "True" else "external_objects"'])),
370 ],
371 parameters=[
372 motion_computation_param_file,
373 vehicle_config_param_file,
374 global_params_override_file
375 ]
376 ),
377 ComposableNode(
378 package='motion_prediction_visualizer',
379 plugin='motion_prediction_visualizer::Node',
380 name='motion_prediction_visualizer',
381 extra_arguments=[
382 {'use_intra_process_comms': True},
383 {'--log-level' : GetLogLevel('motion_prediction_visualizer', env_log_levels) }
384 ],
385 remappings=[
386 ("external_objects", "external_object_predictions" ),
387 ],
388 parameters=[ vehicle_config_param_file, global_params_override_file ]
389 ),
390 ComposableNode(
391 package='traffic_incident_parser',
392 plugin='traffic_incident_parser::TrafficIncidentParserNode',
393 name='traffic_incident_parser_node',
394 extra_arguments=[
395 {'use_intra_process_comms': True},
396 {'--log-level' : GetLogLevel('traffic_incident_parser', env_log_levels) }
397 ],
398 remappings=[
399 ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ),
400 ("geofence", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_geofence_control" ] ),
401 ("incoming_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_operation" ] ),
402 ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ),
403 ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] )
404 ],
405 parameters = [
406 vehicle_config_param_file, global_params_override_file
407 ]
408
409 ),
410 ]
411 )
412
413
414 lanelet2_map_loader_container = ComposableNodeContainer(
415 package='carma_ros2_utils',
416 name='lanelet2_map_loader_container',
417 executable='lifecycle_component_wrapper_mt',
418 namespace=GetCurrentNamespace(),
419 composable_node_descriptions=[
420 ComposableNode(
421 package='map_file_ros2',
422 plugin='lanelet2_map_loader::Lanelet2MapLoader',
423 name='lanelet2_map_loader',
424 extra_arguments=[
425 {'use_intra_process_comms': True},
426 {'--log-level' : GetLogLevel('lanelet2_map_loader', env_log_levels) },
427 {'is_lifecycle_node': True}
428 ],
429 remappings=[
430 ("lanelet_map_bin", "base_map"),
431 ("change_state", "disabled_change_state"),
432 ("get_state", "disabled_get_state")
433 ],
434 parameters=[
435 { "lanelet2_filename" : vector_map_file},
436 vehicle_config_param_file,
437 global_params_override_file
438 ]
439 )
440 ]
441 )
442
443
444 lanelet2_map_visualization_container = ComposableNodeContainer(
445 package='carma_ros2_utils',
446 name='lanelet2_map_visualization_container',
447 executable='lifecycle_component_wrapper_mt',
448 namespace= GetCurrentNamespace(),
449 composable_node_descriptions=[
450 ComposableNode(
451 package='map_file_ros2',
452 plugin='lanelet2_map_visualization::Lanelet2MapVisualization',
453 name='lanelet2_map_visualization',
454 extra_arguments=[
455 {'use_intra_process_comms': True},
456 {'--log-level' : GetLogLevel('lanelet2_map_visualization', env_log_levels) },
457 {'is_lifecycle_node': True}
458 ],
459 remappings=[
460 ("lanelet_map_bin", "semantic_map"),
461 ("change_state", "disabled_change_state"),
462 ("get_state", "disabled_get_state")
463 ],
464 parameters=[
465 vehicle_config_param_file,
466 global_params_override_file
467 ]
468 )
469 ]
470 )
471
472
473 carma_cooperative_perception_container = ComposableNodeContainer(
474 condition=IfCondition(is_cp_mot_enabled),
475 package='carma_ros2_utils',
476 name='carma_cooperative_perception_container',
477 executable='carma_component_container_mt',
478 namespace= GetCurrentNamespace(),
479 output='screen',
480 composable_node_descriptions=[
481 ComposableNode(
482 package='carma_cooperative_perception',
483 plugin='carma_cooperative_perception::ExternalObjectListToDetectionListNode',
484 name='cp_external_object_list_to_detection_list_node',
485 extra_arguments=[
486 {'use_intra_process_comms': True},
487 {'--log-level' : GetLogLevel('cp_external_object_list_to_detection_list_node', env_log_levels) },
488 ],
489 remappings=[
490 ("input/georeference", [EnvironmentVariable("CARMA_LOCZ_NS", default_value=""), "/map_param_loader/georeference"]),
491 ("output/detections", "full_detection_list"),
492 ("input/external_objects", "external_objects"),
493 ],
494 parameters=[
495 vehicle_config_param_file,
496 global_params_override_file
497 ]
498 ),
499 ComposableNode(
500 package='carma_cooperative_perception',
501 plugin='carma_cooperative_perception::ExternalObjectListToSdsmNode',
502 name='cp_external_object_list_to_sdsm_node',
503 extra_arguments=[
504 {'use_intra_process_comms': True},
505 {'--log-level' : GetLogLevel('cp_external_object_list_to_sdsm_node', env_log_levels) },
506 ],
507 remappings=[
508 ("input/georeference", [EnvironmentVariable("CARMA_LOCZ_NS", default_value=""), "/map_param_loader/georeference"]),
509 ("output/sdsms", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_sdsm" ] ),
510 ("input/pose_stamped", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
511 ("input/external_objects", "external_objects"),
512 ],
513 parameters=[
514 vehicle_config_param_file,
515 global_params_override_file
516 ]
517 ),
518 ComposableNode(
519 package='carma_cooperative_perception',
520 plugin='carma_cooperative_perception::HostVehicleFilterNode',
521 name='cp_host_vehicle_filter_node',
522 extra_arguments=[
523 {'use_intra_process_comms': True},
524 {'--log-level' : GetLogLevel('cp_host_vehicle_filter_node', env_log_levels) },
525 ],
526 remappings=[
527 ("input/host_vehicle_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
528 ("input/detection_list", "full_detection_list"),
529 ("output/detection_list", "filtered_detection_list")
530 ],
531 parameters=[
532 cp_host_vehicle_filter_node_file,
533 vehicle_config_param_file,
534 global_params_override_file
535 ]
536 ),
537 ComposableNode(
538 package='carma_cooperative_perception',
539 plugin='carma_cooperative_perception::SdsmToDetectionListNode',
540 name='cp_sdsm_to_detection_list_node',
541 extra_arguments=[
542 {'use_intra_process_comms': True},
543 {'--log-level' : GetLogLevel('cp_sdsm_to_detection_list_node', env_log_levels) },
544 ],
545 remappings=[
546 ("input/georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ),
547 ("input/sdsm", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_sdsm" ] ),
548 ("input/cdasim_clock", "/sim_clock"),
549 ("output/detections", "full_detection_list"),
550 ],
551 parameters=[
552 vehicle_config_param_file,
553 cp_sdsm_to_detection_list_node_file,
554 global_params_override_file
555 ]
556 ),
557 ComposableNode(
558 package='carma_cooperative_perception',
559 plugin='carma_cooperative_perception::TrackListToExternalObjectListNode',
560 name='cp_track_list_to_external_object_list_node',
561 extra_arguments=[
562 {'use_intra_process_comms': True},
563 {'--log-level' : GetLogLevel('cp_track_list_to_external_object_list_node', env_log_levels) },
564 ],
565 remappings=[
566 ("input/track_list", "cooperative_perception_track_list"),
567 ("output/external_object_list", "fused_external_objects"),
568 ],
569 parameters=[
570 vehicle_config_param_file,
571 global_params_override_file
572 ]
573 ),
574 ComposableNode(
575 package='carma_cooperative_perception',
576 plugin='carma_cooperative_perception::MultipleObjectTrackerNode',
577 name='cp_multiple_object_tracker_node',
578 extra_arguments=[
579 {'use_intra_process_comms': True},
580 {'--log-level' : GetLogLevel('cp_multiple_object_tracker_node', env_log_levels) },
581 ],
582 remappings=[
583 ("output/track_list", "cooperative_perception_track_list"),
584 ("input/detection_list", "filtered_detection_list"),
585 ],
586 parameters=[
587 cp_multiple_object_tracker_node_file,
588 vehicle_config_param_file,
589 global_params_override_file
590 ]
591
592 ),
593
594 ]
595 )
596
597
598 subsystem_controller = Node(
599 package='subsystem_controllers',
600 name='environment_perception_controller',
601 executable='environment_perception_controller',
602 parameters=[
603 subsystem_controller_default_param_file,
604 subsystem_controller_param_file,
605 {"use_sim_time" : use_sim_time}],
606 on_exit= Shutdown(),
607 arguments=['--ros-args', '--log-level', GetLogLevel('subsystem_controllers', env_log_levels)]
608 )
609
610 return LaunchDescription([
611 declare_vehicle_characteristics_param_file_arg,
612 declare_vehicle_config_param_file_arg,
613 declare_vehicle_config_dir_arg,
614 declare_global_params_override_file_arg,
615 declare_use_sim_time_arg,
616 declare_is_autoware_lidar_obj_detection_enabled,
617 declare_is_cp_mot_enabled,
618 declare_subsystem_controller_param_file_arg,
619 declare_vector_map_file,
620 lidar_perception_container,
621 carma_external_objects_container,
622 lanelet2_map_loader_container,
623 lanelet2_map_visualization_container,
624 carma_cooperative_perception_container,
625 subsystem_controller
626 ])
def generate_launch_description()