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