created a new parameter manager object to standardize across estimato…

…r, path planner, etc.

.gitignore

@@ -8,4 +8,5 @@ log/
 
 # IDE files
 .vscode/
-.idea/
+.idea/
+__pycache__/

rosplane/CMakeLists.txt

@@ -51,7 +51,8 @@ add_executable(rosplane_controller
             src/controller_base.cpp
             src/controller_state_machine.cpp
             src/controller_successive_loop.cpp
-            src/controller_total_energy.cpp)
+            src/controller_total_energy.cpp
+            src/param_manager.cpp)
 ament_target_dependencies(rosplane_controller rosplane_msgs rosflight_msgs rclcpp rclpy Eigen3)
 install(TARGETS
   rosplane_controller

rosplane/include/controller_base.hpp

@@ -20,6 +20,7 @@
 #include <rosplane_msgs/msg/controller_internals_debug.hpp>
 #include <rosplane_msgs/msg/detail/controller_internals_debug__struct.hpp>
 #include <rosplane_msgs/msg/state.hpp>
+#include <param_manager.hpp>
 
 using std::placeholders::_1;
 using namespace std::chrono_literals;
@@ -84,25 +85,9 @@ class controller_base : public rclcpp::Node
   };
 
   /**
-   * Returns a std::variant that holds the value of the given parameter
+   * Parameter manager object. Contains helper functions to interface parameters with ROS.
   */
-  double get_double(std::string param_name);
-  bool get_bool(std::string param_name);
-  int64_t get_int(std::string param_name);
-  std::string get_string(std::string param_name);
-
-  void declare_param(std::string param_name, double value);
-  void declare_param(std::string param_name, bool value);
-  void declare_int(std::string param_name, int64_t value);  
-  void declare_param(std::string param_name, std::string value);
-
-  /**
-   * This sets the parameters with the values in the params_ object from the supplied parameter file, or sets them to
-   * the default if no value is given for a parameter.
-   */
-  // 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();
+  param_manager params;
 
   /**
    * Interface for control algorithm.

rosplane/include/param_manager.hpp

@@ -0,0 +1,64 @@
+/**
+ * @file param_manager.hpp
+ * 
+ * Manager for the parameters in ROSplane. Includes helper functions to call parameters
+ * 
+ * @author Jacob Moore <[email protected]>
+*/
+
+#ifndef PARAM_MANAGER_H
+#define PARAM_MANAGER_H
+
+#include <rclcpp/rclcpp.hpp>
+#include <variant>
+
+namespace rosplane
+{
+
+class param_manager
+{
+public:
+  /**
+   * Public constructor
+   * 
+   * @param node: the ROS2 node that has this parameter object. Used to poll the ROS2 parameters
+  */
+  param_manager(rclcpp::Node * node);
+
+  /**
+   * Helper functions to access parameter values stored in param_manager object
+   * Returns a std::variant that holds the value of the given parameter
+  */
+  double get_double(std::string param_name);
+  bool get_bool(std::string param_name);
+  int64_t get_int(std::string param_name);
+  std::string get_string(std::string param_name);
+
+  /**
+   * Helper functions to declare parameters in the param_manager object
+   * Inserts a parameter into the parameter object and declares it with the ROS system
+  */
+  void declare_param(std::string param_name, double value);
+  void declare_param(std::string param_name, bool value);
+  void declare_int(std::string param_name, int64_t value);  
+  void declare_param(std::string param_name, std::string value);
+
+  /**
+   * This sets the parameters with the values in the params_ object from the supplied parameter file, or sets them to
+   * the default if no value is given for a parameter.
+   */
+  // 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();
+
+private:
+  /**
+   * Data structure to hold all of the parameters
+  */
+  std::map<std::string, std::variant<double, bool, int64_t, std::string>> params_;   // Can I cast ROS int to int?
+  rclcpp::Node * container_node_;
+};
+
+
+}   // namespace rosplane
+#endif // PARAM_MANAGER_H

rosplane/src/controller_base.cpp

