From 7131dedc9edc17edac7043267ad3c805dc5e46e1 Mon Sep 17 00:00:00 2001
From: Brandon Sutherland <brandonsutherland2@gmail.com>
Date: Thu, 2 Nov 2023 21:37:22 -0600
Subject: [PATCH] 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 <rcl_interfaces/msg/set_parameters_result.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <std_srvs/srv/trigger.hpp>
 
@@ -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<std_srvs::srv::Trigger>::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<rclcpp::Parameter> & params);
+
   /// Callback to toggle step signal
   bool step_toggle_service_callback(const std_srvs::srv::Trigger::Request::SharedPtr & req,
                                     const std_srvs::srv::Trigger::Response::SharedPtr & res);
@@ -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<long>(1000 / publish_rate_hz_)),
-                            std::bind(&TuningSignalGenerator::publish_timer_callback, this));
-
   internals_publisher_ = 
     this->create_publisher<rosplane_msgs::msg::ControllerInternalsDebug>("/tuning_debug", 1);
   command_publisher_ =
     this->create_publisher<rosplane_msgs::msg::ControllerCommands>("/controller_commands", 1);
+  
+  publish_timer_ =
+    this->create_wall_timer(std::chrono::milliseconds(static_cast<long>(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<std_srvs::srv::Trigger>(
     "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<rclcpp::Parameter> & 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)