Signal Generator for Tuning

data_viz/data_viz/data_storage.py

@@ -5,7 +5,7 @@
 import threading
 from rosplane_msgs.msg import State
 from rosflight_msgs.msg import Command
-from rosplane_msgs.msg import ControllerInternals
+from rosplane_msgs.msg import ControllerInternalsDebug
 from rosplane_msgs.msg import ControllerCommands
 
 
@@ -147,7 +147,7 @@ def append(self, cmd: Command) -> None:
             del self.rudder[0:ind]
             del self.throttle[0:ind]
 
-class ControllerInternalsStorage:
+class ControllerInternalsDebugStorage:
 
     def __init__(self, t_horizon: float) -> None:
 
@@ -157,7 +157,7 @@ def __init__(self, t_horizon: float) -> None:
         self.pitch_c: list[float] = []
         self.roll_c: list[float] = []
 
-    def append(self, internals: ControllerInternals) -> None:
+    def append(self, internals: ControllerInternalsDebug) -> None:
         """ Stores the command data and trims the vectors
         """
         # Append data
@@ -227,7 +227,7 @@ def __init__(self, node: Node, t_horizon: float) -> None:
         self._sub_state = self.node.create_subscription(State, "/state", self.state_callback, 1)
         self._sub_est = self.node.create_subscription(State, "/estimated_state", self.estimate_callback, 1)
         self._sub_cmd = self.node.create_subscription(Command, "/command", self.command_callback, 1)
-        self._sub_cmd_internals = self.node.create_subscription(ControllerInternals, "/controller_inners", self.cmd_internal_callback, 1)
+        self._sub_cmd_internals = self.node.create_subscription(ControllerInternalsDebug, "/controller_inners_debug", self.cmd_internal_callback, 1)
         self._sub_con_cmd = self.node.create_subscription(ControllerCommands, "/controller_commands", self.con_command_callback, 1)
 
         self.con_cmd = ControllerCommands()
@@ -236,7 +236,7 @@ def __init__(self, node: Node, t_horizon: float) -> None:
         self.true = StateStorage(t_horizon=self.t_horizon)
         self.cmd = CommandStorage(t_horizon=self.t_horizon)
         self.est = StateStorage(t_horizon=self.t_horizon)
-        self.con_inners = ControllerInternalsStorage(t_horizon=self.t_horizon)
+        self.con_inners = ControllerInternalsDebugStorage(t_horizon=self.t_horizon)
         self.con_cmd = ControllerCommandsStorage(t_horizon=self.t_horizon)
 
     def state_callback(self, msg: State) -> None:
@@ -260,12 +260,12 @@ def command_callback(self, msg: Command) -> None:
             self.cmd.append(cmd=msg)
 
 
-    def cmd_internal_callback(self, msg: ControllerInternals) -> None:
+    def cmd_internal_callback(self, msg: ControllerInternalsDebug) -> None:
 
         with self.lock:
             self.con_inners.append(internals=msg)
 
     def con_command_callback(self, msg: ControllerCommands) -> None:
 
         with self.lock:
-            self.con_cmd.append(con_cmd=msg)
+            self.con_cmd.append(con_cmd=msg)

rosplane/CMakeLists.txt

@@ -24,6 +24,7 @@ find_package(rosidl_default_generators REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclpy REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
@@ -92,6 +93,15 @@ install(TARGETS
         rosplane_estimator_node
         DESTINATION lib/${PROJECT_NAME})
 
+# Signal Generator
+add_executable(tuning_signal_generator
+               src/tuning_signal_generator.cpp)
+ament_target_dependencies(tuning_signal_generator rosplane_msgs std_srvs rclcpp)
+target_compile_options(tuning_signal_generator PRIVATE -Wno-unused-parameter)
+install(TARGETS
+        tuning_signal_generator
+        DESTINATION lib/${PROJECT_NAME})
+
 #### END OF EXECUTABLES ###
 
 

rosplane/include/controller_base.hpp

@@ -11,9 +11,10 @@
 #define CONTROLLER_BASE_H
 
 #include <rclcpp/rclcpp.hpp>
+#include <rosplane_msgs/msg/detail/controller_internals_debug__struct.hpp>
 #include <rosplane_msgs/msg/state.hpp>
 #include <rosplane_msgs/msg/controller_commands.hpp>
-#include <rosplane_msgs/msg/controller_internals.hpp>
+#include <rosplane_msgs/msg/controller_internals_debug.hpp>
 #include <rosflight_msgs/msg/command.hpp>
 #include <chrono>
 #include <iostream>
@@ -127,6 +128,8 @@ class controller_base : public rclcpp::Node
     double max_takeoff_throttle; /**< maximum throttle allowed at takeoff */
     double mass;                 /**< mass of the aircraft */
     double gravity;              /**< gravity in m/s^2 */
+    bool pitch_tuning_debug_override;
+    bool roll_tuning_debug_override;
   };
 
   /**
@@ -137,6 +140,11 @@ class controller_base : public rclcpp::Node
    */
   virtual void control(const struct params_s &params, const struct input_s &input, struct output_s &output) = 0;
 
