From 0d04ac0e6fb0069ba22fcf050ccc5630996b274f Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Thu, 19 Oct 2023 11:48:11 -0600 Subject: [PATCH 01/25] Renamed ControllerInternalsDebug for clarity. --- data_viz/data_viz/data_storage.py | 14 +++++++------- rosplane/include/controller_base.hpp | 8 +++++--- rosplane/src/controller_base.cpp | 7 +++++-- rosplane_msgs/CMakeLists.txt | 2 +- ...rInternals.msg => ControllerInternalsDebug.msg} | 0 5 files changed, 18 insertions(+), 13 deletions(-) rename rosplane_msgs/msg/{ControllerInternals.msg => ControllerInternalsDebug.msg} (100%) diff --git a/data_viz/data_viz/data_storage.py b/data_viz/data_viz/data_storage.py index b34a6b4..f87a4ac 100644 --- a/data_viz/data_viz/data_storage.py +++ b/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", 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,7 +260,7 @@ 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) @@ -268,4 +268,4 @@ def cmd_internal_callback(self, msg: ControllerInternals) -> None: def con_command_callback(self, msg: ControllerCommands) -> None: with self.lock: - self.con_cmd.append(con_cmd=msg) \ No newline at end of file + self.con_cmd.append(con_cmd=msg) diff --git a/rosplane/include/controller_base.hpp b/rosplane/include/controller_base.hpp index 9cff660..40f9dbc 100644 --- a/rosplane/include/controller_base.hpp +++ b/rosplane/include/controller_base.hpp @@ -13,7 +13,7 @@ #include #include #include -#include +#include #include #include #include @@ -127,6 +127,7 @@ 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 tuning_debug_override; }; /** @@ -147,7 +148,7 @@ class controller_base : public rclcpp::Node /** * This publisher publishes the intermediate commands in the control algorithm. */ - rclcpp::Publisher::SharedPtr internals_pub_; + rclcpp::Publisher::SharedPtr internals_pub_; /** * This subscriber subscribes to the commands the controller uses to calculate control effort. @@ -204,7 +205,8 @@ class controller_base : public rclcpp::Node /* pwm_rad_r */ 1.0, /* max_takeoff_throttle */ .55, /* mass */ 2.28, - /* gravity */ 9.8}; + /* gravity */ 9.8, + /* tuning_debug_override*/ false}; /** * The stored value for the most up to date commands for the controller. diff --git a/rosplane/src/controller_base.cpp b/rosplane/src/controller_base.cpp index c7df383..976633a 100644 --- a/rosplane/src/controller_base.cpp +++ b/rosplane/src/controller_base.cpp @@ -10,7 +10,7 @@ controller_base::controller_base() : Node("controller_base") // Advertise published topics. actuators_pub_ = this->create_publisher("command",10); - internals_pub_ = this->create_publisher("controller_inners",10); + internals_pub_ = this->create_publisher("controller_inners",10); // Set timer to trigger bound callback (actuator_controls_publish) at the given periodicity. timer_ = this->create_wall_timer(10ms, std::bind(&controller_base::actuator_controls_publish, this)); // TODO add the period to the params. @@ -113,7 +113,7 @@ void controller_base::actuator_controls_publish() // control values, phi_c (commanded roll angle), and theta_c (commanded pitch angle). if (internals_pub_->get_subscription_count() > 0) { - rosplane_msgs::msg::ControllerInternals inners; + rosplane_msgs::msg::ControllerInternalsDebug inners; inners.header.stamp = now; @@ -181,6 +181,7 @@ void controller_base::actuator_controls_publish() this->declare_parameter("max_takeoff_throttle", params_.max_takeoff_throttle); this->declare_parameter("mass", params_.mass); this->declare_parameter("gravity", params_.gravity); + this->declare_parameter("tuning_debug_override", params_.tuning_debug_override); } void controller_base::set_parameters() { @@ -226,6 +227,7 @@ void controller_base::set_parameters() { params_.max_takeoff_throttle = this->get_parameter("pwm_rad_r").as_double(); params_.mass = this->get_parameter("mass").as_double(); params_.gravity = this->get_parameter("gravity").as_double(); + params_.gravity = this->get_parameter("tuning_debug_override").as_bool(); } @@ -276,6 +278,7 @@ rcl_interfaces::msg::SetParametersResult controller_base::parametersCallback(con else if (param.get_name() == "max_takeoff_throttle") params_.max_takeoff_throttle = param.as_double(); else if (param.get_name() == "mass") params_.mass = param.as_double(); else if (param.get_name() == "gravity") params_.gravity = param.as_double(); + else if (param.get_name() == "tuning_debug_override") params_.tuning_debug_override = param.as_bool(); else{ // If the parameter given doesn't match any of the parameters return false. diff --git a/rosplane_msgs/CMakeLists.txt b/rosplane_msgs/CMakeLists.txt index a84efcd..50571b9 100644 --- a/rosplane_msgs/CMakeLists.txt +++ b/rosplane_msgs/CMakeLists.txt @@ -27,7 +27,7 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/Command.msg" "msg/ControllerCommands.msg" - "msg/ControllerInternals.msg" + "msg/ControllerInternalsDebug.msg" "msg/CurrentPath.msg" "msg/State.msg" "msg/Waypoint.msg" diff --git a/rosplane_msgs/msg/ControllerInternals.msg b/rosplane_msgs/msg/ControllerInternalsDebug.msg similarity index 100% rename from rosplane_msgs/msg/ControllerInternals.msg rename to rosplane_msgs/msg/ControllerInternalsDebug.msg From 39b0c3aee2b9f5e62c979a5cb5999fd5471f60dc Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Thu, 19 Oct 2023 12:13:32 -0600 Subject: [PATCH 02/25] Added the callbacks for the tuning override. --- rosplane/include/controller_base.hpp | 14 ++++++++++++++ rosplane/src/controller_base.cpp | 17 +++++++++++++++-- 2 files changed, 29 insertions(+), 2 deletions(-) diff --git a/rosplane/include/controller_base.hpp b/rosplane/include/controller_base.hpp index 40f9dbc..334dec4 100644 --- a/rosplane/include/controller_base.hpp +++ b/rosplane/include/controller_base.hpp @@ -11,6 +11,7 @@ #define CONTROLLER_BASE_H #include +#include #include #include #include @@ -160,6 +161,7 @@ class controller_base : public rclcpp::Node */ rclcpp::Subscription::SharedPtr vehicle_state_sub_; + rclcpp::Subscription::SharedPtr tuning_debug_sub_; /** * This timer controls how often commands are published by the autopilot. */ @@ -217,6 +219,11 @@ class controller_base : public rclcpp::Node * The stored value for the most up to date vehicle state (pose). */ rosplane_msgs::msg::State vehicle_state_; + + /** + * The override for the intermediate values for the controller. + */ + rosplane_msgs::msg::ControllerInternalsDebug tuning_debug_override_msg_; /** * Flag to indicate if the first command has been received. @@ -248,6 +255,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. */ diff --git a/rosplane/src/controller_base.cpp b/rosplane/src/controller_base.cpp index 976633a..9bfdb85 100644 --- a/rosplane/src/controller_base.cpp +++ b/rosplane/src/controller_base.cpp @@ -1,6 +1,8 @@ #include "controller_base.hpp" #include "controller_successive_loop.hpp" #include "controller_total_energy.hpp" +#include +#include namespace rosplane { @@ -10,7 +12,7 @@ controller_base::controller_base() : Node("controller_base") // Advertise published topics. actuators_pub_ = this->create_publisher("command",10); - internals_pub_ = this->create_publisher("controller_inners",10); + internals_pub_ = this->create_publisher("controller_inners_debug",10); // Set timer to trigger bound callback (actuator_controls_publish) at the given periodicity. timer_ = this->create_wall_timer(10ms, std::bind(&controller_base::actuator_controls_publish, this)); // TODO add the period to the params. @@ -29,8 +31,13 @@ controller_base::controller_base() : Node("controller_base") // Set the values for the parameters, from the param file or use the deafault value. set_parameters(); + + if (params_.tuning_debug_override){ + tuning_debug_sub_ = this->create_subscription( + "/tuning_debug", 10, std::bind(&controller_base::tuning_debug_callback, this, _1)); + } - // Set the parameter callback, for when callbacks are changed. + // Set the parameter callback, for when parameters are changed. parameter_callback_handle_ = this->add_on_set_parameters_callback( std::bind(&controller_base::parametersCallback, this, std::placeholders::_1)); @@ -54,6 +61,12 @@ void controller_base::vehicle_state_callback(const rosplane_msgs::msg::State::Sh vehicle_state_ = *msg; } +void controller_base::tuning_debug_callback(const rosplane_msgs::msg::ControllerInternalsDebug::SharedPtr msg) +{ + // Save the message to use in calculations. + tuning_debug_override_msg_ = *msg; +} + void controller_base::actuator_controls_publish() { From accf114c500820616796c2e309fd3ae32459baf0 Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Thu, 19 Oct 2023 13:10:20 -0600 Subject: [PATCH 03/25] Implemented tuning override. --- data_viz/data_viz/data_storage.py | 2 +- rosplane/include/controller_base.hpp | 10 +++++----- rosplane/src/controller_successive_loop.cpp | 10 ++++++++++ 3 files changed, 16 insertions(+), 6 deletions(-) diff --git a/data_viz/data_viz/data_storage.py b/data_viz/data_viz/data_storage.py index f87a4ac..d9bf28b 100644 --- a/data_viz/data_viz/data_storage.py +++ b/data_viz/data_viz/data_storage.py @@ -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(ControllerInternalsDebug, "/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() diff --git a/rosplane/include/controller_base.hpp b/rosplane/include/controller_base.hpp index 334dec4..9937ff8 100644 --- a/rosplane/include/controller_base.hpp +++ b/rosplane/include/controller_base.hpp @@ -139,6 +139,11 @@ class controller_base : public rclcpp::Node */ virtual void control(const struct params_s ¶ms, 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: /** @@ -219,11 +224,6 @@ class controller_base : public rclcpp::Node * The stored value for the most up to date vehicle state (pose). */ rosplane_msgs::msg::State vehicle_state_; - - /** - * The override for the intermediate values for the controller. - */ - rosplane_msgs::msg::ControllerInternalsDebug tuning_debug_override_msg_; /** * Flag to indicate if the first command has been received. diff --git a/rosplane/src/controller_successive_loop.cpp b/rosplane/src/controller_successive_loop.cpp index a238859..ab8a7bb 100644 --- a/rosplane/src/controller_successive_loop.cpp +++ b/rosplane/src/controller_successive_loop.cpp @@ -66,6 +66,11 @@ void controller_successive_loop::alt_hold_lateral_control(const struct params_s // Find aileron deflection required to acheive required roll angle. output.delta_r = 0; //cooridinated_turn_hold(input.beta, params, input.Ts) output.phi_c = course_hold(input.chi_c, input.chi, input.phi_ff, input.r, params, input.Ts); + + if (params.tuning_debug_override){ + output.phi_c = tuning_debug_override_msg_.phi_c; + } + output.delta_a = roll_hold(output.phi_c, input.phi, input.p, params, input.Ts); } @@ -77,6 +82,11 @@ void controller_successive_loop::alt_hold_longitudinal_control(const struct para // Control airspeed with throttle loop and altitude with commanded pitch and drive aircraft to commanded pitch. output.delta_t = airspeed_with_throttle_hold(input.Va_c, input.va, params, input.Ts); output.theta_c = altitude_hold_control(adjusted_hc, input.h, params, input.Ts); + + if (params.tuning_debug_override){ + output.theta_c = tuning_debug_override_msg_.theta_c; + } + output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts); } From f40c61257c1434b183855c28fbeeac725cabad59 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Thu, 19 Oct 2023 17:38:01 -0600 Subject: [PATCH 04/25] Got tuning_signal_generator compiling and mostly stubbed out --- rosplane/CMakeLists.txt | 7 + rosplane/include/tuning_signal_generator.hpp | 159 +++++++++++++++++++ rosplane/src/tuning_signal_generator.cpp | 150 +++++++++++++++++ 3 files changed, 316 insertions(+) create mode 100644 rosplane/include/tuning_signal_generator.hpp create mode 100644 rosplane/src/tuning_signal_generator.cpp diff --git a/rosplane/CMakeLists.txt b/rosplane/CMakeLists.txt index 9da8f4b..742a9cb 100644 --- a/rosplane/CMakeLists.txt +++ b/rosplane/CMakeLists.txt @@ -92,6 +92,13 @@ install(TARGETS rosplane_estimator_node DESTINATION lib/${PROJECT_NAME}) +add_executable(tuning_signal_generator + src/tuning_signal_generator.cpp) +ament_target_dependencies(tuning_signal_generator rosplane_msgs rclcpp) +install(TARGETS + tuning_signal_generator + DESTINATION lib/${PROJECT_NAME}) + #### END OF EXECUTABLES ### diff --git a/rosplane/include/tuning_signal_generator.hpp b/rosplane/include/tuning_signal_generator.hpp new file mode 100644 index 0000000..bfc5a3f --- /dev/null +++ b/rosplane/include/tuning_signal_generator.hpp @@ -0,0 +1,159 @@ +/* + * 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 + */ + +#ifndef TUNING_SIGNAL_GENERATOR_HPP +#define TUNING_SIGNAL_GENERATOR_HPP + +#include +#include "rosplane_msgs/msg/controller_commands.hpp" +#include "rosplane_msgs/msg/controller_internals_debug.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: + /** + * @brief 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 { + SQUARE, + SAWTOOTH, + TRIANGLE, + SINE + }; + + ControllerOutput controller_output_; ///< Controller to output command signals to. + SignalType signal_type_; ///< Signal type to output. + double dt_hz_; ///< Frequency to publish commands. + double amplitude_; ///< Amplitude of signal. + double frequency_hz_; ///< Frequency of the signal. + double offset_; ///< Offset of signal from 0. + double initial_time_; ///< Initial time of the signal. + + /// Controller command ROS message publisher. + rclcpp::Publisher::SharedPtr command_publisher_; + /// Controller internals ROS message publisher. + rclcpp::Publisher::SharedPtr internals_publisher_; + + /// ROS timer to run timer callback, which publishes commands + rclcpp::TimerBase::SharedPtr publish_timer_; + + /** + * Callback to publish command on topic. + */ + void publish_timer_callback(); + + /** + * @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 initial_value Inital value of the signal. The in other words, the signal 'offset'. + */ + static double get_square_signal(double elapsed_time, double amplitude, double frequency, + double initial_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 initial_value Inital value of the signal. The in other words, the signal 'offset'. + */ + static double get_sawtooth_signal(double elapsed_time, double amplitude, double frequency, + double initial_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 initial_value Inital value of the signal. The in other words, the signal 'offset'. + */ + static double get_triangle_signal(double elapsed_time, double amplitude, double frequency, + double initial_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 initial_value Inital value of the signal. The in other words, the signal 'offset'. + */ + static double get_sine_signal(double elapsed_time, double amplitude, double frequency, + double initial_value); + + /** + * Updates the parameters within the class with the latest values from ROS. + */ + void update_params(); +}; +} // namespace rosplane + +#endif // TUNING_SIGNAL_GENERATOR_HPP diff --git a/rosplane/src/tuning_signal_generator.cpp b/rosplane/src/tuning_signal_generator.cpp new file mode 100644 index 0000000..5f306bd --- /dev/null +++ b/rosplane/src/tuning_signal_generator.cpp @@ -0,0 +1,150 @@ +/* + * 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 + * + * @author Brandon Sutherland + */ + +#include +#include + +#include "tuning_signal_generator.hpp" + +namespace rosplane +{ + TuningSignalGenerator::TuningSignalGenerator() : + Node("signal_generator"), + controller_output_(ControllerOutput::ROLL), + signal_type_(SignalType::SQUARE), + dt_hz_(0), + amplitude_(0), + frequency_hz_(0), + offset_(0), + initial_time_(0) + { + this->declare_parameter("controller_output", "roll"); + this->declare_parameter("signal_type", "square"); + this->declare_parameter("dt_hz", 100.0); + this->declare_parameter("amplitude", 1.0); + this->declare_parameter("frequency_hz", 0.2); + this->declare_parameter("offset", 0.0); + + update_params(); + + publish_timer_ = this->create_wall_timer( + std::chrono::milliseconds(static_cast(1000 / dt_hz_)), + std::bind(&TuningSignalGenerator::publish_timer_callback, this)); + } + +void TuningSignalGenerator::publish_timer_callback() { + +} + +double TuningSignalGenerator::get_square_signal(double elapsed_time, double amplitude, + double frequency, double initial_value) { + +} + +double TuningSignalGenerator::get_sawtooth_signal(double elapsed_time, double amplitude, + double frequency, double initial_value) { + +} + +double TuningSignalGenerator::get_triangle_signal(double elapsed_time, double amplitude, + double frequency, double initial_value) { + +} + +double TuningSignalGenerator::get_sine_signal(double elapsed_time, double amplitude, + double frequency, double initial_value) { + +} + +void TuningSignalGenerator::update_params() { + // controller output + std::string controller_output_string = + this->get_parameter("controller_output").as_string(); + if (controller_output_string == "roll") { + controller_output_ = ControllerOutput::ROLL; + } else if (controller_output_string == "pitch") { + 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 == "airspeed") { + controller_output_ = ControllerOutput::AIRSPEED; + } else { + RCLCPP_ERROR(this->get_logger(), "Param controller_output set to invalid type %s!", + controller_output_string); + } + + // signal type + std::string signal_type_string = this->get_parameter("signal_type").as_string(); + if (signal_type_string == "square") { + signal_type_ = SignalType::SQUARE; + } else if (signal_type_string == "sawtooth") { + signal_type_ = SignalType::SAWTOOTH; + } else if (signal_type_string == "triangle") { + signal_type_ = SignalType::TRIANGLE; + } else if (signal_type_string == "sine") { + signal_type_ = SignalType::SINE; + } else { + RCLCPP_ERROR(this->get_logger(), "Param signal_type set to invalid type %s!", + signal_type_string); + } + + // dt hz + dt_hz_ = this->get_parameter("dt_hz").as_double(); + + // amplitude + amplitude_ = this->get_parameter("amplitude").as_double(); + + // frequency hz + frequency_hz_ = this->get_parameter("frequency_hz").as_double(); + + // offset + offset_ = this->get_parameter("offset").as_double(); +} + +} // namespace rosplane + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} From c737ec2e590d1286a2ef7a5e0e0527f708be6390 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Thu, 19 Oct 2023 18:24:34 -0600 Subject: [PATCH 05/25] Finished update_param function --- rosplane/include/tuning_signal_generator.hpp | 20 ++++----------- rosplane/src/tuning_signal_generator.cpp | 26 ++++++++++++++++---- 2 files changed, 26 insertions(+), 20 deletions(-) diff --git a/rosplane/include/tuning_signal_generator.hpp b/rosplane/include/tuning_signal_generator.hpp index bfc5a3f..8925bb8 100644 --- a/rosplane/include/tuning_signal_generator.hpp +++ b/rosplane/include/tuning_signal_generator.hpp @@ -56,15 +56,11 @@ namespace rosplane class TuningSignalGenerator : public rclcpp::Node { public: - /** - * @brief Contructor for signal generator. - */ + /// Contructor for signal generator. TuningSignalGenerator(); private: - /** - * This defines what controller to publish the generated signal to. - */ + /// This defines what controller to publish the generated signal to. enum class ControllerOutput { ROLL, PITCH, @@ -73,9 +69,7 @@ class TuningSignalGenerator : public rclcpp::Node AIRSPEED }; - /** - * This defines what type of signal to publish to the selected controller. - */ + /// This defines what type of signal to publish to the selected controller. enum class SignalType { SQUARE, SAWTOOTH, @@ -99,9 +93,7 @@ class TuningSignalGenerator : public rclcpp::Node /// ROS timer to run timer callback, which publishes commands rclcpp::TimerBase::SharedPtr publish_timer_; - /** - * Callback to publish command on topic. - */ + /// Callback to publish command on topic. void publish_timer_callback(); /** @@ -149,9 +141,7 @@ class TuningSignalGenerator : public rclcpp::Node static double get_sine_signal(double elapsed_time, double amplitude, double frequency, double initial_value); - /** - * Updates the parameters within the class with the latest values from ROS. - */ + /// Updates the parameters within the class with the latest values from ROS. void update_params(); }; } // namespace rosplane diff --git a/rosplane/src/tuning_signal_generator.cpp b/rosplane/src/tuning_signal_generator.cpp index 5f306bd..37d97dc 100644 --- a/rosplane/src/tuning_signal_generator.cpp +++ b/rosplane/src/tuning_signal_generator.cpp @@ -69,7 +69,7 @@ namespace rosplane } void TuningSignalGenerator::publish_timer_callback() { - + update_params(); } double TuningSignalGenerator::get_square_signal(double elapsed_time, double amplitude, @@ -108,7 +108,7 @@ void TuningSignalGenerator::update_params() { controller_output_ = ControllerOutput::AIRSPEED; } else { RCLCPP_ERROR(this->get_logger(), "Param controller_output set to invalid type %s!", - controller_output_string); + controller_output_string.c_str()); } // signal type @@ -123,17 +123,33 @@ void TuningSignalGenerator::update_params() { signal_type_ = SignalType::SINE; } else { RCLCPP_ERROR(this->get_logger(), "Param signal_type set to invalid type %s!", - signal_type_string); + signal_type_string.c_str()); } // dt hz - dt_hz_ = this->get_parameter("dt_hz").as_double(); + double dt_hz_value = this->get_parameter("dt_hz").as_double(); + if (dt_hz_value <= 0) { + RCLCPP_ERROR(this->get_logger(), "Param dt_hz must be greater than 0!"); + } else { + // Parameter has changed, create new timer with updated value + if (dt_hz_ != dt_hz_value) { + dt_hz_ = dt_hz_value; + publish_timer_ = this->create_wall_timer( + std::chrono::milliseconds(static_cast(1000 / dt_hz_)), + std::bind(&TuningSignalGenerator::publish_timer_callback, this)); + } + } // amplitude amplitude_ = this->get_parameter("amplitude").as_double(); // frequency hz - frequency_hz_ = this->get_parameter("frequency_hz").as_double(); + double frequency_hz_value = this->get_parameter("frequency_hz").as_double(); + if (frequency_hz_value <= 0) { + RCLCPP_ERROR(this->get_logger(), "Param frequency_hz must be greater than 0!"); + } else { + frequency_hz_ = frequency_hz_value; + } // offset offset_ = this->get_parameter("offset").as_double(); From eca25cd15ced414896fdae03fe9123769379422b Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Thu, 19 Oct 2023 19:13:13 -0600 Subject: [PATCH 06/25] Got messages publishing --- rosplane/src/tuning_signal_generator.cpp | 122 ++++++++++++++++++----- 1 file changed, 98 insertions(+), 24 deletions(-) diff --git a/rosplane/src/tuning_signal_generator.cpp b/rosplane/src/tuning_signal_generator.cpp index 37d97dc..75319da 100644 --- a/rosplane/src/tuning_signal_generator.cpp +++ b/rosplane/src/tuning_signal_generator.cpp @@ -44,7 +44,7 @@ namespace rosplane { - TuningSignalGenerator::TuningSignalGenerator() : +TuningSignalGenerator::TuningSignalGenerator() : Node("signal_generator"), controller_output_(ControllerOutput::ROLL), signal_type_(SignalType::SQUARE), @@ -53,45 +53,119 @@ namespace rosplane frequency_hz_(0), offset_(0), initial_time_(0) - { - this->declare_parameter("controller_output", "roll"); - this->declare_parameter("signal_type", "square"); - this->declare_parameter("dt_hz", 100.0); - this->declare_parameter("amplitude", 1.0); - this->declare_parameter("frequency_hz", 0.2); - this->declare_parameter("offset", 0.0); - - update_params(); - - publish_timer_ = this->create_wall_timer( - std::chrono::milliseconds(static_cast(1000 / dt_hz_)), - std::bind(&TuningSignalGenerator::publish_timer_callback, this)); - } +{ + this->declare_parameter("controller_output", "roll"); + this->declare_parameter("signal_type", "square"); + this->declare_parameter("dt_hz", 100.0); + this->declare_parameter("amplitude", 1.0); + this->declare_parameter("frequency_hz", 0.2); + this->declare_parameter("offset", 0.0); + + update_params(); + initial_time_ = this->get_clock()->now().seconds(); + + publish_timer_ = this->create_wall_timer( + std::chrono::milliseconds(static_cast(1000 / dt_hz_)), + std::bind(&TuningSignalGenerator::publish_timer_callback, this)); +} void TuningSignalGenerator::publish_timer_callback() { update_params(); + + double signal_value = 0; + double elapsed_time = this->get_clock()->now().seconds() - initial_time_; + + // Get value for signal + switch (signal_type_) { + case SignalType::SQUARE: + signal_value = get_square_signal(elapsed_time, amplitude_, frequency_hz_, offset_); + break; + case SignalType::SAWTOOTH: + signal_value = get_sawtooth_signal(elapsed_time, amplitude_, frequency_hz_, offset_); + break; + case SignalType::TRIANGLE: + signal_value = get_triangle_signal(elapsed_time, amplitude_, frequency_hz_, offset_); + break; + case SignalType::SINE: + signal_value = get_sine_signal(elapsed_time, amplitude_, frequency_hz_, offset_); + break; + } + + // Create required publishers if they don't already exist + if ((controller_output_ == ControllerOutput::ROLL || + controller_output_ == ControllerOutput::PITCH) && internals_publisher_ == nullptr) { + internals_publisher_ = + this->create_publisher("/tuning_debug", 1); + } else if (command_publisher_ == nullptr) { + command_publisher_ = + this->create_publisher("/controller_command", 1); + } + + // Publish message + switch (controller_output_) { + case ControllerOutput::ROLL: { + rosplane_msgs::msg::ControllerInternalsDebug message; + message.header.stamp = this->get_clock()->now(); + message.phi_c = signal_value; + internals_publisher_->publish(message); + break; + } + case ControllerOutput::PITCH: { + rosplane_msgs::msg::ControllerInternalsDebug message; + message.header.stamp = this->get_clock()->now(); + message.theta_c = signal_value; + internals_publisher_->publish(message); + break; + } + case ControllerOutput::ALTITUDE: { + rosplane_msgs::msg::ControllerCommands message; + message.header.stamp = this->get_clock()->now(); + message.h_c = signal_value; + command_publisher_->publish(message); + break; + } + case ControllerOutput::HEADING: { + rosplane_msgs::msg::ControllerCommands message; + message.header.stamp = this->get_clock()->now(); + message.chi_c = signal_value; + command_publisher_->publish(message); + break; + } + case ControllerOutput::AIRSPEED: { + rosplane_msgs::msg::ControllerCommands message; + message.header.stamp = this->get_clock()->now(); + message.va_c = signal_value; + command_publisher_->publish(message); + break; + } + } } -double TuningSignalGenerator::get_square_signal(double elapsed_time, double amplitude, - double frequency, double initial_value) { +double TuningSignalGenerator::get_square_signal(double elapsed_time, double amplitude, + double frequency, double offset) { + return 1; } -double TuningSignalGenerator::get_sawtooth_signal(double elapsed_time, double amplitude, - double frequency, double initial_value) { +double TuningSignalGenerator::get_sawtooth_signal(double elapsed_time, double amplitude, + double frequency, double offset) { + return 2; } -double TuningSignalGenerator::get_triangle_signal(double elapsed_time, double amplitude, - double frequency, double initial_value) { +double TuningSignalGenerator::get_triangle_signal(double elapsed_time, double amplitude, + double frequency, double offset) { + return 3; } -double TuningSignalGenerator::get_sine_signal(double elapsed_time, double amplitude, - double frequency, double initial_value) { +double TuningSignalGenerator::get_sine_signal(double elapsed_time, double amplitude, + double frequency, double offset) { + return 4; } + void TuningSignalGenerator::update_params() { // controller output std::string controller_output_string = @@ -154,9 +228,9 @@ void TuningSignalGenerator::update_params() { // offset offset_ = this->get_parameter("offset").as_double(); } - } // namespace rosplane + int main(int argc, char ** argv) { rclcpp::init(argc, argv); From 457b2f43bf2753c84d741399ac37f98134322732 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Fri, 20 Oct 2023 01:10:37 -0600 Subject: [PATCH 07/25] Got signal generation working --- rosplane/src/tuning_signal_generator.cpp | 25 ++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/rosplane/src/tuning_signal_generator.cpp b/rosplane/src/tuning_signal_generator.cpp index 75319da..9ebaf60 100644 --- a/rosplane/src/tuning_signal_generator.cpp +++ b/rosplane/src/tuning_signal_generator.cpp @@ -69,6 +69,7 @@ TuningSignalGenerator::TuningSignalGenerator() : std::bind(&TuningSignalGenerator::publish_timer_callback, this)); } + void TuningSignalGenerator::publish_timer_callback() { update_params(); @@ -92,10 +93,12 @@ void TuningSignalGenerator::publish_timer_callback() { } // Create required publishers if they don't already exist - if ((controller_output_ == ControllerOutput::ROLL || - controller_output_ == ControllerOutput::PITCH) && internals_publisher_ == nullptr) { - internals_publisher_ = - this->create_publisher("/tuning_debug", 1); + if (controller_output_ == ControllerOutput::ROLL || + controller_output_ == ControllerOutput::PITCH) { + if (internals_publisher_ == nullptr) { + internals_publisher_ = + this->create_publisher("/tuning_debug", 1); + } } else if (command_publisher_ == nullptr) { command_publisher_ = this->create_publisher("/controller_command", 1); @@ -144,25 +147,31 @@ void TuningSignalGenerator::publish_timer_callback() { double TuningSignalGenerator::get_square_signal(double elapsed_time, double amplitude, double frequency, double offset) { - return 1; + // amplitude * (1 to -1 switching value) + offset + return amplitude * ((static_cast(elapsed_time * frequency * 2) % 2) * 2 - 1) + offset; } double TuningSignalGenerator::get_sawtooth_signal(double elapsed_time, double amplitude, double frequency, double offset) { - return 2; + // slope * elapsed_time - num_cycles * offset_per_cycle + offset + return 2 * amplitude * (elapsed_time * frequency - static_cast(elapsed_time * frequency) - + 0.5) + offset; } double TuningSignalGenerator::get_triangle_signal(double elapsed_time, double amplitude, double frequency, double offset) { - return 3; + // (1 to -1 switching value) * sawtooth_at_twice_the_rate + offset + return ((static_cast(elapsed_time * frequency * 2) % 2) * 2 - 1) * 2 * + amplitude * (2 * elapsed_time * frequency - static_cast(2 * elapsed_time * frequency) - + 0.5) + offset; } double TuningSignalGenerator::get_sine_signal(double elapsed_time, double amplitude, double frequency, double offset) { - return 4; + return sin(elapsed_time * frequency * 2 * M_PI) * amplitude + offset; } From b73b9d78ed06adb97eb92db5da6d45440ff313d3 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Fri, 20 Oct 2023 02:08:06 -0600 Subject: [PATCH 08/25] Added ROS services to signal generator --- rosplane/CMakeLists.txt | 4 +- rosplane/include/tuning_signal_generator.hpp | 24 ++++++++ rosplane/package.xml | 1 + rosplane/src/tuning_signal_generator.cpp | 58 +++++++++++++++++++- 4 files changed, 83 insertions(+), 4 deletions(-) diff --git a/rosplane/CMakeLists.txt b/rosplane/CMakeLists.txt index 742a9cb..b063a32 100644 --- a/rosplane/CMakeLists.txt +++ b/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,9 +93,10 @@ 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 rclcpp) +ament_target_dependencies(tuning_signal_generator rosplane_msgs std_srvs rclcpp) install(TARGETS tuning_signal_generator DESTINATION lib/${PROJECT_NAME}) diff --git a/rosplane/include/tuning_signal_generator.hpp b/rosplane/include/tuning_signal_generator.hpp index 8925bb8..82f4783 100644 --- a/rosplane/include/tuning_signal_generator.hpp +++ b/rosplane/include/tuning_signal_generator.hpp @@ -43,6 +43,7 @@ #define TUNING_SIGNAL_GENERATOR_HPP #include +#include #include "rosplane_msgs/msg/controller_commands.hpp" #include "rosplane_msgs/msg/controller_internals_debug.hpp" @@ -84,6 +85,7 @@ class TuningSignalGenerator : public rclcpp::Node double frequency_hz_; ///< Frequency of the signal. double offset_; ///< Offset of signal from 0. double initial_time_; ///< Initial time of the signal. + bool is_paused_; ///< Flag to specify if signal should be paused. /// Controller command ROS message publisher. rclcpp::Publisher::SharedPtr command_publisher_; @@ -93,9 +95,31 @@ class TuningSignalGenerator : public rclcpp::Node /// ROS timer to run timer callback, which publishes commands rclcpp::TimerBase::SharedPtr publish_timer_; + /// ROS service for reset signal + rclcpp::Service::SharedPtr reset_service_; + /// ROS service for pause signal + rclcpp::Service::SharedPtr pause_service_; + /// ROS service for start signal continuously + rclcpp::Service::SharedPtr start_continuous_service_; + /// ROS service for start signal for one period + rclcpp::Service::SharedPtr start_single_service_; + /// Callback to publish command on topic. void publish_timer_callback(); + /// 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 square signal at the given time with the given conditions. * diff --git a/rosplane/package.xml b/rosplane/package.xml index 4eec9b5..db7e5c8 100644 --- a/rosplane/package.xml +++ b/rosplane/package.xml @@ -12,6 +12,7 @@ rclcpp rclpy std_msgs + std_srvs sensor_msgs rosplane_msgs rosflight_msgs diff --git a/rosplane/src/tuning_signal_generator.cpp b/rosplane/src/tuning_signal_generator.cpp index 9ebaf60..35922d2 100644 --- a/rosplane/src/tuning_signal_generator.cpp +++ b/rosplane/src/tuning_signal_generator.cpp @@ -39,6 +39,7 @@ #include #include +#include #include "tuning_signal_generator.hpp" @@ -67,6 +68,19 @@ TuningSignalGenerator::TuningSignalGenerator() : publish_timer_ = this->create_wall_timer( std::chrono::milliseconds(static_cast(1000 / dt_hz_)), std::bind(&TuningSignalGenerator::publish_timer_callback, this)); + + reset_service_ = this->create_service("reset_signal", + std::bind(&TuningSignalGenerator::reset_service_callback, this, + std::placeholders::_1, std::placeholders::_2)); + pause_service_ = this->create_service("pause_signal", + std::bind(&TuningSignalGenerator::pause_service_callback, this, + std::placeholders::_1, std::placeholders::_2)); + start_continuous_service_ = this->create_service("start_continuous_signal", + std::bind(&TuningSignalGenerator::start_continuous_service_callback, this, + std::placeholders::_1, std::placeholders::_2)); + start_single_service_ = this->create_service("start_single_period", + std::bind(&TuningSignalGenerator::start_single_service_callback, this, + std::placeholders::_1, std::placeholders::_2)); } @@ -99,9 +113,11 @@ void TuningSignalGenerator::publish_timer_callback() { internals_publisher_ = this->create_publisher("/tuning_debug", 1); } - } else if (command_publisher_ == nullptr) { - command_publisher_ = - this->create_publisher("/controller_command", 1); + } else { + if (command_publisher_ == nullptr) { + command_publisher_ = + this->create_publisher("/controller_command", 1); + } } // Publish message @@ -145,6 +161,42 @@ void TuningSignalGenerator::publish_timer_callback() { } +bool TuningSignalGenerator::reset_service_callback( + const std_srvs::srv::Trigger::Request::SharedPtr & req, + const std_srvs::srv::Trigger::Response::SharedPtr & res) { + + res->success = true; + return true; +} + + +bool TuningSignalGenerator::pause_service_callback( + const std_srvs::srv::Trigger::Request::SharedPtr & req, + const std_srvs::srv::Trigger::Response::SharedPtr & res) { + + res->success = true; + return true; +} + + +bool TuningSignalGenerator::start_continuous_service_callback( + const std_srvs::srv::Trigger::Request::SharedPtr & req, + const std_srvs::srv::Trigger::Response::SharedPtr & res) { + + res->success = true; + return true; +} + + +bool TuningSignalGenerator::start_single_service_callback( + const std_srvs::srv::Trigger::Request::SharedPtr & req, + const std_srvs::srv::Trigger::Response::SharedPtr & res) { + + res->success = true; + return true; +} + + double TuningSignalGenerator::get_square_signal(double elapsed_time, double amplitude, double frequency, double offset) { // amplitude * (1 to -1 switching value) + offset From 91988adf3dcf52e4922e416438c62b8c3fba1843 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Fri, 20 Oct 2023 03:20:06 -0600 Subject: [PATCH 09/25] Implemented service functionality for signal generator --- rosplane/include/tuning_signal_generator.hpp | 3 ++ rosplane/src/tuning_signal_generator.cpp | 34 +++++++++++++++++--- 2 files changed, 33 insertions(+), 4 deletions(-) diff --git a/rosplane/include/tuning_signal_generator.hpp b/rosplane/include/tuning_signal_generator.hpp index 82f4783..9c0f7d4 100644 --- a/rosplane/include/tuning_signal_generator.hpp +++ b/rosplane/include/tuning_signal_generator.hpp @@ -84,8 +84,11 @@ class TuningSignalGenerator : public rclcpp::Node double amplitude_; ///< Amplitude of signal. double frequency_hz_; ///< Frequency of the signal. double offset_; ///< Offset of signal from 0. + 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::SharedPtr command_publisher_; diff --git a/rosplane/src/tuning_signal_generator.cpp b/rosplane/src/tuning_signal_generator.cpp index 35922d2..5855ad3 100644 --- a/rosplane/src/tuning_signal_generator.cpp +++ b/rosplane/src/tuning_signal_generator.cpp @@ -53,7 +53,10 @@ TuningSignalGenerator::TuningSignalGenerator() : amplitude_(0), frequency_hz_(0), offset_(0), - initial_time_(0) + initial_time_(0), + is_paused_(true), + paused_time_(0), + single_period_start_time_(0) { this->declare_parameter("controller_output", "roll"); this->declare_parameter("signal_type", "square"); @@ -78,7 +81,7 @@ TuningSignalGenerator::TuningSignalGenerator() : start_continuous_service_ = this->create_service("start_continuous_signal", std::bind(&TuningSignalGenerator::start_continuous_service_callback, this, std::placeholders::_1, std::placeholders::_2)); - start_single_service_ = this->create_service("start_single_period", + start_single_service_ = this->create_service("start_single_period_signal", std::bind(&TuningSignalGenerator::start_single_service_callback, this, std::placeholders::_1, std::placeholders::_2)); } @@ -87,10 +90,23 @@ TuningSignalGenerator::TuningSignalGenerator() : void TuningSignalGenerator::publish_timer_callback() { update_params(); - double signal_value = 0; - double elapsed_time = this->get_clock()->now().seconds() - initial_time_; + double elapsed_time = this->get_clock()->now().seconds() - initial_time_ - paused_time_; + + // Check if only suppose to run for single period, pausing when complete + if (abs(single_period_start_time_) > 0.01 && + (single_period_start_time_ + (1 / frequency_hz_)) <= this->get_clock()->now().seconds()) { + single_period_start_time_ = 0; + is_paused_ = true; + RCLCPP_INFO(this->get_logger(), "Single period active!"); + } + + // If paused, negate passing of time but keep publishing + if (is_paused_) { + paused_time_ += 1 / dt_hz_; + } // Get value for signal + double signal_value = 0; switch (signal_type_) { case SignalType::SQUARE: signal_value = get_square_signal(elapsed_time, amplitude_, frequency_hz_, offset_); @@ -164,6 +180,10 @@ void TuningSignalGenerator::publish_timer_callback() { bool TuningSignalGenerator::reset_service_callback( const std_srvs::srv::Trigger::Request::SharedPtr & req, const std_srvs::srv::Trigger::Response::SharedPtr & res) { + initial_time_ = this->get_clock()->now().seconds(); + paused_time_ = 0; + is_paused_ = true; + single_period_start_time_ = 0; res->success = true; return true; @@ -173,6 +193,8 @@ bool TuningSignalGenerator::reset_service_callback( bool TuningSignalGenerator::pause_service_callback( const std_srvs::srv::Trigger::Request::SharedPtr & req, const std_srvs::srv::Trigger::Response::SharedPtr & res) { + is_paused_ = true; + single_period_start_time_ = 0; res->success = true; return true; @@ -182,6 +204,8 @@ bool TuningSignalGenerator::pause_service_callback( bool TuningSignalGenerator::start_continuous_service_callback( const std_srvs::srv::Trigger::Request::SharedPtr & req, const std_srvs::srv::Trigger::Response::SharedPtr & res) { + is_paused_ = false; + single_period_start_time_ = 0; res->success = true; return true; @@ -191,6 +215,8 @@ bool TuningSignalGenerator::start_continuous_service_callback( bool TuningSignalGenerator::start_single_service_callback( const std_srvs::srv::Trigger::Request::SharedPtr & req, const std_srvs::srv::Trigger::Response::SharedPtr & res) { + is_paused_ = false; + single_period_start_time_ = this->get_clock()->now().seconds(); res->success = true; return true; From b792bd63f18f8466a5239c82c1e72d1bb869e591 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Fri, 20 Oct 2023 03:27:44 -0600 Subject: [PATCH 10/25] Formatting on signal generator --- rosplane/include/tuning_signal_generator.hpp | 38 +-- rosplane/src/tuning_signal_generator.cpp | 229 ++++++++++--------- 2 files changed, 135 insertions(+), 132 deletions(-) diff --git a/rosplane/include/tuning_signal_generator.hpp b/rosplane/include/tuning_signal_generator.hpp index 9c0f7d4..132f86d 100644 --- a/rosplane/include/tuning_signal_generator.hpp +++ b/rosplane/include/tuning_signal_generator.hpp @@ -42,10 +42,10 @@ #ifndef TUNING_SIGNAL_GENERATOR_HPP #define TUNING_SIGNAL_GENERATOR_HPP -#include -#include #include "rosplane_msgs/msg/controller_commands.hpp" #include "rosplane_msgs/msg/controller_internals_debug.hpp" +#include +#include namespace rosplane { @@ -56,45 +56,47 @@ namespace rosplane */ class TuningSignalGenerator : public rclcpp::Node { -public: +public: /// Contructor for signal generator. TuningSignalGenerator(); private: /// This defines what controller to publish the generated signal to. - enum class ControllerOutput { + enum class ControllerOutput + { ROLL, PITCH, ALTITUDE, HEADING, AIRSPEED }; - + /// This defines what type of signal to publish to the selected controller. - enum class SignalType { + enum class SignalType + { SQUARE, SAWTOOTH, TRIANGLE, SINE }; - ControllerOutput controller_output_; ///< Controller to output command signals to. - SignalType signal_type_; ///< Signal type to output. - double dt_hz_; ///< Frequency to publish commands. - double amplitude_; ///< Amplitude of signal. - double frequency_hz_; ///< Frequency of the signal. - double offset_; ///< Offset of signal from 0. + ControllerOutput controller_output_; ///< Controller to output command signals to. + SignalType signal_type_; ///< Signal type to output. + double dt_hz_; ///< Frequency to publish commands. + double amplitude_; ///< Amplitude of signal. + double frequency_hz_; ///< Frequency of the signal. + double offset_; ///< Offset of signal from 0. - 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. + 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::SharedPtr command_publisher_; /// Controller internals ROS message publisher. rclcpp::Publisher::SharedPtr internals_publisher_; - + /// ROS timer to run timer callback, which publishes commands rclcpp::TimerBase::SharedPtr publish_timer_; @@ -132,7 +134,7 @@ class TuningSignalGenerator : public rclcpp::Node * @param frequency The frequency of the signal. * @param initial_value Inital value of the signal. The in other words, the signal 'offset'. */ - static double get_square_signal(double elapsed_time, double amplitude, double frequency, + static double get_square_signal(double elapsed_time, double amplitude, double frequency, double initial_value); /** * @brief Get the value for a sawtooth signal at the given time with the given conditions. diff --git a/rosplane/src/tuning_signal_generator.cpp b/rosplane/src/tuning_signal_generator.cpp index 5855ad3..28da963 100644 --- a/rosplane/src/tuning_signal_generator.cpp +++ b/rosplane/src/tuning_signal_generator.cpp @@ -38,25 +38,25 @@ */ #include -#include #include +#include #include "tuning_signal_generator.hpp" namespace rosplane { -TuningSignalGenerator::TuningSignalGenerator() : - Node("signal_generator"), - controller_output_(ControllerOutput::ROLL), - signal_type_(SignalType::SQUARE), - dt_hz_(0), - amplitude_(0), - frequency_hz_(0), - offset_(0), - initial_time_(0), - is_paused_(true), - paused_time_(0), - single_period_start_time_(0) +TuningSignalGenerator::TuningSignalGenerator() + : Node("signal_generator") + , controller_output_(ControllerOutput::ROLL) + , signal_type_(SignalType::SQUARE) + , dt_hz_(0) + , amplitude_(0) + , frequency_hz_(0) + , offset_(0) + , initial_time_(0) + , is_paused_(true) + , paused_time_(0) + , single_period_start_time_(0) { this->declare_parameter("controller_output", "roll"); this->declare_parameter("signal_type", "square"); @@ -68,42 +68,44 @@ TuningSignalGenerator::TuningSignalGenerator() : update_params(); initial_time_ = this->get_clock()->now().seconds(); - publish_timer_ = this->create_wall_timer( - std::chrono::milliseconds(static_cast(1000 / dt_hz_)), - std::bind(&TuningSignalGenerator::publish_timer_callback, this)); - - reset_service_ = this->create_service("reset_signal", - std::bind(&TuningSignalGenerator::reset_service_callback, this, - std::placeholders::_1, std::placeholders::_2)); - pause_service_ = this->create_service("pause_signal", - std::bind(&TuningSignalGenerator::pause_service_callback, this, - std::placeholders::_1, std::placeholders::_2)); - start_continuous_service_ = this->create_service("start_continuous_signal", - std::bind(&TuningSignalGenerator::start_continuous_service_callback, this, - std::placeholders::_1, std::placeholders::_2)); - start_single_service_ = this->create_service("start_single_period_signal", - std::bind(&TuningSignalGenerator::start_single_service_callback, this, - std::placeholders::_1, std::placeholders::_2)); + publish_timer_ = + this->create_wall_timer(std::chrono::milliseconds(static_cast(1000 / dt_hz_)), + std::bind(&TuningSignalGenerator::publish_timer_callback, this)); + + reset_service_ = this->create_service( + "reset_signal", + std::bind(&TuningSignalGenerator::reset_service_callback, this, std::placeholders::_1, + std::placeholders::_2)); + pause_service_ = this->create_service( + "pause_signal", + std::bind(&TuningSignalGenerator::pause_service_callback, this, std::placeholders::_1, + std::placeholders::_2)); + start_continuous_service_ = this->create_service( + "start_continuous_signal", + std::bind(&TuningSignalGenerator::start_continuous_service_callback, this, + std::placeholders::_1, std::placeholders::_2)); + start_single_service_ = this->create_service( + "start_single_period_signal", + std::bind(&TuningSignalGenerator::start_single_service_callback, this, std::placeholders::_1, + std::placeholders::_2)); } - -void TuningSignalGenerator::publish_timer_callback() { +void TuningSignalGenerator::publish_timer_callback() +{ update_params(); double elapsed_time = this->get_clock()->now().seconds() - initial_time_ - paused_time_; // Check if only suppose to run for single period, pausing when complete - if (abs(single_period_start_time_) > 0.01 && - (single_period_start_time_ + (1 / frequency_hz_)) <= this->get_clock()->now().seconds()) { + if (abs(single_period_start_time_) > 0.01 + && (single_period_start_time_ + (1 / frequency_hz_)) <= this->get_clock()->now().seconds()) { single_period_start_time_ = 0; is_paused_ = true; RCLCPP_INFO(this->get_logger(), "Single period active!"); } - + // If paused, negate passing of time but keep publishing - if (is_paused_) { - paused_time_ += 1 / dt_hz_; - } + if (is_paused_) { paused_time_ += 1 / dt_hz_; } // Get value for signal double signal_value = 0; @@ -121,65 +123,65 @@ void TuningSignalGenerator::publish_timer_callback() { signal_value = get_sine_signal(elapsed_time, amplitude_, frequency_hz_, offset_); break; } - + // Create required publishers if they don't already exist - if (controller_output_ == ControllerOutput::ROLL || - controller_output_ == ControllerOutput::PITCH) { + if (controller_output_ == ControllerOutput::ROLL + || controller_output_ == ControllerOutput::PITCH) { if (internals_publisher_ == nullptr) { - internals_publisher_ = - this->create_publisher("/tuning_debug", 1); + internals_publisher_ = + this->create_publisher("/tuning_debug", 1); } } else { if (command_publisher_ == nullptr) { - command_publisher_ = - this->create_publisher("/controller_command", 1); + command_publisher_ = + this->create_publisher("/controller_command", 1); } } // Publish message switch (controller_output_) { case ControllerOutput::ROLL: { - rosplane_msgs::msg::ControllerInternalsDebug message; - message.header.stamp = this->get_clock()->now(); - message.phi_c = signal_value; - internals_publisher_->publish(message); - break; - } + rosplane_msgs::msg::ControllerInternalsDebug message; + message.header.stamp = this->get_clock()->now(); + message.phi_c = signal_value; + internals_publisher_->publish(message); + break; + } case ControllerOutput::PITCH: { - rosplane_msgs::msg::ControllerInternalsDebug message; - message.header.stamp = this->get_clock()->now(); - message.theta_c = signal_value; - internals_publisher_->publish(message); - break; - } + rosplane_msgs::msg::ControllerInternalsDebug message; + message.header.stamp = this->get_clock()->now(); + message.theta_c = signal_value; + internals_publisher_->publish(message); + break; + } case ControllerOutput::ALTITUDE: { - rosplane_msgs::msg::ControllerCommands message; - message.header.stamp = this->get_clock()->now(); - message.h_c = signal_value; - command_publisher_->publish(message); - break; - } + rosplane_msgs::msg::ControllerCommands message; + message.header.stamp = this->get_clock()->now(); + message.h_c = signal_value; + command_publisher_->publish(message); + break; + } case ControllerOutput::HEADING: { - rosplane_msgs::msg::ControllerCommands message; - message.header.stamp = this->get_clock()->now(); - message.chi_c = signal_value; - command_publisher_->publish(message); - break; - } + rosplane_msgs::msg::ControllerCommands message; + message.header.stamp = this->get_clock()->now(); + message.chi_c = signal_value; + command_publisher_->publish(message); + break; + } case ControllerOutput::AIRSPEED: { - rosplane_msgs::msg::ControllerCommands message; - message.header.stamp = this->get_clock()->now(); - message.va_c = signal_value; - command_publisher_->publish(message); - break; - } + rosplane_msgs::msg::ControllerCommands message; + message.header.stamp = this->get_clock()->now(); + message.va_c = signal_value; + command_publisher_->publish(message); + break; + } } } - bool TuningSignalGenerator::reset_service_callback( - const std_srvs::srv::Trigger::Request::SharedPtr & req, - const std_srvs::srv::Trigger::Response::SharedPtr & res) { + const std_srvs::srv::Trigger::Request::SharedPtr & req, + const std_srvs::srv::Trigger::Response::SharedPtr & res) +{ initial_time_ = this->get_clock()->now().seconds(); paused_time_ = 0; is_paused_ = true; @@ -189,10 +191,10 @@ bool TuningSignalGenerator::reset_service_callback( return true; } - bool TuningSignalGenerator::pause_service_callback( - const std_srvs::srv::Trigger::Request::SharedPtr & req, - const std_srvs::srv::Trigger::Response::SharedPtr & res) { + const std_srvs::srv::Trigger::Request::SharedPtr & req, + const std_srvs::srv::Trigger::Response::SharedPtr & res) +{ is_paused_ = true; single_period_start_time_ = 0; @@ -200,10 +202,10 @@ bool TuningSignalGenerator::pause_service_callback( return true; } - bool TuningSignalGenerator::start_continuous_service_callback( - const std_srvs::srv::Trigger::Request::SharedPtr & req, - const std_srvs::srv::Trigger::Response::SharedPtr & res) { + const std_srvs::srv::Trigger::Request::SharedPtr & req, + const std_srvs::srv::Trigger::Response::SharedPtr & res) +{ is_paused_ = false; single_period_start_time_ = 0; @@ -211,10 +213,10 @@ bool TuningSignalGenerator::start_continuous_service_callback( return true; } - bool TuningSignalGenerator::start_single_service_callback( - const std_srvs::srv::Trigger::Request::SharedPtr & req, - const std_srvs::srv::Trigger::Response::SharedPtr & res) { + const std_srvs::srv::Trigger::Request::SharedPtr & req, + const std_srvs::srv::Trigger::Response::SharedPtr & res) +{ is_paused_ = false; single_period_start_time_ = this->get_clock()->now().seconds(); @@ -222,41 +224,41 @@ bool TuningSignalGenerator::start_single_service_callback( return true; } - -double TuningSignalGenerator::get_square_signal(double elapsed_time, double amplitude, - double frequency, double offset) { +double TuningSignalGenerator::get_square_signal(double elapsed_time, double amplitude, + double frequency, double offset) +{ // amplitude * (1 to -1 switching value) + offset return amplitude * ((static_cast(elapsed_time * frequency * 2) % 2) * 2 - 1) + offset; } - -double TuningSignalGenerator::get_sawtooth_signal(double elapsed_time, double amplitude, - double frequency, double offset) { +double TuningSignalGenerator::get_sawtooth_signal(double elapsed_time, double amplitude, + double frequency, double offset) +{ // slope * elapsed_time - num_cycles * offset_per_cycle + offset - return 2 * amplitude * (elapsed_time * frequency - static_cast(elapsed_time * frequency) - - 0.5) + offset; + return 2 * amplitude + * (elapsed_time * frequency - static_cast(elapsed_time * frequency) - 0.5) + + offset; } - double TuningSignalGenerator::get_triangle_signal(double elapsed_time, double amplitude, - double frequency, double offset) { + double frequency, double offset) +{ // (1 to -1 switching value) * sawtooth_at_twice_the_rate + offset - return ((static_cast(elapsed_time * frequency * 2) % 2) * 2 - 1) * 2 * - amplitude * (2 * elapsed_time * frequency - static_cast(2 * elapsed_time * frequency) - - 0.5) + offset; + return ((static_cast(elapsed_time * frequency * 2) % 2) * 2 - 1) * 2 * amplitude + * (2 * elapsed_time * frequency - static_cast(2 * elapsed_time * frequency) - 0.5) + + offset; } - -double TuningSignalGenerator::get_sine_signal(double elapsed_time, double amplitude, - double frequency, double offset) { +double TuningSignalGenerator::get_sine_signal(double elapsed_time, double amplitude, + double frequency, double offset) +{ return sin(elapsed_time * frequency * 2 * M_PI) * amplitude + offset; } - -void TuningSignalGenerator::update_params() { +void TuningSignalGenerator::update_params() +{ // controller output - std::string controller_output_string = - this->get_parameter("controller_output").as_string(); + std::string controller_output_string = this->get_parameter("controller_output").as_string(); if (controller_output_string == "roll") { controller_output_ = ControllerOutput::ROLL; } else if (controller_output_string == "pitch") { @@ -268,7 +270,7 @@ void TuningSignalGenerator::update_params() { } else if (controller_output_string == "airspeed") { controller_output_ = ControllerOutput::AIRSPEED; } else { - RCLCPP_ERROR(this->get_logger(), "Param controller_output set to invalid type %s!", + RCLCPP_ERROR(this->get_logger(), "Param controller_output set to invalid type %s!", controller_output_string.c_str()); } @@ -295,15 +297,15 @@ void TuningSignalGenerator::update_params() { // Parameter has changed, create new timer with updated value if (dt_hz_ != dt_hz_value) { dt_hz_ = dt_hz_value; - publish_timer_ = this->create_wall_timer( - std::chrono::milliseconds(static_cast(1000 / dt_hz_)), - std::bind(&TuningSignalGenerator::publish_timer_callback, this)); + publish_timer_ = + this->create_wall_timer(std::chrono::milliseconds(static_cast(1000 / dt_hz_)), + std::bind(&TuningSignalGenerator::publish_timer_callback, this)); } } - + // amplitude amplitude_ = this->get_parameter("amplitude").as_double(); - + // frequency hz double frequency_hz_value = this->get_parameter("frequency_hz").as_double(); if (frequency_hz_value <= 0) { @@ -311,13 +313,12 @@ void TuningSignalGenerator::update_params() { } else { frequency_hz_ = frequency_hz_value; } - + // offset offset_ = this->get_parameter("offset").as_double(); } } // namespace rosplane - int main(int argc, char ** argv) { rclcpp::init(argc, argv); From 2d4faba9757c8fefd43b168f0d7a34b484c3cbad Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Fri, 20 Oct 2023 11:31:47 -0600 Subject: [PATCH 11/25] Created launch file for tuning. --- rosplane/launch/rosplane_tuning.launch.py | 46 +++++++++++++++++++++++ 1 file changed, 46 insertions(+) create mode 100644 rosplane/launch/rosplane_tuning.launch.py diff --git a/rosplane/launch/rosplane_tuning.launch.py b/rosplane/launch/rosplane_tuning.launch.py new file mode 100644 index 0000000..f0ec535 --- /dev/null +++ b/rosplane/launch/rosplane_tuning.launch.py @@ -0,0 +1,46 @@ +import os +import sys +from launch import LaunchDescription +from launch.descriptions import executable +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + # Create the package directory + rosplane_dir = get_package_share_directory('rosplane') + + autopilot_params = os.path.join( + rosplane_dir, + 'params', + 'succesive_loop_controller_params.yaml' + ) + + # Determine the appropriate control scheme. + control_type = "default" + + for arg in sys.argv: + if arg.startswith("control_type:="): + control_type = arg.split(":=")[1] + + return LaunchDescription([ + Node( + package='rosplane', + executable='rosplane_controller', + name='autopilot', + parameters = [autopilot_params, + {'tuning_debug_override': False}], + output = 'screen', + arguments = [control_type] + ), + Node( + package='rosplane', + executable='rosplane_estimator_node', + name='estimator' + ), + Node( + package='rosplane', + executable='tuning_signal_generator', + name='signal_generator', + output = 'screen' + ) + ]) From bbc72963f93c5edcd2590ce6fe3a83c02ebb4bab Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 25 Oct 2023 17:22:25 -0600 Subject: [PATCH 12/25] Set topics to publish all the time --- rosplane/src/tuning_signal_generator.cpp | 73 ++++++++++-------------- 1 file changed, 29 insertions(+), 44 deletions(-) diff --git a/rosplane/src/tuning_signal_generator.cpp b/rosplane/src/tuning_signal_generator.cpp index 28da963..cfca622 100644 --- a/rosplane/src/tuning_signal_generator.cpp +++ b/rosplane/src/tuning_signal_generator.cpp @@ -72,6 +72,11 @@ TuningSignalGenerator::TuningSignalGenerator() this->create_wall_timer(std::chrono::milliseconds(static_cast(1000 / dt_hz_)), std::bind(&TuningSignalGenerator::publish_timer_callback, this)); + internals_publisher_ = + this->create_publisher("/tuning_debug", 1); + command_publisher_ = + this->create_publisher("/controller_command", 1); + reset_service_ = this->create_service( "reset_signal", std::bind(&TuningSignalGenerator::reset_service_callback, this, std::placeholders::_1, @@ -101,7 +106,6 @@ void TuningSignalGenerator::publish_timer_callback() && (single_period_start_time_ + (1 / frequency_hz_)) <= this->get_clock()->now().seconds()) { single_period_start_time_ = 0; is_paused_ = true; - RCLCPP_INFO(this->get_logger(), "Single period active!"); } // If paused, negate passing of time but keep publishing @@ -124,58 +128,39 @@ void TuningSignalGenerator::publish_timer_callback() break; } - // Create required publishers if they don't already exist - if (controller_output_ == ControllerOutput::ROLL - || controller_output_ == ControllerOutput::PITCH) { - if (internals_publisher_ == nullptr) { - internals_publisher_ = - this->create_publisher("/tuning_debug", 1); - } - } else { - if (command_publisher_ == nullptr) { - command_publisher_ = - this->create_publisher("/controller_command", 1); - } - } + // Creates message with default values + rosplane_msgs::msg::ControllerCommands command_message; + command_message.header.stamp = this->get_clock()->now(); + command_message.va_c = 0; + command_message.h_c = 0; + command_message.chi_c = 0; + command_message.phi_ff = 0; + rosplane_msgs::msg::ControllerInternalsDebug internals_message; + internals_message.header.stamp = this->get_clock()->now(); + internals_message.theta_c = 0; + internals_message.phi_c = 0; + internals_message.alt_zone = 0; // Publish message switch (controller_output_) { - case ControllerOutput::ROLL: { - rosplane_msgs::msg::ControllerInternalsDebug message; - message.header.stamp = this->get_clock()->now(); - message.phi_c = signal_value; - internals_publisher_->publish(message); + case ControllerOutput::ROLL: + internals_message.phi_c = signal_value; break; - } - case ControllerOutput::PITCH: { - rosplane_msgs::msg::ControllerInternalsDebug message; - message.header.stamp = this->get_clock()->now(); - message.theta_c = signal_value; - internals_publisher_->publish(message); + case ControllerOutput::PITCH: + internals_message.theta_c = signal_value; break; - } - case ControllerOutput::ALTITUDE: { - rosplane_msgs::msg::ControllerCommands message; - message.header.stamp = this->get_clock()->now(); - message.h_c = signal_value; - command_publisher_->publish(message); + case ControllerOutput::ALTITUDE: + command_message.h_c = signal_value; break; - } - case ControllerOutput::HEADING: { - rosplane_msgs::msg::ControllerCommands message; - message.header.stamp = this->get_clock()->now(); - message.chi_c = signal_value; - command_publisher_->publish(message); + case ControllerOutput::HEADING: + command_message.chi_c = signal_value; break; - } - case ControllerOutput::AIRSPEED: { - rosplane_msgs::msg::ControllerCommands message; - message.header.stamp = this->get_clock()->now(); - message.va_c = signal_value; - command_publisher_->publish(message); + case ControllerOutput::AIRSPEED: + command_message.va_c = signal_value; break; - } } + command_publisher_->publish(command_message); + internals_publisher_->publish(internals_message); } bool TuningSignalGenerator::reset_service_callback( From ec908318d40a9a9e6f3f1a851220281897433636 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 25 Oct 2023 17:48:19 -0600 Subject: [PATCH 13/25] Added ROS params for default command values --- rosplane/include/tuning_signal_generator.hpp | 7 ++++ rosplane/src/tuning_signal_generator.cpp | 40 ++++++++++++++------ 2 files changed, 36 insertions(+), 11 deletions(-) diff --git a/rosplane/include/tuning_signal_generator.hpp b/rosplane/include/tuning_signal_generator.hpp index 132f86d..a78265d 100644 --- a/rosplane/include/tuning_signal_generator.hpp +++ b/rosplane/include/tuning_signal_generator.hpp @@ -80,13 +80,20 @@ class TuningSignalGenerator : public rclcpp::Node SINE }; + // Parameters ControllerOutput controller_output_; ///< Controller to output command signals to. SignalType signal_type_; ///< Signal type to output. double dt_hz_; ///< Frequency to publish commands. double amplitude_; ///< Amplitude of signal. double frequency_hz_; ///< Frequency of the signal. double offset_; ///< Offset of signal from 0. + double default_va_c_; ///< Default for va_c, used when not controlling airspeed. + double default_h_c_; ///< Default for h_c, used when not controlling altitude. + double default_chi_c_; ///< Default for chi_c, used when not controlling heading. + double default_theta_c_; ///< Default for theta_c, used when not controlling pitch. + double default_phi_c_; ///< Default for phi_c, used when not controlling roll. + // Internal values 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. diff --git a/rosplane/src/tuning_signal_generator.cpp b/rosplane/src/tuning_signal_generator.cpp index cfca622..d9dbdc2 100644 --- a/rosplane/src/tuning_signal_generator.cpp +++ b/rosplane/src/tuning_signal_generator.cpp @@ -64,6 +64,11 @@ TuningSignalGenerator::TuningSignalGenerator() this->declare_parameter("amplitude", 1.0); this->declare_parameter("frequency_hz", 0.2); this->declare_parameter("offset", 0.0); + this->declare_parameter("default_va_c", 15.0); + this->declare_parameter("default_h_c", 40.0); + this->declare_parameter("default_chi_c", 0.0); + this->declare_parameter("default_theta_c", 0.0); + this->declare_parameter("default_phi_c", 0.0); update_params(); initial_time_ = this->get_clock()->now().seconds(); @@ -131,15 +136,13 @@ void TuningSignalGenerator::publish_timer_callback() // Creates message with default values rosplane_msgs::msg::ControllerCommands command_message; command_message.header.stamp = this->get_clock()->now(); - command_message.va_c = 0; - command_message.h_c = 0; - command_message.chi_c = 0; - command_message.phi_ff = 0; + command_message.va_c = default_va_c_; + command_message.h_c = default_h_c_; + command_message.chi_c = default_chi_c_; rosplane_msgs::msg::ControllerInternalsDebug internals_message; internals_message.header.stamp = this->get_clock()->now(); - internals_message.theta_c = 0; - internals_message.phi_c = 0; - internals_message.alt_zone = 0; + internals_message.theta_c = default_theta_c_; + internals_message.phi_c = default_phi_c_; // Publish message switch (controller_output_) { @@ -242,7 +245,7 @@ double TuningSignalGenerator::get_sine_signal(double elapsed_time, double amplit void TuningSignalGenerator::update_params() { - // controller output + // controller_output std::string controller_output_string = this->get_parameter("controller_output").as_string(); if (controller_output_string == "roll") { controller_output_ = ControllerOutput::ROLL; @@ -259,7 +262,7 @@ void TuningSignalGenerator::update_params() controller_output_string.c_str()); } - // signal type + // signal_type std::string signal_type_string = this->get_parameter("signal_type").as_string(); if (signal_type_string == "square") { signal_type_ = SignalType::SQUARE; @@ -274,7 +277,7 @@ void TuningSignalGenerator::update_params() signal_type_string.c_str()); } - // dt hz + // dt_hz double dt_hz_value = this->get_parameter("dt_hz").as_double(); if (dt_hz_value <= 0) { RCLCPP_ERROR(this->get_logger(), "Param dt_hz must be greater than 0!"); @@ -291,7 +294,7 @@ void TuningSignalGenerator::update_params() // amplitude amplitude_ = this->get_parameter("amplitude").as_double(); - // frequency hz + // frequency_hz double frequency_hz_value = this->get_parameter("frequency_hz").as_double(); if (frequency_hz_value <= 0) { RCLCPP_ERROR(this->get_logger(), "Param frequency_hz must be greater than 0!"); @@ -301,6 +304,21 @@ void TuningSignalGenerator::update_params() // offset offset_ = this->get_parameter("offset").as_double(); + + // default_va_c + default_va_c_ = this->get_parameter("default_va_c").as_double(); + + // default_h_c + default_h_c_ = this->get_parameter("default_h_c").as_double(); + + // default_chi_c + default_chi_c_ = this->get_parameter("default_chi_c").as_double(); + + // default_theta_c + default_theta_c_ = this->get_parameter("default_theta_c").as_double(); + + // default_phi_c + default_phi_c_ = this->get_parameter("default_phi_c").as_double(); } } // namespace rosplane From 9371a23c09d139d1874ee113ee403b53a7335831 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 25 Oct 2023 18:02:20 -0600 Subject: [PATCH 14/25] Added ROS callback for toggle step signal --- rosplane/include/tuning_signal_generator.hpp | 5 +++++ rosplane/src/tuning_signal_generator.cpp | 12 ++++++++++++ 2 files changed, 17 insertions(+) diff --git a/rosplane/include/tuning_signal_generator.hpp b/rosplane/include/tuning_signal_generator.hpp index a78265d..afb285b 100644 --- a/rosplane/include/tuning_signal_generator.hpp +++ b/rosplane/include/tuning_signal_generator.hpp @@ -107,6 +107,8 @@ class TuningSignalGenerator : public rclcpp::Node /// ROS timer to run timer callback, which publishes commands rclcpp::TimerBase::SharedPtr publish_timer_; + /// ROS service for toggling step signal + rclcpp::Service::SharedPtr step_toggle_service_; /// ROS service for reset signal rclcpp::Service::SharedPtr reset_service_; /// ROS service for pause signal @@ -119,6 +121,9 @@ class TuningSignalGenerator : public rclcpp::Node /// Callback to publish command on topic. void publish_timer_callback(); + /// 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); diff --git a/rosplane/src/tuning_signal_generator.cpp b/rosplane/src/tuning_signal_generator.cpp index d9dbdc2..426343b 100644 --- a/rosplane/src/tuning_signal_generator.cpp +++ b/rosplane/src/tuning_signal_generator.cpp @@ -82,6 +82,10 @@ TuningSignalGenerator::TuningSignalGenerator() command_publisher_ = this->create_publisher("/controller_command", 1); + step_toggle_service_ = this->create_service( + "toggle_step_signal", + std::bind(&TuningSignalGenerator::step_toggle_service_callback, this, std::placeholders::_1, + std::placeholders::_2)); reset_service_ = this->create_service( "reset_signal", std::bind(&TuningSignalGenerator::reset_service_callback, this, std::placeholders::_1, @@ -166,6 +170,14 @@ void TuningSignalGenerator::publish_timer_callback() internals_publisher_->publish(internals_message); } +bool TuningSignalGenerator::step_toggle_service_callback( + const std_srvs::srv::Trigger::Request::SharedPtr & req, + const std_srvs::srv::Trigger::Response::SharedPtr & res) +{ + RCLCPP_INFO(this->get_logger(), "Toggle callback called!"); + return true; +} + bool TuningSignalGenerator::reset_service_callback( const std_srvs::srv::Trigger::Request::SharedPtr & req, const std_srvs::srv::Trigger::Response::SharedPtr & res) From 35d4824f8f0c106e70e65a8e0f4313489f3bd2f9 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 25 Oct 2023 18:37:17 -0600 Subject: [PATCH 15/25] Completed step response behavior --- rosplane/include/tuning_signal_generator.hpp | 30 +++++++--- rosplane/src/tuning_signal_generator.cpp | 61 ++++++++++++-------- 2 files changed, 59 insertions(+), 32 deletions(-) diff --git a/rosplane/include/tuning_signal_generator.hpp b/rosplane/include/tuning_signal_generator.hpp index afb285b..cb35560 100644 --- a/rosplane/include/tuning_signal_generator.hpp +++ b/rosplane/include/tuning_signal_generator.hpp @@ -74,6 +74,7 @@ class TuningSignalGenerator : public rclcpp::Node /// This defines what type of signal to publish to the selected controller. enum class SignalType { + STEP, SQUARE, SAWTOOTH, TRIANGLE, @@ -86,7 +87,7 @@ class TuningSignalGenerator : public rclcpp::Node double dt_hz_; ///< Frequency to publish commands. double amplitude_; ///< Amplitude of signal. double frequency_hz_; ///< Frequency of the signal. - double offset_; ///< Offset of signal from 0. + double center_value_; ///< Offset of signal from 0. double default_va_c_; ///< Default for va_c, used when not controlling airspeed. double default_h_c_; ///< Default for h_c, used when not controlling altitude. double default_chi_c_; ///< Default for chi_c, used when not controlling heading. @@ -94,6 +95,7 @@ class TuningSignalGenerator : public rclcpp::Node double default_phi_c_; ///< Default for phi_c, used when not controlling roll. // 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. @@ -137,6 +139,16 @@ class TuningSignalGenerator : public rclcpp::Node 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. * @@ -144,10 +156,10 @@ class TuningSignalGenerator : public rclcpp::Node * in seconds. * @param amplitude The amplitude of the signal. * @param frequency The frequency of the signal. - * @param initial_value Inital value of the signal. The in other words, the signal 'offset'. + * @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 initial_value); + double center_value); /** * @brief Get the value for a sawtooth signal at the given time with the given conditions. * @@ -155,10 +167,10 @@ class TuningSignalGenerator : public rclcpp::Node * in seconds. * @param amplitude The amplitude of the signal. * @param frequency The frequency of the signal. - * @param initial_value Inital value of the signal. The in other words, the signal 'offset'. + * @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 initial_value); + double center_value); /** * @brief Get the value for a triangle signal at the given time with the given conditions. * @@ -166,10 +178,10 @@ class TuningSignalGenerator : public rclcpp::Node * in seconds. * @param amplitude The amplitude of the signal. * @param frequency The frequency of the signal. - * @param initial_value Inital value of the signal. The in other words, the signal 'offset'. + * @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 initial_value); + double center_value); /** * @brief Get the value for a sine signal at the given time with the given conditions. * @@ -177,10 +189,10 @@ class TuningSignalGenerator : public rclcpp::Node * in seconds. * @param amplitude The amplitude of the signal. * @param frequency The frequency of the signal. - * @param initial_value Inital value of the signal. The in other words, the signal 'offset'. + * @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 initial_value); + double center_value); /// Updates the parameters within the class with the latest values from ROS. void update_params(); diff --git a/rosplane/src/tuning_signal_generator.cpp b/rosplane/src/tuning_signal_generator.cpp index 426343b..9e454d5 100644 --- a/rosplane/src/tuning_signal_generator.cpp +++ b/rosplane/src/tuning_signal_generator.cpp @@ -52,18 +52,18 @@ TuningSignalGenerator::TuningSignalGenerator() , dt_hz_(0) , amplitude_(0) , frequency_hz_(0) - , offset_(0) + , center_value_(0) , initial_time_(0) , is_paused_(true) , paused_time_(0) , single_period_start_time_(0) { this->declare_parameter("controller_output", "roll"); - this->declare_parameter("signal_type", "square"); + this->declare_parameter("signal_type", "step"); this->declare_parameter("dt_hz", 100.0); this->declare_parameter("amplitude", 1.0); this->declare_parameter("frequency_hz", 0.2); - this->declare_parameter("offset", 0.0); + this->declare_parameter("center_value", 0.0); this->declare_parameter("default_va_c", 15.0); this->declare_parameter("default_h_c", 40.0); this->declare_parameter("default_chi_c", 0.0); @@ -123,17 +123,20 @@ void TuningSignalGenerator::publish_timer_callback() // Get value for signal double signal_value = 0; switch (signal_type_) { + case SignalType::STEP: + signal_value = get_step_signal(step_toggled_, amplitude_, center_value_); + break; case SignalType::SQUARE: - signal_value = get_square_signal(elapsed_time, amplitude_, frequency_hz_, offset_); + signal_value = get_square_signal(elapsed_time, amplitude_, frequency_hz_, center_value_); break; case SignalType::SAWTOOTH: - signal_value = get_sawtooth_signal(elapsed_time, amplitude_, frequency_hz_, offset_); + signal_value = get_sawtooth_signal(elapsed_time, amplitude_, frequency_hz_, center_value_); break; case SignalType::TRIANGLE: - signal_value = get_triangle_signal(elapsed_time, amplitude_, frequency_hz_, offset_); + signal_value = get_triangle_signal(elapsed_time, amplitude_, frequency_hz_, center_value_); break; case SignalType::SINE: - signal_value = get_sine_signal(elapsed_time, amplitude_, frequency_hz_, offset_); + signal_value = get_sine_signal(elapsed_time, amplitude_, frequency_hz_, center_value_); break; } @@ -174,8 +177,12 @@ bool TuningSignalGenerator::step_toggle_service_callback( const std_srvs::srv::Trigger::Request::SharedPtr & req, const std_srvs::srv::Trigger::Response::SharedPtr & res) { - RCLCPP_INFO(this->get_logger(), "Toggle callback called!"); - return true; + if (step_toggled_) { + step_toggled_ = false; + } else { + step_toggled_ = true; + } + return true; } bool TuningSignalGenerator::reset_service_callback( @@ -224,35 +231,41 @@ bool TuningSignalGenerator::start_single_service_callback( return true; } +double TuningSignalGenerator::get_step_signal(bool step_toggled, double amplitude, + double center_value) +{ + return (step_toggled - 0.5) * amplitude + center_value; +} + double TuningSignalGenerator::get_square_signal(double elapsed_time, double amplitude, - double frequency, double offset) + double frequency, double center_value) { - // amplitude * (1 to -1 switching value) + offset - return amplitude * ((static_cast(elapsed_time * frequency * 2) % 2) * 2 - 1) + offset; + // amplitude * (1 to -1 switching value) + center value + return amplitude * ((static_cast(elapsed_time * frequency * 2) % 2) * 2 - 1) + center_value; } double TuningSignalGenerator::get_sawtooth_signal(double elapsed_time, double amplitude, - double frequency, double offset) + double frequency, double center_value) { - // slope * elapsed_time - num_cycles * offset_per_cycle + offset + // slope * elapsed_time - num_cycles * offset_per_cycle + center value return 2 * amplitude * (elapsed_time * frequency - static_cast(elapsed_time * frequency) - 0.5) - + offset; + + center_value; } double TuningSignalGenerator::get_triangle_signal(double elapsed_time, double amplitude, - double frequency, double offset) + double frequency, double center_value) { - // (1 to -1 switching value) * sawtooth_at_twice_the_rate + offset + // (1 to -1 switching value) * sawtooth_at_twice_the_rate + center_value return ((static_cast(elapsed_time * frequency * 2) % 2) * 2 - 1) * 2 * amplitude * (2 * elapsed_time * frequency - static_cast(2 * elapsed_time * frequency) - 0.5) - + offset; + + center_value; } double TuningSignalGenerator::get_sine_signal(double elapsed_time, double amplitude, - double frequency, double offset) + double frequency, double center_value) { - return sin(elapsed_time * frequency * 2 * M_PI) * amplitude + offset; + return sin(elapsed_time * frequency * 2 * M_PI) * amplitude + center_value; } void TuningSignalGenerator::update_params() @@ -276,7 +289,9 @@ void TuningSignalGenerator::update_params() // signal_type std::string signal_type_string = this->get_parameter("signal_type").as_string(); - if (signal_type_string == "square") { + if (signal_type_string == "step") { + signal_type_ = SignalType::STEP; + } else if (signal_type_string == "square") { signal_type_ = SignalType::SQUARE; } else if (signal_type_string == "sawtooth") { signal_type_ = SignalType::SAWTOOTH; @@ -314,8 +329,8 @@ void TuningSignalGenerator::update_params() frequency_hz_ = frequency_hz_value; } - // offset - offset_ = this->get_parameter("offset").as_double(); + // center_value + center_value_ = this->get_parameter("center_value").as_double(); // default_va_c default_va_c_ = this->get_parameter("default_va_c").as_double(); From 9d11ef53ba450cd49d773204db6187f1fe9d63d2 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Thu, 26 Oct 2023 11:15:41 -0600 Subject: [PATCH 16/25] Improved step signal behavior --- rosplane/src/tuning_signal_generator.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/rosplane/src/tuning_signal_generator.cpp b/rosplane/src/tuning_signal_generator.cpp index 9e454d5..4b63cec 100644 --- a/rosplane/src/tuning_signal_generator.cpp +++ b/rosplane/src/tuning_signal_generator.cpp @@ -117,6 +117,11 @@ void TuningSignalGenerator::publish_timer_callback() is_paused_ = true; } + // Check if step toggle needs to be reset + if (signal_type_ != SignalType::STEP) { + step_toggled_ = false; + } + // If paused, negate passing of time but keep publishing if (is_paused_) { paused_time_ += 1 / dt_hz_; } @@ -234,7 +239,7 @@ bool TuningSignalGenerator::start_single_service_callback( double TuningSignalGenerator::get_step_signal(bool step_toggled, double amplitude, double center_value) { - return (step_toggled - 0.5) * amplitude + center_value; + return (step_toggled - 0.5) * 2 * amplitude + center_value; } double TuningSignalGenerator::get_square_signal(double elapsed_time, double amplitude, From 0102e0e59246be6859bcdfad41fcd8bb3b066b32 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Thu, 26 Oct 2023 11:16:54 -0600 Subject: [PATCH 17/25] Suppressed unused params warning for ROS callbacks --- rosplane/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/rosplane/CMakeLists.txt b/rosplane/CMakeLists.txt index b063a32..1617f18 100644 --- a/rosplane/CMakeLists.txt +++ b/rosplane/CMakeLists.txt @@ -97,6 +97,7 @@ install(TARGETS 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}) From f8ced5186ecb72e06bc4c22d9cfa4b66be1a8334 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Thu, 26 Oct 2023 11:24:35 -0600 Subject: [PATCH 18/25] Renamed dt_hz to publish_rate_hz --- rosplane/include/tuning_signal_generator.hpp | 2 +- rosplane/src/tuning_signal_generator.cpp | 26 ++++++++++---------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/rosplane/include/tuning_signal_generator.hpp b/rosplane/include/tuning_signal_generator.hpp index cb35560..08749e5 100644 --- a/rosplane/include/tuning_signal_generator.hpp +++ b/rosplane/include/tuning_signal_generator.hpp @@ -84,7 +84,7 @@ class TuningSignalGenerator : public rclcpp::Node // Parameters ControllerOutput controller_output_; ///< Controller to output command signals to. SignalType signal_type_; ///< Signal type to output. - double dt_hz_; ///< Frequency to publish commands. + double publish_rate_hz_; ///< Frequency to publish commands. double amplitude_; ///< Amplitude of signal. double frequency_hz_; ///< Frequency of the signal. double center_value_; ///< Offset of signal from 0. diff --git a/rosplane/src/tuning_signal_generator.cpp b/rosplane/src/tuning_signal_generator.cpp index 4b63cec..c9dfade 100644 --- a/rosplane/src/tuning_signal_generator.cpp +++ b/rosplane/src/tuning_signal_generator.cpp @@ -49,7 +49,7 @@ TuningSignalGenerator::TuningSignalGenerator() : Node("signal_generator") , controller_output_(ControllerOutput::ROLL) , signal_type_(SignalType::SQUARE) - , dt_hz_(0) + , publish_rate_hz_(0) , amplitude_(0) , frequency_hz_(0) , center_value_(0) @@ -60,7 +60,7 @@ TuningSignalGenerator::TuningSignalGenerator() { this->declare_parameter("controller_output", "roll"); this->declare_parameter("signal_type", "step"); - this->declare_parameter("dt_hz", 100.0); + this->declare_parameter("publish_rate_hz", 100.0); this->declare_parameter("amplitude", 1.0); this->declare_parameter("frequency_hz", 0.2); this->declare_parameter("center_value", 0.0); @@ -74,7 +74,7 @@ TuningSignalGenerator::TuningSignalGenerator() initial_time_ = this->get_clock()->now().seconds(); publish_timer_ = - this->create_wall_timer(std::chrono::milliseconds(static_cast(1000 / dt_hz_)), + this->create_wall_timer(std::chrono::milliseconds(static_cast(1000 / publish_rate_hz_)), std::bind(&TuningSignalGenerator::publish_timer_callback, this)); internals_publisher_ = @@ -123,7 +123,7 @@ void TuningSignalGenerator::publish_timer_callback() } // If paused, negate passing of time but keep publishing - if (is_paused_) { paused_time_ += 1 / dt_hz_; } + if (is_paused_) { paused_time_ += 1 / publish_rate_hz_; } // Get value for signal double signal_value = 0; @@ -309,17 +309,17 @@ void TuningSignalGenerator::update_params() signal_type_string.c_str()); } - // dt_hz - double dt_hz_value = this->get_parameter("dt_hz").as_double(); - if (dt_hz_value <= 0) { - RCLCPP_ERROR(this->get_logger(), "Param dt_hz must be greater than 0!"); + // publish_rate_hz + double publish_rate_hz_value = this->get_parameter("publish_rate_hz").as_double(); + if (publish_rate_hz_value <= 0) { + RCLCPP_ERROR(this->get_logger(), "Param publish_rate_hz must be greater than 0!"); } else { // Parameter has changed, create new timer with updated value - if (dt_hz_ != dt_hz_value) { - dt_hz_ = dt_hz_value; - publish_timer_ = - this->create_wall_timer(std::chrono::milliseconds(static_cast(1000 / dt_hz_)), - std::bind(&TuningSignalGenerator::publish_timer_callback, this)); + if (publish_rate_hz_ != publish_rate_hz_value) { + publish_rate_hz_ = publish_rate_hz_value; + publish_timer_ = this->create_wall_timer( + std::chrono::milliseconds(static_cast(1000 / publish_rate_hz_)), + std::bind(&TuningSignalGenerator::publish_timer_callback, this)); } } From 27553339cc597c22261c44fc99507fa58548a657 Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Thu, 26 Oct 2023 12:35:40 -0600 Subject: [PATCH 19/25] Added a simulator launch file for tuning. --- rosplane_sim/launch/sim_tuning.launch.py | 50 ++++++++++++++++++++++++ 1 file changed, 50 insertions(+) create mode 100644 rosplane_sim/launch/sim_tuning.launch.py diff --git a/rosplane_sim/launch/sim_tuning.launch.py b/rosplane_sim/launch/sim_tuning.launch.py new file mode 100644 index 0000000..5ea49f9 --- /dev/null +++ b/rosplane_sim/launch/sim_tuning.launch.py @@ -0,0 +1,50 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import IncludeLaunchDescription +import os + +def generate_launch_description(): + # Create the package directory + rosplane_dir = get_package_share_directory('rosplane') + + # Define the urdf file for visualizing the uav + urdf_file_name = 'fixed_wing_uav.urdf' + urdf = os.path.join( + get_package_share_directory('rosplane'), + 'urdf', + urdf_file_name) + + # Flag for enabling/disabling use of simulation time instead of wall clock + use_sim_time = True + + base_launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory('rosplane'), + 'launch/rosplane_tuning.launch.py' + ) + ) + ) + + return LaunchDescription([ + base_launch_include, + Node ( + package = 'rosplane_sim', + executable='rosplane_gazebo_truth_publisher', + name='gazebo_truth' + ), + Node( + package='data_viz', + executable='viz_data', + name='uav_plotter', + parameters=[{ + 'use_sim_time': False, + 't_horizon': 100., + 'plot_sensors': False + }], + output='screen' + ) + ]) + From 94b2f390b5a66c355a301b81378cd352e99fefde Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Mon, 30 Oct 2023 13:48:22 -0600 Subject: [PATCH 20/25] Fixed debug issues and topic name issues. --- rosplane/include/controller_base.hpp | 7 +++++-- rosplane/launch/rosplane_tuning.launch.py | 3 ++- rosplane/src/controller_base.cpp | 11 +++++++---- rosplane/src/controller_successive_loop.cpp | 4 ++-- rosplane/src/tuning_signal_generator.cpp | 2 +- 5 files changed, 17 insertions(+), 10 deletions(-) diff --git a/rosplane/include/controller_base.hpp b/rosplane/include/controller_base.hpp index 9937ff8..6859331 100644 --- a/rosplane/include/controller_base.hpp +++ b/rosplane/include/controller_base.hpp @@ -128,7 +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 tuning_debug_override; + bool pitch_tuning_debug_override; + bool roll_tuning_debug_override; }; /** @@ -213,7 +214,9 @@ class controller_base : public rclcpp::Node /* max_takeoff_throttle */ .55, /* mass */ 2.28, /* gravity */ 9.8, - /* tuning_debug_override*/ false}; + /* pitch_tuning_debug_override*/ false, + /* roll_tuning_debug_override*/ false + }; /** * The stored value for the most up to date commands for the controller. diff --git a/rosplane/launch/rosplane_tuning.launch.py b/rosplane/launch/rosplane_tuning.launch.py index f0ec535..c5abb09 100644 --- a/rosplane/launch/rosplane_tuning.launch.py +++ b/rosplane/launch/rosplane_tuning.launch.py @@ -28,7 +28,8 @@ def generate_launch_description(): executable='rosplane_controller', name='autopilot', parameters = [autopilot_params, - {'tuning_debug_override': False}], + {'pitch_tuning_debug_override': False}, + {'roll_tuning_debug_override': True}], output = 'screen', arguments = [control_type] ), diff --git a/rosplane/src/controller_base.cpp b/rosplane/src/controller_base.cpp index 9bfdb85..d32e3b2 100644 --- a/rosplane/src/controller_base.cpp +++ b/rosplane/src/controller_base.cpp @@ -32,7 +32,7 @@ controller_base::controller_base() : Node("controller_base") // Set the values for the parameters, from the param file or use the deafault value. set_parameters(); - if (params_.tuning_debug_override){ + 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)); } @@ -194,7 +194,8 @@ void controller_base::actuator_controls_publish() this->declare_parameter("max_takeoff_throttle", params_.max_takeoff_throttle); this->declare_parameter("mass", params_.mass); this->declare_parameter("gravity", params_.gravity); - this->declare_parameter("tuning_debug_override", params_.tuning_debug_override); + this->declare_parameter("pitch_tuning_debug_override", params_.pitch_tuning_debug_override); + this->declare_parameter("roll_tuning_debug_override", params_.roll_tuning_debug_override); } void controller_base::set_parameters() { @@ -240,7 +241,8 @@ void controller_base::set_parameters() { params_.max_takeoff_throttle = this->get_parameter("pwm_rad_r").as_double(); params_.mass = this->get_parameter("mass").as_double(); params_.gravity = this->get_parameter("gravity").as_double(); - params_.gravity = this->get_parameter("tuning_debug_override").as_bool(); + params_.roll_tuning_debug_override = this->get_parameter("roll_tuning_debug_override").as_bool(); + params_.pitch_tuning_debug_override = this->get_parameter("pitch_tuning_debug_override").as_bool(); } @@ -291,7 +293,8 @@ rcl_interfaces::msg::SetParametersResult controller_base::parametersCallback(con else if (param.get_name() == "max_takeoff_throttle") params_.max_takeoff_throttle = param.as_double(); else if (param.get_name() == "mass") params_.mass = param.as_double(); else if (param.get_name() == "gravity") params_.gravity = param.as_double(); - else if (param.get_name() == "tuning_debug_override") params_.tuning_debug_override = param.as_bool(); + else if (param.get_name() == "roll_tuning_debug_override") params_.roll_tuning_debug_override = param.as_bool(); + else if (param.get_name() == "pitch_tuning_debug_override") params_.pitch_tuning_debug_override = param.as_bool(); else{ // If the parameter given doesn't match any of the parameters return false. diff --git a/rosplane/src/controller_successive_loop.cpp b/rosplane/src/controller_successive_loop.cpp index ab8a7bb..56da8ec 100644 --- a/rosplane/src/controller_successive_loop.cpp +++ b/rosplane/src/controller_successive_loop.cpp @@ -67,7 +67,7 @@ void controller_successive_loop::alt_hold_lateral_control(const struct params_s output.delta_r = 0; //cooridinated_turn_hold(input.beta, params, input.Ts) output.phi_c = course_hold(input.chi_c, input.chi, input.phi_ff, input.r, params, input.Ts); - if (params.tuning_debug_override){ + if (params.roll_tuning_debug_override){ output.phi_c = tuning_debug_override_msg_.phi_c; } @@ -83,7 +83,7 @@ void controller_successive_loop::alt_hold_longitudinal_control(const struct para output.delta_t = airspeed_with_throttle_hold(input.Va_c, input.va, params, input.Ts); output.theta_c = altitude_hold_control(adjusted_hc, input.h, params, input.Ts); - if (params.tuning_debug_override){ + if (params.pitch_tuning_debug_override){ output.theta_c = tuning_debug_override_msg_.theta_c; } diff --git a/rosplane/src/tuning_signal_generator.cpp b/rosplane/src/tuning_signal_generator.cpp index c9dfade..93dd5ac 100644 --- a/rosplane/src/tuning_signal_generator.cpp +++ b/rosplane/src/tuning_signal_generator.cpp @@ -80,7 +80,7 @@ TuningSignalGenerator::TuningSignalGenerator() internals_publisher_ = this->create_publisher("/tuning_debug", 1); command_publisher_ = - this->create_publisher("/controller_command", 1); + this->create_publisher("/controller_commands", 1); step_toggle_service_ = this->create_service( "toggle_step_signal", From 1fb3696dd0cad49118394db18e847c741ccc1d76 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Thu, 2 Nov 2023 13:42:22 -0600 Subject: [PATCH 21/25] Made triangle and sine signal begin at minimum value --- rosplane/src/tuning_signal_generator.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rosplane/src/tuning_signal_generator.cpp b/rosplane/src/tuning_signal_generator.cpp index 93dd5ac..2dfcdb9 100644 --- a/rosplane/src/tuning_signal_generator.cpp +++ b/rosplane/src/tuning_signal_generator.cpp @@ -262,7 +262,7 @@ double TuningSignalGenerator::get_triangle_signal(double elapsed_time, double am double frequency, double center_value) { // (1 to -1 switching value) * sawtooth_at_twice_the_rate + center_value - return ((static_cast(elapsed_time * frequency * 2) % 2) * 2 - 1) * 2 * amplitude + return -((static_cast(elapsed_time * frequency * 2) % 2) * 2 - 1) * 2 * amplitude * (2 * elapsed_time * frequency - static_cast(2 * elapsed_time * frequency) - 0.5) + center_value; } @@ -270,7 +270,7 @@ double TuningSignalGenerator::get_triangle_signal(double elapsed_time, double am double TuningSignalGenerator::get_sine_signal(double elapsed_time, double amplitude, double frequency, double center_value) { - return sin(elapsed_time * frequency * 2 * M_PI) * amplitude + center_value; + return -cos(elapsed_time * frequency * 2 * M_PI) * amplitude + center_value; } void TuningSignalGenerator::update_params() From 8790853e18751ede311c4c32d794d57d8b75e798 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Thu, 2 Nov 2023 20:58:37 -0600 Subject: [PATCH 22/25] Changed to use default-magnitude system instead of center-amplitude --- rosplane/include/tuning_signal_generator.hpp | 13 +++--- rosplane/src/tuning_signal_generator.cpp | 43 +++++++++++++------- 2 files changed, 35 insertions(+), 21 deletions(-) diff --git a/rosplane/include/tuning_signal_generator.hpp b/rosplane/include/tuning_signal_generator.hpp index 08749e5..e8dc736 100644 --- a/rosplane/include/tuning_signal_generator.hpp +++ b/rosplane/include/tuning_signal_generator.hpp @@ -85,14 +85,13 @@ class TuningSignalGenerator : public rclcpp::Node 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 amplitude_; ///< Amplitude of signal. + double signal_magnitude_; ///< The the magnitude of the signal being generated. double frequency_hz_; ///< Frequency of the signal. - double center_value_; ///< Offset of signal from 0. - double default_va_c_; ///< Default for va_c, used when not controlling airspeed. - double default_h_c_; ///< Default for h_c, used when not controlling altitude. - double default_chi_c_; ///< Default for chi_c, used when not controlling heading. - double default_theta_c_; ///< Default for theta_c, used when not controlling pitch. - double default_phi_c_; ///< Default for phi_c, used when not controlling roll. + 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. diff --git a/rosplane/src/tuning_signal_generator.cpp b/rosplane/src/tuning_signal_generator.cpp index 2dfcdb9..a4f2e67 100644 --- a/rosplane/src/tuning_signal_generator.cpp +++ b/rosplane/src/tuning_signal_generator.cpp @@ -50,9 +50,8 @@ TuningSignalGenerator::TuningSignalGenerator() , controller_output_(ControllerOutput::ROLL) , signal_type_(SignalType::SQUARE) , publish_rate_hz_(0) - , amplitude_(0) + , signal_magnitude_(0) , frequency_hz_(0) - , center_value_(0) , initial_time_(0) , is_paused_(true) , paused_time_(0) @@ -61,9 +60,8 @@ TuningSignalGenerator::TuningSignalGenerator() this->declare_parameter("controller_output", "roll"); this->declare_parameter("signal_type", "step"); this->declare_parameter("publish_rate_hz", 100.0); - this->declare_parameter("amplitude", 1.0); + this->declare_parameter("signal_magnitude", 1.0); this->declare_parameter("frequency_hz", 0.2); - this->declare_parameter("center_value", 0.0); this->declare_parameter("default_va_c", 15.0); this->declare_parameter("default_h_c", 40.0); this->declare_parameter("default_chi_c", 0.0); @@ -126,22 +124,42 @@ void TuningSignalGenerator::publish_timer_callback() if (is_paused_) { paused_time_ += 1 / publish_rate_hz_; } // Get value for signal + double amplitude = signal_magnitude_ / 2; + double center_value = 0; + switch (controller_output_) { + case ControllerOutput::ROLL: + center_value = default_phi_c_; + break; + case ControllerOutput::PITCH: + center_value = default_theta_c_; + break; + case ControllerOutput::ALTITUDE: + center_value = default_h_c_; + break; + case ControllerOutput::HEADING: + center_value = default_chi_c_; + break; + case ControllerOutput::AIRSPEED: + center_value = default_va_c_; + break; + } + center_value += amplitude; double signal_value = 0; switch (signal_type_) { case SignalType::STEP: - signal_value = get_step_signal(step_toggled_, amplitude_, center_value_); + signal_value = get_step_signal(step_toggled_, amplitude, center_value); break; case SignalType::SQUARE: - signal_value = get_square_signal(elapsed_time, amplitude_, frequency_hz_, center_value_); + signal_value = get_square_signal(elapsed_time, amplitude, frequency_hz_, center_value); break; case SignalType::SAWTOOTH: - signal_value = get_sawtooth_signal(elapsed_time, amplitude_, frequency_hz_, center_value_); + signal_value = get_sawtooth_signal(elapsed_time, amplitude, frequency_hz_, center_value); break; case SignalType::TRIANGLE: - signal_value = get_triangle_signal(elapsed_time, amplitude_, frequency_hz_, center_value_); + signal_value = get_triangle_signal(elapsed_time, amplitude, frequency_hz_, center_value); break; case SignalType::SINE: - signal_value = get_sine_signal(elapsed_time, amplitude_, frequency_hz_, center_value_); + signal_value = get_sine_signal(elapsed_time, amplitude, frequency_hz_, center_value); break; } @@ -323,8 +341,8 @@ void TuningSignalGenerator::update_params() } } - // amplitude - amplitude_ = this->get_parameter("amplitude").as_double(); + // signal_magnitude_ + signal_magnitude_ = this->get_parameter("signal_magnitude").as_double(); // frequency_hz double frequency_hz_value = this->get_parameter("frequency_hz").as_double(); @@ -334,9 +352,6 @@ void TuningSignalGenerator::update_params() frequency_hz_ = frequency_hz_value; } - // center_value - center_value_ = this->get_parameter("center_value").as_double(); - // default_va_c default_va_c_ = this->get_parameter("default_va_c").as_double(); From 82fbd807e77da1a6bf98a2363b81f8cec4d9fb10 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Thu, 2 Nov 2023 21:10:42 -0600 Subject: [PATCH 23/25] Added logic to check if service calls make sense for signal type --- rosplane/src/tuning_signal_generator.cpp | 31 ++++++++++++++++++++++-- 1 file changed, 29 insertions(+), 2 deletions(-) diff --git a/rosplane/src/tuning_signal_generator.cpp b/rosplane/src/tuning_signal_generator.cpp index a4f2e67..dd33e7c 100644 --- a/rosplane/src/tuning_signal_generator.cpp +++ b/rosplane/src/tuning_signal_generator.cpp @@ -200,11 +200,19 @@ bool TuningSignalGenerator::step_toggle_service_callback( const std_srvs::srv::Trigger::Request::SharedPtr & req, const std_srvs::srv::Trigger::Response::SharedPtr & res) { + if (signal_type_ != SignalType::STEP) { + res->success = false; + res->message = "Service valid only for step type signal"; + return true; + } + if (step_toggled_) { - step_toggled_ = false; + step_toggled_ = false; } else { - step_toggled_ = true; + step_toggled_ = true; } + + res->success = true; return true; } @@ -216,6 +224,7 @@ bool TuningSignalGenerator::reset_service_callback( paused_time_ = 0; is_paused_ = true; single_period_start_time_ = 0; + step_toggled_ = false; res->success = true; return true; @@ -225,6 +234,12 @@ bool TuningSignalGenerator::pause_service_callback( const std_srvs::srv::Trigger::Request::SharedPtr & req, const std_srvs::srv::Trigger::Response::SharedPtr & res) { + if (signal_type_ == SignalType::STEP) { + res->success = false; + res->message = "Service not valid for step type signal"; + return true; + } + is_paused_ = true; single_period_start_time_ = 0; @@ -236,6 +251,12 @@ bool TuningSignalGenerator::start_continuous_service_callback( const std_srvs::srv::Trigger::Request::SharedPtr & req, const std_srvs::srv::Trigger::Response::SharedPtr & res) { + if (signal_type_ == SignalType::STEP) { + res->success = false; + res->message = "Service not valid for step type signal"; + return true; + } + is_paused_ = false; single_period_start_time_ = 0; @@ -247,6 +268,12 @@ bool TuningSignalGenerator::start_single_service_callback( const std_srvs::srv::Trigger::Request::SharedPtr & req, const std_srvs::srv::Trigger::Response::SharedPtr & res) { + if (signal_type_ == SignalType::STEP) { + res->success = false; + res->message = "Service not valid for step type signal"; + return true; + } + is_paused_ = false; single_period_start_time_ = this->get_clock()->now().seconds(); From 7131dedc9edc17edac7043267ad3c805dc5e46e1 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Thu, 2 Nov 2023 21:37:22 -0600 Subject: [PATCH 24/25] Added reset on signal type or output change --- rosplane/include/tuning_signal_generator.hpp | 11 +++++ rosplane/src/tuning_signal_generator.cpp | 42 +++++++++++++++----- 2 files changed, 43 insertions(+), 10 deletions(-) diff --git a/rosplane/include/tuning_signal_generator.hpp b/rosplane/include/tuning_signal_generator.hpp index e8dc736..36e8e65 100644 --- a/rosplane/include/tuning_signal_generator.hpp +++ b/rosplane/include/tuning_signal_generator.hpp @@ -44,6 +44,7 @@ #include "rosplane_msgs/msg/controller_commands.hpp" #include "rosplane_msgs/msg/controller_internals_debug.hpp" +#include #include #include @@ -107,6 +108,9 @@ class TuningSignalGenerator : public rclcpp::Node /// 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::SharedPtr step_toggle_service_; @@ -122,6 +126,10 @@ class TuningSignalGenerator : public rclcpp::Node /// Callback to publish command on topic. void publish_timer_callback(); + /// Callback for parameter changes. + rcl_interfaces::msg::SetParametersResult param_callback( + const std::vector & 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); @@ -195,6 +203,9 @@ class TuningSignalGenerator : public rclcpp::Node /// Updates the parameters within the class with the latest values from ROS. void update_params(); + + /// Reset the signal generator + void reset(); }; } // namespace rosplane diff --git a/rosplane/src/tuning_signal_generator.cpp b/rosplane/src/tuning_signal_generator.cpp index dd33e7c..e7c2ad3 100644 --- a/rosplane/src/tuning_signal_generator.cpp +++ b/rosplane/src/tuning_signal_generator.cpp @@ -71,14 +71,17 @@ TuningSignalGenerator::TuningSignalGenerator() update_params(); initial_time_ = this->get_clock()->now().seconds(); - publish_timer_ = - this->create_wall_timer(std::chrono::milliseconds(static_cast(1000 / publish_rate_hz_)), - std::bind(&TuningSignalGenerator::publish_timer_callback, this)); - internals_publisher_ = this->create_publisher("/tuning_debug", 1); command_publisher_ = this->create_publisher("/controller_commands", 1); + + publish_timer_ = + this->create_wall_timer(std::chrono::milliseconds(static_cast(1000 / publish_rate_hz_)), + std::bind(&TuningSignalGenerator::publish_timer_callback, this)); + + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&TuningSignalGenerator::param_callback, this, std::placeholders::_1)); step_toggle_service_ = this->create_service( "toggle_step_signal", @@ -196,6 +199,21 @@ void TuningSignalGenerator::publish_timer_callback() internals_publisher_->publish(internals_message); } +rcl_interfaces::msg::SetParametersResult TuningSignalGenerator::param_callback( + const std::vector & params) +{ + for (const auto & param : params) + { + if (param.get_name() == "controller_output" || param.get_name() == "signal_type") { + reset(); + } + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; +} + bool TuningSignalGenerator::step_toggle_service_callback( const std_srvs::srv::Trigger::Request::SharedPtr & req, const std_srvs::srv::Trigger::Response::SharedPtr & res) @@ -220,12 +238,7 @@ bool TuningSignalGenerator::reset_service_callback( const std_srvs::srv::Trigger::Request::SharedPtr & req, const std_srvs::srv::Trigger::Response::SharedPtr & res) { - initial_time_ = this->get_clock()->now().seconds(); - paused_time_ = 0; - is_paused_ = true; - single_period_start_time_ = 0; - step_toggled_ = false; - + reset(); res->success = true; return true; } @@ -394,6 +407,15 @@ void TuningSignalGenerator::update_params() // default_phi_c default_phi_c_ = this->get_parameter("default_phi_c").as_double(); } + +void TuningSignalGenerator::reset() { + initial_time_ = this->get_clock()->now().seconds(); + paused_time_ = 0; + is_paused_ = true; + single_period_start_time_ = 0; + step_toggled_ = false; +} + } // namespace rosplane int main(int argc, char ** argv) From 4c533174a9f2d49eb479c137198daf327d6fb671 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Thu, 2 Nov 2023 21:42:31 -0600 Subject: [PATCH 25/25] Misc formatting --- rosplane/include/tuning_signal_generator.hpp | 28 +++++++++--------- rosplane/src/tuning_signal_generator.cpp | 30 +++++++++----------- 2 files changed, 27 insertions(+), 31 deletions(-) diff --git a/rosplane/include/tuning_signal_generator.hpp b/rosplane/include/tuning_signal_generator.hpp index 36e8e65..c97a06f 100644 --- a/rosplane/include/tuning_signal_generator.hpp +++ b/rosplane/include/tuning_signal_generator.hpp @@ -108,41 +108,41 @@ class TuningSignalGenerator : public rclcpp::Node /// 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 + /// ROS service for toggling step signal. rclcpp::Service::SharedPtr step_toggle_service_; - /// ROS service for reset signal + /// ROS service for reset signal. rclcpp::Service::SharedPtr reset_service_; - /// ROS service for pause signal + /// ROS service for pause signal. rclcpp::Service::SharedPtr pause_service_; - /// ROS service for start signal continuously + /// ROS service for start signal continuously. rclcpp::Service::SharedPtr start_continuous_service_; - /// ROS service for start signal for one period + /// ROS service for start signal for one period. rclcpp::Service::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 & params); + rcl_interfaces::msg::SetParametersResult + param_callback(const std::vector & params); - /// Callback to toggle step signal + /// 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 + /// 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 + /// 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 + /// 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 + /// 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); @@ -204,7 +204,7 @@ class TuningSignalGenerator : public rclcpp::Node /// Updates the parameters within the class with the latest values from ROS. void update_params(); - /// Reset the signal generator + /// Reset the signal generator. void reset(); }; } // namespace rosplane diff --git a/rosplane/src/tuning_signal_generator.cpp b/rosplane/src/tuning_signal_generator.cpp index e7c2ad3..c3f87c9 100644 --- a/rosplane/src/tuning_signal_generator.cpp +++ b/rosplane/src/tuning_signal_generator.cpp @@ -71,11 +71,11 @@ TuningSignalGenerator::TuningSignalGenerator() update_params(); initial_time_ = this->get_clock()->now().seconds(); - internals_publisher_ = + internals_publisher_ = this->create_publisher("/tuning_debug", 1); command_publisher_ = this->create_publisher("/controller_commands", 1); - + publish_timer_ = this->create_wall_timer(std::chrono::milliseconds(static_cast(1000 / publish_rate_hz_)), std::bind(&TuningSignalGenerator::publish_timer_callback, this)); @@ -119,9 +119,7 @@ void TuningSignalGenerator::publish_timer_callback() } // Check if step toggle needs to be reset - if (signal_type_ != SignalType::STEP) { - step_toggled_ = false; - } + if (signal_type_ != SignalType::STEP) { step_toggled_ = false; } // If paused, negate passing of time but keep publishing if (is_paused_) { paused_time_ += 1 / publish_rate_hz_; } @@ -167,7 +165,7 @@ void TuningSignalGenerator::publish_timer_callback() } // Creates message with default values - rosplane_msgs::msg::ControllerCommands command_message; + rosplane_msgs::msg::ControllerCommands command_message; command_message.header.stamp = this->get_clock()->now(); command_message.va_c = default_va_c_; command_message.h_c = default_h_c_; @@ -199,15 +197,12 @@ void TuningSignalGenerator::publish_timer_callback() internals_publisher_->publish(internals_message); } -rcl_interfaces::msg::SetParametersResult TuningSignalGenerator::param_callback( - const std::vector & params) +rcl_interfaces::msg::SetParametersResult +TuningSignalGenerator::param_callback(const std::vector & params) { - for (const auto & param : params) - { - if (param.get_name() == "controller_output" || param.get_name() == "signal_type") { - reset(); - } - } + for (const auto & param : params) { + if (param.get_name() == "controller_output" || param.get_name() == "signal_type") { reset(); } + } rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -310,7 +305,7 @@ double TuningSignalGenerator::get_square_signal(double elapsed_time, double ampl double TuningSignalGenerator::get_sawtooth_signal(double elapsed_time, double amplitude, double frequency, double center_value) { - // slope * elapsed_time - num_cycles * offset_per_cycle + center value + // slope * elapsed_time - num_cycles * offset_per_cycle + center value return 2 * amplitude * (elapsed_time * frequency - static_cast(elapsed_time * frequency) - 0.5) + center_value; @@ -394,7 +389,7 @@ void TuningSignalGenerator::update_params() // default_va_c default_va_c_ = this->get_parameter("default_va_c").as_double(); - + // default_h_c default_h_c_ = this->get_parameter("default_h_c").as_double(); @@ -408,7 +403,8 @@ void TuningSignalGenerator::update_params() default_phi_c_ = this->get_parameter("default_phi_c").as_double(); } -void TuningSignalGenerator::reset() { +void TuningSignalGenerator::reset() +{ initial_time_ = this->get_clock()->now().seconds(); paused_time_ = 0; is_paused_ = true;