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, ExecuteProcess
26from launch.conditions
import IfCondition
31from launch.actions
import IncludeLaunchDescription
32from launch.launch_description_sources
import PythonLaunchDescriptionSource
33from launch.actions
import GroupAction
34from launch_ros.actions
import set_remap
41 Launch V2X subsystem nodes.
44 env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value=
'{ "default_level" : "WARN" }')
46 subsystem_controller_default_param_file = os.path.join(
47 get_package_share_directory(
'subsystem_controllers'),
'config/v2x_controller_config.yaml')
49 mobilitypath_publisher_param_file = os.path.join(
50 get_package_share_directory(
'mobilitypath_publisher'),
'config/parameters.yaml')
52 bsm_generator_param_file = os.path.join(
53 get_package_share_directory(
'bsm_generator'),
'config/parameters.yaml')
55 vehicle_characteristics_param_file = LaunchConfiguration(
'vehicle_characteristics_param_file')
56 declare_vehicle_characteristics_param_file_arg = DeclareLaunchArgument(
57 name =
'vehicle_characteristics_param_file',
58 default_value =
"/opt/carma/vehicle/calibration/identifiers/UniqueVehicleParams.yaml",
59 description =
"Path to file containing unique vehicle calibrations"
63 vehicle_config_param_file = LaunchConfiguration(
'vehicle_config_param_file')
64 declare_vehicle_config_param_file_arg = DeclareLaunchArgument(
65 name =
'vehicle_config_param_file',
66 default_value =
"/opt/carma/vehicle/config/VehicleConfigParams.yaml",
67 description =
"Path to file contain vehicle configuration parameters"
70 subsystem_controller_param_file = LaunchConfiguration(
'subsystem_controller_param_file')
71 declare_subsystem_controller_param_file_arg = DeclareLaunchArgument(
72 name =
'subsystem_controller_param_file',
73 default_value = subsystem_controller_default_param_file,
74 description =
"Path to file containing override parameters for the subsystem controller"
77 vehicle_config_dir = LaunchConfiguration(
'vehicle_config_dir')
78 declare_vehicle_config_dir_arg = DeclareLaunchArgument(
79 name =
'vehicle_config_dir',
80 default_value =
"/opt/carma/vehicle/config",
81 description =
"Path to vehicle configuration directory populated by carma-config"
86 global_params_override_file = LaunchConfiguration(
'global_params_override_file')
87 declare_global_params_override_file_arg = DeclareLaunchArgument(
88 name =
'global_params_override_file',
89 default_value = [vehicle_config_dir,
"/GlobalParamsOverride.yaml"],
90 description =
"Path to global file containing the parameters overwrite"
94 enable_opening_tunnels = LaunchConfiguration(
'enable_opening_tunnels')
95 declare_enable_opening_tunnels = DeclareLaunchArgument(
96 name =
'enable_opening_tunnels',
97 default_value=
'True',
98 description=
'Flag to enable opening http tunnesl to CARMA Cloud'
101 carma_cloud_client_param_file = os.path.join(
102 get_package_share_directory(
'carma_cloud_client'),
'config/parameters.yaml')
104 use_sim_time = LaunchConfiguration(
'use_sim_time')
105 declare_use_sim_time_arg = DeclareLaunchArgument(
106 name =
'use_sim_time',
107 default_value =
"False",
108 description =
"True if simulation mode is on"
112 carma_v2x_container = ComposableNodeContainer(
113 package=
'carma_ros2_utils',
114 name=
'carma_v2x_container',
115 executable=
'carma_component_container_mt',
116 namespace=GetCurrentNamespace(),
117 composable_node_descriptions=[
120 package=
'mobilitypath_publisher',
121 plugin=
'mobilitypath_publisher::MobilityPathPublication',
122 name=
'mobilitypath_publisher_node',
124 {
'use_intra_process_comms':
True},
125 {
'--log-level' : GetLogLevel(
'mobilitypath_publisher', env_log_levels) }
128 (
"plan_trajectory", [ EnvironmentVariable(
'CARMA_GUIDE_NS', default_value=
''),
"/plan_trajectory" ] ),
129 (
"guidance_state", [ EnvironmentVariable(
'CARMA_GUIDE_NS', default_value=
''),
"/state" ] ),
130 (
"georeference", [ EnvironmentVariable(
'CARMA_LOCZ_NS', default_value=
''),
"/map_param_loader/georeference" ] ),
131 (
"mobility_path_msg", [ EnvironmentVariable(
'CARMA_MSG_NS', default_value=
''),
"/outgoing_mobility_path" ] )
134 mobilitypath_publisher_param_file,
135 vehicle_characteristics_param_file,
136 vehicle_config_param_file,
137 global_params_override_file
141 package=
'bsm_generator',
142 plugin=
'bsm_generator::BSMGenerator',
143 name=
'bsm_generator_node',
145 {
'use_intra_process_comms':
True},
146 {
'--log-level' : GetLogLevel(
'bsm_generator', env_log_levels) }
149 (
"velocity_accel_cov", [ EnvironmentVariable(
'CARMA_INTR_NS', default_value=
''),
"/velocity_accel_cov" ] ),
150 (
"ekf_twist", [ EnvironmentVariable(
'CARMA_INTR_NS', default_value=
''),
"/vehicle/twist" ] ),
151 (
"imu_raw", [ EnvironmentVariable(
'CARMA_INTR_NS', default_value=
''),
"/imu_raw" ] ),
152 (
"transmission_state", [ EnvironmentVariable(
'CARMA_INTR_NS', default_value=
''),
"/can/transmission_state" ] ),
153 (
"brake_position", [ EnvironmentVariable(
'CARMA_INTR_NS', default_value=
''),
"/can/brake_position" ] ),
154 (
"steering_wheel_angle", [ EnvironmentVariable(
'CARMA_INTR_NS', default_value=
''),
"/can/steering_wheel_angle" ] ),
155 (
"gnss_fix_fused", [ EnvironmentVariable(
'CARMA_INTR_NS', default_value=
''),
"/gnss_fix_fused" ] ),
156 (
"pose", [ EnvironmentVariable(
'CARMA_LOCZ_NS', default_value=
''),
"/current_pose" ] ),
157 (
"georeference", [ EnvironmentVariable(
'CARMA_LOCZ_NS', default_value=
''),
"/map_param_loader/georeference" ] )
160 bsm_generator_param_file,
161 vehicle_characteristics_param_file,
162 vehicle_config_param_file,
163 global_params_override_file
167 package=
'cpp_message',
168 plugin=
'cpp_message::Node',
169 name=
'cpp_message_node',
171 {
'use_intra_process_comms':
True},
172 {
'--log-level' : GetLogLevel(
'cpp_message', env_log_levels) }
175 (
"inbound_binary_msg", [ EnvironmentVariable(
'CARMA_INTR_NS', default_value=
''),
"/comms/inbound_binary_msg" ] ),
176 (
"outbound_binary_msg", [ EnvironmentVariable(
'CARMA_INTR_NS', default_value=
''),
"/comms/outbound_binary_msg" ] ),
179 vehicle_config_param_file,
180 global_params_override_file
184 package=
'j2735_convertor',
185 plugin=
'j2735_convertor::Node',
186 name=
'j2735_convertor_node',
188 {
'use_intra_process_comms':
True},
189 {
'--log-level' : GetLogLevel(
'j2735_convertor', env_log_levels) }
192 (
"outgoing_bsm",
"bsm_outbound" )
195 vehicle_config_param_file,
196 global_params_override_file
200 package=
'carma_cloud_client',
201 plugin=
'carma_cloud_client::CarmaCloudClient',
202 name=
'carma_cloud_client_node',
204 {
'use_intra_process_comms':
True},
205 {
'--log-level' : GetLogLevel(
'carma_cloud_client', env_log_levels) }
208 (
"incoming_geofence_control", [ EnvironmentVariable(
'CARMA_MSG_NS', default_value=
''),
"/incoming_geofence_control" ] ),
211 vehicle_config_param_file,
212 carma_cloud_client_param_file,
213 global_params_override_file
221 subsystem_controller = Node(
222 package=
'subsystem_controllers',
223 name=
'v2x_controller',
224 executable=
'v2x_controller',
226 subsystem_controller_default_param_file,
227 subsystem_controller_param_file,
228 {
"use_sim_time" : use_sim_time}],
230 arguments=[
'--ros-args',
'--log-level', GetLogLevel(
'subsystem_controllers', env_log_levels)]
236 REMOTE_ADDR=
"carma-cloud.com"
237 KEY_FILE=
"carma-cloud-1.pem"
240 param_launch_path = os.path.join(
241 get_package_share_directory(
'carma_cloud_client'),
'launch/scripts')
243 script = param_launch_path +
'/open_tunnels.sh'
245 subprocess.check_call([
'chmod',
'u+x', script])
247 key_path =
"/opt/carma/vehicle/calibration/cloud_permission"
249 keyfile = key_path +
'/' + KEY_FILE
251 subprocess.check_call([
'sudo',
'chmod',
'400', keyfile])
254 open_tunnels_action = ExecuteProcess(
256 condition=IfCondition(enable_opening_tunnels),
257 cmd = [
'sudo', script,
'-u', REMOTE_USER,
'-a', REMOTE_ADDR,
'-k', keyfile,
'-p', REMOTE_PORT,
'-r', HOST_PORT],
262 return LaunchDescription([
263 declare_vehicle_config_param_file_arg,
264 declare_vehicle_config_dir_arg,
265 declare_global_params_override_file_arg,
266 declare_use_sim_time_arg,
267 declare_vehicle_characteristics_param_file_arg,
268 declare_subsystem_controller_param_file_arg,
269 declare_enable_opening_tunnels,
def generate_launch_description()