+  /**
+   * The override for the intermediate values for the controller.
+   */
+  rosplane_msgs::msg::ControllerInternalsDebug tuning_debug_override_msg_; // TODO find a better and more permanent place for this.
+
 private:
 
   /**
@@ -147,7 +155,7 @@ class controller_base : public rclcpp::Node
     /**
    * This publisher publishes the intermediate commands in the control algorithm.
    */
-  rclcpp::Publisher<rosplane_msgs::msg::ControllerInternals>::SharedPtr internals_pub_;
+  rclcpp::Publisher<rosplane_msgs::msg::ControllerInternalsDebug>::SharedPtr internals_pub_;
 
   /**
    * This subscriber subscribes to the commands the controller uses to calculate control effort.
@@ -159,6 +167,7 @@ class controller_base : public rclcpp::Node
    */
   rclcpp::Subscription<rosplane_msgs::msg::State>::SharedPtr vehicle_state_sub_;
 
+  rclcpp::Subscription<rosplane_msgs::msg::ControllerInternalsDebug>::SharedPtr tuning_debug_sub_;
   /**
    * This timer controls how often commands are published by the autopilot.
    */
@@ -204,7 +213,10 @@ class controller_base : public rclcpp::Node
           /* pwm_rad_r */ 1.0,
           /* max_takeoff_throttle */ .55,
           /* mass */ 2.28,
-          /* gravity */ 9.8};
+          /* gravity */ 9.8,
+          /* pitch_tuning_debug_override*/ false,
+          /* roll_tuning_debug_override*/ false
+    };
 
   /**
    * The stored value for the most up to date commands for the controller.
@@ -246,6 +258,13 @@ class controller_base : public rclcpp::Node
    */
   void vehicle_state_callback(const rosplane_msgs::msg::State::SharedPtr msg);
 
+  /**
+   * Callback for the overrided intermediate values of the controller interface for tuning.
+   * This saves the message as the member variable tuing_debug_override_msg_.
+   * @param msg
+   */
+  void tuning_debug_callback(const rosplane_msgs::msg::ControllerInternalsDebug::SharedPtr msg);
+
   /**
    * ROS2 parameter system interface. This connects ROS2 parameters with the defined update callback, parametersCallback.
    */

rosplane/include/tuning_signal_generator.hpp