@@ -9,7 +9,7 @@ namespace rosplane
 {
 
 controller_base::controller_base()
-    : Node("controller_base")
+    : Node("controller_base"), params(this)
 {
 
   // Advertise published topics.
@@ -30,10 +30,10 @@ controller_base::controller_base()
   declare_parameters();
 
   // Set the values for the parameters, from the param file or use the deafault value.
-  set_parameters();
+  params.set_parameters();
 
-  bool roll_tuning_debug_override = get_bool("roll_tuning_debug_override");
-  bool pitch_tuning_debug_override = get_bool("pitch_tuning_debug_override");
+  bool roll_tuning_debug_override = params.get_bool("roll_tuning_debug_override");
+  bool pitch_tuning_debug_override = params.get_bool("pitch_tuning_debug_override");
 
   if (roll_tuning_debug_override || pitch_tuning_debug_override) {
     tuning_debug_sub_ = this->create_subscription<rosplane_msgs::msg::ControllerInternalsDebug>(
@@ -50,12 +50,12 @@ controller_base::controller_base()
 void controller_base::declare_parameters()
 {
   // Declare default parameters associated with this controller, controller_base
-  declare_param("roll_tuning_debug_override", false);
-  declare_param("pitch_tuning_debug_override", false);
-  declare_param("pwm_rad_e", 1.0);
-  declare_param("pwm_rad_a", 1.0);
-  declare_param("pwm_rad_r", 1.0);
-  declare_int("frequency", 100);
+  params.declare_param("roll_tuning_debug_override", false);
+  params.declare_param("pitch_tuning_debug_override", false);
+  params.declare_param("pwm_rad_e", 1.0);
+  params.declare_param("pwm_rad_a", 1.0);
+  params.declare_param("pwm_rad_r", 1.0);
+  params.declare_int("frequency", 100);
 }
 
 void controller_base::controller_commands_callback(
@@ -163,79 +163,6 @@ void controller_base::actuator_controls_publish()
   }
 }
 
-void controller_base::declare_param(std::string param_name, double value)
-{
-  // Insert the parameter into the parameter struct
-  params1_[param_name] = value;
-  // Declare each of the parameters, making it visible to the ROS2 param system.
-  this->declare_parameter(param_name, value);
-}
-
-void controller_base::declare_param(std::string param_name, bool value)
-{
-  // Insert the parameter into the parameter struct
-  params1_[param_name] = value;
-  // Declare each of the parameters, making it visible to the ROS2 param system.
-  this->declare_parameter(param_name, value);
-}
-
-void controller_base::declare_int(std::string param_name, int64_t value)
-{
-  // Insert the parameter into the parameter struct
-  params1_[param_name] = value;
-  // Declare each of the parameters, making it visible to the ROS2 param system.
-  this->declare_parameter(param_name, value);
-}
-
-void controller_base::declare_param(std::string param_name, std::string value)
-{
-  // Insert the parameter into the parameter struct
-  params1_[param_name] = value;
-  // Declare each of the parameters, making it visible to the ROS2 param system.
-  this->declare_parameter(param_name, value);
-}
-
-double controller_base::get_double(std::string param_name)
-{
-    return std::get<double>(params1_[param_name]);
-} 
-
-bool controller_base::get_bool(std::string param_name)
-{
-  return std::get<bool>(params1_[param_name]);
-} 
-
-int64_t controller_base::get_int(std::string param_name)
-{
-  return std::get<int64_t>(params1_[param_name]);
-} 
-
-std::string controller_base::get_string(std::string param_name)
-{
-  return std::get<std::string>(params1_[param_name]);
-} 
-
-void controller_base::set_parameters()
-{
-
-  // Get the parameters from the launch file, if given.
-  // If not, use the default value defined in the header file.
-  for (const auto& [key, value] : params1_)
-  {
-    auto type = this->get_parameter(key).get_type();
-    if (type == rclcpp::ParameterType::PARAMETER_DOUBLE)
-      params1_[key] = this->get_parameter(key).as_double();
-    else if (type == rclcpp::ParameterType::PARAMETER_BOOL)
-      params1_[key] = this->get_parameter(key).as_bool();
-    else if (type == rclcpp::ParameterType::PARAMETER_INTEGER)
-      params1_[key] = this->get_parameter(key).as_int();
-    else if (type == rclcpp::ParameterType::PARAMETER_STRING)
-      params1_[key] = this->get_parameter(key).as_string();
-    else  
-      RCLCPP_ERROR_STREAM(this->get_logger(), "Unable to set parameter: " + key + ". Error casting parameter as double, int, string, or bool!");
-  }
-}
-
 rcl_interfaces::msg::SetParametersResult 
 controller_base::parametersCallback(const std::vector<rclcpp::Parameter> & parameters)
 {
@@ -247,21 +174,21 @@ controller_base::parametersCallback(const std::vector<rclcpp::Parameter> & param
   for (const auto & param : parameters) {
 
     // Check if the parameter is in the params object or return an error
-    if (params1_.find(param.get_name()) == params1_.end()) {
+    if (params_.find(param.get_name()) == params_.end()) {
       result.successful = false;
       result.reason =
         "One of the parameters given does not is not a parameter of the controller node. Parameter: " + param.get_name();
       break;
     }
 
     if (param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
-      params1_[param.get_name()] = param.as_double();
+      params_[param.get_name()] = param.as_double();
     else if (param.get_type() == rclcpp::ParameterType::PARAMETER_BOOL)
-      params1_[param.get_name()] = param.as_bool();
+      params_[param.get_name()] = param.as_bool();
     else if (param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER)
-      params1_[param.get_name()] = param.as_int();
+      params_[param.get_name()] = param.as_int();
     else if (param.get_type() == rclcpp::ParameterType::PARAMETER_STRING)
-      params1_[param.get_name()] = param.as_string();
+      params_[param.get_name()] = param.as_string();
     else
       RCLCPP_ERROR_STREAM(this->get_logger(), "Unable to determine parameter type in controller. Type is " + std::to_string(param.get_type()));
 
@@ -273,7 +200,7 @@ controller_base::parametersCallback(const std::vector<rclcpp::Parameter> & param
 void controller_base::set_timer()
 {
 
-  int64_t frequency = get_int("frequency");
+  int64_t frequency = params.get_int("frequency");
   auto timer_period =
     std::chrono::microseconds(static_cast<long long>(1.0 / frequency * 1'000'000));
 
@@ -287,9 +214,9 @@ void controller_base::convert_to_pwm(controller_base::output_s & output)
 {
 
   // Assign parameters from parameters object
-  double pwm_rad_e = get_double("pwm_rad_e");
-  double pwm_rad_a = get_double("pwm_rad_a");
-  double pwm_rad_r = get_double("pwm_rad_r");
+  double pwm_rad_e = params.get_double("pwm_rad_e");
+  double pwm_rad_a = params.get_double("pwm_rad_a");
+  double pwm_rad_r = params.get_double("pwm_rad_r");
   // double test = get_double("frequency");
 
   // Multiply each control effort (in radians) by a scaling factor to a pwm.

rosplane/src/controller_state_machine.cpp

@@ -15,16 +15,16 @@ controller_state_machine::controller_state_machine()
   declare_parameters();
 
   // Set parameters according to the parameters in the launch file, otherwise use the default values
-  set_parameters();
+  params.set_parameters();
 }
 
 void controller_state_machine::control(const input_s & input,
                                        output_s & output)
 {
 
   // For readability, declare parameters that will be used in this controller
-  double alt_toz = get_double("alt_toz");
-  double alt_hz = get_double("alt_hz");
+  double alt_toz = params.get_double("alt_toz");
+  double alt_hz = params.get_double("alt_hz");
 
   // This state machine changes the controls used based on the zone of flight path the aircraft is currently on.
   switch (current_zone) {
@@ -96,8 +96,8 @@ void controller_state_machine::control(const input_s & input,
 void controller_state_machine::declare_parameters()
 {
   // Declare param with ROS2 and set the default value.
-  declare_param("alt_toz", 5.0);
-  declare_param("alt_hz", 10.0);
+  params.declare_param("alt_toz", 5.0);
+  params.declare_param("alt_hz", 10.0);
 }
 
 } // namespace rosplane