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
134 env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }')
135
136 carma_wm_ctrl_param_file = os.path.join(
137 get_package_share_directory('carma_wm_ctrl'), 'config/parameters.yaml')
138
139 cp_multiple_object_tracker_node_file =
str(
140 PurePath(get_package_share_directory("carma_cooperative_perception"),
141 "config/cp_multiple_object_tracker_node.yaml"))
142 cp_host_vehicle_filter_node_file =
str(
143 PurePath(get_package_share_directory("carma_cooperative_perception"),
144 "config/cp_host_vehicle_filter_node.yaml"))
145 cp_sdsm_to_detection_list_node_file =
str(
146 PurePath(get_package_share_directory("carma_cooperative_perception"),
147 "config/cp_sdsm_to_detection_list_node.yaml"))
148
149
150
151
152
153 lidar_perception_container = ComposableNodeContainer(
154 condition=IfCondition(is_autoware_lidar_obj_detection_enabled),
155 package='carma_ros2_utils',
156 name='perception_points_filter_container',
157 executable='lifecycle_component_wrapper_mt',
158 namespace=GetCurrentNamespace(),
159 composable_node_descriptions=[
160 ComposableNode(
161 package='frame_transformer',
162 plugin='frame_transformer::Node',
163 name='lidar_to_map_frame_transformer',
164 extra_arguments=[
165 {'use_intra_process_comms': True},
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 {'is_lifecycle_node': True}
190 ],
191 remappings=[
192 ("points_raw", "points_in_map" ),
193 ("filtered_points", "map_filtered_points"),
194 ("lanelet2_map", "semantic_map"),
195 ("change_state", "disabled_change_state"),
196 ("get_state", "disabled_get_state")
197 ],
198 parameters=[ points_map_filter_param_file,
199 vehicle_config_param_file,
200 global_params_override_file]
201 ),
202 ComposableNode(
203 package='frame_transformer',
204 plugin='frame_transformer::Node',
205 name='lidar_frame_transformer',
206 extra_arguments=[
207 {'use_intra_process_comms': True},
208 {'is_lifecycle_node': True}
209 ],
210 remappings=[
211 ("input", "map_filtered_points" ),
212 ("output", "points_in_base_link"),
213 ("change_state", "disabled_change_state"),
214 ("get_state", "disabled_get_state")
215 ],
216 parameters=[frame_transformer_param_file,
217 vehicle_config_param_file,
218 global_params_override_file]
219 ),
220 ComposableNode(
221 package='ray_ground_classifier_nodes',
222 name='ray_ground_filter',
223 plugin='autoware::perception::filters::ray_ground_classifier_nodes::RayGroundClassifierCloudNode',
224 extra_arguments=[
225 {'use_intra_process_comms': True},
226 ],
227 remappings=[
228 ("points_in", "points_in_base_link"),
229 ("points_nonground", "points_no_ground")
230 ],
231 parameters=[ray_ground_classifier_param_file,
232 vehicle_config_param_file,
233 global_params_override_file]
234 ),
235 ComposableNode(
236 package='euclidean_cluster_nodes',
237 name='euclidean_cluster',
238 plugin='autoware::perception::segmentation::euclidean_cluster_nodes::EuclideanClusterNode',
239 extra_arguments=[
240 {'use_intra_process_comms': True},
241 ],
242 remappings=[
243 ("points_in", "points_no_ground")
244 ],
245 parameters=[euclidean_cluster_param_file,
246 vehicle_config_param_file,
247 global_params_override_file]
248 ),
249 ComposableNode(
250 package='object_detection_tracking',
251 plugin='bounding_box_to_detected_object::Node',
252 name='bounding_box_converter',
253 extra_arguments=[
254 {'use_intra_process_comms': True},
255 {'is_lifecycle_node': True}
256 ],
257 remappings=[
258 ("bounding_boxes", "lidar_bounding_boxes"),
259 ("lidar_detected_objects", "detected_objects"),
260 ],
261 parameters=[vehicle_config_param_file, global_params_override_file]
262 ),
263 ComposableNode(
264 package='tracking_nodes',
265 plugin='autoware::tracking_nodes::MultiObjectTrackerNode',
266 name='tracking_nodes_node',
267 extra_arguments=[
268 {'use_intra_process_comms': True},
269 ],
270 remappings=[
271 ("ego_state", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose_with_covariance" ] ),
272
273
274 ],
275 parameters=[tracking_nodes_param_file,
276 vehicle_config_param_file,
277 global_params_override_file]
278 )
279 ]
280 )
281
282
283
284
285 carma_external_objects_container = ComposableNodeContainer(
286 package='carma_ros2_utils',
287 name='external_objects_container',
288 executable='carma_component_container_mt',
289 namespace=GetCurrentNamespace(),
290 composable_node_descriptions=[
291 ComposableNode(
292 package='carma_wm_ctrl',
293 plugin='carma_wm_ctrl::WMBroadcasterNode',
294 name='carma_wm_broadcaster',
295 extra_arguments=[
296 {'use_intra_process_comms': True},
297 ],
298 remappings=[
299 ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ),
300 ("geofence", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_geofence_control" ] ),
301 ("incoming_map", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_map" ] ),
302 ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
303 ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] ),
304 ("outgoing_geofence_ack", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_operation" ] ),
305 ("outgoing_geofence_request", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_geofence_request" ] )
306 ],
307 parameters=[carma_wm_ctrl_param_file,
308 vehicle_config_param_file,
309 {'tim_icon_path': [
310 'file:///',
311 vehicle_calibration_dir,
312 '/visualization_meshes/cop.obj']},
313 vehicle_characteristics_param_file,
314 global_params_override_file]
315 ),
316 ComposableNode(
317 package='object_detection_tracking',
318 plugin='object::ObjectDetectionTrackingNode',
319 name='external_object',
320 extra_arguments=[
321 {'use_intra_process_comms': True},
322 ],
323 remappings=[
324 ("detected_objects", "tracked_objects"),
325 ],
326 parameters=[object_detection_tracking_param_file,
327 vehicle_config_param_file,
328 global_params_override_file]
329 ),
330 ComposableNode(
331 package='object_visualizer',
332 plugin='object_visualizer::Node',
333 name='object_visualizer_node',
334 extra_arguments=[
335 {'use_intra_process_comms': True},
336 ],
337 remappings=[
338 ("external_objects", "external_object_predictions"),
339 ("external_objects_viz", "fused_external_objects_viz")
340 ],
341 parameters=[object_visualizer_param_file, vehicle_config_param_file,
342 {'pedestrian_icon_path': [
343 'file:///',
344 vehicle_calibration_dir,
345 '/visualization_meshes/pedestrian.stl']},
346 global_params_override_file
347 ]
348 ),
349 ComposableNode(
350 package='motion_computation',
351 plugin='motion_computation::MotionComputationNode',
352 name='motion_computation_node',
353 extra_arguments=[
354 {'use_intra_process_comms': True},
355 ],
356 remappings=[
357 ("incoming_mobility_path", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_path" ] ),
358 ("incoming_psm", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_psm" ] ),
359 ("incoming_bsm", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_bsm" ] ),
360 ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ),
361
362 ("external_objects", PythonExpression(['"fused_external_objects" if "', is_cp_mot_enabled, '" == "True" else "external_objects"'])),
363 ],
364 parameters=[
365 motion_computation_param_file,
366 vehicle_config_param_file,
367 global_params_override_file
368 ]
369 ),
370 ComposableNode(
371 package='motion_prediction_visualizer',
372 plugin='motion_prediction_visualizer::Node',
373 name='motion_prediction_visualizer',
374 extra_arguments=[
375 {'use_intra_process_comms': True},
376 ],
377 remappings=[
378 ("external_objects", "external_object_predictions" ),
379 ],
380 parameters=[ vehicle_config_param_file, global_params_override_file ]
381 ),
382 ComposableNode(
383 package='traffic_incident_parser',
384 plugin='traffic_incident_parser::TrafficIncidentParserNode',
385 name='traffic_incident_parser_node',
386 extra_arguments=[
387 {'use_intra_process_comms': True},
388 ],
389 remappings=[
390 ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ),
391 ("geofence", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_geofence_control" ] ),
392 ("incoming_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_operation" ] ),
393 ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ),
394 ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] )
395 ],
396 parameters = [
397 vehicle_config_param_file, global_params_override_file
398 ]
399
400 ),
401 ]
402 )
403
404
405 lanelet2_map_loader_container = ComposableNodeContainer(
406 package='carma_ros2_utils',
407 name='lanelet2_map_loader_container',
408 executable='lifecycle_component_wrapper_mt',
409 namespace=GetCurrentNamespace(),
410 composable_node_descriptions=[
411 ComposableNode(
412 package='map_file_ros2',
413 plugin='lanelet2_map_loader::Lanelet2MapLoader',
414 name='lanelet2_map_loader',
415 extra_arguments=[
416 {'use_intra_process_comms': True},
417 {'is_lifecycle_node': True}
418 ],
419 remappings=[
420 ("lanelet_map_bin", "base_map"),
421 ("change_state", "disabled_change_state"),
422 ("get_state", "disabled_get_state")
423 ],
424 parameters=[
425 { "lanelet2_filename" : vector_map_file},
426 vehicle_config_param_file,
427 global_params_override_file
428 ]
429 )
430 ]
431 )
432
433
434 lanelet2_map_visualization_container = ComposableNodeContainer(
435 package='carma_ros2_utils',
436 name='lanelet2_map_visualization_container',
437 executable='lifecycle_component_wrapper_mt',
438 namespace= GetCurrentNamespace(),
439 composable_node_descriptions=[
440 ComposableNode(
441 package='map_file_ros2',
442 plugin='lanelet2_map_visualization::Lanelet2MapVisualization',
443 name='lanelet2_map_visualization',
444 extra_arguments=[
445 {'use_intra_process_comms': True},
446 {'is_lifecycle_node': True}
447 ],
448 remappings=[
449 ("lanelet_map_bin", "semantic_map"),
450 ("change_state", "disabled_change_state"),
451 ("get_state", "disabled_get_state")
452 ],
453 parameters=[
454 vehicle_config_param_file,
455 global_params_override_file
456 ]
457 )
458 ]
459 )
460
461
462 carma_cooperative_perception_container = ComposableNodeContainer(
463 condition=IfCondition(is_cp_mot_enabled),
464 package='carma_ros2_utils',
465 name='carma_cooperative_perception_container',
466 executable='carma_component_container_mt',
467 namespace= GetCurrentNamespace(),
468 output='screen',
469 composable_node_descriptions=[
470 ComposableNode(
471 package='carma_cooperative_perception',
472 plugin='carma_cooperative_perception::ExternalObjectListToDetectionListNode',
473 name='cp_external_object_list_to_detection_list_node',
474 extra_arguments=[
475 {'use_intra_process_comms': True},
476 ],
477 remappings=[
478 ("input/georeference", [EnvironmentVariable("CARMA_LOCZ_NS", default_value=""), "/map_param_loader/georeference"]),
479 ("output/detections", "full_detection_list"),
480 ("input/external_objects", "external_objects"),
481 ],
482 parameters=[
483 vehicle_config_param_file,
484 global_params_override_file
485 ]
486 ),
487 ComposableNode(
488 package='carma_cooperative_perception',
489 plugin='carma_cooperative_perception::ExternalObjectListToSdsmNode',
490 name='cp_external_object_list_to_sdsm_node',
491 extra_arguments=[
492 {'use_intra_process_comms': True},
493 ],
494 remappings=[
495 ("input/georeference", [EnvironmentVariable("CARMA_LOCZ_NS", default_value=""), "/map_param_loader/georeference"]),
496 ("output/sdsms", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_sdsm" ] ),
497 ("input/pose_stamped", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
498 ("input/external_objects", "external_objects"),
499 ],
500 parameters=[
501 vehicle_config_param_file,
502 global_params_override_file
503 ]
504 ),
505 ComposableNode(
506 package='carma_cooperative_perception',
507 plugin='carma_cooperative_perception::HostVehicleFilterNode',
508 name='cp_host_vehicle_filter_node',
509 extra_arguments=[
510 {'use_intra_process_comms': True},
511 ],
512 remappings=[
513 ("input/host_vehicle_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
514 ("input/detection_list", "full_detection_list"),
515 ("output/detection_list", "filtered_detection_list")
516 ],
517 parameters=[
518 cp_host_vehicle_filter_node_file,
519 vehicle_config_param_file,
520 global_params_override_file
521 ]
522 ),
523 ComposableNode(
524 package='carma_cooperative_perception',
525 plugin='carma_cooperative_perception::SdsmToDetectionListNode',
526 name='cp_sdsm_to_detection_list_node',
527 extra_arguments=[
528 {'use_intra_process_comms': True},
529 ],
530 remappings=[
531 ("input/georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ),
532 ("input/sdsm", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_sdsm" ] ),
533 ("input/cdasim_clock", "/sim_clock"),
534 ("output/detections", "full_detection_list"),
535 ],
536 parameters=[
537 vehicle_config_param_file,
538 cp_sdsm_to_detection_list_node_file,
539 global_params_override_file
540 ]
541 ),
542 ComposableNode(
543 package='carma_cooperative_perception',
544 plugin='carma_cooperative_perception::TrackListToExternalObjectListNode',
545 name='cp_track_list_to_external_object_list_node',
546 extra_arguments=[
547 {'use_intra_process_comms': True},
548 ],
549 remappings=[
550 ("input/track_list", "cooperative_perception_track_list"),
551 ("output/external_object_list", "fused_external_objects"),
552 ],
553 parameters=[
554 vehicle_config_param_file,
555 global_params_override_file
556 ]
557 ),
558 ComposableNode(
559 package='carma_cooperative_perception',
560 plugin='carma_cooperative_perception::MultipleObjectTrackerNode',
561 name='cp_multiple_object_tracker_node',
562 extra_arguments=[
563 {'use_intra_process_comms': True},
564 ],
565 remappings=[
566 ("output/track_list", "cooperative_perception_track_list"),
567 ("input/detection_list", "filtered_detection_list"),
568 ],
569 parameters=[
570 cp_multiple_object_tracker_node_file,
571 vehicle_config_param_file,
572 global_params_override_file
573 ]
574
575 ),
576
577 ]
578 )
579
580
581 subsystem_controller = Node(
582 package='subsystem_controllers',
583 name='environment_perception_controller',
584 executable='environment_perception_controller',
585 parameters=[
586 subsystem_controller_default_param_file,
587 subsystem_controller_param_file,
588 {"use_sim_time" : use_sim_time}],
589 on_exit= Shutdown(),
590 arguments=['--ros-args', '--log-level', GetLogLevel('subsystem_controllers', env_log_levels)]
591 )
592
593 return LaunchDescription([
594 declare_vehicle_characteristics_param_file_arg,
595 declare_vehicle_config_param_file_arg,
596 declare_vehicle_config_dir_arg,
597 declare_global_params_override_file_arg,
598 declare_use_sim_time_arg,
599 declare_is_autoware_lidar_obj_detection_enabled,
600 declare_is_cp_mot_enabled,
601 declare_subsystem_controller_param_file_arg,
602 declare_vector_map_file,
603 lidar_perception_container,
604 carma_external_objects_container,
605 lanelet2_map_loader_container,
606 lanelet2_map_visualization_container,
607 carma_cooperative_perception_container,
608 subsystem_controller
609 ])
def generate_launch_description()