diff --git a/.gitignore b/.gitignore index 301eb2e..8059276 100644 --- a/.gitignore +++ b/.gitignore @@ -8,4 +8,8 @@ log/ # IDE files .vscode/ -.idea/ \ No newline at end of file +.idea/ + +# Python files +__pycache__/ +venv/ diff --git a/rosplane/src/controller_base.cpp b/rosplane/src/controller_base.cpp index f3bb364..e65e1ae 100644 --- a/rosplane/src/controller_base.cpp +++ b/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( - "/tuning_debug", 10, std::bind(&controller_base::tuning_debug_callback, this, _1)); - } + tuning_debug_sub_ = this->create_subscription( + "/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( diff --git a/rosplane_tuning/CMakeLists.txt b/rosplane_tuning/CMakeLists.txt index 631836a..08a9c47 100644 --- a/rosplane_tuning/CMakeLists.txt +++ b/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() diff --git a/rosplane_tuning/README.md b/rosplane_tuning/README.md index 0a4703e..e263768 100644 --- a/rosplane_tuning/README.md +++ b/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. diff --git a/rosplane_tuning/include/signal_generator.hpp b/rosplane_tuning/include/signal_generator.hpp index c97a06f..8319fdf 100644 --- a/rosplane_tuning/include/signal_generator.hpp +++ b/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 }; diff --git a/rosplane_tuning/launch/airspeed_autotune.launch.py b/rosplane_tuning/launch/airspeed_autotune.launch.py new file mode 100644 index 0000000..fb843d1 --- /dev/null +++ b/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 + ]) diff --git a/rosplane_tuning/launch/altitude_autotune.launch.py b/rosplane_tuning/launch/altitude_autotune.launch.py new file mode 100644 index 0000000..72619aa --- /dev/null +++ b/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 + ]) diff --git a/rosplane_tuning/launch/autotune.launch.py b/rosplane_tuning/launch/autotune.launch.py new file mode 100644 index 0000000..84587a3 --- /dev/null +++ b/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 + ]) diff --git a/rosplane_tuning/launch/course_autotune.launch.py b/rosplane_tuning/launch/course_autotune.launch.py new file mode 100644 index 0000000..bcdb0b5 --- /dev/null +++ b/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 + ]) diff --git a/rosplane_tuning/launch/pitch_autotune.launch.py b/rosplane_tuning/launch/pitch_autotune.launch.py new file mode 100644 index 0000000..a221ea9 --- /dev/null +++ b/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 + ]) diff --git a/rosplane_tuning/launch/roll_autotune.launch.py b/rosplane_tuning/launch/roll_autotune.launch.py new file mode 100644 index 0000000..9eb2c8d --- /dev/null +++ b/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 + ]) diff --git a/rosplane_tuning/launch/rosplane_tuning.launch.py b/rosplane_tuning/launch/rosplane_tuning.launch.py index 8e9a1b8..2291138 100644 --- a/rosplane_tuning/launch/rosplane_tuning.launch.py +++ b/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] ), diff --git a/rosplane_tuning/Waveforms.svg b/rosplane_tuning/media/Waveforms.svg similarity index 100% rename from rosplane_tuning/Waveforms.svg rename to rosplane_tuning/media/Waveforms.svg diff --git a/rosplane_tuning/media/roll_response.png b/rosplane_tuning/media/roll_response.png new file mode 100644 index 0000000..4f085ae Binary files /dev/null and b/rosplane_tuning/media/roll_response.png differ diff --git a/rosplane_tuning/package.xml b/rosplane_tuning/package.xml index 9323df2..1e51c63 100644 --- a/rosplane_tuning/package.xml +++ b/rosplane_tuning/package.xml @@ -17,6 +17,7 @@ sensor_msgs rosplane_msgs rosflight_msgs + python3-numpy ament_lint_auto ament_lint_common diff --git a/rosplane_tuning/src/autotune/__pycache__/optimizer.cpython-312.pyc b/rosplane_tuning/src/autotune/__pycache__/optimizer.cpython-312.pyc new file mode 100644 index 0000000..6584961 Binary files /dev/null and b/rosplane_tuning/src/autotune/__pycache__/optimizer.cpython-312.pyc differ diff --git a/rosplane_tuning/src/autotune/autotune.py b/rosplane_tuning/src/autotune/autotune.py new file mode 100644 index 0000000..fb182f5 --- /dev/null +++ b/rosplane_tuning/src/autotune/autotune.py @@ -0,0 +1,585 @@ +#!/usr/bin/env python3 + +from rosplane_msgs.msg import ControllerCommands +from rosplane_msgs.msg import ControllerInternalsDebug +from rosplane_msgs.msg import State +from plot_util import Optimizer + +import rclpy +from rclpy.executors import MultiThreadedExecutor +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.node import Node +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterType +from rcl_interfaces.srv import GetParameters, SetParameters +from std_srvs.srv import Trigger + +from enum import Enum, auto +from queue import Queue +import numpy as np +import time + + +class CurrentAutopilot(Enum): + """ + This class defines which autopilots are available for tuning. + """ + ROLL = auto() + COURSE = auto() + PITCH = auto() + ALTITUDE = auto() + AIRSPEED = auto() + + +class AutoTuneState(Enum): + """ + This class defines the possible states of the autotune node. + """ + ORBITING = auto() + STABILIZING = auto() + STEP_TESTING = auto() + RETURN_TESTING = auto() + + +class Autotune(Node): + """ + This class is an auto-tuning node for the ROSplane autopilot. The node calculates the error + between the state estimate of the system and the commanded setpoint for a given autopilot. + A gradient-based optimization is then run to find the optimal gains to minimize the error. + + This class itself contains the ROS-specific code for the autotune node. The optimization + algorithms are contained in the Optimizer class. The core of the autotune procedure and logic + is in the run_tuning_iteration_callback function and the stabilize_period_timer_callback + function. + + Va is airspeed, phi is roll angle, chi is course angle, theta is pitch angle, and h is altitude. + """ + + def __init__(self): + super().__init__('autotune') + + self.autotune_state = AutoTuneState.ORBITING + + # Callback groups, used for allowing external services to run mid-internal callback + self.internal_callback_group = MutuallyExclusiveCallbackGroup() + self.external_callback_group = MutuallyExclusiveCallbackGroup() + + # Class state variables + self.collecting_data = False + self.gain_queue = Queue() + self.error_queue = Queue() + + # Data storage + self.state = [] + self.commands = [] + self.internals_debug = [] + + # ROS parameters + # The amount of time to collect data for calculating the error + self.declare_parameter('stabilize_period', rclpy.Parameter.Type.DOUBLE) + # Whether to run the autotune continuously or not + self.declare_parameter('continuous_tuning', rclpy.Parameter.Type.BOOL) + # The autopilot that is currently being tuned + self.declare_parameter('current_tuning_autopilot', rclpy.Parameter.Type.STRING) + # Get the autopilot to tune + if self.get_parameter('current_tuning_autopilot').value == 'roll': + self.current_autopilot = CurrentAutopilot.ROLL + elif self.get_parameter('current_tuning_autopilot').value == 'course': + self.current_autopilot = CurrentAutopilot.COURSE + elif self.get_parameter('current_tuning_autopilot').value == 'pitch': + self.current_autopilot = CurrentAutopilot.PITCH + elif self.get_parameter('current_tuning_autopilot').value == 'altitude': + self.current_autopilot = CurrentAutopilot.ALTITUDE + elif self.get_parameter('current_tuning_autopilot').value == 'airspeed': + self.current_autopilot = CurrentAutopilot.AIRSPEED + else: + raise ValueError(self.get_parameter('current_tuning_autopilot').value + + ' is not a valid value for current_tuning_autopilot.' + + ' Please select one of the' + + ' following: roll, course, pitch, altitude, airspeed.') + + # Subscriptions + self.state_subscription = self.create_subscription( + State, + 'estimated_state', + self.state_callback, + 10, + callback_group=self.internal_callback_group) + self.commands_subscription = self.create_subscription( + ControllerCommands, + 'controller_commands', + self.commands_callback, + 10, + callback_group=self.internal_callback_group) + self.internals_debug_subscription = self.create_subscription( + ControllerInternalsDebug, + 'tuning_debug', + self.internals_debug_callback, + 10, + callback_group=self.internal_callback_group) + + # Timers + self.stabilize_period_timer = self.create_timer( + self.get_parameter('stabilize_period').value, + self.stabilize_period_timer_callback, + callback_group=self.internal_callback_group) + self.stabilize_period_timer.cancel() + + # Services + self.run_tuning_iteration_service = self.create_service( + Trigger, + 'run_tuning_iteration', + self.run_tuning_iteration_callback, + callback_group=self.internal_callback_group) + + # Clients + self.toggle_step_signal_client = self.create_client( + Trigger, + 'toggle_step_signal', + callback_group=self.external_callback_group) + self.get_parameter_client = self.create_client( + GetParameters, + '/autopilot/get_parameters', + callback_group=self.external_callback_group) + self.autopilot_set_param_client = self.create_client( + SetParameters, + '/autopilot/set_parameters', + callback_group=self.external_callback_group) + self.signal_generator_set_param_client = self.create_client( + SetParameters, + '/signal_generator/set_parameters', + callback_group=self.external_callback_group) + + # Optimization + self.initial_gains = None # get_gains function cannot be called in __init__ since the node + # has not yet been passed to the executor + + # As we conduct optimizations, these parameters will be changed to be more + # efficient to optimize the specific gains, based on the design spaces. + if self.current_autopilot == CurrentAutopilot.ROLL: + self.optimization_params = {'alpha': 0.01, + 'h': 0.04} + elif self.current_autopilot == CurrentAutopilot.PITCH: + self.optimization_params = {'alpha': 0.01, + 'h': -0.04} + elif self.current_autopilot == CurrentAutopilot.COURSE: + self.optimization_params = {'alpha': 0.01, + 'h': 0.04} + elif self.current_autopilot == CurrentAutopilot.ALTITUDE: + self.optimization_params = {'alpha': 0.01, + 'h': 0.04} + elif self.current_autopilot == CurrentAutopilot.AIRSPEED: + self.optimization_params = {'alpha': 0.01, + 'h': 0.04} + else: + raise ValueError(self.get_parameter('current_tuning_autopilot').value + + ' is not a valid value for current_tuning_autopilot.' + + ' Please select one of the' + + ' following: roll, course, pitch, altitude, airspeed.') + + self.optimizer = None + + + ## ROS Callbacks ## + def state_callback(self, msg): + """ + This function is called when a new state estimate is received. It stores the state estimate + if the node is currently collecting data. + """ + + if self.collecting_data: + time = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9 + self.state.append([time, msg.va, msg.phi, msg.chi, msg.theta, + -msg.position[2]]) # h = -msg.position[2] + + def commands_callback(self, msg): + """ + This function is called when new commands are received. It stores the commands if the node + is currently collecting data. + """ + + if self.collecting_data: + time = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9 + self.commands.append([time, msg.va_c, msg.chi_c, msg.h_c]) + + def internals_debug_callback(self, msg): + """ + This function is called when new debug information is received. It stores the debug + information if the node is currently collecting data. + """ + + if self.collecting_data: + time = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9 + self.internals_debug.append([time, msg.phi_c, msg.theta_c]) + + def stabilize_period_timer_callback(self): + """ + This function is called when the stability timer callback occurs. It starts/stops data + collection and sets up ROSplane to perform a step manuever. + """ + + if self.autotune_state == AutoTuneState.STABILIZING: + # Stabilization period is over, start collecting data + self.get_logger().info('Setting gains: ' + str(self.new_gains)) + self.set_current_gains(self.new_gains) + self.get_logger().info('Stepping command for ' + + str(self.get_parameter('stabilize_period').value) + + ' seconds...') + self.collecting_data = True + self.call_toggle_step_signal() + self.autotune_state = AutoTuneState.STEP_TESTING + + elif self.autotune_state == AutoTuneState.STEP_TESTING: + # Step test is over, reverse step + self.get_logger().info('Reversing step command for ' + + str(self.get_parameter('stabilize_period').value) + + ' seconds...') + self.call_toggle_step_signal() + self.autotune_state = AutoTuneState.RETURN_TESTING + + else: + # Data collection is over, stop collecting data and calculate gains for next iteration + self.get_logger().info('Data collection complete, restoring original gains and ' + + 'returning to orbit.') + self.collecting_data = False + self.stabilize_period_timer.cancel() + self.set_current_gains(self.initial_gains) + self.disable_autopilot_override() + self.autotune_state = AutoTuneState.ORBITING + + if self.get_parameter('continuous_tuning').value: + self.run_tuning_iteration_callback(Trigger.Request(), Trigger.Response()) + + def run_tuning_iteration_callback(self, request, response): + """ + This function is called when the run_tuning_iteration service is called. It starts the + next iteration of the optimization process. + """ + + if self.optimizer is None: + # Initialize optimizer + self.initial_gains = self.get_current_gains() + self.optimizer = Optimizer(self.initial_gains, self.optimization_params) + self.new_gains = self.initial_gains + self.set_signal_generator_params() + + elif not self.optimizer.optimization_terminated(): + self.new_gains = self.get_next_gains() + + if not self.optimizer.optimization_terminated(): + self.stabilize_period_timer.timer_period_ns = \ + int(self.get_parameter('stabilize_period').value * 1e9) + self.stabilize_period_timer.reset() + self.enable_autopilot_override() + self.get_logger().info('Stabilizing autopilot for ' + + str(self.get_parameter('stabilize_period').value) + + ' seconds...') + self.autotune_state = AutoTuneState.STABILIZING + self.get_logger().info(f"State: {self.optimizer.get_optimization_status()}") + + else: + self.get_logger().info('Optimization terminated with: ' + + self.optimizer.get_optimization_status()) + self.stabilize_period_timer.cancel() + + response.success = True + response.message = self.optimizer.get_optimization_status() + + return response + + + ## Helper Functions ## + def set_signal_generator_params(self): + """ + Sets the signal generators parameters to the initial values needed for the optimization. + """ + + request = SetParameters.Request() + if self.current_autopilot == CurrentAutopilot.ROLL: + request.parameters = [ + Parameter(name='controller_output', value='roll').to_parameter_msg(), + Parameter(name='signal_type', value='step').to_parameter_msg(), + Parameter(name='signal_magnitude', value=np.deg2rad(30)).to_parameter_msg(), + Parameter(name='default_phi_c', value=0.0).to_parameter_msg() + ] + elif self.current_autopilot == CurrentAutopilot.COURSE: + request.parameters = [ + Parameter(name='controller_output', value='course').to_parameter_msg(), + Parameter(name='signal_type', value='step').to_parameter_msg(), + Parameter(name='signal_magnitude', value=np.deg2rad(90)).to_parameter_msg(), + ] + elif self.current_autopilot == CurrentAutopilot.PITCH: + request.parameters = [ + Parameter(name='controller_output', value='pitch').to_parameter_msg(), + Parameter(name='signal_type', value='step').to_parameter_msg(), + Parameter(name='signal_magnitude', value=np.deg2rad(20)).to_parameter_msg(), + Parameter(name='default_theta_c', value=0.0).to_parameter_msg() + ] + elif self.current_autopilot == CurrentAutopilot.ALTITUDE: + request.parameters = [ + Parameter(name='controller_output', value='altitude').to_parameter_msg(), + Parameter(name='signal_type', value='step').to_parameter_msg(), + Parameter(name='signal_magnitude', value=10.0).to_parameter_msg(), + ] + else: # CurrentAutopilot.AIRSPEED + request.parameters = [ + Parameter(name='controller_output', value='airspeed').to_parameter_msg(), + Parameter(name='signal_type', value='step').to_parameter_msg(), + Parameter(name='signal_magnitude', value=5.0).to_parameter_msg(), + Parameter(name='default_theta_c', value=0.0).to_parameter_msg() + ] + + + # Call the service + while not self.signal_generator_set_param_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('Service {self.signal_generator_set_param_client.srv_name}' + + ' not available, waiting...') + future = self.signal_generator_set_param_client.call_async(request) + + # Wait for the service to complete, exiting if it takes too long + call_time = time.time() + callback_complete = False + while call_time + 5 > time.time(): + if future.done(): + callback_complete = True + break + if not callback_complete or future.result() is None: + raise RuntimeError('Unable to set signal generator gains.') + + # Print any errors that occurred + for response in future.result().results: + if not response.successful: + raise RuntimeError(f'Failed to set parameter: {response.reason}') + + def enable_autopilot_override(self): + """ + Enable to autopilot override for the current autopilot, if an override is needed. + This enables direct control of that autopilot, rather than the outer loop controlling + the autopilot. + """ + request = SetParameters.Request() + if self.current_autopilot == CurrentAutopilot.ROLL: + request.parameters = [ + Parameter(name='roll_tuning_debug_override', value=True).to_parameter_msg() + ] + elif self.current_autopilot == CurrentAutopilot.PITCH \ + or self.current_autopilot == CurrentAutopilot.AIRSPEED: + request.parameters = [ + Parameter(name='pitch_tuning_debug_override', value=True).to_parameter_msg() + ] + self.set_autopilot_params(request) + + def disable_autopilot_override(self): + """ + Disable the autopilot override for the current autopilot, if an override is needed. + This allows the outer loop to control the autopilot again. + """ + request = SetParameters.Request() + if self.current_autopilot == CurrentAutopilot.ROLL: + request.parameters = [ + Parameter(name='roll_tuning_debug_override', value=False).to_parameter_msg() + ] + elif self.current_autopilot == CurrentAutopilot.PITCH \ + or self.current_autopilot == CurrentAutopilot.AIRSPEED: + request.parameters = [ + Parameter(name='pitch_tuning_debug_override', value=False).to_parameter_msg() + ] + self.set_autopilot_params(request) + + def call_toggle_step_signal(self): + """ + Call the signal_generator's toggle step service to toggle the step input. + """ + + while not self.toggle_step_signal_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info(f'Service {self.toggle_step_signal_client.srv_name} ' + + 'not available, waiting...') + + self.toggle_step_signal_client.call_async(Trigger.Request()) + + def get_current_gains(self): + """ + Gets the current gains of the autopilot. + + Returns: + np.array, size num_gains, dtype float: The gains of the autopilot. [gain1, gain2, ...] + """ + + request = GetParameters.Request() + if self.current_autopilot == CurrentAutopilot.ROLL: + request.names = ['r_kp', 'r_kd'] + elif self.current_autopilot == CurrentAutopilot.COURSE: + request.names = ['c_kp', 'c_ki'] + elif self.current_autopilot == CurrentAutopilot.PITCH: + request.names = ['p_kp', 'p_kd'] + elif self.current_autopilot == CurrentAutopilot.ALTITUDE: + request.names = ['a_kp', 'a_ki'] + else: # CurrentAutopilot.AIRSPEED + request.names = ['a_t_kp', 'a_t_ki'] + + # Call the service + while not self.get_parameter_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info(f'Service {self.get_parameter_client.srv_name}' + + ' not available, waiting...') + future = self.get_parameter_client.call_async(request) + + # Wait for the service to complete, exiting if it takes too long + call_time = time.time() + callback_complete = False + while call_time + 5 > time.time(): + if future.done(): + callback_complete = True + break + if not callback_complete or future.result() is None: + raise RuntimeError('Unable to get autopilot gains.') + + # Check that values were returned. No values are returned if the parameter does not exist. + if len(future.result().values) == 0: + raise RuntimeError(f'Parameter values for {request.names} were not returned, ' + + 'have they been set?') + + # Put the gains into a numpy array + gains = [] + for value in future.result().values: + if value.type != ParameterType.PARAMETER_DOUBLE: + raise RuntimeError('Parameter type returned by get_parameters is not DOUBLE.') + gains.append(value.double_value) + + return np.array(gains) + + def set_current_gains(self, gains): + """ + Set the gains of the autopilot to the given values. + + Parameters: + gains (np.array, size num_gains, dtype float): The gains to set for the autopilot. + [gain1, gain2, ...] + """ + + request = SetParameters.Request() + if self.current_autopilot == CurrentAutopilot.ROLL: + request.parameters = [Parameter(name='r_kp', value=gains[0]).to_parameter_msg(), + Parameter(name='r_kd', value=gains[1]).to_parameter_msg()] + elif self.current_autopilot == CurrentAutopilot.COURSE: + request.parameters = [Parameter(name='c_kp', value=gains[0]).to_parameter_msg(), + Parameter(name='c_ki', value=gains[1]).to_parameter_msg()] + elif self.current_autopilot == CurrentAutopilot.PITCH: + request.parameters = [Parameter(name='p_kp', value=gains[0]).to_parameter_msg(), + Parameter(name='p_kd', value=gains[1]).to_parameter_msg()] + elif self.current_autopilot == CurrentAutopilot.ALTITUDE: + request.parameters = [Parameter(name='a_kp', value=gains[0]).to_parameter_msg(), + Parameter(name='a_ki', value=gains[1]).to_parameter_msg()] + else: # CurrentAutopilot.AIRSPEED + request.parameters = [Parameter(name='a_t_kp', value=gains[0]).to_parameter_msg(), + Parameter(name='a_t_ki', value=gains[1]).to_parameter_msg()] + + self.set_autopilot_params(request) + + def set_autopilot_params(self, request): + """ + Set parameters for the autopilot. Moved to a separate function to avoid code repetition. + + Parameters: + request (SetParameters.Request): A ROS2 object for setting parameters, already populated + with the desired parameters and values. + """ + + # Call the service + while not self.autopilot_set_param_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('Service {self.set_parameter_client.srv_name}' + + ' not available, waiting...') + future = self.autopilot_set_param_client.call_async(request) + + # Wait for the service to complete, exiting if it takes too long + call_time = time.time() + callback_complete = False + while call_time + 5 > time.time(): + if future.done(): + callback_complete = True + break + if not callback_complete or future.result() is None: + raise RuntimeError('Unable to set autopilot params.') + + # Print any errors that occurred + for response in future.result().results: + if not response.successful: + raise RuntimeError(f'Failed to set parameter: {response.reason}') + + def calculate_error(self): + """ + Calculate the error between the state estimate and the commanded setpoint using the + collected data. + """ + + # Get the estimate and command data from the right autopilot + if self.current_autopilot == CurrentAutopilot.ROLL: + estimate = np.array(self.state)[:, [0, 2]] + command = np.array(self.internals_debug)[:, [0, 1]] + elif self.current_autopilot == CurrentAutopilot.COURSE: + estimate = np.array(self.state)[:, [0, 3]] + command = np.array(self.commands)[:, [0, 2]] + elif self.current_autopilot == CurrentAutopilot.PITCH: + estimate = np.array(self.state)[:, [0, 4]] + command = np.array(self.internals_debug)[:, [0, 2]] + elif self.current_autopilot == CurrentAutopilot.ALTITUDE: + estimate = np.array(self.state)[:, [0, 5]] + command = np.array(self.commands)[:, [0, 3]] + else: # CurrentAutopilot.AIRSPEED + estimate = np.array(self.state)[:, [0, 1]] + command = np.array(self.commands)[:, [0, 1]] + + # Trim any measurements that happened before the first command + estimate = estimate[np.where(estimate[:, 0] >= command[0, 0])[0], :] + + # Integrate across the square of the error + cumulative_error = 0.0 + for i in range(estimate.shape[0] - 1): + # Get command that was just previous + command_index = np.where(command[:, 0] <= estimate[i + 1, 0])[0][-1] + # Numerically integrate + cumulative_error += (estimate[i + 1, 1] - command[command_index, 1])**2 \ + * (estimate[i + 1, 0] - estimate[i, 0]) + + return cumulative_error + + def get_next_gains(self): + """ + Gets the next set of gains to test from either the queue or the optimizer. + + Returns: + np.array, size num_gains, dtype float: The gains to test. [gain1, gain2, ...] + """ + + self.error_queue.put(self.calculate_error()) + self.state = [] + self.commands = [] + self.internals_debug = [] + + if self.gain_queue.empty(): + # Empty the error queue and store it in a numpy array + error_array = [] + while not self.error_queue.empty(): + error_array.append(self.error_queue.get()) + error_array = np.array(error_array) + + # Store the next set of gains in the queue + for set in self.optimizer.get_next_parameter_set(error_array): + self.gain_queue.put(set) + + return self.gain_queue.get() + + +def main(args=None): + rclpy.init(args=args) + + autotune = Autotune() + executor = MultiThreadedExecutor() + executor.add_node(autotune) + executor.spin() + + autotune.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/rosplane_tuning/src/autotune/optimizer.py b/rosplane_tuning/src/autotune/optimizer.py new file mode 100644 index 0000000..bf2e8a3 --- /dev/null +++ b/rosplane_tuning/src/autotune/optimizer.py @@ -0,0 +1,128 @@ +#!/usr/bin/env python3 + +from enum import Enum, auto +import numpy as np + + +class OptimizerState(Enum): + """ + This class defines the various states that exist within the optimizer. + """ + UNINITIALIZED = auto() + FIRST_ITERATION = auto() + OPTIMIZING = auto() + TERMINATED = auto() + + +class Optimizer: + """ + This class contains the optimization algorithms used by the autotune node. It is able to + optimize functions of any number of parameters. + """ + + def __init__(self, initial_point, optimization_params): + """ + Parameters: + initial_point (np.array, size num_gains, dtype float): The starting point for the gains to + be optimized (x_0). [gain1, gain2, ...] + optimization_params (dictionary): Parameters needed for the optimization operation. + - alpha (float): The step size. + - h (float): Finite difference step size, used to calculate the gradient. + """ + + # Save passed optimization parameters. See above for parameters details. + self.alpha = optimization_params['alpha'] + self.h = optimization_params['h'] + + # The current state of the optimization process. + self.state = OptimizerState.UNINITIALIZED + + # Reason for termination + self.reason = "" + + # Initialize the gains to the initial point + self.x = initial_point + self.prev_error = None + + + def optimization_terminated(self): + """ + This function checks if the optimization algorithm has terminated. + + Returns: + bool: True if optimization is terminated, False otherwise. + """ + if self.state == OptimizerState.TERMINATED: + return True + else: + return False + + def get_optimization_status(self): + """ + This function returns the status of the optimization algorithm. + + Returns: + str: The status of the optimization algorithm. + """ + if self.state == OptimizerState.UNINITIALIZED: + return "Optimizer awaiting first iteration." + elif self.state == OptimizerState.FIRST_ITERATION: + return "First iteration." + elif self.state == OptimizerState.OPTIMIZING: + return "Optimizing." + else: # self.state == OptimizerState.TERMINATED: + return str(self.reason) + " Final gains: " + str(self.x) + + def get_next_parameter_set(self, error): + """ + This function returns the next set of gains to be tested by the optimization algorithm. + Needs to return valid array even if optimization is terminated. + + Parameters: + error (np.array, size num_gains, dtype float): The error from the list of gains to test that + was returned by this function previously. If this is the first iteration, the error of + the initial gains will be given. [error1, error2, ...] + + Returns: + np.array, size num_gains*num_sets, dtype float: The next series of gains to be tested. Any + number of gain sets can be returned, but each set should correspond to the initial gains + given at initialization. [[gain1_1, gain2_1, ...], [gain1_2, gain2_2, ...], ...] + """ + + if self.state == OptimizerState.UNINITIALIZED: + # Find the gradient at the new point + self.state = OptimizerState.FIRST_ITERATION + self.prev_error = error.item(0) + + # Find the values for finite difference, where each dimension is individually + # stepped by h + return np.tile(self.x, (self.x.shape[0], 1)) \ + + np.identity(self.x.shape[0]) * self.h + + if self.state == OptimizerState.FIRST_ITERATION or self.state == OptimizerState.OPTIMIZING: + # Calculate the jacobean at the current point + if self.state == OptimizerState.FIRST_ITERATION: + J = (error - self.prev_error) / self.h + self.state = OptimizerState.OPTIMIZING + else: + J = (error[1:] - error[0]) / self.h + + # Check if any of the new points are better + if np.min(error) > self.prev_error: + self.reason = "Current iteration worse than previous." + self.state = OptimizerState.TERMINATED + return self.x.reshape(1, 2) + self.prev_error = error.item(0) + + + # Take a step in the search direction + self.x = self.x - J * self.alpha + + # Find the gradient at the new point + points = np.tile(self.x, (self.x.shape[0], 1)) \ + + np.identity(self.x.shape[0]) * self.h + points = np.vstack((self.x, points)) + return points + + else: # Process Terminated + return self.x.reshape(1, 2) diff --git a/rosplane_tuning/src/autotune/optimizer_tester.py b/rosplane_tuning/src/autotune/optimizer_tester.py new file mode 100644 index 0000000..ca41315 --- /dev/null +++ b/rosplane_tuning/src/autotune/optimizer_tester.py @@ -0,0 +1,71 @@ +from optimizer import Optimizer + +import numpy as np +import matplotlib.pyplot as plt + + +def FakeRollController(x, include_noise): + # Function that roughly mimics the roll controller landscape + std = 0.1 if include_noise else 0.0 + + rotation = np.deg2rad(35) + x_offset = 0.20 + y_offset = 0.03 + x_scale = 15 + y_scale = 3 + + x_rot = x[0] * np.cos(rotation) - x[1] * np.sin(rotation) + y_rot = x[0] * np.sin(rotation) + x[1] * np.cos(rotation) + x_trans = x_rot - (x_offset*np.cos(rotation) - y_offset*np.sin(rotation)) + y_trans = y_rot - (x_offset*np.sin(rotation) + y_offset*np.cos(rotation)) + x_scaled = x_trans**2 * x_scale + y_scaled = y_trans**2 * y_scale + return x_scaled + y_scaled + np.random.normal(0, std) + +# Initialize optimizer +curr_points = np.array([[0.06, 0.04]]) # Initial point +optimization_params = {'alpha': 0.01, + 'h': 0.04} +optimizer = Optimizer(curr_points[0], optimization_params) + +# Print initial point and value +print('Initial point: {}'.format(curr_points[0])) +print('Initial value: {}'.format(FakeRollController(curr_points[0], False))) + +# Run optimization +all_points = [] +x_points = [] +while not optimizer.optimization_terminated(): + # Calculate error for current points + error = [] + for point in curr_points: + error.append(FakeRollController(point, True)) + error = np.array(error) + # Pass points to optimizer + curr_points = optimizer.get_next_parameter_set(error) + + # Store points + for point in curr_points: + all_points.append(point) + x_points.append(optimizer.x) + + # End interation step +all_points = np.array(all_points) +x_points = np.array(x_points) + +print('Optimization terminated with status: {}'.format(optimizer.get_optimization_status())) +print('Final value: {}'.format(FakeRollController(curr_points[0], False))) +print('Total number of points tested: {}'.format(len(all_points))) + + +# Plot the function with the optimization path +x = np.linspace(0., 0.4, 100) +y = np.linspace(-0.1, 0.2, 100) +X, Y = np.meshgrid(x, y) +Z = FakeRollController([X, Y], False) +plt.contour(X, Y, Z, 50) +plt.plot(x_points[:, 0], x_points[:, 1], color='b', marker='o') +# Lock the x and y axis to be equal +plt.axis('equal') +plt.show() + diff --git a/rosplane_tuning/src/autotune/plot_util.py b/rosplane_tuning/src/autotune/plot_util.py new file mode 100644 index 0000000..91d9f22 --- /dev/null +++ b/rosplane_tuning/src/autotune/plot_util.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 + +import numpy as np +import matplotlib.pyplot as plt + +class Optimizer: + """ + This class is a utility that can be used to plot the function landscape of the autopilot + being tuned. It acts as a dummy optimizer that instead of running an optimization, it collects + data based on a grid of points and then plots the function landscape. + + WARNING: Plotting is intended for simulation only, do not attempt to plot on real hardware + as the autopilot may be given non-functional gains. As such, no launch files are provided for + this utility. If you wish to use it, modify the source code of the autotune node to + include this class instead of the optimizer. + """ + + def __init__(self, *args): + # None of the optimization parameters are needed, so they are ignored. + + # Define the grid of points to test and plot + self.x_values = np.linspace(0.1, 0.3, 10) + self.y_values = np.linspace(0.0, 0.1, 10) + + self.points_given = False + self.plotting_complete = False + + def optimization_terminated(self): + return self.plotting_complete + + def get_optimization_status(self): + if self.plotting_complete: + return "Plotting complete." + else: + return "Collecting data for plotting." + + def get_next_parameter_set(self, error): + if not self.points_given: + self.points_given = True + x, y = np.meshgrid(self.x_values, self.y_values) + return np.column_stack((x.ravel(), y.ravel())) + else: + self.plot(error) + self.plotting_complete = True + return np.zeros((1, 2)) + + def plot(self, error): + x, y = np.meshgrid(self.x_values, self.y_values) + z = error.reshape(x.shape) + + # Save to npy file, iterate the filename if it already exists + xyz = np.stack((x, y, z), axis=-1) + file_index = 0 + while True: + filename = f'plot_data_{file_index}.npy' + try: + with open(filename, 'xb') as f: # Use 'xb' mode to create the file exclusively + np.save(f, xyz) + break # Exit loop if file creation is successful + except FileExistsError: + file_index += 1 + + # Plot the function landscape in a 3D plot + fig = plt.figure() + ax = fig.add_subplot(111, projection='3d') + ax.plot_surface(x, y, z, cmap='viridis', alpha=0.8) + ax.set_xlabel('Gain 1') + ax.set_ylabel('Gain 2') + ax.set_zlabel('Error') + plt.show() + diff --git a/rosplane_tuning/src/signal_generator.cpp b/rosplane_tuning/src/signal_generator.cpp index 21d0b48..1f30770 100644 --- a/rosplane_tuning/src/signal_generator.cpp +++ b/rosplane_tuning/src/signal_generator.cpp @@ -137,7 +137,7 @@ void TuningSignalGenerator::publish_timer_callback() case ControllerOutput::ALTITUDE: center_value = default_h_c_; break; - case ControllerOutput::HEADING: + case ControllerOutput::COURSE: center_value = default_chi_c_; break; case ControllerOutput::AIRSPEED: @@ -186,7 +186,7 @@ void TuningSignalGenerator::publish_timer_callback() case ControllerOutput::ALTITUDE: command_message.h_c = signal_value; break; - case ControllerOutput::HEADING: + case ControllerOutput::COURSE: command_message.chi_c = signal_value; break; case ControllerOutput::AIRSPEED: @@ -336,8 +336,8 @@ void TuningSignalGenerator::update_params() controller_output_ = ControllerOutput::PITCH; } else if (controller_output_string == "altitude") { controller_output_ = ControllerOutput::ALTITUDE; - } else if (controller_output_string == "heading") { - controller_output_ = ControllerOutput::HEADING; + } else if (controller_output_string == "course") { + controller_output_ = ControllerOutput::COURSE; } else if (controller_output_string == "airspeed") { controller_output_ = ControllerOutput::AIRSPEED; } else {