@@ -0,0 +1,212 @@
+/*
+ * Software License Agreement (BSD-3 License)
+ *
+ * Copyright (c) 2023 Brandon Sutherland, BYU MAGICC Lab.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice, this
+ *   list of conditions and the following disclaimer.
+ *
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ *
+ * * Neither the name of the copyright holder nor the names of its
+ *   contributors may be used to endorse or promote products derived from
+ *   this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/**
+ * @file tuning_signal_generator.hpp
+ *
+ * ROS2 node for generating various input signals for tuning any/all layers of the controller.
+ *
+ * @author Brandon Sutherland <[email protected]>
+ */
+
+#ifndef TUNING_SIGNAL_GENERATOR_HPP
+#define TUNING_SIGNAL_GENERATOR_HPP
+
+#include "rosplane_msgs/msg/controller_commands.hpp"
+#include "rosplane_msgs/msg/controller_internals_debug.hpp"
+#include <rcl_interfaces/msg/set_parameters_result.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <std_srvs/srv/trigger.hpp>
+
+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. 
+ */
+class TuningSignalGenerator : public rclcpp::Node
+{
+public:
+  /// Contructor for signal generator.
+  TuningSignalGenerator();
+
+private:
+  /// This defines what controller to publish the generated signal to.
+  enum class ControllerOutput
+  {
+    ROLL,
+    PITCH,
+    ALTITUDE,
+    HEADING,
+    AIRSPEED
+  };
+
+  /// This defines what type of signal to publish to the selected controller.
+  enum class SignalType
+  {
+    STEP,
+    SQUARE,
+    SAWTOOTH,
+    TRIANGLE,
+    SINE
+  };
+
+  // Parameters
+  ControllerOutput controller_output_; ///< Controller to output command signals to.
+  SignalType signal_type_;             ///< Signal type to output.
+  double publish_rate_hz_;             ///< Frequency to publish commands.
+  double signal_magnitude_;            ///< The the magnitude of the signal being generated.
+  double frequency_hz_;                ///< Frequency of the signal.
+  double default_va_c_;                ///< Default for va_c.
+  double default_h_c_;                 ///< Default for h_c.
+  double default_chi_c_;               ///< Default for chi_c.
+  double default_theta_c_;             ///< Default for theta_c.
+  double default_phi_c_;               ///< Default for phi_c.
+
+  // Internal values
+  bool step_toggled_;               ///< Flag for when step signal has been toggled.
+  double initial_time_;             ///< Initial time of the signal.
+  bool is_paused_;                  ///< Flag to specify if signal should be paused.
+  double paused_time_;              ///< Amount of time that has been spent paused.
+  double single_period_start_time_; ///< Epoch time of when single period start was called.
+
+  /// Controller command ROS message publisher.
+  rclcpp::Publisher<rosplane_msgs::msg::ControllerCommands>::SharedPtr command_publisher_;
+  /// Controller internals ROS message publisher.
+  rclcpp::Publisher<rosplane_msgs::msg::ControllerInternalsDebug>::SharedPtr internals_publisher_;
+
+  /// ROS timer to run timer callback, which publishes commands
+  rclcpp::TimerBase::SharedPtr publish_timer_;
+
+  /// ROS parameter change callback handler.
+  OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;
+
+  /// ROS service for toggling step signal.
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr step_toggle_service_;
+  /// ROS service for reset signal.
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reset_service_;
+  /// ROS service for pause signal.
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr pause_service_;
+  /// ROS service for start signal continuously.
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr start_continuous_service_;
+  /// ROS service for start signal for one period.
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr start_single_service_;
+
+  /// Callback to publish command on topic.
+  void publish_timer_callback();
+
+  /// Callback for parameter changes.
+  rcl_interfaces::msg::SetParametersResult
+  param_callback(const std::vector<rclcpp::Parameter> & params);
+
+  /// Callback to toggle step signal.
+  bool step_toggle_service_callback(const std_srvs::srv::Trigger::Request::SharedPtr & req,
+                                    const std_srvs::srv::Trigger::Response::SharedPtr & res);
+  /// Callback to reset signal.
+  bool reset_service_callback(const std_srvs::srv::Trigger::Request::SharedPtr & req,
+                              const std_srvs::srv::Trigger::Response::SharedPtr & res);
+  /// Callback to pause signal.
+  bool pause_service_callback(const std_srvs::srv::Trigger::Request::SharedPtr & req,
+                              const std_srvs::srv::Trigger::Response::SharedPtr & res);
+  /// Callback to start signal continuously.
+  bool start_continuous_service_callback(const std_srvs::srv::Trigger::Request::SharedPtr & req,
+                                         const std_srvs::srv::Trigger::Response::SharedPtr & res);
+  /// Callback to start signal for a single period.
+  bool start_single_service_callback(const std_srvs::srv::Trigger::Request::SharedPtr & req,
+                                     const std_srvs::srv::Trigger::Response::SharedPtr & res);
+
+  /**
+   * @brief Get the value for a step signal at the given time with the given conditions.
+   *
+   * @param step_toggled Flag to specify if signal is "stepped up" or not.
+   * @param amplitude The amplitude of the signal.
+   * @param center_value The central value of the signal. Not the initial value of the signal,
+   *  but the value directly in the middle of the step values.
+   */
+  static double get_step_signal(bool step_toggled, double amplitude, double center_value);
+
+  /**
+   * @brief Get the value for a square signal at the given time with the given conditions.
+   *
+   * @param elapsed_time The amount of time that has passed since the 'start' of the signal
+   *   in seconds.
+   * @param amplitude The amplitude of the signal.
+   * @param frequency The frequency of the signal.
+   * @param center_value The central value of the signal. The in other words, the signal 'offset'.
+   */
+  static double get_square_signal(double elapsed_time, double amplitude, double frequency,
+                                  double center_value);
+  /**
+   * @brief Get the value for a sawtooth signal at the given time with the given conditions.
+   *
+   * @param elapsed_time The amount of time that has passed since the 'start' of the signal
+   *   in seconds.
+   * @param amplitude The amplitude of the signal.
+   * @param frequency The frequency of the signal.
+   * @param center_value The central value of the signal. The in other words, the signal 'offset'.
+   */
+  static double get_sawtooth_signal(double elapsed_time, double amplitude, double frequency,
+                                    double center_value);
+  /**
+   * @brief Get the value for a triangle signal at the given time with the given conditions.
+   *
+   * @param elapsed_time The amount of time that has passed since the 'start' of the signal
+   *   in seconds.
+   * @param amplitude The amplitude of the signal.
+   * @param frequency The frequency of the signal.
+   * @param center_value The central value of the signal. The in other words, the signal 'offset'.
+   */
+  static double get_triangle_signal(double elapsed_time, double amplitude, double frequency,
+                                    double center_value);
+  /**
+   * @brief Get the value for a sine signal at the given time with the given conditions.
+   *
+   * @param elapsed_time The amount of time that has passed since the 'start' of the signal
+   *   in seconds.
+   * @param amplitude The amplitude of the signal.
+   * @param frequency The frequency of the signal.
+   * @param center_value The central value of the signal. The in other words, the signal 'offset'.
+   */
+  static double get_sine_signal(double elapsed_time, double amplitude, double frequency,
+                                double center_value);
+
+  /// Updates the parameters within the class with the latest values from ROS.
+  void update_params();
+
+  /// Reset the signal generator.
+  void reset();
+};
+} // namespace rosplane
+
+#endif // TUNING_SIGNAL_GENERATOR_HPP