[feat] new launchers

config/mavros_config.yaml

@@ -0,0 +1,9 @@
+/**:
+  ros__parameters:
+    fcu_url: "udp://:[email protected]:14557" # Autopilot address for connection
+    gcs_url: "" # URL for the GCS
+    tgt_system: 1 # Target system ID
+    tgt_component: 1 # Target component ID
+    log_output: "screen" # Logging output type
+    fcu_protocol: "v2.0" # Protocol version for the FCU
+    respawn_mavros: "false" # Respawn mavros node if it dies

config/platform_config_file.yaml

@@ -4,4 +4,4 @@
     info_freq: 10.0  # Hz of platform info publish
     max_thrust: 15.0  # Max thrust to send to platform (N)
     min_thrust: 0.15  # Min thrust to send to platform (N)
-    external_odom: false  # Flag to use external odometry source
+    external_odom: false  # Flag to use external odometry source

launch/as2_platform_mavlink_launch.py

@@ -33,42 +33,43 @@
 __copyright__ = 'Copyright (c) 2022 Universidad Politécnica de Madrid'
 __license__ = 'BSD-3-Clause'
 
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from as2_core.declare_launch_arguments_from_config_file import DeclareLaunchArgumentsFromConfigFile
+from as2_core.launch_configuration_from_config_file import LaunchConfigurationFromConfigFile
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.launch_description_sources import AnyLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
 
 
-def generate_launch_description():
-    """Entrypoint."""
-    control_modes = PathJoinSubstitution([
-        FindPackageShare('as2_platform_mavlink'),
-        'config', 'control_modes.yaml'
-    ])
+def get_platform_config_file():
+    """Return the platform config file."""
+    package_folder = get_package_share_directory('as2_platform_mavlink')
+    return os.path.join(package_folder,
+                        'config/platform_config_file.yaml')
 
-    platform_config_file = PathJoinSubstitution([
-        FindPackageShare('as2_platform_mavlink'),
-        'config', 'platform_config_file.yaml'
-    ])
 
-    mavros_launch_file = PathJoinSubstitution([
-        FindPackageShare('as2_platform_mavlink'), 'launch', 'mavros_launch.py'])
+def get_control_modes_file():
+    """Return the control modes file."""
+    package_folder = get_package_share_directory('as2_platform_mavlink')
+    return os.path.join(package_folder,
+                        'config/control_modes.yaml')
+
 
+def generate_launch_description():
+    """Entrypoint."""
     return LaunchDescription([
         DeclareLaunchArgument('namespace',
                               default_value='drone0',
                               description='Drone namespace'),
         DeclareLaunchArgument('control_modes_file',
-                              default_value=control_modes,
+                              default_value=get_control_modes_file(),
                               description='Platform control modes file'),
-        DeclareLaunchArgument('platform_config_file',
-                              default_value=platform_config_file,
-                              description='Platform configuration file'),
-        DeclareLaunchArgument('fcu_url',
-                              default_value='udp://:[email protected]:14557',
-                              description='Mavlink connection URL'),
+        DeclareLaunchArgumentsFromConfigFile(name='platform_config_file',
+                                             source_file=get_platform_config_file(),
+                                             description='Platform configuration file'),
         Node(
             package='as2_platform_mavlink',
             executable='as2_platform_mavlink_node',
@@ -80,14 +81,8 @@ def generate_launch_description():
                 {
                     'control_modes_file': LaunchConfiguration('control_modes_file'),
                 },
-                LaunchConfiguration('platform_config_file')
+                LaunchConfigurationFromConfigFile('platform_config_file',
+                                                  default_file=get_platform_config_file())
             ]
         ),
-        IncludeLaunchDescription(
-            AnyLaunchDescriptionSource(mavros_launch_file),
-            launch_arguments={
-                'fcu_url': LaunchConfiguration('fcu_url'),
-                'namespace': LaunchConfiguration('namespace')
-            }.items()
-        )
     ])

launch/mavros_launch.py

@@ -33,13 +33,25 @@
 __copyright__ = 'Copyright (c) 2022 Universidad Politécnica de Madrid'
 __license__ = 'BSD-3-Clause'
 
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from as2_core.declare_launch_arguments_from_config_file import DeclareLaunchArgumentsFromConfigFile
+from as2_core.launch_configuration_from_config_file import LaunchConfigurationFromConfigFile
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
 
+def get_mavros_config_file():
+    """Return the mavros config file."""
+    package_folder = get_package_share_directory('as2_platform_mavlink')
+    return os.path.join(package_folder,
+                        'config/mavros_config.yaml')
+
+
 def get_node(context, *args, **kwargs) -> list:
     """
     Get node.
@@ -67,17 +79,15 @@ def get_node(context, *args, **kwargs) -> list:
             executable='mavros_node',
             namespace=ns,
             output=LaunchConfiguration('log_output'),
-            parameters=[{'fcu_url': LaunchConfiguration('fcu_url'),
-                         'gcs_url': LaunchConfiguration('gcs_url'),
-                         'tgt_system': LaunchConfiguration('tgt_system'),
-                         'tgt_component': LaunchConfiguration('tgt_component'),
-                         'fcu_protocol': LaunchConfiguration('fcu_protocol'),
-                         'respawn_mavros': LaunchConfiguration('respawn_mavros')},
-                        pluginlists_yaml, config_yaml],
+            parameters=[config_yaml,
+                        pluginlists_yaml,
+                        LaunchConfigurationFromConfigFile(
+                            'mavros_config_file',
+                            default_file=get_mavros_config_file())
+                        ],
             remappings=[('imu/data', f'/{namespace}/sensor_measurements/imu'),
                         ('global_position/global', f'/{namespace}/sensor_measurements/gps'),
                         ('battery', f'/{namespace}/sensor_measurements/battery'),
-
                         ],
         )]
 
@@ -86,46 +96,14 @@ def generate_launch_description():
     """Entrypoint."""
     # Declare the launch arguments
     return LaunchDescription([
-        DeclareLaunchArgument(
-            'fcu_url',
-            default_value='/dev/ttyACM0:57600',
-            description='URL for the FCU'
-        ),
-        DeclareLaunchArgument(
-            'gcs_url',
-            default_value='',
-            description='URL for the GCS'
-        ),
-        DeclareLaunchArgument(
-            'tgt_system',
-            default_value='1',
-            description='Target system ID'
-        ),
-        DeclareLaunchArgument(
-            'tgt_component',
-            default_value='1',
-            description='Target component ID'
-        ),
-        DeclareLaunchArgument(
-            'log_output',
-            default_value='screen',
-            description='Logging output type'
-        ),
-        DeclareLaunchArgument(
-            'fcu_protocol',
-            default_value='v2.0',
-            description='Protocol version for the FCU'
-        ),
-        DeclareLaunchArgument(
-            'respawn_mavros',
-            default_value='false',
-            description='Respawn mavros node if it dies'
-        ),
         DeclareLaunchArgument(
             'namespace',
             default_value='mavros',
             description='Namespace for the mavros node'
         ),
+        DeclareLaunchArgumentsFromConfigFile(name='mavros_config_file',
+                                             source_file=get_mavros_config_file(),
+                                             description='Mavros configuration file'),
 
         OpaqueFunction(function=get_node),
 

src/mavlink_platform.cpp

@@ -225,34 +225,42 @@ bool MavlinkPlatform::ownSendCommand()
 {
   as2_msgs::msg::ControlMode platform_control_mode = this->getControlMode();
 
+  auto thrust_normalized = this->command_thrust_msg_.thrust / max_thrust_;
+  thrust_normalized = std::clamp<float>(thrust_normalized, min_thrust_, 1.0);
+
   // Switch case to set setpoint
   switch (platform_control_mode.control_mode) {
     case as2_msgs::msg::ControlMode::POSITION: {
         if (platform_control_mode.yaw_mode == as2_msgs::msg::ControlMode::YAW_ANGLE) {
           mavlink_publishPoseSetpoint(this->command_pose_msg_);
         } else {
-          // ENU --> NED
-          notImplemented();
+          RCLCPP_WARN(
+            this->get_logger(),
+            "NOT IMPLEMENTED FOR POSITION WITH YAW_RATE SETPOINT, sending  yaw 0.0");
+          command_pose_msg_.pose.orientation = tf2::toMsg(tf2::Quaternion(0, 0, 0, 1));
+          mavlink_publishPoseSetpoint(this->command_pose_msg_);
         }
       } break;
     case as2_msgs::msg::ControlMode::SPEED: {
         if (platform_control_mode.yaw_mode == as2_msgs::msg::ControlMode::YAW_ANGLE) {
-          notImplemented();
+          RCLCPP_WARN(this->get_logger(), "NOT IMPLEMENTED FOR SPEED WITH YAW_ANGLE SETPOINT");
+          this->command_twist_msg_.twist.angular.z = 0.0;
+          mavlink_publishTwistSetpoint(this->command_twist_msg_);
         } else {
           mavlink_publishTwistSetpoint(this->command_twist_msg_);
         }
       } break;
     case as2_msgs::msg::ControlMode::ATTITUDE: {
         mavlink_publishAttitudeSetpoint(
           this->command_pose_msg_.pose.orientation,
-          this->command_thrust_msg_.thrust);
+          thrust_normalized);
       } break;
     case as2_msgs::msg::ControlMode::ACRO: {
         mavlink_publishRatesSetpoint(
           this->command_twist_msg_.twist.angular.x,
           this->command_twist_msg_.twist.angular.y,
           this->command_twist_msg_.twist.angular.z,
-          this->command_thrust_msg_.thrust);
+          thrust_normalized);
       } break;
     default:
       return false;