64 """
65 Launch CARMA System.
66 """
67
68 system_controller_param_file = os.path.join(
69 get_package_share_directory('system_controller'), 'config/config.yaml')
70
71 env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }')
72
73
74 vehicle_calibration_dir = LaunchConfiguration('vehicle_calibration_dir')
75 declare_vehicle_calibration_dir_arg = DeclareLaunchArgument(
76 name = 'vehicle_calibration_dir',
77 default_value = "/opt/carma/vehicle/calibration",
78 description = "Path to folder containing vehicle calibration directories"
79 )
80
81 vehicle_config_dir = LaunchConfiguration('vehicle_config_dir')
82 declare_vehicle_config_dir_arg = DeclareLaunchArgument(
83 name = 'vehicle_config_dir',
84 default_value = "/opt/carma/vehicle/config",
85 description = "Path to file containing vehicle config directories"
86 )
87
88 vehicle_characteristics_param_file = LaunchConfiguration('vehicle_characteristics_param_file')
89 declare_vehicle_characteristics_param_file_arg = DeclareLaunchArgument(
90 name = 'vehicle_characteristics_param_file',
91 default_value = [vehicle_calibration_dir, "/identifiers/UniqueVehicleParams.yaml"],
92 description = "Path to file containing unique vehicle characteristics"
93 )
94
95
96 vehicle_config_param_file = LaunchConfiguration('vehicle_config_param_file')
97 declare_vehicle_config_param_file_arg = DeclareLaunchArgument(
98 name = 'vehicle_config_param_file',
99 default_value = [vehicle_config_dir, "/VehicleConfigParams.yaml"],
100 description = "Path to file contain vehicle configuration parameters"
101 )
102
103 use_sim_time = LaunchConfiguration('use_sim_time')
104 declare_use_sim_time_arg = DeclareLaunchArgument(
105 name = 'use_sim_time',
106 default_value = 'False',
107 description = "True of simulation mode is on"
108 )
109
110 use_real_time_spat_in_sim = LaunchConfiguration('use_real_time_spat_in_sim')
111 declare_use_real_time_spat_in_sim_arg = DeclareLaunchArgument(
112 name = 'use_real_time_spat_in_sim',
113 default_value = 'False',
114 description = "True if SPaT is being published based on wall clock"
115 )
116
117
118 route_file_folder = LaunchConfiguration('route_file_folder')
119 declare_route_file_folder = DeclareLaunchArgument(
120 name = 'route_file_folder',
121 default_value='/opt/carma/routes/',
122 description = 'Path of folder containing routes to load'
123 )
124
125
126 enable_guidance_plugin_validator = LaunchConfiguration('enable_guidance_plugin_validator')
127 declare_enable_guidance_plugin_validator = DeclareLaunchArgument(
128 name = 'enable_guidance_plugin_validator',
129 default_value='false',
130 description='Flag indicating whether the Guidance Plugin Validator node will actively validate guidance strategic, tactical, and control plugins'
131 )
132
133
134 strategic_plugins_to_validate = LaunchConfiguration('strategic_plugins_to_validate')
135 declare_strategic_plugins_to_validate = DeclareLaunchArgument(
136 name = 'strategic_plugins_to_validate',
137 default_value = '[]',
138 description = 'List of String: Guidance Strategic Plugins that will be validated by the Guidance Plugin Validator Node if enabled'
139 )
140
141
142 tactical_plugins_to_validate = LaunchConfiguration('tactical_plugins_to_validate')
143 declare_tactical_plugins_to_validate = DeclareLaunchArgument(
144 name = 'tactical_plugins_to_validate',
145 default_value='[]',
146 description='List of String: Guidance Tactical Plugins that will be validated by the Guidance Plugin Validator Node if enabled'
147 )
148
149
150 control_plugins_to_validate = LaunchConfiguration('control_plugins_to_validate')
151 declare_control_plugins_to_validate = DeclareLaunchArgument(
152 name = 'control_plugins_to_validate',
153 default_value= '[]',
154 description='List of String: Guidance Control Plugins that will be validated by the Guidance Plugin Validator Node if enabled'
155 )
156
157
158 enable_opening_tunnels = LaunchConfiguration('enable_opening_tunnels')
159 declare_enable_opening_tunnels = DeclareLaunchArgument(
160 name = 'enable_opening_tunnels',
161 default_value= 'False',
162 description='Flag to enable opening http tunnesl to CARMA Cloud'
163 )
164
165
166 port = LaunchConfiguration('port')
167 declare_port = DeclareLaunchArgument(
168 name = 'port',
169 default_value= "9090",
170 description='The default port for rosbridge is 909'
171 )
172
173
174 load_type = LaunchConfiguration('load_type')
175 declare_load_type= DeclareLaunchArgument(name = 'load_type', default_value = "noupdate")
176
177 single_pcd_path = LaunchConfiguration('single_pcd_path')
178 declare_single_pcd_path = DeclareLaunchArgument(name='single_pcd_path', default_value="['/opt/carma/maps/pcd_map.pcd']")
179
180 area = LaunchConfiguration('area')
181 declare_area = DeclareLaunchArgument(name='area', default_value="1x1")
182
183 arealist_path = LaunchConfiguration('arealist_path')
184 declare_arealist_path = DeclareLaunchArgument(name='arealist_path', default_value="/opt/carma/maps/arealist.txt")
185
186 vector_map_file = LaunchConfiguration('vector_map_file')
187 declare_vector_map_file = DeclareLaunchArgument(name='vector_map_file', default_value='/opt/carma/maps/vector_map.osm')
188
189 is_ros2_tracing_enabled = LaunchConfiguration('is_ros2_tracing_enabled')
190 declare_is_ros2_tracing_enabled = DeclareLaunchArgument(
191 name='is_ros2_tracing_enabled',
192 default_value = 'False',
193 description = 'True if user wants ROS 2 Tracing logs to be generated from CARMA Platform.')
194
195
196
197 is_cp_mot_enabled = LaunchConfiguration('is_cp_mot_enabled')
198 declare_is_cp_mot_enabled = DeclareLaunchArgument(
199 name='is_cp_mot_enabled',
200 default_value = 'False',
201 description = 'True if user wants Cooperative Perception capability using Multiple Object Tracking to be enabled'
202 )
203
204
205
206 is_autoware_lidar_obj_detection_enabled = LaunchConfiguration('is_autoware_lidar_obj_detection_enabled')
207 declare_is_autoware_lidar_obj_detection_enabled = DeclareLaunchArgument(
208 name='is_autoware_lidar_obj_detection_enabled',
209 default_value = 'False',
210 description = 'True if user wants Autoware Lidar Object Detection to be enabled'
211 )
212
213
214 ros2_rosbag_launch = GroupAction(
215 actions=[
216 IncludeLaunchDescription(
217 PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/ros2_rosbag.launch.py']),
218 launch_arguments = {
219 'vehicle_config_dir' : vehicle_config_dir,
220 'vehicle_config_param_file' : vehicle_config_param_file
221 }.items()
222 )
223 ]
224 )
225
226
227
228 transform_group = GroupAction(
229 actions=[
230 PushRosNamespace(EnvironmentVariable('CARMA_TF_NS', default_value='/')),
231 IncludeLaunchDescription(
232 PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/transforms.launch.py']),
233 launch_arguments = {
234 'vehicle_config_param_file' : vehicle_config_param_file,
235 'use_sim_time' : use_sim_time
236 }.items()
237 ),
238 ]
239 )
240
241 environment_group = GroupAction(
242 actions=[
243 PushRosNamespace(EnvironmentVariable('CARMA_ENV_NS', default_value='environment')),
244 IncludeLaunchDescription(
245 PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/environment.launch.py']),
246 launch_arguments = {
247 'subsystem_controller_param_file' : [vehicle_config_dir, '/SubsystemControllerParams.yaml'],
248 'vehicle_config_param_file' : vehicle_config_param_file,
249 'vehicle_calibration_dir': vehicle_calibration_dir,
250 'vehicle_characteristics_param_file' : vehicle_characteristics_param_file,
251 'vector_map_file' : vector_map_file,
252 'use_sim_time' : use_sim_time,
253 'is_cp_mot_enabled' : is_cp_mot_enabled,
254 'is_autoware_lidar_obj_detection_enabled' : is_autoware_lidar_obj_detection_enabled
255 }.items()
256 ),
257 ]
258 )
259
260 localization_group = GroupAction(
261 actions=[
262 PushRosNamespace(EnvironmentVariable('CARMA_LOCZ_NS', default_value='localization')),
263 IncludeLaunchDescription(
264 PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/localization.launch.py']),
265 launch_arguments = {
266 'vehicle_config_param_file' : vehicle_config_param_file,
267 'subsystem_controller_param_file' : [vehicle_config_dir, '/SubsystemControllerParams.yaml'],
268 'load_type' : load_type,
269 'single_pcd_path' : single_pcd_path,
270 'area' : area,
271 'arealist_path' : arealist_path,
272 'vector_map_file' : vector_map_file,
273 'vehicle_calibration_dir': vehicle_calibration_dir,
274 'use_sim_time' : use_sim_time
275 }.items()
276 )
277 ]
278 )
279
280 v2x_group = GroupAction(
281 actions=[
282 PushRosNamespace(EnvironmentVariable('CARMA_MSG_NS', default_value='message')),
283 IncludeLaunchDescription(
284 PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/message.launch.py']),
285 launch_arguments = {
286 'vehicle_characteristics_param_file' : vehicle_characteristics_param_file,
287 'vehicle_config_param_file' : vehicle_config_param_file,
288 'enable_opening_tunnels' : enable_opening_tunnels,
289 'subsystem_controller_param_file' : [vehicle_config_dir, '/SubsystemControllerParams.yaml'],
290 'use_sim_time' : use_sim_time
291 }.items()
292 ),
293 ]
294 )
295
296 guidance_group = GroupAction(
297 actions=[
298 PushRosNamespace(EnvironmentVariable('CARMA_GUIDE_NS', default_value='guidance')),
299
300 IncludeLaunchDescription(
301 PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/guidance.launch.py']),
302 launch_arguments={
303 'route_file_folder' : route_file_folder,
304 'vehicle_characteristics_param_file' : vehicle_characteristics_param_file,
305 'vehicle_config_param_file' : vehicle_config_param_file,
306 'enable_guidance_plugin_validator' : enable_guidance_plugin_validator,
307 'strategic_plugins_to_validate' : strategic_plugins_to_validate,
308 'tactical_plugins_to_validate' : tactical_plugins_to_validate,
309 'control_plugins_to_validate' : control_plugins_to_validate,
310 'subsystem_controller_param_file' : [vehicle_config_dir, '/SubsystemControllerParams.yaml'],
311 'use_sim_time' : use_sim_time,
312 'use_real_time_spat_in_sim' : use_real_time_spat_in_sim
313 }.items()
314 ),
315 ]
316 )
317
318 drivers_group = GroupAction(
319 actions=[
320 PushRosNamespace(EnvironmentVariable('CARMA_INTR_NS', default_value='hardware_interface')),
321 IncludeLaunchDescription(
322 PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/drivers.launch.py']),
323 launch_arguments = {
324 'subsystem_controller_param_file' : [vehicle_config_dir, '/SubsystemControllerParams.yaml'],
325 'vehicle_config_param_file' : vehicle_config_param_file,
326 'use_sim_time' : use_sim_time
327 }.items()
328 ),
329 ]
330 )
331
332 system_controller = Node(
333 package='system_controller',
334 name='system_controller',
335 executable='system_controller',
336 parameters=[
337 system_controller_param_file,
338 {"use_sim_time" : use_sim_time}],
339 on_exit = Shutdown(),
340 arguments=['--ros-args', '--log-level', GetLogLevel('system_controller', env_log_levels)]
341 )
342
343 ui_group = GroupAction(
344 actions=[
345 PushRosNamespace(EnvironmentVariable('CARMA_UI_NS', default_value='ui')),
346
347 IncludeLaunchDescription(
348 PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/ui.launch.py']),
349 launch_arguments={
350 'port' : port
351 }.items()
352 ),
353 ]
354 )
355
356 return LaunchDescription([
357 declare_vehicle_calibration_dir_arg,
358 declare_vehicle_config_dir_arg,
359 declare_vehicle_characteristics_param_file_arg,
360 declare_vehicle_config_param_file_arg,
361 declare_use_sim_time_arg,
362 declare_use_real_time_spat_in_sim_arg,
363 declare_route_file_folder,
364 declare_enable_guidance_plugin_validator,
365 declare_strategic_plugins_to_validate,
366 declare_tactical_plugins_to_validate,
367 declare_control_plugins_to_validate,
368 declare_enable_opening_tunnels,
369 declare_port,
370 declare_load_type,
371 declare_single_pcd_path,
372 declare_area,
373 declare_arealist_path,
374 declare_vector_map_file,
375 declare_is_ros2_tracing_enabled,
376 declare_is_cp_mot_enabled,
377 declare_is_autoware_lidar_obj_detection_enabled,
378 ros2_rosbag_launch,
379 OpaqueFunction(function=create_ros2_tracing_action),
380 drivers_group,
381 transform_group,
382 environment_group,
383 localization_group,
384 v2x_group,
385 guidance_group,
386 ui_group,
387 system_controller
388 ])
def generate_launch_description()