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
29from launch.actions
import IncludeLaunchDescription
30from launch.launch_description_sources
import PythonLaunchDescriptionSource
31from launch.actions
import GroupAction
32from launch_ros.actions
import set_remap
33from launch.conditions
import UnlessCondition
34from launch.substitutions
import LaunchConfiguration, PythonExpression
39 Launch Localization subsystem nodes
42 env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value=
'{ "default_level" : "WARN" }')
43 vehicle_config_param_file = LaunchConfiguration(
'vehicle_config_param_file')
44 subsystem_controller_default_param_file = os.path.join(
45 get_package_share_directory(
'subsystem_controllers'),
'config/localization_controller_config.yaml')
47 subsystem_controller_param_file = LaunchConfiguration(
'subsystem_controller_param_file')
48 declare_subsystem_controller_param_file_arg = DeclareLaunchArgument(
49 name =
'subsystem_controller_param_file',
50 default_value = subsystem_controller_default_param_file,
51 description =
"Path to file containing override parameters for the subsystem controller"
57 gnss_to_map_convertor_param_file = os.path.join(
58 get_package_share_directory(
'gnss_to_map_convertor'),
'config/parameters.yaml')
60 localization_manager_convertor_param_file = os.path.join(
61 get_package_share_directory(
'localization_manager'),
'config/parameters.yaml')
63 vehicle_calibration_dir = LaunchConfiguration(
'vehicle_calibration_dir')
64 ndt_matching_param_file = [vehicle_calibration_dir,
"/lidar_localizer/ndt_matching/params.yaml"]
67 load_type = LaunchConfiguration(
'load_type')
68 declare_load_type= DeclareLaunchArgument(name =
'load_type', default_value =
"noupdate")
70 single_pcd_path = LaunchConfiguration(
'single_pcd_path')
71 declare_single_pcd_path = DeclareLaunchArgument(name=
'single_pcd_path', default_value=
"['/opt/carma/maps/pcd_map.pcd']", description=
'Path to the map pcd file if using the noupdate load type')
73 vehicle_config_dir = LaunchConfiguration(
'vehicle_config_dir')
74 declare_vehicle_config_dir_arg = DeclareLaunchArgument(
75 name =
'vehicle_config_dir',
76 default_value =
"/opt/carma/vehicle/config",
77 description =
"Path to vehicle configuration directory populated by carma-config"
82 global_params_override_file = LaunchConfiguration(
'global_params_override_file')
83 declare_global_params_override_file_arg = DeclareLaunchArgument(
84 name =
'global_params_override_file',
85 default_value = [vehicle_config_dir,
"/GlobalParamsOverride.yaml"],
86 description =
"Path to global file containing the parameters overwrite"
89 area = LaunchConfiguration(
'area')
90 declare_area = DeclareLaunchArgument(name=
'area', default_value=
"1x1")
92 arealist_path = LaunchConfiguration(
'arealist_path')
93 declare_arealist_path = DeclareLaunchArgument(name=
'arealist_path', default_value=
"/opt/carma/maps/arealist.txt")
95 vector_map_file = LaunchConfiguration(
'vector_map_file')
96 declare_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')
98 use_sim_time = LaunchConfiguration(
'use_sim_time')
99 declare_use_sim_time_arg = DeclareLaunchArgument(
100 name =
'use_sim_time',
101 default_value =
'False',
102 description =
"True if simulation mode is on"
105 gnss_to_map_convertor_container = ComposableNodeContainer(
106 package=
'carma_ros2_utils',
107 name=
'gnss_to_map_convertor_container',
108 executable=
'carma_component_container_mt',
109 namespace=GetCurrentNamespace(),
110 composable_node_descriptions=[
113 package=
'gnss_to_map_convertor',
114 plugin=
'gnss_to_map_convertor::Node',
115 name=
'gnss_to_map_convertor',
117 {
'use_intra_process_comms':
True},
118 {
'--log-level' : GetLogLevel(
'gnss_to_map_convertor', env_log_levels) }
121 (
"gnss_fix_fused", [EnvironmentVariable(
'CARMA_INTR_NS', default_value=
''),
"/gnss_fix_fused"]),
122 (
"georeference",
"map_param_loader/georeference"),
124 parameters=[ gnss_to_map_convertor_param_file,
125 vehicle_config_param_file,
126 global_params_override_file]
130 localization_manager_container = ComposableNodeContainer(
131 package=
'carma_ros2_utils',
132 name=
'localization_manager_container',
133 executable=
'carma_component_container_mt',
134 namespace=GetCurrentNamespace(),
135 composable_node_descriptions=[
137 package=
'localization_manager',
138 plugin=
'localization_manager::Node',
139 name=
'localization_manager',
141 {
'use_intra_process_comms':
True},
142 {
'--log-level' : GetLogLevel(
'localization_manager', env_log_levels) }
147 parameters=[ localization_manager_convertor_param_file,
148 vehicle_config_param_file,
149 global_params_override_file]
156 map_param_loader_container = ComposableNodeContainer(
157 package=
'carma_ros2_utils',
158 name=
'map_param_loader_container',
159 executable=
'carma_component_container_mt',
160 namespace=GetCurrentNamespace(),
161 composable_node_descriptions=[
163 package=
'map_file_ros2',
164 plugin=
'map_param_loader::MapParamLoader',
165 name=
'map_param_loader',
167 {
'use_intra_process_comms':
True},
168 {
'--log-level' : GetLogLevel(
'map_param_loader', env_log_levels) }
171 (
"georeference",
"map_param_loader/georeference"),
173 parameters=[ {
'file_name' : vector_map_file },
174 vehicle_config_param_file,
175 global_params_override_file]
180 pcd_map_file_loader_container = ComposableNodeContainer(
181 package=
'carma_ros2_utils',
182 name=
'map_file_nodes_container',
183 namespace=GetCurrentNamespace(),
184 executable=
'carma_component_container_mt',
185 composable_node_descriptions=[
188 package=
'map_file_ros2',
189 plugin=
'points_map_loader::PointsMapLoader',
190 name=
'points_map_loader',
192 {
'use_intra_process_comms':
True},
193 {
'--log-level' : GetLogLevel(
'points_map_loader', env_log_levels) }
196 {
'load_type' : load_type },
197 {
'pcd_path_parameter' : single_pcd_path },
199 {
'path_area_list' : arealist_path },
200 vehicle_config_param_file,
201 global_params_override_file
208 dead_reckoner_container = ComposableNodeContainer(
209 package=
'carma_ros2_utils',
210 name=
'dead_reckoner_container',
211 namespace=GetCurrentNamespace(),
212 executable=
'carma_component_container_mt',
213 composable_node_descriptions=[
216 package=
'dead_reckoner',
217 plugin=
'dead_reckoner::DeadReckoner',
218 name=
'dead_reckoner',
220 {
'use_intra_process_comms':
True},
221 {
'--log-level' : GetLogLevel(
'dead_reckoner', env_log_levels) }
224 (
"current_twist", [EnvironmentVariable(
'CARMA_INTR_NS', default_value=
''),
"/vehicle/twist" ]),
225 (
"current_odom",
"vehicle/odom")
228 vehicle_config_param_file, global_params_override_file
238 ndt_matching_container = ComposableNodeContainer(
239 package=
'carma_ros2_utils',
240 name=
'ndt_matching_container',
241 namespace=GetCurrentNamespace(),
242 executable=
'carma_component_container_mt',
243 composable_node_descriptions=[
246 package=
'lidar_localizer_ros2',
247 plugin=
'ndt_matching::NDTMatching',
250 {
'use_intra_process_comms':
True},
251 {
'--log-level' : GetLogLevel(
'ndt_matching', env_log_levels) }
254 (
"/config/ndt",
"config/ndt"),
255 (
"/imu_raw", [ EnvironmentVariable(
'CARMA_INTR_NS', default_value=
''),
"/imu_raw" ] ),
256 (
'filtered_points',
'random_points'),
257 (
'initialpose',
'managed_initialpose'),
258 (
'/tf',
'/tf_ndt_UNUSED'),
261 ndt_matching_param_file,
262 {
'get_height' :
True },
263 {
'use_odom' :
True },
265 {
'gnss_reinit_fitness' : 10000.0 },
266 {
'base_frame':
"base_link"},
267 global_params_override_file
275 ekf_localizer_container = ComposableNodeContainer(
276 condition=UnlessCondition(use_sim_time),
277 package=
'carma_ros2_utils',
278 name=
'ekf_localizer_container',
279 namespace=GetCurrentNamespace(),
280 executable=
'carma_component_container_mt',
281 composable_node_descriptions=[
284 package=
'ekf_localizer',
285 plugin=
'ekf_localizer::EKFLocalizer',
286 name=
'ekf_localizer',
288 {
'use_intra_process_comms':
True},
289 {
'--log-level' : GetLogLevel(
'ekf_localizer', env_log_levels) }
292 (
"in_pose",
"selected_pose"),
293 (
"in_pose_with_covariance",
"input_pose_with_cov_UNUSED"),
294 (
"in_twist", [EnvironmentVariable(
'CARMA_INTR_NS', default_value=
''),
"/vehicle/twist" ]),
295 (
"in_twist_with_covariance",
"input_twist_with_covariance_UNUSED"),
296 (
"initialpose",
"managed_initialpose"),
297 (
"ekf_pose",
"current_pose"),
298 (
"ekf_pose_with_covariance",
"current_pose_with_covariance"),
300 (
"debug",
"~/debug"),
301 (
"debug/measured_pose",
"~/debug/measured_pose"),
302 (
"estimated_yaw_bias",
"~/estimated_yaw_bias")
305 {
'show_debug_info':
False},
306 {
'predict_frequency': 50.0},
307 {
'enable_yaw_bias_estimation':
True},
308 {
'extend_state_step': 50},
309 {
'pose_frame_id':
'map'},
310 {
'child_frame_id':
'base_link'},
311 {
'pose_additional_delay': 0.0},
312 {
'pose_measure_uncertainty_time': 0.01},
314 {
'pose_gate_dist': 10000.0},
315 {
'pose_stddev_x': 0.05},
316 {
'pose_stddev_y': 0.05},
317 {
'pose_stddev_yaw': 0.025},
318 {
'use_pose_with_covariance':
False},
319 {
'twist_additional_delay': 0.0},
320 {
'twist_rate': 30.0},
321 {
'twist_gate_dist': 10000.0},
322 {
'twist_stddev_vx': 0.2},
323 {
'twist_stddev_wz': 0.03},
324 {
'proc_stddev_yaw_c': 0.005},
325 {
'proc_stddev_yaw_bias_c': 0.001},
326 {
'proc_stddev_vx_c': 0.1},
327 {
'proc_stddev_wz_c': 0.05},
328 global_params_override_file
336 voxel_grid_filter_container = ComposableNodeContainer(
337 package=
'carma_ros2_utils',
338 name=
'voxel_grid_filter_container',
339 namespace=GetCurrentNamespace(),
340 executable=
'carma_component_container_mt',
341 composable_node_descriptions=[
345 package=
'points_downsampler',
346 plugin=
'voxel_grid_filter::VoxelGridFilter',
347 name=
'voxel_grid_filter_node',
349 {
'use_intra_process_comms':
True},
350 {
'--log-level' : GetLogLevel(
'voxel_grid_filter', env_log_levels) }
353 {
"points_topic": [EnvironmentVariable(
'CARMA_INTR_NS', default_value=
''),
"/lidar/points_raw" ]},
354 {
"output_log":
False},
355 {
"measurement_range": 200.0},
356 {
"voxel_leaf_size": 3.0},
357 global_params_override_file
363 random_filter_container = ComposableNodeContainer(
364 package=
'carma_ros2_utils',
365 name=
'random_filter_container',
366 namespace=GetCurrentNamespace(),
367 executable=
'carma_component_container_mt',
368 composable_node_descriptions=[
372 package=
'points_downsampler',
373 plugin=
'random_filter::RandomFilter',
374 name=
'random_filter_node',
376 {
'use_intra_process_comms':
True},
377 {
'--log-level' : GetLogLevel(
'random_filter', env_log_levels) }
380 {
"points_topic":
"filtered_points"},
381 {
"output_log":
False},
382 {
"measurement_range": 200.0},
384 global_params_override_file
392 subsystem_controller = Node(
393 package=
'subsystem_controllers',
394 name=
'localization_controller',
395 executable=
'localization_controller',
397 subsystem_controller_default_param_file,
398 subsystem_controller_param_file,
399 {
"use_sim_time" : use_sim_time}],
401 arguments=[
'--ros-args',
'--log-level', GetLogLevel(
'subsystem_controllers', env_log_levels)]
404 return LaunchDescription([
405 declare_subsystem_controller_param_file_arg,
407 declare_single_pcd_path,
409 declare_arealist_path,
411 declare_use_sim_time_arg,
412 declare_vehicle_config_dir_arg,
413 declare_global_params_override_file_arg,
414 gnss_to_map_convertor_container,
415 localization_manager_container,
416 dead_reckoner_container,
417 voxel_grid_filter_container,
418 random_filter_container,
419 map_param_loader_container,
420 pcd_map_file_loader_container,
421 ndt_matching_container,
422 ekf_localizer_container,
def generate_launch_description()