15from ament_index_python
import get_package_share_directory
16from launch.actions
import Shutdown
17from launch
import LaunchDescription
18from launch_ros.actions
import Node
19from launch_ros.actions
import ComposableNodeContainer
20from launch_ros.descriptions
import ComposableNode
21from launch.substitutions
import EnvironmentVariable
22from carma_ros2_utils.launch.get_log_level
import GetLogLevel
23from carma_ros2_utils.launch.get_current_namespace
import GetCurrentNamespace
24from launch.substitutions
import LaunchConfiguration
25from launch.actions
import DeclareLaunchArgument
26from launch.conditions
import IfCondition
27from launch.substitutions
import PythonExpression
28from pathlib
import PurePath
34 Launch perception nodes.
36 vehicle_calibration_dir = LaunchConfiguration('vehicle_calibration_dir')
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"
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"
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"
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")
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'
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'
80 autoware_auto_launch_pkg_prefix = get_package_share_directory(
81 'autoware_auto_launch')
83 euclidean_cluster_param_file = os.path.join(
84 autoware_auto_launch_pkg_prefix,
'param/component_style/euclidean_cluster.param.yaml')
86 ray_ground_classifier_param_file = os.path.join(
87 autoware_auto_launch_pkg_prefix,
'param/component_style/ray_ground_classifier.param.yaml')
89 tracking_nodes_param_file = os.path.join(
90 autoware_auto_launch_pkg_prefix,
'param/component_style/tracking_nodes.param.yaml')
92 object_detection_tracking_param_file = os.path.join(
93 get_package_share_directory(
'object_detection_tracking'),
'config/parameters.yaml')
95 subsystem_controller_default_param_file = os.path.join(
96 get_package_share_directory(
'subsystem_controllers'),
'config/environment_perception_controller_config.yaml')
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"
105 frame_transformer_param_file = os.path.join(
106 get_package_share_directory(
'frame_transformer'),
'config/parameters.yaml')
108 object_visualizer_param_file = os.path.join(
109 get_package_share_directory(
'object_visualizer'),
'config/parameters.yaml')
111 points_map_filter_param_file = os.path.join(
112 get_package_share_directory(
'points_map_filter'),
'config/parameters.yaml')
114 motion_computation_param_file = os.path.join(
115 get_package_share_directory(
'motion_computation'),
'config/parameters.yaml')
117 env_log_levels = EnvironmentVariable(
'CARMA_ROS_LOGGING_CONFIG', default_value=
'{ "default_level" : "WARN" }')
119 carma_wm_ctrl_param_file = os.path.join(
120 get_package_share_directory(
'carma_wm_ctrl'),
'config/parameters.yaml')
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"))
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=[
144 package=
'frame_transformer',
145 plugin=
'frame_transformer::Node',
146 name=
'lidar_to_map_frame_transformer',
148 {
'use_intra_process_comms':
True},
149 {
'--log-level' : GetLogLevel(
'frame_transformer', env_log_levels) },
150 {
'is_lifecycle_node':
True}
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")
159 {
"target_frame" :
"map"},
160 {
"message_type" :
"sensor_msgs/PointCloud2"},
163 vehicle_config_param_file
167 package=
'points_map_filter',
168 plugin=
'points_map_filter::Node',
169 name=
'points_map_filter',
171 {
'use_intra_process_comms':
True},
172 {
'--log-level' : GetLogLevel(
'points_map_filter', env_log_levels) },
173 {
'is_lifecycle_node':
True}
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")
182 parameters=[ points_map_filter_param_file, vehicle_config_param_file ]
185 package=
'frame_transformer',
186 plugin=
'frame_transformer::Node',
187 name=
'lidar_frame_transformer',
189 {
'use_intra_process_comms':
True},
190 {
'--log-level' : GetLogLevel(
'frame_transformer', env_log_levels) },
191 {
'is_lifecycle_node':
True}
194 (
"input",
"map_filtered_points" ),
195 (
"output",
"points_in_base_link"),
196 (
"change_state",
"disabled_change_state"),
197 (
"get_state",
"disabled_get_state")
199 parameters=[ frame_transformer_param_file, vehicle_config_param_file ]
202 package=
'ray_ground_classifier_nodes',
203 name=
'ray_ground_filter',
204 plugin=
'autoware::perception::filters::ray_ground_classifier_nodes::RayGroundClassifierCloudNode',
206 {
'use_intra_process_comms':
True},
207 {
'--log-level' : GetLogLevel(
'ray_ground_classifier_nodes', env_log_levels) }
210 (
"points_in",
"points_in_base_link"),
211 (
"points_nonground",
"points_no_ground")
213 parameters=[ ray_ground_classifier_param_file, vehicle_config_param_file]
216 package=
'euclidean_cluster_nodes',
217 name=
'euclidean_cluster',
218 plugin=
'autoware::perception::segmentation::euclidean_cluster_nodes::EuclideanClusterNode',
220 {
'use_intra_process_comms':
True},
221 {
'--log-level' : GetLogLevel(
'euclidean_cluster_nodes', env_log_levels) }
224 (
"points_in",
"points_no_ground")
226 parameters=[ euclidean_cluster_param_file, vehicle_config_param_file]
229 package=
'object_detection_tracking',
230 plugin=
'bounding_box_to_detected_object::Node',
231 name=
'bounding_box_converter',
233 {
'use_intra_process_comms':
True},
234 {
'--log-level' : GetLogLevel(
'object_detection_tracking', env_log_levels) },
235 {
'is_lifecycle_node':
True}
238 (
"bounding_boxes",
"lidar_bounding_boxes"),
239 (
"lidar_detected_objects",
"detected_objects"),
241 parameters=[vehicle_config_param_file]
244 package=
'tracking_nodes',
245 plugin=
'autoware::tracking_nodes::MultiObjectTrackerNode',
246 name=
'tracking_nodes_node',
248 {
'use_intra_process_comms':
True},
249 {
'--log-level' : GetLogLevel(
'tracking_nodes', env_log_levels) }
252 (
"ego_state", [ EnvironmentVariable(
'CARMA_LOCZ_NS', default_value=
''),
"/current_pose_with_covariance" ] ),
256 parameters=[ tracking_nodes_param_file, vehicle_config_param_file]
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=[
272 package=
'carma_wm_ctrl',
273 plugin=
'carma_wm_ctrl::WMBroadcasterNode',
274 name=
'carma_wm_broadcaster',
276 {
'use_intra_process_comms':
True},
277 {
'--log-level' : GetLogLevel(
'carma_wm_ctrl', env_log_levels) }
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" ] )
288 parameters=[ carma_wm_ctrl_param_file, vehicle_config_param_file, vehicle_characteristics_param_file ]
291 package=
'object_detection_tracking',
292 plugin=
'object::ObjectDetectionTrackingNode',
293 name=
'external_object',
295 {
'use_intra_process_comms':
True},
296 {
'--log-level' : GetLogLevel(
'object_detection_tracking', env_log_levels) }
299 (
"detected_objects",
"tracked_objects"),
301 parameters=[ object_detection_tracking_param_file, vehicle_config_param_file]
304 package=
'object_visualizer',
305 plugin=
'object_visualizer::Node',
306 name=
'object_visualizer_node',
308 {
'use_intra_process_comms':
True},
309 {
'--log-level' : GetLogLevel(
'object_visualizer', env_log_levels) }
312 (
"external_objects",
"external_object_predictions"),
313 (
"external_objects_viz",
"fused_external_objects_viz")
315 parameters=[object_visualizer_param_file, vehicle_config_param_file,
316 {
'pedestrian_icon_path': [
'file:///', vehicle_calibration_dir,
'/visualization_meshes/pedestrian.stl']}
320 package=
'motion_computation',
321 plugin=
'motion_computation::MotionComputationNode',
322 name=
'motion_computation_node',
324 {
'use_intra_process_comms':
True},
325 {
'--log-level' : GetLogLevel(
'motion_computation', env_log_levels) }
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" ] ),
333 (
"external_objects", PythonExpression([
'"fused_external_objects" if "', is_cp_mot_enabled,
'" == "True" else "external_objects"'])),
336 motion_computation_param_file, vehicle_config_param_file
340 package=
'motion_prediction_visualizer',
341 plugin=
'motion_prediction_visualizer::Node',
342 name=
'motion_prediction_visualizer',
344 {
'use_intra_process_comms':
True},
345 {
'--log-level' : GetLogLevel(
'motion_prediction_visualizer', env_log_levels) }
348 (
"external_objects",
"external_object_predictions" ),
350 parameters=[ vehicle_config_param_file ]
353 package=
'traffic_incident_parser',
354 plugin=
'traffic_incident_parser::TrafficIncidentParserNode',
355 name=
'traffic_incident_parser_node',
357 {
'use_intra_process_comms':
True},
358 {
'--log-level' : GetLogLevel(
'traffic_incident_parser', env_log_levels) }
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" ] )
368 vehicle_config_param_file
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=[
383 package=
'map_file_ros2',
384 plugin=
'lanelet2_map_loader::Lanelet2MapLoader',
385 name=
'lanelet2_map_loader',
387 {
'use_intra_process_comms':
True},
388 {
'--log-level' : GetLogLevel(
'lanelet2_map_loader', env_log_levels) },
389 {
'is_lifecycle_node':
True}
392 (
"lanelet_map_bin",
"base_map"),
393 (
"change_state",
"disabled_change_state"),
394 (
"get_state",
"disabled_get_state")
397 {
"lanelet2_filename" : vector_map_file},
398 vehicle_config_param_file
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=[
412 package=
'map_file_ros2',
413 plugin=
'lanelet2_map_visualization::Lanelet2MapVisualization',
414 name=
'lanelet2_map_visualization',
416 {
'use_intra_process_comms':
True},
417 {
'--log-level' : GetLogLevel(
'lanelet2_map_visualization', env_log_levels) },
418 {
'is_lifecycle_node':
True}
421 (
"lanelet_map_bin",
"semantic_map"),
422 (
"change_state",
"disabled_change_state"),
423 (
"get_state",
"disabled_get_state")
426 vehicle_config_param_file
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=[
441 package=
'carma_cooperative_perception',
442 plugin=
'carma_cooperative_perception::ExternalObjectListToDetectionListNode',
443 name=
'cp_external_object_list_to_detection_list_node',
445 {
'use_intra_process_comms':
True},
446 {
'--log-level' : GetLogLevel(
'cp_external_object_list_to_detection_list_node', env_log_levels) },
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"),
454 vehicle_config_param_file
458 package=
'carma_cooperative_perception',
459 plugin=
'carma_cooperative_perception::ExternalObjectListToSdsmNode',
460 name=
'cp_external_object_list_to_sdsm_node',
462 {
'use_intra_process_comms':
True},
463 {
'--log-level' : GetLogLevel(
'cp_external_object_list_to_sdsm_node', env_log_levels) },
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"),
472 vehicle_config_param_file
476 package=
'carma_cooperative_perception',
477 plugin=
'carma_cooperative_perception::HostVehicleFilterNode',
478 name=
'cp_host_vehicle_filter_node',
480 {
'use_intra_process_comms':
True},
481 {
'--log-level' : GetLogLevel(
'cp_host_vehicle_filter_node', env_log_levels) },
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")
489 cp_host_vehicle_filter_node_file,
490 vehicle_config_param_file
494 package=
'carma_cooperative_perception',
495 plugin=
'carma_cooperative_perception::SdsmToDetectionListNode',
496 name=
'cp_sdsm_to_detection_list_node',
498 {
'use_intra_process_comms':
True},
499 {
'--log-level' : GetLogLevel(
'cp_sdsm_to_detection_list_node', env_log_levels) },
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"),
508 vehicle_config_param_file,
509 cp_sdsm_to_detection_list_node_file
513 package=
'carma_cooperative_perception',
514 plugin=
'carma_cooperative_perception::TrackListToExternalObjectListNode',
515 name=
'cp_track_list_to_external_object_list_node',
517 {
'use_intra_process_comms':
True},
518 {
'--log-level' : GetLogLevel(
'cp_track_list_to_external_object_list_node', env_log_levels) },
521 (
"input/track_list",
"cooperative_perception_track_list"),
522 (
"output/external_object_list",
"fused_external_objects"),
525 vehicle_config_param_file
529 package=
'carma_cooperative_perception',
530 plugin=
'carma_cooperative_perception::MultipleObjectTrackerNode',
531 name=
'cp_multiple_object_tracker_node',
533 {
'use_intra_process_comms':
True},
534 {
'--log-level' : GetLogLevel(
'cp_multiple_object_tracker_node', env_log_levels) },
537 (
"output/track_list",
"cooperative_perception_track_list"),
538 (
"input/detection_list",
"filtered_detection_list"),
541 cp_multiple_object_tracker_node_file,
542 vehicle_config_param_file
551 subsystem_controller = Node(
552 package=
'subsystem_controllers',
553 name=
'environment_perception_controller',
554 executable=
'environment_perception_controller',
556 subsystem_controller_default_param_file,
557 subsystem_controller_param_file,
558 {
"use_sim_time" : use_sim_time}],
560 arguments=[
'--ros-args',
'--log-level', GetLogLevel(
'subsystem_controllers', env_log_levels)]
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,
def generate_launch_description()