Carma-platform v4.11.0
CARMA Platform is built on robot operating system (ROS) and utilizes open source software (OSS) that enables Cooperative Driving Automation (CDA) features to allow Automated Driving Systems to interact and cooperate with infrastructure and other vehicles through communication.
Classes | Functions
guidance Namespace Reference

Classes

struct  Config
 Stuct containing the algorithm configuration values for GuidanceWorker. More...
 
class  GuidanceStateMachine
 
class  GuidanceWorker
 Worker class for the Guidance node. More...
 

Functions

def generate_launch_description ()
 

Function Documentation

◆ generate_launch_description()

def guidance.generate_launch_description ( )

Definition at line 39 of file guidance.launch.py.

39def generate_launch_description():
40
41 route_file_folder = LaunchConfiguration('route_file_folder')
42 vehicle_calibration_dir = LaunchConfiguration('vehicle_calibration_dir')
43 vehicle_characteristics_param_file = LaunchConfiguration('vehicle_characteristics_param_file')
44 enable_guidance_plugin_validator = LaunchConfiguration('enable_guidance_plugin_validator')
45 strategic_plugins_to_validate = LaunchConfiguration('strategic_plugins_to_validate')
46 tactical_plugins_to_validate = LaunchConfiguration('tactical_plugins_to_validate')
47 control_plugins_to_validate = LaunchConfiguration('control_plugins_to_validate')
48 vehicle_config_param_file = LaunchConfiguration('vehicle_config_param_file')
49 declare_vehicle_config_param_file_arg = DeclareLaunchArgument(
50 name = 'vehicle_config_param_file',
51 default_value = "/opt/carma/vehicle/config/VehicleConfigParams.yaml",
52 description = "Path to file contain vehicle configuration parameters"
53 )
54 vehicle_config_dir = LaunchConfiguration('vehicle_config_dir')
55 declare_vehicle_config_dir_arg = DeclareLaunchArgument(
56 name = 'vehicle_config_dir',
57 default_value = "/opt/carma/vehicle/config",
58 description = "Path to vehicle configuration directory populated by carma-config"
59 )
60
61 use_sim_time = LaunchConfiguration('use_sim_time')
62 declare_use_sim_time_arg = DeclareLaunchArgument(
63 name = 'use_sim_time',
64 default_value = "False",
65 description = "True if simulation mode is on"
66 )
67
68 use_real_time_spat_in_sim = LaunchConfiguration('use_real_time_spat_in_sim')
69 declare_use_real_time_spat_in_sim_arg = DeclareLaunchArgument(
70 name = 'use_real_time_spat_in_sim',
71 default_value = 'False',
72 description = "True if SPaT is being published based on wall clock"
73 )
74
75 subsystem_controller_default_param_file = os.path.join(
76 get_package_share_directory('subsystem_controllers'), 'config/guidance_controller_config.yaml')
77
78 # Declare the global_params_override_file launch argument
79 # Parameters in this file will override any parameters loaded in their respective packages
80 global_params_override_file = LaunchConfiguration('global_params_override_file')
81 declare_global_params_override_file_arg = DeclareLaunchArgument(
82 name = 'global_params_override_file',
83 default_value = [vehicle_config_dir, "/GlobalParamsOverride.yaml"],
84 description = "Path to global file containing the parameters overwrite"
85 )
86
87 mobilitypath_visualizer_param_file = os.path.join(
88 get_package_share_directory('mobilitypath_visualizer'), 'config/params.yaml')
89
90 trajectory_executor_param_file = os.path.join(
91 get_package_share_directory('trajectory_executor'), 'config/parameters.yaml')
92
93 route_param_file = os.path.join(
94 get_package_share_directory('route'), 'config/parameters.yaml')
95
96 trajectory_visualizer_param_file = os.path.join(
97 get_package_share_directory('trajectory_visualizer'), 'config/parameters.yaml')
98
99 guidance_param_file = os.path.join(
100 get_package_share_directory('guidance'), 'config/parameters.yaml')
101
102 arbitrator_param_file_path = os.path.join(
103 get_package_share_directory('arbitrator'), 'config/arbitrator_params.yaml')
104
105 plan_delegator_param_file = os.path.join(
106 get_package_share_directory('plan_delegator'), 'config/plan_delegator_params.yaml')
107
108 port_drayage_plugin_param_file = os.path.join(
109 get_package_share_directory('port_drayage_plugin'), 'config/parameters.yaml')
110
111 # Log level is set from CARMA_ROS_LOGGING_CONFIG, generated from carma_rosconsole.conf in the vehicle config dir (carma-config)
112 env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }')
113
114 subsystem_controller_param_file = LaunchConfiguration('subsystem_controller_param_file')
115 declare_subsystem_controller_param_file_arg = DeclareLaunchArgument(
116 name = 'subsystem_controller_param_file',
117 default_value = subsystem_controller_default_param_file,
118 description = "Path to file containing override parameters for the subsystem controller"
119 )
120
121 # Below nodes are separated to individual container such that the nodes with reentrant services are within their separate container.
122 # When all nodes are within single container, it is prone to fail throwing runtime_error, and it is currently hypothesized to be
123 # because of this issue: https://github.com/ros2/rclcpp/issues/1212, where fix in the rclcpp library, so not able to be integrated at this moment:
124 # https://github.com/ros2/rclcpp/pull/1241. This issue was first discovered in this carma issue: https://github.com/usdot-fhwa-stol/carma-platform/issues/1961
125
126 # Nodes
127 carma_guidance_visualizer_container = ComposableNodeContainer(
128 package='carma_ros2_utils',
129 name='carma_guidance_visualizer_container',
130 executable='carma_component_container_mt',
131 namespace=GetCurrentNamespace(),
132 composable_node_descriptions=[
133 ComposableNode(
134 package='mobilitypath_visualizer',
135 plugin='mobilitypath_visualizer::MobilityPathVisualizer',
136 name='mobilitypath_visualizer_node',
137 extra_arguments=[
138 {'use_intra_process_comms': True},
139 ],
140 remappings = [
141 ("mobility_path_msg", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_path" ] ),
142 ("incoming_mobility_path", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_path" ] ),
143 ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference"])
144 ],
145 parameters=[
146 vehicle_characteristics_param_file,
147 mobilitypath_visualizer_param_file,
148 vehicle_config_param_file,
149 global_params_override_file
150 ]
151 ),
152 ComposableNode(
153 package='trajectory_visualizer',
154 plugin='trajectory_visualizer::TrajectoryVisualizer',
155 name='trajectory_visualizer_node',
156 extra_arguments=[
157 {'use_intra_process_comms': True},
158 ],
159 parameters=[
160 trajectory_visualizer_param_file,
161 vehicle_config_param_file,
162 global_params_override_file
163 ]
164 )
165 ]
166 )
167
168 carma_plan_delegator_container = ComposableNodeContainer(
169 package='carma_ros2_utils',
170 name='carma_plan_delegator_container',
171 executable='carma_component_container_mt',
172 namespace=GetCurrentNamespace(),
173 composable_node_descriptions=[
174 ComposableNode(
175 package='plan_delegator',
176 plugin='plan_delegator::PlanDelegator',
177 name='plan_delegator',
178 extra_arguments=[
179 {'use_intra_process_comms': True},
180 ],
181 remappings = [
182 ("current_velocity", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ),
183 ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
184 ("vehicle_status", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle_status" ] ),
185 ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ),
186 ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ),
187 ("roadway_objects", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/roadway_objects" ] ),
188 ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ),
189 ("guidance_state", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/state" ] ),
190 ],
191 parameters=[
192 plan_delegator_param_file,
193 vehicle_config_param_file,
194 global_params_override_file
195 ]
196 )
197 ]
198 )
199
200 carma_trajectory_executor_and_route_container = ComposableNodeContainer(
201 package='carma_ros2_utils',
202 name='carma_trajectory_executor_and_route_container',
203 executable='carma_component_container_mt',
204 namespace=GetCurrentNamespace(),
205 composable_node_descriptions=[
206 ComposableNode(
207 package='route',
208 plugin='route::Route',
209 name='route_node',
210 extra_arguments=[
211 {'use_intra_process_comms': True},
212 ],
213 remappings = [
214 ("current_velocity", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ),
215 ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ),
216 ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ),
217 ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ),
218 ("roadway_objects", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/roadway_objects" ] ),
219 ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] )
220 ],
221 parameters=[
222 {'route_file_path': route_file_folder},
223 route_param_file,
224 vehicle_config_param_file,
225 global_params_override_file
226 ]
227 ),
228 ComposableNode(
229 package='trajectory_executor',
230 plugin='trajectory_executor::TrajectoryExecutor',
231 name='trajectory_executor_node',
232 extra_arguments=[
233 {'use_intra_process_comms': True},
234 ],
235 remappings = [
236 ("trajectory", "plan_trajectory"),
237 ],
238 parameters=[
239 trajectory_executor_param_file,
240 vehicle_config_param_file,
241 global_params_override_file
242 ]
243 )
244 ]
245 )
246
247 carma_arbitrator_container = ComposableNodeContainer(
248 package='carma_ros2_utils',
249 name='carma_arbitrator_container',
250 executable='carma_component_container_mt',
251 namespace=GetCurrentNamespace(),
252 composable_node_descriptions=[
253 ComposableNode(
254 package='arbitrator',
255 plugin='arbitrator::ArbitratorNode',
256 name='arbitrator',
257 extra_arguments=[
258 {'use_intra_process_comms': True},
259 ],
260 remappings = [
261 ("current_velocity", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ),
262 ("guidance_state", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/state" ] ),
263 ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ),
264 ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ),
265 ("roadway_objects", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/roadway_objects" ] ),
266 ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] )
267 ],
268 parameters=[
269 arbitrator_param_file_path,
270 vehicle_config_param_file,
271 global_params_override_file
272 ]
273 )
274 ]
275 )
276 carma_guidance_worker_container = ComposableNodeContainer(
277 package='carma_ros2_utils',
278 name='carma_guidance_worker_container',
279 executable='carma_component_container_mt',
280 namespace=GetCurrentNamespace(),
281 composable_node_descriptions=[
282 ComposableNode(
283 package='guidance',
284 plugin='guidance::GuidanceWorker',
285 name='guidance_node',
286 extra_arguments=[
287 {'use_intra_process_comms': True},
288 ],
289 remappings = [
290 ("vehicle_status", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle_status" ] ),
291 ("robot_status", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/controller/robot_status" ] ),
292 ("enable_robotic", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/controller/enable_robotic" ] ),
293 ],
294 parameters=[
295 guidance_param_file,
296 vehicle_config_param_file,
297 global_params_override_file
298 ]
299 )
300 ]
301 )
302
303 carma_port_drayage_plugin_container = ComposableNodeContainer(
304 package='carma_ros2_utils',
305 name='carma_port_drayage_plugin_container',
306 executable='carma_component_container_mt',
307 namespace=GetCurrentNamespace(),
308 composable_node_descriptions=[
309 ComposableNode(
310 package='port_drayage_plugin',
311 plugin='port_drayage_plugin::PortDrayagePlugin',
312 name='port_drayage_plugin_node',
313 extra_arguments=[
314 {'use_intra_process_comms': True},
315 ],
316 remappings = [
317 ("guidance_state", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/state" ] ),
318 ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ),
319 ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
320 ("incoming_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_operation" ] ),
321 ("outgoing_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_operation" ] ),
322 ("ui_instructions", [ EnvironmentVariable('CARMA_UI_NS', default_value=''), "/ui_instructions" ] )
323 ],
324 parameters=[
325 port_drayage_plugin_param_file,
326 vehicle_characteristics_param_file,
327 vehicle_config_param_file,
328 global_params_override_file
329 ]
330 )
331 ]
332 )
333
334 twist_filter_container = ComposableNodeContainer(
335 package='carma_ros2_utils',
336 name='twist_filter_container',
337 executable='carma_component_container_mt',
338 namespace=GetCurrentNamespace(),
339 composable_node_descriptions=[
340 ComposableNode(
341 package='twist_filter',
342 plugin='twist_filter::TwistFilter',
343 name='twist_filter_node',
344 extra_arguments=[
345 {'use_intra_process_comms': True},
346 ],
347 remappings = [
348 ("/accel_cmd", ["accel_cmd" ] ),
349 ("/brake_cmd", ["brake_cmd" ] ),
350 ("/gear_cmd", ["gear_cmd" ] ),
351 ("/mode_cmd", ["mode_cmd" ] ),
352 ("/remote_cmd", ["remote_cmd" ] ),
353 ("/steer_cmd", ["steer_cmd" ] ),
354 ("/emergency_stop", ["emergency_stop" ] ),
355 ("/state_cmd", ["state_cmd" ] )
356 ],
357 parameters=[
358 vehicle_config_param_file,
359 {'lowpass_gain_linear_x':0.1},
360 {'lowpass_gain_angular_z':0.0},
361 {'lowpass_gain_steering_angle':0.1},
362 global_params_override_file
363 ]
364 ),
365 ComposableNode(
366 package='twist_gate',
367 plugin='TwistGate',
368 name='twist_gate_node',
369 extra_arguments=[
370 {'use_intra_process_comms': True},
371 ],
372 remappings = [
373 ("vehicle_cmd", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle_cmd" ] ),
374 ("/lamp_cmd", ["lamp_cmd" ] ),
375 ("/twist_cmd", ["twist_cmd" ] ),
376 ("/decision_maker/state", ["decision_maker/state" ] ),
377 ("/ctrl_cmd", ["ctrl_cmd" ] ),
378 ],
379 parameters = [
380 {'loop_rate':30.0},
381 {'use_decision_maker':False},
382 vehicle_config_param_file,
383 global_params_override_file
384 ]
385 )
386 ]
387 )
388
389 # Launch plugins
390 plugins_group = GroupAction(
391 actions=[
392 PushRosNamespace("plugins"),
393 IncludeLaunchDescription(
394 PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/plugins.launch.py']),
395 launch_arguments={
396 'route_file_folder' : route_file_folder,
397 'global_params_override_file' : global_params_override_file,
398 'vehicle_calibration_dir' : vehicle_calibration_dir,
399 'vehicle_characteristics_param_file' : vehicle_characteristics_param_file,
400 'vehicle_config_param_file' : vehicle_config_param_file,
401 'enable_guidance_plugin_validator' : enable_guidance_plugin_validator,
402 'strategic_plugins_to_validate' : strategic_plugins_to_validate,
403 'tactical_plugins_to_validate' : tactical_plugins_to_validate,
404 'control_plugins_to_validate' : control_plugins_to_validate,
405 'subsystem_controller_param_file' : [vehicle_config_dir, '/SubsystemControllerParams.yaml'],
406 }.items()
407 ),
408 ]
409 )
410
411 # subsystem_controller which orchestrates the lifecycle of this subsystem's components
412 subsystem_controller = Node(
413 package='subsystem_controllers',
414 name='guidance_controller',
415 executable='guidance_controller',
416 parameters=[
417 subsystem_controller_default_param_file,
418 subsystem_controller_param_file,
419 {"use_sim_time" : use_sim_time},
420 {"use_real_time_spat_in_sim" : use_real_time_spat_in_sim}],
421 on_exit= Shutdown(), # Mark the subsystem controller as required
422 arguments=['--ros-args', '--log-level', GetLogLevel('subsystem_controllers', env_log_levels)]
423 )
424
425 return LaunchDescription([
426 declare_vehicle_config_param_file_arg,
427 declare_vehicle_config_dir_arg,
428 declare_global_params_override_file_arg,
429 declare_use_sim_time_arg,
430 declare_subsystem_controller_param_file_arg,
431 declare_use_real_time_spat_in_sim_arg,
432 carma_trajectory_executor_and_route_container,
433 carma_guidance_visualizer_container,
434 carma_guidance_worker_container,
435 carma_plan_delegator_container,
436 carma_arbitrator_container,
437 carma_port_drayage_plugin_container,
438 twist_filter_container,
439 plugins_group,
440 subsystem_controller
441 ])
guidance.generate_launch_description
def generate_launch_description()
Definition: guidance.launch.py:39