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