From d42d840d81c6164bc010f3044dd37c89dd2ef73c Mon Sep 17 00:00:00 2001
From: Brandon Sutherland <brandonsutherland2@gmail.com>
Date: Mon, 17 Jun 2024 13:03:17 -0600
Subject: [PATCH] Added individual set functions

---
 rosplane/include/param_manager.hpp | 11 +++++-
 rosplane/src/param_manager.cpp     | 60 ++++++++++++++++++++++++++++++
 2 files changed, 70 insertions(+), 1 deletion(-)

diff --git a/rosplane/include/param_manager.hpp b/rosplane/include/param_manager.hpp
index c57f0df..27d4279 100644
--- a/rosplane/include/param_manager.hpp
+++ b/rosplane/include/param_manager.hpp
@@ -49,7 +49,16 @@ class param_manager
    */
   // TODO: Check to make sure that setting a parameter before declaring it won't give an error.
   // Hypothesis is that it will break, but is that not desired behavior?
-  void set_parameters();    
+  void set_parameters();
+
+  /**
+   * This function sets a previously declared parameter to a new value in both the parameter object
+   * and the ROS system.
+   */
+  void set_parameter(std::string param_name, double value);
+  void set_parameter(std::string param_name, bool value);
+  void set_int(std::string param_name, int64_t value);
+  void set_parameter(std::string param_name, std::string value);
   
   /**
    * This function should be called in the parametersCallback function in a containing ROS node.
diff --git a/rosplane/src/param_manager.cpp b/rosplane/src/param_manager.cpp
index 6f8dc1d..2b7ac29 100644
--- a/rosplane/src/param_manager.cpp
+++ b/rosplane/src/param_manager.cpp
@@ -37,6 +37,66 @@ void param_manager::declare_string(std::string param_name, std::string value)
   container_node_->declare_parameter(param_name, value);
 }
 
+void param_manager::set_parameter(std::string param_name, double value)
+{
+  // Check that the parameter is in the parameter struct
+  if (params_.find(param_name) == params_.end())
+  {
+    RCLCPP_ERROR_STREAM(container_node_->get_logger(), "Parameter not found in parameter struct: " + param_name);
+    return;
+  }
+
+  // Set the parameter in the parameter struct
+  params_[param_name] = value;
+  // Set the parameter in the ROS2 param system
+  container_node_->set_parameter(rclcpp::Parameter(param_name, value));
+}
+
+void param_manager::set_parameter(std::string param_name, bool value)
+{
+  // Check that the parameter is in the parameter struct
+  if (params_.find(param_name) == params_.end())
+  {
+    RCLCPP_ERROR_STREAM(container_node_->get_logger(), "Parameter not found in parameter struct: " + param_name);
+    return;
+  }
+
+  // Set the parameter in the parameter struct
+  params_[param_name] = value;
+  // Set the parameter in the ROS2 param system
+  container_node_->set_parameter(rclcpp::Parameter(param_name, value));
+}
+
+void param_manager::set_int(std::string param_name, int64_t value)
+{
+  // Check that the parameter is in the parameter struct
+  if (params_.find(param_name) == params_.end())
+  {
+    RCLCPP_ERROR_STREAM(container_node_->get_logger(), "Parameter not found in parameter struct: " + param_name);
+    return;
+  }
+
+  // Set the parameter in the parameter struct
+  params_[param_name] = value;
+  // Set the parameter in the ROS2 param system
+  container_node_->set_parameter(rclcpp::Parameter(param_name, value));
+}
+
+void param_manager::set_parameter(std::string param_name, std::string value)
+{
+  // Check that the parameter is in the parameter struct
+  if (params_.find(param_name) == params_.end())
+  {
+    RCLCPP_ERROR_STREAM(container_node_->get_logger(), "Parameter not found in parameter struct: " + param_name);
+    return;
+  }
+
+  // Set the parameter in the parameter struct
+  params_[param_name] = value;
+  // Set the parameter in the ROS2 param system
+  container_node_->set_parameter(rclcpp::Parameter(param_name, value));
+}
+
 double param_manager::get_double(std::string param_name)
 {
     try