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 is_spat_wall_time = LaunchConfiguration('is_spat_wall_time')
111 declare_is_spat_wall_time_arg = DeclareLaunchArgument(
112 name = 'is_spat_wall_time',
113 default_value = 'True',
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 ros2_rosbag_launch = GroupAction(
197 actions=[
198 IncludeLaunchDescription(
199 PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/ros2_rosbag.launch.py']),
200 launch_arguments = {
201 'vehicle_config_dir' : vehicle_config_dir,
202 'vehicle_config_param_file' : vehicle_config_param_file
203 }.items()
204 )
205 ]
206 )
207
208
209
210 transform_group = GroupAction(
211 actions=[
212 PushRosNamespace(EnvironmentVariable('CARMA_TF_NS', default_value='/')),
213 IncludeLaunchDescription(
214 PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/transforms.launch.py']),
215 launch_arguments = {
216 'vehicle_config_param_file' : vehicle_config_param_file,
217 'use_sim_time' : use_sim_time
218 }.items()
219 ),
220 ]
221 )
222
223 environment_group = GroupAction(
224 actions=[
225 PushRosNamespace(EnvironmentVariable('CARMA_ENV_NS', default_value='environment')),
226 IncludeLaunchDescription(
227 PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/environment.launch.py']),
228 launch_arguments = {
229 'subsystem_controller_param_file' : [vehicle_config_dir, '/SubsystemControllerParams.yaml'],
230 'vehicle_config_param_file' : vehicle_config_param_file,
231 'vehicle_characteristics_param_file' : vehicle_characteristics_param_file,
232 'vector_map_file' : vector_map_file,
233 'use_sim_time' : use_sim_time
234 }.items()
235 ),
236 ]
237 )
238
239 localization_group = GroupAction(
240 actions=[
241 PushRosNamespace(EnvironmentVariable('CARMA_LOCZ_NS', default_value='localization')),
242 IncludeLaunchDescription(
243 PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/localization.launch.py']),
244 launch_arguments = {
245 'vehicle_config_param_file' : vehicle_config_param_file,
246 'subsystem_controller_param_file' : [vehicle_config_dir, '/SubsystemControllerParams.yaml'],
247 'load_type' : load_type,
248 'single_pcd_path' : single_pcd_path,
249 'area' : area,
250 'arealist_path' : arealist_path,
251 'vector_map_file' : vector_map_file,
252 'vehicle_calibration_dir': vehicle_calibration_dir,
253 'use_sim_time' : use_sim_time
254 }.items()
255 )
256 ]
257 )
258
259 v2x_group = GroupAction(
260 actions=[
261 PushRosNamespace(EnvironmentVariable('CARMA_MSG_NS', default_value='message')),
262 IncludeLaunchDescription(
263 PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/message.launch.py']),
264 launch_arguments = {
265 'vehicle_characteristics_param_file' : vehicle_characteristics_param_file,
266 'vehicle_config_param_file' : vehicle_config_param_file,
267 'enable_opening_tunnels' : enable_opening_tunnels,
268 'subsystem_controller_param_file' : [vehicle_config_dir, '/SubsystemControllerParams.yaml'],
269 'use_sim_time' : use_sim_time
270 }.items()
271 ),
272 ]
273 )
274
275 guidance_group = GroupAction(
276 actions=[
277 PushRosNamespace(EnvironmentVariable('CARMA_GUIDE_NS', default_value='guidance')),
278
279 IncludeLaunchDescription(
280 PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/guidance.launch.py']),
281 launch_arguments={
282 'route_file_folder' : route_file_folder,
283 'vehicle_characteristics_param_file' : vehicle_characteristics_param_file,
284 'vehicle_config_param_file' : vehicle_config_param_file,
285 'enable_guidance_plugin_validator' : enable_guidance_plugin_validator,
286 'strategic_plugins_to_validate' : strategic_plugins_to_validate,
287 'tactical_plugins_to_validate' : tactical_plugins_to_validate,
288 'control_plugins_to_validate' : control_plugins_to_validate,
289 'subsystem_controller_param_file' : [vehicle_config_dir, '/SubsystemControllerParams.yaml'],
290 'use_sim_time' : use_sim_time,
291 'is_spat_wall_time' : is_spat_wall_time
292 }.items()
293 ),
294 ]
295 )
296
297 drivers_group = GroupAction(
298 actions=[
299 PushRosNamespace(EnvironmentVariable('CARMA_INTR_NS', default_value='hardware_interface')),
300 IncludeLaunchDescription(
301 PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/drivers.launch.py']),
302 launch_arguments = {
303 'subsystem_controller_param_file' : [vehicle_config_dir, '/SubsystemControllerParams.yaml'],
304 'vehicle_config_param_file' : vehicle_config_param_file,
305 'use_sim_time' : use_sim_time
306 }.items()
307 ),
308 ]
309 )
310
311 system_controller = Node(
312 package='system_controller',
313 name='system_controller',
314 executable='system_controller',
315 parameters=[
316 system_controller_param_file,
317 {"use_sim_time" : use_sim_time}],
318 on_exit = Shutdown(),
319 arguments=['--ros-args', '--log-level', GetLogLevel('system_controller', env_log_levels)]
320 )
321
322 ui_group = GroupAction(
323 actions=[
324 PushRosNamespace(EnvironmentVariable('CARMA_UI_NS', default_value='ui')),
325
326 IncludeLaunchDescription(
327 PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/ui.launch.py']),
328 launch_arguments={
329 'port' : port
330 }.items()
331 ),
332 ]
333 )
334
335 return LaunchDescription([
336 declare_vehicle_calibration_dir_arg,
337 declare_vehicle_config_dir_arg,
338 declare_vehicle_characteristics_param_file_arg,
339 declare_vehicle_config_param_file_arg,
340 declare_use_sim_time_arg,
341 declare_is_spat_wall_time_arg,
342 declare_route_file_folder,
343 declare_enable_guidance_plugin_validator,
344 declare_strategic_plugins_to_validate,
345 declare_tactical_plugins_to_validate,
346 declare_control_plugins_to_validate,
347 declare_enable_opening_tunnels,
348 declare_port,
349 declare_load_type,
350 declare_single_pcd_path,
351 declare_area,
352 declare_arealist_path,
353 declare_vector_map_file,
354 declare_is_ros2_tracing_enabled,
355 drivers_group,
356 transform_group,
357 environment_group,
358 localization_group,
359 v2x_group,
360 guidance_group,
361 ros2_rosbag_launch,
362 ui_group,
363 system_controller,
364 OpaqueFunction(function=create_ros2_tracing_action)
365 ])
def generate_launch_description()