Autotuner

.gitignore

@@ -8,4 +8,8 @@ log/
 
 # IDE files
 .vscode/
-.idea/
+.idea/
+
+# Python files
+__pycache__/
+venv/

rosplane/src/controller_base.cpp

@@ -32,10 +32,8 @@ controller_base::controller_base()
   // Set the values for the parameters, from the param file or use the deafault value.
   set_parameters();
 
-  if (params_.roll_tuning_debug_override || params_.pitch_tuning_debug_override) {
-    tuning_debug_sub_ = this->create_subscription<rosplane_msgs::msg::ControllerInternalsDebug>(
-      "/tuning_debug", 10, std::bind(&controller_base::tuning_debug_callback, this, _1));
-  }
+  tuning_debug_sub_ = this->create_subscription<rosplane_msgs::msg::ControllerInternalsDebug>(
+    "/tuning_debug", 10, std::bind(&controller_base::tuning_debug_callback, this, _1));
 
   // Set the parameter callback, for when parameters are changed.
   parameter_callback_handle_ = this->add_on_set_parameters_callback(

rosplane_tuning/CMakeLists.txt

@@ -63,6 +63,14 @@ install(PROGRAMS
   DESTINATION lib/${PROJECT_NAME}
 )
 
+# Autotune
+install(PROGRAMS
+  src/autotune/autotune.py
+  src/autotune/optimizer.py
+  src/autotune/plot_util.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 #### END OF EXECUTABLES ###
 
 ament_package()

rosplane_tuning/README.md

@@ -4,13 +4,50 @@ This ROS2 package contains tools useful for tuning the autopilot performance of
 
 We recommend using [PlotJuggler](https://github.com/facontidavide/PlotJuggler) to visualize command input and response.
 
+## Autotuner
+
+To make using ROSplane easier, we have implemented an autotune node. This node performs a gradient-based optimization routine where the autotune adjusts the autopilot gains in the direction that minimizes the error between the autopilot command and estimated state.
+
+The optimization tests a series of gains using step commands from which error is calculated and gains are slowly adjusted to minimize the error. Error is calculated by giving a step input to an autopilot, waiting a few seconds for the system to respond, stepping back, waiting a few more seconds, and then calculuating the error. The autopilot then returns back to its previous state. This results in a responce like this figure:
+
+![Autotune Response](media/roll_response.png)
+
+### Usage
+
+WARNING: Make sure to monitor the UAV during all portions of tuning, and be ready to take manual control in case the autopilot becomes unstable or does something terrible. We don't guarantee that the autotuner will not crash your plane.
+
+1. Launch the launch file for the portion of the autopilot you want to tune. This will launch both ROSplane and the autotuner. Set the `continuous_tuning` parameter to `true` if you are confident in the autopilot's current ability to stay stable and in the air. Leave it as `false` if you'd rather manually begin each iteration.
+    ```
+    ros2 launch rosplane_tuning roll_autotune.launch.py continuous_tuning:=true
+    ```
+
+2. Launch ROSplane if you haven't already done so. The autotuner will start from whichever gains are currently set in ROSplane, so set those to whichever values you'd like to start the optimization from.
+    ```
+    ros2 launch rosplane_tuning rosplane_tuning.launch.py
+    ```
+
+3. Get ROSplane into the desired state, like an orbit high above the launch site. The autotuner will return to this state between every iteration.
+
+4. Call the `run_tuning_iteration` service to start tuning.  If you set `continuous_tuning` to `false`, you will need to manually call the `run_tuning_iteration` service to start each iteration.
+
+5. Wait until the autotuner reports that it has finished or you are satisfied with the current response. The autotuner will update the current gains with the best response it has found so far.
+
+Repeat for each autopilot you wish to tune. We recommend tuning the autopilots in this order: roll, pitch, airspeed, altitude, course. The airspeed and altitude autopilots are closely coupled, so you may need to iterate between them to get the best performance.
+
+Make sure to set the new gains as the default values for ROSplane in the `rosplane/params` directory. Gains are reported by the autotuner in this order:
+- Roll: `r_kp`, `r_kd`
+- Pitch: `p_kp`, `p_kd`
+- Airspeed: `a_t_kp`, `a_t_ki`
+- Altitude: `a_kp`, `a_ki`
+- Course: `c_kp`, `c_ki`
+
 ## Signal Generator
 
-Signal generator is a ROS2 node that will generate step inputs, square waves, sine waves, sawtooth waves, and triangle waves to be used as command input for ROSplane. It has support for roll, pitch, altitude, heading, and airspeed command input.
+Signal generator is a ROS2 node that will generate step inputs, square waves, sine waves, sawtooth waves, and triangle waves to be used as command input for ROSplane. It has support for roll, pitch, altitude, course, and airspeed command input.
 
 This is useful for tuning autopilots as we can give a clear and repeatable command to any portion of the autopilot and observe its response to that input. We can then tune gains, re-issue the same commands, and observe whether performance improved or worsened.
 
-Signal generator works by publishing autopilot commands on the ROS network on the `controller_commands` and `tuning_debug` topics. Each control output (roll, pitch, altitude, heading, airspeed) has a default values that is set by a ROS parameter. The signal generator will then add the generated signal to one of the control outputs with a set magnitude and frequency.
+Signal generator works by publishing autopilot commands on the ROS network on the `controller_commands` and `tuning_debug` topics. Each control output (roll, pitch, altitude, course, airspeed) has a default values that is set by a ROS parameter. The signal generator will then add the generated signal to one of the control outputs with a set magnitude and frequency.
 
 ### Signal Types
 - Step: This is a non-continuous signal, where the generated signal will jump between the default value and the default+magnitude every time the `toggle_step_signal` service is called.
@@ -19,19 +56,19 @@ Signal generator works by publishing autopilot commands on the ROS network on th
 - Triangle: This is a continuous signal that ramps up and down between the minimum and maximum value of the signal, creating a triangle like pattern.
 - Sawtooth: This is a continuous signal (sometimes call a ramp signal) that creates a constantly increasing signal that resets to its minimum value once the maximum value is reached.
 
-![Waveforms](Waveforms.svg)
+![Waveforms](media/Waveforms.svg)
 
 *By Omegatron - Own work, CC BY-SA 3.0, https://commons.wikimedia.org/w/index.php?curid=343520*
 
 ### Parameters
-- `controller_output`: Specifies what controller to apply the generated signal to. All other controllers will be constant at their default values. Valid values are `roll`, `pitch`, `altitude`, `heading`, and `airspeed`.
+- `controller_output`: Specifies what controller to apply the generated signal to. All other controllers will be constant at their default values. Valid values are `roll`, `pitch`, `altitude`, `course`, and `airspeed`.
 - `signal_type`: Specified what kind of signal to generate. Valid values are `step`, `square`, `sawtooth`, `triangle`, and `sine`.
 - `publish_rate_hz`: Specifies the rate to publish control commands. Must be greater than 0.
 - `signal_magnitude`: Specifies the magnitude of the signal to generate. The signal will only be added to the default value, rather than subtracted. For example, if the signal has a magnitude of 2 and a default value of 5, the generated signal will range from 5 to 7.
 - `frequency_hz`: Specifies the frequency of the generated signal. Must be greater than 0, and does not apply to step signals. For step signals, manually toggle the signal up and down with the `toggle_step_signal` service.
 - `default_va_c`: The default value for the commanded airspeed, in meters per second.
 - `default_h_c`: The default value for the commanded altitude, in meters above takeoff altitude.
-- `default_chi_c`: The default value for the commanded heading, in radians clockwise from north.
+- `default_chi_c`: The default value for the commanded course, in radians clockwise from north.
 - `default_theta_c`: The default value for the commanded pitch, in radians pitched up from horizontal.
 - `default_phi_c`: The default value for the commanded roll, in radians 'rolled right' from horizontal.
 

rosplane_tuning/include/signal_generator.hpp

@@ -53,7 +53,7 @@ namespace rosplane
 /**
  * This class is used to generate various input signals to test and tune all the control layers
  * in ROSplane. It currently supports square, sawtooth, triangle, and sine signals, and supports
- * outputting to the roll, pitch, altitude, heading, and airspeed controllers. 
+ * outputting to the roll, pitch, altitude, course, and airspeed controllers. 
  */
 class TuningSignalGenerator : public rclcpp::Node
 {
@@ -68,7 +68,7 @@ class TuningSignalGenerator : public rclcpp::Node
     ROLL,
     PITCH,
     ALTITUDE,
-    HEADING,
+    COURSE,
     AIRSPEED
   };
 

rosplane_tuning/launch/airspeed_autotune.launch.py

@@ -0,0 +1,38 @@
+import os
+
+from ament_index_python import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.substitutions import TextSubstitution, LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def generate_launch_description():
+    stabilize_period = LaunchConfiguration('stabilize_period')
+    stabilize_period_launch_arg = DeclareLaunchArgument(
+        'stabilize_period',
+        default_value=TextSubstitution(text='5.0'),
+        description='Whether to run the tuning sequence continuously or to wait for manual input'
+    )
+    continuous_tuning = LaunchConfiguration('continuous_tuning')
+    continuous_tuning_launch_arg = DeclareLaunchArgument(
+        'continuous_tuning',
+        default_value=TextSubstitution(text='False'),
+        description='Whether to run the tuning sequence continuously or to wait for manual input'
+    )
+    rosplane_tuning_launch_arg = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('rosplane_tuning'),
+                         'launch/autotune.launch.py')
+        ),
+        launch_arguments={
+            'stabilize_period': stabilize_period,
+            'continuous_tuning': continuous_tuning,
+            'current_tuning_autopilot': 'airspeed'
+        }.items()
+    )
+
+    return LaunchDescription([
+        stabilize_period_launch_arg,
+        continuous_tuning_launch_arg,
+        rosplane_tuning_launch_arg
+        ])

