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.
37 vehicle_config_param_file = LaunchConfiguration('vehicle_config_param_file')
38 declare_vehicle_config_param_file_arg = DeclareLaunchArgument(
39 name =
'vehicle_config_param_file',
40 default_value =
"/opt/carma/vehicle/config/VehicleConfigParams.yaml",
41 description =
"Path to file contain vehicle configuration parameters"
44 use_sim_time = LaunchConfiguration(
'use_sim_time')
45 declare_use_sim_time_arg = DeclareLaunchArgument(
46 name =
'use_sim_time',
47 default_value =
"False",
48 description =
"True if simulation mode is on"
51 vehicle_characteristics_param_file = LaunchConfiguration(
'vehicle_characteristics_param_file')
52 declare_vehicle_characteristics_param_file_arg = DeclareLaunchArgument(
53 name =
'vehicle_characteristics_param_file',
54 default_value =
"/opt/carma/vehicle/calibration/identifiers/UniqueVehicleParams.yaml",
55 description =
"Path to file containing unique vehicle calibrations"
58 vector_map_file = LaunchConfiguration(
'vector_map_file')
59 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")
63 is_cp_mot_enabled = LaunchConfiguration(
'is_cp_mot_enabled')
64 declare_is_cp_mot_enabled = DeclareLaunchArgument(
65 name=
'is_cp_mot_enabled',
66 default_value =
'False',
67 description =
'True if user wants Cooperative Perception capability using Multiple Object Tracking to be enabled'
72 is_autoware_lidar_obj_detection_enabled = LaunchConfiguration(
'is_autoware_lidar_obj_detection_enabled')
73 declare_is_autoware_lidar_obj_detection_enabled = DeclareLaunchArgument(
74 name=
'is_autoware_lidar_obj_detection_enabled',
75 default_value =
'False',
76 description =
'True if user wants Autoware Lidar Object Detection to be enabled'
79 autoware_auto_launch_pkg_prefix = get_package_share_directory(
80 'autoware_auto_launch')
82 euclidean_cluster_param_file = os.path.join(
83 autoware_auto_launch_pkg_prefix,
'param/component_style/euclidean_cluster.param.yaml')
85 ray_ground_classifier_param_file = os.path.join(
86 autoware_auto_launch_pkg_prefix,
'param/component_style/ray_ground_classifier.param.yaml')
88 tracking_nodes_param_file = os.path.join(
89 autoware_auto_launch_pkg_prefix,
'param/component_style/tracking_nodes.param.yaml')
91 object_detection_tracking_param_file = os.path.join(
92 get_package_share_directory(
'object_detection_tracking'),
'config/parameters.yaml')
94 subsystem_controller_default_param_file = os.path.join(
95 get_package_share_directory(
'subsystem_controllers'),
'config/environment_perception_controller_config.yaml')
97 subsystem_controller_param_file = LaunchConfiguration(
'subsystem_controller_param_file')
98 declare_subsystem_controller_param_file_arg = DeclareLaunchArgument(
99 name =
'subsystem_controller_param_file',
100 default_value = subsystem_controller_default_param_file,
101 description =
"Path to file containing override parameters for the subsystem controller"
104 frame_transformer_param_file = os.path.join(
105 get_package_share_directory(
'frame_transformer'),
'config/parameters.yaml')
107 object_visualizer_param_file = os.path.join(
108 get_package_share_directory(
'object_visualizer'),
'config/parameters.yaml')
110 points_map_filter_param_file = os.path.join(
111 get_package_share_directory(
'points_map_filter'),
'config/parameters.yaml')
113 motion_computation_param_file = os.path.join(
114 get_package_share_directory(
'motion_computation'),
'config/parameters.yaml')
116 env_log_levels = EnvironmentVariable(
'CARMA_ROS_LOGGING_CONFIG', default_value=
'{ "default_level" : "WARN" }')
118 carma_wm_ctrl_param_file = os.path.join(
119 get_package_share_directory(
'carma_wm_ctrl'),
'config/parameters.yaml')
121 cp_multiple_object_tracker_node_file = str(PurePath(get_package_share_directory(
"carma_cooperative_perception"),
"config/cp_multiple_object_tracker_node.yaml"))
122 cp_host_vehicle_filter_node_file = str(PurePath(get_package_share_directory(
"carma_cooperative_perception"),
"config/cp_host_vehicle_filter_node.yaml"))
129 lidar_perception_container = ComposableNodeContainer(
130 condition=IfCondition(is_autoware_lidar_obj_detection_enabled),
131 package=
'carma_ros2_utils',
132 name=
'perception_points_filter_container',
133 executable=
'lifecycle_component_wrapper_mt',
134 namespace=GetCurrentNamespace(),
135 composable_node_descriptions=[
137 package=
'frame_transformer',
138 plugin=
'frame_transformer::Node',
139 name=
'lidar_to_map_frame_transformer',
141 {
'use_intra_process_comms':
True},
142 {
'--log-level' : GetLogLevel(
'frame_transformer', env_log_levels) },
143 {
'is_lifecycle_node':
True}
146 (
"input", [ EnvironmentVariable(
'CARMA_INTR_NS', default_value=
''),
"/lidar/points_raw" ] ),
147 (
"output",
"points_in_map"),
148 (
"change_state",
"disabled_change_state"),
149 (
"get_state",
"disabled_get_state")
152 {
"target_frame" :
"map"},
153 {
"message_type" :
"sensor_msgs/PointCloud2"},
156 vehicle_config_param_file
160 package=
'points_map_filter',
161 plugin=
'points_map_filter::Node',
162 name=
'points_map_filter',
164 {
'use_intra_process_comms':
True},
165 {
'--log-level' : GetLogLevel(
'points_map_filter', env_log_levels) },
166 {
'is_lifecycle_node':
True}
169 (
"points_raw",
"points_in_map" ),
170 (
"filtered_points",
"map_filtered_points"),
171 (
"lanelet2_map",
"semantic_map"),
172 (
"change_state",
"disabled_change_state"),
173 (
"get_state",
"disabled_get_state")
175 parameters=[ points_map_filter_param_file, vehicle_config_param_file ]
178 package=
'frame_transformer',
179 plugin=
'frame_transformer::Node',
180 name=
'lidar_frame_transformer',
182 {
'use_intra_process_comms':
True},
183 {
'--log-level' : GetLogLevel(
'frame_transformer', env_log_levels) },
184 {
'is_lifecycle_node':
True}
187 (
"input",
"map_filtered_points" ),
188 (
"output",
"points_in_base_link"),
189 (
"change_state",
"disabled_change_state"),
190 (
"get_state",
"disabled_get_state")
192 parameters=[ frame_transformer_param_file, vehicle_config_param_file ]
195 package=
'ray_ground_classifier_nodes',
196 name=
'ray_ground_filter',
197 plugin=
'autoware::perception::filters::ray_ground_classifier_nodes::RayGroundClassifierCloudNode',
199 {
'use_intra_process_comms':
True},
200 {
'--log-level' : GetLogLevel(
'ray_ground_classifier_nodes', env_log_levels) }
203 (
"points_in",
"points_in_base_link"),
204 (
"points_nonground",
"points_no_ground")
206 parameters=[ ray_ground_classifier_param_file, vehicle_config_param_file]
209 package=
'euclidean_cluster_nodes',
210 name=
'euclidean_cluster',
211 plugin=
'autoware::perception::segmentation::euclidean_cluster_nodes::EuclideanClusterNode',
213 {
'use_intra_process_comms':
True},
214 {
'--log-level' : GetLogLevel(
'euclidean_cluster_nodes', env_log_levels) }
217 (
"points_in",
"points_no_ground")
219 parameters=[ euclidean_cluster_param_file, vehicle_config_param_file]
222 package=
'object_detection_tracking',
223 plugin=
'bounding_box_to_detected_object::Node',
224 name=
'bounding_box_converter',
226 {
'use_intra_process_comms':
True},
227 {
'--log-level' : GetLogLevel(
'object_detection_tracking', env_log_levels) },
228 {
'is_lifecycle_node':
True}
231 (
"bounding_boxes",
"lidar_bounding_boxes"),
232 (
"lidar_detected_objects",
"detected_objects"),
234 parameters=[vehicle_config_param_file]
237 package=
'tracking_nodes',
238 plugin=
'autoware::tracking_nodes::MultiObjectTrackerNode',
239 name=
'tracking_nodes_node',
241 {
'use_intra_process_comms':
True},
242 {
'--log-level' : GetLogLevel(
'tracking_nodes', env_log_levels) }
245 (
"ego_state", [ EnvironmentVariable(
'CARMA_LOCZ_NS', default_value=
''),
"/current_pose_with_covariance" ] ),
249 parameters=[ tracking_nodes_param_file, vehicle_config_param_file]
258 carma_external_objects_container = ComposableNodeContainer(
259 package=
'carma_ros2_utils',
260 name=
'external_objects_container',
261 executable=
'carma_component_container_mt',
262 namespace=GetCurrentNamespace(),
263 composable_node_descriptions=[
265 package=
'carma_wm_ctrl',
266 plugin=
'carma_wm_ctrl::WMBroadcasterNode',
267 name=
'carma_wm_broadcaster',
269 {
'use_intra_process_comms':
True},
270 {
'--log-level' : GetLogLevel(
'carma_wm_ctrl', env_log_levels) }
273 (
"georeference", [ EnvironmentVariable(
'CARMA_LOCZ_NS', default_value=
''),
"/map_param_loader/georeference" ] ),
274 (
"geofence", [ EnvironmentVariable(
'CARMA_MSG_NS', default_value=
''),
"/incoming_geofence_control" ] ),
275 (
"incoming_map", [ EnvironmentVariable(
'CARMA_MSG_NS', default_value=
''),
"/incoming_map" ] ),
276 (
"current_pose", [ EnvironmentVariable(
'CARMA_LOCZ_NS', default_value=
''),
"/current_pose" ] ),
277 (
"route", [ EnvironmentVariable(
'CARMA_GUIDE_NS', default_value=
''),
"/route" ] ),
278 (
"outgoing_geofence_ack", [ EnvironmentVariable(
'CARMA_MSG_NS', default_value=
''),
"/outgoing_mobility_operation" ] ),
279 (
"outgoing_geofence_request", [ EnvironmentVariable(
'CARMA_MSG_NS', default_value=
''),
"/outgoing_geofence_request" ] )
281 parameters=[ carma_wm_ctrl_param_file, vehicle_config_param_file, vehicle_characteristics_param_file ]
284 package=
'object_detection_tracking',
285 plugin=
'object::ObjectDetectionTrackingNode',
286 name=
'external_object',
288 {
'use_intra_process_comms':
True},
289 {
'--log-level' : GetLogLevel(
'object_detection_tracking', env_log_levels) }
292 (
"detected_objects",
"tracked_objects"),
294 parameters=[ object_detection_tracking_param_file, vehicle_config_param_file]
297 package=
'object_visualizer',
298 plugin=
'object_visualizer::Node',
299 name=
'object_visualizer_node',
301 {
'use_intra_process_comms':
True},
302 {
'--log-level' : GetLogLevel(
'object_visualizer', env_log_levels) }
305 (
"external_objects",
"external_object_predictions"),
306 (
"external_objects_viz",
"fused_external_objects_viz")
308 parameters=[ object_visualizer_param_file, vehicle_config_param_file ]
311 package=
'motion_computation',
312 plugin=
'motion_computation::MotionComputationNode',
313 name=
'motion_computation_node',
315 {
'use_intra_process_comms':
True},
316 {
'--log-level' : GetLogLevel(
'motion_computation', env_log_levels) }
319 (
"incoming_mobility_path", [ EnvironmentVariable(
'CARMA_MSG_NS', default_value=
''),
"/incoming_mobility_path" ] ),
320 (
"incoming_psm", [ EnvironmentVariable(
'CARMA_MSG_NS', default_value=
''),
"/incoming_psm" ] ),
321 (
"incoming_bsm", [ EnvironmentVariable(
'CARMA_MSG_NS', default_value=
''),
"/incoming_bsm" ] ),
322 (
"georeference", [ EnvironmentVariable(
'CARMA_LOCZ_NS', default_value=
''),
"/map_param_loader/georeference" ] ),
324 (
"external_objects", PythonExpression([
'"fused_external_objects" if "', is_cp_mot_enabled,
'" == "True" else "external_objects"'])),
327 motion_computation_param_file, vehicle_config_param_file
331 package=
'motion_prediction_visualizer',
332 plugin=
'motion_prediction_visualizer::Node',
333 name=
'motion_prediction_visualizer',
335 {
'use_intra_process_comms':
True},
336 {
'--log-level' : GetLogLevel(
'motion_prediction_visualizer', env_log_levels) }
339 (
"external_objects",
"external_object_predictions" ),
341 parameters=[ vehicle_config_param_file ]
344 package=
'traffic_incident_parser',
345 plugin=
'traffic_incident_parser::TrafficIncidentParserNode',
346 name=
'traffic_incident_parser_node',
348 {
'use_intra_process_comms':
True},
349 {
'--log-level' : GetLogLevel(
'traffic_incident_parser', env_log_levels) }
352 (
"georeference", [ EnvironmentVariable(
'CARMA_LOCZ_NS', default_value=
''),
"/map_param_loader/georeference" ] ),
353 (
"geofence", [ EnvironmentVariable(
'CARMA_MSG_NS', default_value=
''),
"/incoming_geofence_control" ] ),
354 (
"incoming_mobility_operation", [ EnvironmentVariable(
'CARMA_MSG_NS', default_value=
''),
"/incoming_mobility_operation" ] ),
355 (
"incoming_spat", [ EnvironmentVariable(
'CARMA_MSG_NS', default_value=
''),
"/incoming_spat" ] ),
356 (
"route", [ EnvironmentVariable(
'CARMA_GUIDE_NS', default_value=
''),
"/route" ] )
359 vehicle_config_param_file
367 lanelet2_map_loader_container = ComposableNodeContainer(
368 package=
'carma_ros2_utils',
369 name=
'lanelet2_map_loader_container',
370 executable=
'lifecycle_component_wrapper_mt',
371 namespace=GetCurrentNamespace(),
372 composable_node_descriptions=[
374 package=
'map_file_ros2',
375 plugin=
'lanelet2_map_loader::Lanelet2MapLoader',
376 name=
'lanelet2_map_loader',
378 {
'use_intra_process_comms':
True},
379 {
'--log-level' : GetLogLevel(
'lanelet2_map_loader', env_log_levels) },
380 {
'is_lifecycle_node':
True}
383 (
"lanelet_map_bin",
"base_map"),
384 (
"change_state",
"disabled_change_state"),
385 (
"get_state",
"disabled_get_state")
388 {
"lanelet2_filename" : vector_map_file},
389 vehicle_config_param_file
396 lanelet2_map_visualization_container = ComposableNodeContainer(
397 package=
'carma_ros2_utils',
398 name=
'lanelet2_map_visualization_container',
399 executable=
'lifecycle_component_wrapper_mt',
400 namespace= GetCurrentNamespace(),
401 composable_node_descriptions=[
403 package=
'map_file_ros2',
404 plugin=
'lanelet2_map_visualization::Lanelet2MapVisualization',
405 name=
'lanelet2_map_visualization',
407 {
'use_intra_process_comms':
True},
408 {
'--log-level' : GetLogLevel(
'lanelet2_map_visualization', env_log_levels) },
409 {
'is_lifecycle_node':
True}
412 (
"lanelet_map_bin",
"semantic_map"),
413 (
"change_state",
"disabled_change_state"),
414 (
"get_state",
"disabled_get_state")
417 vehicle_config_param_file
424 carma_cooperative_perception_container = ComposableNodeContainer(
425 condition=IfCondition(is_cp_mot_enabled),
426 package=
'carma_ros2_utils',
427 name=
'carma_cooperative_perception_container',
428 executable=
'carma_component_container_mt',
429 namespace= GetCurrentNamespace(),
430 composable_node_descriptions=[
432 package=
'carma_cooperative_perception',
433 plugin=
'carma_cooperative_perception::ExternalObjectListToDetectionListNode',
434 name=
'cp_external_object_list_to_detection_list_node',
436 {
'use_intra_process_comms':
True},
437 {
'--log-level' : GetLogLevel(
'cp_external_object_list_to_detection_list_node', env_log_levels) },
440 (
"input/georeference", [EnvironmentVariable(
"CARMA_LOCZ_NS", default_value=
""),
"/map_param_loader/georeference"]),
441 (
"output/detections",
"full_detection_list"),
442 (
"input/external_objects",
"external_objects"),
445 vehicle_config_param_file
449 package=
'carma_cooperative_perception',
450 plugin=
'carma_cooperative_perception::ExternalObjectListToSdsmNode',
451 name=
'cp_external_object_list_to_sdsm_node',
453 {
'use_intra_process_comms':
True},
454 {
'--log-level' : GetLogLevel(
'cp_external_object_list_to_sdsm_node', env_log_levels) },
457 (
"input/georeference", [EnvironmentVariable(
"CARMA_LOCZ_NS", default_value=
""),
"/map_param_loader/georeference"]),
458 (
"output/sdsms", [ EnvironmentVariable(
'CARMA_MSG_NS', default_value=
''),
"/outgoing_sdsm" ] ),
459 (
"input/pose_stamped", [ EnvironmentVariable(
'CARMA_LOCZ_NS', default_value=
''),
"/current_pose" ] ),
460 (
"input/external_objects",
"external_objects"),
463 vehicle_config_param_file
467 package=
'carma_cooperative_perception',
468 plugin=
'carma_cooperative_perception::HostVehicleFilterNode',
469 name=
'cp_host_vehicle_filter_node',
471 {
'use_intra_process_comms':
True},
472 {
'--log-level' : GetLogLevel(
'cp_host_vehicle_filter_node', env_log_levels) },
475 (
"input/host_vehicle_pose", [ EnvironmentVariable(
'CARMA_LOCZ_NS', default_value=
''),
"/current_pose" ] ),
476 (
"input/detection_list",
"full_detection_list"),
477 (
"output/detection_list",
"filtered_detection_list")
480 cp_host_vehicle_filter_node_file,
481 vehicle_config_param_file
485 package=
'carma_cooperative_perception',
486 plugin=
'carma_cooperative_perception::SdsmToDetectionListNode',
487 name=
'cp_sdsm_to_detection_list_node',
489 {
'use_intra_process_comms':
True},
490 {
'--log-level' : GetLogLevel(
'cp_sdsm_to_detection_list_node', env_log_levels) },
493 (
"input/georeference", [ EnvironmentVariable(
'CARMA_LOCZ_NS', default_value=
''),
"/map_param_loader/georeference" ] ),
494 (
"input/sdsm", [ EnvironmentVariable(
'CARMA_MSG_NS', default_value=
''),
"/incoming_sdsm" ] ),
495 (
"input/cdasim_clock",
"/sim_clock"),
496 (
"output/detections",
"full_detection_list"),
499 vehicle_config_param_file
503 package=
'carma_cooperative_perception',
504 plugin=
'carma_cooperative_perception::TrackListToExternalObjectListNode',
505 name=
'cp_track_list_to_external_object_list_node',
507 {
'use_intra_process_comms':
True},
508 {
'--log-level' : GetLogLevel(
'cp_track_list_to_external_object_list_node', env_log_levels) },
511 (
"input/track_list",
"cooperative_perception_track_list"),
512 (
"output/external_object_list",
"fused_external_objects"),
515 vehicle_config_param_file
519 package=
'carma_cooperative_perception',
520 plugin=
'carma_cooperative_perception::MultipleObjectTrackerNode',
521 name=
'cp_multiple_object_tracker_node',
523 {
'use_intra_process_comms':
True},
524 {
'--log-level' : GetLogLevel(
'cp_multiple_object_tracker_node', env_log_levels) },
527 (
"output/track_list",
"cooperative_perception_track_list"),
528 (
"input/detection_list",
"filtered_detection_list"),
531 cp_multiple_object_tracker_node_file,
532 vehicle_config_param_file
540 subsystem_controller = Node(
541 package=
'subsystem_controllers',
542 name=
'environment_perception_controller',
543 executable=
'environment_perception_controller',
545 subsystem_controller_default_param_file,
546 subsystem_controller_param_file,
547 {
"use_sim_time" : use_sim_time}],
549 arguments=[
'--ros-args',
'--log-level', GetLogLevel(
'subsystem_controllers', env_log_levels)]
552 return LaunchDescription([
553 declare_vehicle_characteristics_param_file_arg,
554 declare_vehicle_config_param_file_arg,
555 declare_use_sim_time_arg,
556 declare_is_autoware_lidar_obj_detection_enabled,
557 declare_is_cp_mot_enabled,
558 declare_subsystem_controller_param_file_arg,
559 declare_vector_map_file,
560 lidar_perception_container,
561 carma_external_objects_container,
562 lanelet2_map_loader_container,
563 lanelet2_map_visualization_container,
564 carma_cooperative_perception_container,
def generate_launch_description()