diff --git a/launch/sensors_launch.py b/launch/sensors_launch.py index 25bddc67..af0cc5e1 100755 --- a/launch/sensors_launch.py +++ b/launch/sensors_launch.py @@ -6,6 +6,7 @@ from launch.substitutions import PythonExpression from launch.substitutions import ThisLaunchFileDir from launch.conditions import IfCondition +from launch.conditions import LaunchConfigurationEquals from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node @@ -28,17 +29,21 @@ def generate_launch_description(): SIMULATION = os.getenv('SIMULATION') simulation_mode = (SIMULATION == "1") - # DRONE_AIRFRAME = - is_robot_holybro_type = False + # DRONE_AIRFRAME = + allowed_airframes = ["holybro", "rover", "t-drone", "m1200"] DRONE_AIRFRAME = os.getenv('DRONE_AIRFRAME') - if DRONE_AIRFRAME == "holybro" or DRONE_AIRFRAME == "rover": - is_robot_holybro_type = True - elif DRONE_AIRFRAME == "" or DRONE_AIRFRAME == "t-drone": - is_robot_holybro_type = False - else: + if DRONE_AIRFRAME not in allowed_airframes: print('ERROR: not valid DRONE_AIRFRAME.') sys.exit(1) + # By default use file with name convention '/static_tf__launch.py' + file = "/static_tf_" + DRONE_AIRFRAME + "_launch.py" + info_string= "--- " + DRONE_AIRFRAME.upper() + " TYPE CONFIGURATION ---" + + # exception - rover uses holybro configuration + if DRONE_AIRFRAME == "rover": + file = '/static_tf_holybro_launch.py' + # By default use raw scan, only if USE_FILTERED_SCAN is set to 1 or True, use filtered filtered_name = "~/scan_filtered" raw_name = "~/scan" @@ -82,26 +87,12 @@ def generate_launch_description(): ), ), - - # sensor tf launch ld.add_action( - LogInfo(msg='--- HOLYBRO/ROVER TYPE CONFIGURATION ---', condition=IfCondition(PythonExpression([str(is_robot_holybro_type)]))), + LogInfo(msg=info_string) ), ld.add_action( IncludeLaunchDescription( - PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/static_tf_holybro_launch.py']), - condition=IfCondition(PythonExpression([str(is_robot_holybro_type)])), + PythonLaunchDescriptionSource([ThisLaunchFileDir(), file]) ), - ), - - ld.add_action( - LogInfo(msg='--- T-DRONE TYPE CONFIGURATION ---', condition=IfCondition(PythonExpression(['not ', str(is_robot_holybro_type)]))), - ), - ld.add_action( - IncludeLaunchDescription( - PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/static_tf_tdrone_launch.py']), - condition=IfCondition(PythonExpression(['not ', str(is_robot_holybro_type)])), - ), - ), - + ) return ld diff --git a/launch/static_tf_m1200_launch.py b/launch/static_tf_m1200_launch.py new file mode 100644 index 00000000..b518dd0a --- /dev/null +++ b/launch/static_tf_m1200_launch.py @@ -0,0 +1,44 @@ +from launch import LaunchDescription +from launch.conditions import IfCondition +from launch_ros.actions import Node +import os + +def generate_launch_description(): + + ld = LaunchDescription() + + # environment variables + DRONE_DEVICE_ID = os.getenv('DRONE_DEVICE_ID') + + #namespace declarations + namespace = DRONE_DEVICE_ID + + # frame names + fcu_frame = DRONE_DEVICE_ID + "/fcu" + rplidar_frame = DRONE_DEVICE_ID + "/rplidar" + garmin_frame = DRONE_DEVICE_ID + "/garmin" + + # node definitions + ld.add_action( + Node( + namespace = namespace, + package = "tf2_ros", + executable = "static_transform_publisher", + name= "fcu_to_rplidar_static_transform_publisher", + arguments = ["-0.15", "0", "0.09", "3.141592", "0", "0", fcu_frame, rplidar_frame], + output='screen', + ), + ) + + ld.add_action( + Node( + namespace = namespace, + package = "tf2_ros", + executable = "static_transform_publisher", + name= "fcu_to_garmin_static_transform_publisher", + arguments = ["-0.13", "0", "-0.13", "0", "1.5708", "0", fcu_frame, garmin_frame], + output='screen', + ), + ) + + return ld diff --git a/launch/static_tf_tdrone_launch.py b/launch/static_tf_t-drone_launch.py similarity index 100% rename from launch/static_tf_tdrone_launch.py rename to launch/static_tf_t-drone_launch.py