rosplane_tuning/launch/altitude_autotune.launch.py

@@ -0,0 +1,38 @@
+import os
+
+from ament_index_python import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.substitutions import TextSubstitution, LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def generate_launch_description():
+    stabilize_period = LaunchConfiguration('stabilize_period')
+    stabilize_period_launch_arg = DeclareLaunchArgument(
+        'stabilize_period',
+        default_value=TextSubstitution(text='10.0'),
+        description='Whether to run the tuning sequence continuously or to wait for manual input'
+    )
+    continuous_tuning = LaunchConfiguration('continuous_tuning')
+    continuous_tuning_launch_arg = DeclareLaunchArgument(
+        'continuous_tuning',
+        default_value=TextSubstitution(text='False'),
+        description='Whether to run the tuning sequence continuously or to wait for manual input'
+    )
+    rosplane_tuning_launch_arg = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('rosplane_tuning'),
+                         'launch/autotune.launch.py')
+        ),
+        launch_arguments={
+            'stabilize_period': stabilize_period,
+            'continuous_tuning': continuous_tuning,
+            'current_tuning_autopilot': 'altitude'
+        }.items()
+    )
+
+    return LaunchDescription([
+        stabilize_period_launch_arg,
+        continuous_tuning_launch_arg,
+        rosplane_tuning_launch_arg
+        ])

rosplane_tuning/launch/autotune.launch.py

@@ -0,0 +1,46 @@
+import os
+
+from ament_index_python import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+def generate_launch_description():
+
+    # Launch arguments
+    stabilize_period = LaunchConfiguration('stabilize_period')
+    stabilize_period_launch_arg = DeclareLaunchArgument(
+        'stabilize_period',
+        description='Amount of time to collect data for calculating error'
+    )
+    continuous_tuning = LaunchConfiguration('continuous_tuning')
+    continuous_tuning_launch_arg = DeclareLaunchArgument(
+        'continuous_tuning',
+        description='Whether to run the tuning sequence continuously or to wait for manual input'
+    )
+    current_tuning_autopilot = LaunchConfiguration('current_tuning_autopilot')
+    current_tuning_autopilot_launch_arg = DeclareLaunchArgument(
+        'current_tuning_autopilot',
+        description='Autopilot to tune'
+    )
+
+    # Launch nodes
+    autotune_node = Node(
+        package='rosplane_tuning',
+        executable='autotune.py',
+        output='screen',
+        parameters=[
+            {'stabilize_period': stabilize_period},
+            {'continuous_tuning': continuous_tuning},
+            {'current_tuning_autopilot': current_tuning_autopilot}
+        ]
+    )
+
+    return LaunchDescription([
+        stabilize_period_launch_arg,
+        continuous_tuning_launch_arg,
+        current_tuning_autopilot_launch_arg,
+        autotune_node
+        ])

rosplane_tuning/launch/course_autotune.launch.py

@@ -0,0 +1,38 @@
+import os
+
+from ament_index_python import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.substitutions import TextSubstitution, LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def generate_launch_description():
+    stabilize_period = LaunchConfiguration('stabilize_period')
+    stabilize_period_launch_arg = DeclareLaunchArgument(
+        'stabilize_period',
+        default_value=TextSubstitution(text='10.0'),
+        description='Whether to run the tuning sequence continuously or to wait for manual input'
+    )
+    continuous_tuning = LaunchConfiguration('continuous_tuning')
+    continuous_tuning_launch_arg = DeclareLaunchArgument(
+        'continuous_tuning',
+        default_value=TextSubstitution(text='False'),
+        description='Whether to run the tuning sequence continuously or to wait for manual input'
+    )
+    rosplane_tuning_launch_arg = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('rosplane_tuning'),
+                         'launch/autotune.launch.py')
+        ),
+        launch_arguments={
+            'stabilize_period': stabilize_period,
+            'continuous_tuning': continuous_tuning,
+            'current_tuning_autopilot': 'course'
+        }.items()
+    )
+
+    return LaunchDescription([
+        stabilize_period_launch_arg,
+        continuous_tuning_launch_arg,
+        rosplane_tuning_launch_arg
+        ])

rosplane_tuning/launch/pitch_autotune.launch.py

@@ -0,0 +1,38 @@
+import os
+
+from ament_index_python import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.substitutions import TextSubstitution, LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def generate_launch_description():
+    stabilize_period = LaunchConfiguration('stabilize_period')
+    stabilize_period_launch_arg = DeclareLaunchArgument(
+        'stabilize_period',
+        default_value=TextSubstitution(text='5.0'),
+        description='Whether to run the tuning sequence continuously or to wait for manual input'
+    )
+    continuous_tuning = LaunchConfiguration('continuous_tuning')
+    continuous_tuning_launch_arg = DeclareLaunchArgument(
+        'continuous_tuning',
+        default_value=TextSubstitution(text='False'),
+        description='Whether to run the tuning sequence continuously or to wait for manual input'
+    )
+    rosplane_tuning_launch_arg = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('rosplane_tuning'),
+                         'launch/autotune.launch.py')
+        ),
+        launch_arguments={
+            'stabilize_period': stabilize_period,
+            'continuous_tuning': continuous_tuning,
+            'current_tuning_autopilot': 'pitch'
+        }.items()
+    )
+
+    return LaunchDescription([
+        stabilize_period_launch_arg,
+        continuous_tuning_launch_arg,
+        rosplane_tuning_launch_arg
+        ])

rosplane_tuning/launch/roll_autotune.launch.py

@@ -0,0 +1,38 @@
+import os
+
+from ament_index_python import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.substitutions import TextSubstitution, LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def generate_launch_description():
+    stabilize_period = LaunchConfiguration('stabilize_period')
+    stabilize_period_launch_arg = DeclareLaunchArgument(
+        'stabilize_period',
+        default_value=TextSubstitution(text='5.0'),
+        description='Whether to run the tuning sequence continuously or to wait for manual input'
+    )
+    continuous_tuning = LaunchConfiguration('continuous_tuning')
+    continuous_tuning_launch_arg = DeclareLaunchArgument(
+        'continuous_tuning',
+        default_value=TextSubstitution(text='False'),
+        description='Whether to run the tuning sequence continuously or to wait for manual input'
+    )
+    rosplane_tuning_launch_arg = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('rosplane_tuning'),
+                         'launch/autotune.launch.py')
+        ),
+        launch_arguments={
+            'stabilize_period': stabilize_period,
+            'continuous_tuning': continuous_tuning,
+            'current_tuning_autopilot': 'roll'
+        }.items()
+    )
+
+    return LaunchDescription([
+        stabilize_period_launch_arg,
+        continuous_tuning_launch_arg,
+        rosplane_tuning_launch_arg
+        ])

rosplane_tuning/launch/rosplane_tuning.launch.py

@@ -23,7 +23,7 @@ def generate_launch_description():
     autopilot_params = os.path.join(
         rosplane_dir,
         'params',
-        aircraft + '_autopilot_commands.yaml'
+        aircraft + '_autopilot_params.yaml'
     )
 
 
@@ -32,9 +32,7 @@ def generate_launch_description():
             package='rosplane',
             executable='rosplane_controller',
             name='autopilot',
-            parameters = [autopilot_params,
-                          {'pitch_tuning_debug_override': False},
-                          {'roll_tuning_debug_override': True}],
+            parameters = [autopilot_params],
             output = 'screen',
             arguments = [control_type]
         ),