Set individual params in manager

rosplane/include/param_manager.hpp

@@ -38,18 +38,27 @@ class param_manager
    * 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_double(std::string param_name, double value);
+  void declare_bool(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);
+  void declare_string(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();    
+  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_double(std::string param_name, double value);
+  void set_bool(std::string param_name, bool value);
+  void set_int(std::string param_name, int64_t value);
+  void set_string(std::string param_name, std::string value);
 
   /**
    * This function should be called in the parametersCallback function in a containing ROS node.

rosplane/src/controller_base.cpp

@@ -41,11 +41,11 @@ controller_base::controller_base()
 void controller_base::declare_parameters()
 {
   // Declare default parameters associated with this controller, controller_base
-  params.declare_param("roll_command_override", false);
-  params.declare_param("pitch_command_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_bool("roll_command_override", false);
+  params.declare_bool("pitch_command_override", false);
+  params.declare_double("pwm_rad_e", 1.0);
+  params.declare_double("pwm_rad_a", 1.0);
+  params.declare_double("pwm_rad_r", 1.0);
   params.declare_int("frequency", 100);
 }
 

rosplane/src/controller_state_machine.cpp

@@ -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.
-  params.declare_param("alt_toz", 5.0);
-  params.declare_param("alt_hz", 10.0);
+  params.declare_double("alt_toz", 5.0);
+  params.declare_double("alt_hz", 10.0);
 }
 
 } // namespace rosplane

rosplane/src/controller_successive_loop.cpp

@@ -447,39 +447,39 @@ float controller_successive_loop::adjust_h_c(float h_c, float h, float max_diff)
 void controller_successive_loop::declare_parameters()
 {
   // Declare param with ROS2 and set the default value.
-  params.declare_param("max_takeoff_throttle", 0.55);
-  params.declare_param("c_kp", 2.37);
-  params.declare_param("c_ki", .4);
-  params.declare_param("c_kd", .0);
-  params.declare_param("max_roll", 25.0);
-  params.declare_param("cmd_takeoff_pitch", 5.0);
-
-  params.declare_param("r_kp", .06);
-  params.declare_param("r_ki", .0);
-  params.declare_param("r_kd", .04);
-  params.declare_param("max_a", .15);
-  params.declare_param("trim_a", 0.0);
-
-  params.declare_param("p_kp", -.15);
-  params.declare_param("p_ki", .0);
-  params.declare_param("p_kd", -.05);
-  params.declare_param("max_e", .15);
-  params.declare_param("max_pitch", 20.0);
-  params.declare_param("trim_e", 0.02);
-
-  params.declare_param("tau", 50.0);
-  params.declare_param("a_t_kp", .05);
-  params.declare_param("a_t_ki", .005);
-  params.declare_param("a_t_kd", 0.0);
-  params.declare_param("max_t", 1.0);
-  params.declare_param("trim_t", 0.5);
-
-  params.declare_param("a_kp", 0.015);
-  params.declare_param("a_ki", 0.003);
-  params.declare_param("a_kd", 0.0);
-
-  params.declare_param("y_pwo", .6349);
-  params.declare_param("y_kr", .85137);
+  params.declare_double("max_takeoff_throttle", 0.55);
+  params.declare_double("c_kp", 2.37);
+  params.declare_double("c_ki", .4);
+  params.declare_double("c_kd", .0);
+  params.declare_double("max_roll", 25.0);
+  params.declare_double("cmd_takeoff_pitch", 5.0);
+
+  params.declare_double("r_kp", .06);
+  params.declare_double("r_ki", .0);
+  params.declare_double("r_kd", .04);
+  params.declare_double("max_a", .15);
+  params.declare_double("trim_a", 0.0);
+
+  params.declare_double("p_kp", -.15);
+  params.declare_double("p_ki", .0);
+  params.declare_double("p_kd", -.05);
+  params.declare_double("max_e", .15);
+  params.declare_double("max_pitch", 20.0);
+  params.declare_double("trim_e", 0.02);
+
+  params.declare_double("tau", 50.0);
+  params.declare_double("a_t_kp", .05);
+  params.declare_double("a_t_ki", .005);
+  params.declare_double("a_t_kd", 0.0);
+  params.declare_double("max_t", 1.0);
+  params.declare_double("trim_t", 0.5);
+
+  params.declare_double("a_kp", 0.015);
+  params.declare_double("a_ki", 0.003);
+  params.declare_double("a_kd", 0.0);
+
+  params.declare_double("y_pwo", .6349);
+  params.declare_double("y_kr", .85137);
 }
 
 } // namespace rosplane

rosplane/src/controller_total_energy.cpp

@@ -170,16 +170,16 @@ void controller_total_energy::update_energies(float va_c, float va, float h_c, f
 void controller_total_energy::declare_parameters()
 {
   // Declare parameter with ROS2 and set the default value
-  params.declare_param("e_kp", 5.0);
-  params.declare_param("e_ki", 0.9);
-  params.declare_param("e_kd", 0.0);
+  params.declare_double("e_kp", 5.0);
+  params.declare_double("e_ki", 0.9);
+  params.declare_double("e_kd", 0.0);
 
-  params.declare_param("l_kp", 1.0);
-  params.declare_param("l_ki", 0.05);
-  params.declare_param("l_kd", 0.0);
+  params.declare_double("l_kp", 1.0);
+  params.declare_double("l_ki", 0.05);
+  params.declare_double("l_kd", 0.0);
 
-  params.declare_param("mass", 2.28);
-  params.declare_param("gravity", 9.8);
-  params.declare_param("max_energy", 5.0);
+  params.declare_double("mass", 2.28);
+  params.declare_double("gravity", 9.8);
+  params.declare_double("max_energy", 5.0);
 }
 } // namespace rosplane

rosplane/src/estimator_base.cpp

@@ -41,8 +41,8 @@ estimator_base::estimator_base()
 
 void estimator_base::declare_parameters()
 {
-  params.declare_param("rho", 1.225);
-  params.declare_param("gravity", 9.8);
+  params.declare_double("rho", 1.225);
+  params.declare_double("gravity", 9.8);
 }
 
 rcl_interfaces::msg::SetParametersResult 

rosplane/src/estimator_example.cpp

@@ -458,16 +458,16 @@ void estimator_example::check_xhat_a()
 
 void estimator_example::declare_parameters()
 {
-  params.declare_param("sigma_n_gps", .01);
-  params.declare_param("sigma_e_gps", .01);
-  params.declare_param("sigma_Vg_gps", .005);
-  params.declare_param("sigma_course_gps", .005 / 20);
-  params.declare_param("sigma_accel", .0025 * 9.81);
+  params.declare_double("sigma_n_gps", .01);
+  params.declare_double("sigma_e_gps", .01);
+  params.declare_double("sigma_Vg_gps", .005);
+  params.declare_double("sigma_course_gps", .005 / 20);
+  params.declare_double("sigma_accel", .0025 * 9.81);
   params.declare_int("frequency", 100);
-  params.declare_param("lpf_a", 50.0);
-  params.declare_param("lpf_a1", 8.0);
-  params.declare_param("gps_n_lim", 10000.);
-  params.declare_param("gps_e_lim", 10000.);
+  params.declare_double("lpf_a", 50.0);
+  params.declare_double("lpf_a1", 8.0);
+  params.declare_double("gps_n_lim", 10000.);
+  params.declare_double("gps_e_lim", 10000.);
 }
 
 } // namespace rosplane

rosplane/src/param_manager.cpp

@@ -5,15 +5,15 @@ namespace rosplane
 
 param_manager::param_manager(rclcpp::Node * node) : container_node_{node} {}
 
-void param_manager::declare_param(std::string param_name, double value)
+void param_manager::declare_double(std::string param_name, double value)
 {
   // Insert the parameter into the parameter struct
   params_[param_name] = value;
   // Declare each of the parameters, making it visible to the ROS2 param system.
   container_node_->declare_parameter(param_name, value);
 }
 
-void param_manager::declare_param(std::string param_name, bool value)
+void param_manager::declare_bool(std::string param_name, bool value)
 {
   // Insert the parameter into the parameter struct
   params_[param_name] = value;
@@ -29,14 +29,74 @@ void param_manager::declare_int(std::string param_name, int64_t value)
   container_node_->declare_parameter(param_name, value);
 }
 
-void param_manager::declare_param(std::string param_name, std::string value)
+void param_manager::declare_string(std::string param_name, std::string value)
 {
   // Insert the parameter into the parameter struct
   params_[param_name] = value;
   // Declare each of the parameters, making it visible to the ROS2 param system.
   container_node_->declare_parameter(param_name, value);
 }
 
+void param_manager::set_double(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_bool(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_string(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
@@ -46,7 +106,7 @@ double param_manager::get_double(std::string param_name)
     catch (std::bad_variant_access & e)
     {
       RCLCPP_ERROR_STREAM(container_node_->get_logger(), "ERROR GETTING PARAMETER: " + param_name);
-      throw std::runtime_error("error");
+      throw std::runtime_error(e.what());
     }
 } 
 
@@ -59,7 +119,7 @@ bool param_manager::get_bool(std::string param_name)
     catch (std::bad_variant_access & e)
     {
       RCLCPP_ERROR_STREAM(container_node_->get_logger(), "ERROR GETTING PARAMETER: " + param_name);
-      throw std::runtime_error("error");
+      throw std::runtime_error(e.what());
     }
 } 
 
@@ -72,7 +132,7 @@ int64_t param_manager::get_int(std::string param_name)
     catch (std::bad_variant_access & e)
     {
       RCLCPP_ERROR_STREAM(container_node_->get_logger(), "ERROR GETTING PARAMETER: " + param_name);
-      throw std::runtime_error("error");
+      throw std::runtime_error(e.what());
     }
 } 
 
@@ -85,7 +145,7 @@ std::string param_manager::get_string(std::string param_name)
     catch (std::bad_variant_access & e)
     {
       RCLCPP_ERROR_STREAM(container_node_->get_logger(), "ERROR GETTING PARAMETER: " + param_name);
-      throw std::runtime_error("error");
+      throw std::runtime_error(e.what());
     }
 } 
 
@@ -120,7 +180,7 @@ bool param_manager::set_parameters_callback(const std::vector<rclcpp::Parameter>
       RCLCPP_ERROR_STREAM(container_node_->get_logger(), "One of the parameters given does not is not a parameter of the controller node. Parameter: " + param.get_name());
       return false;
     }
-    
+
     if (param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
       params_[param.get_name()] = param.as_double();
     else if (param.get_type() == rclcpp::ParameterType::PARAMETER_BOOL)

rosplane/src/path_follower_base.cpp

@@ -113,9 +113,9 @@ path_follower_base::parametersCallback(const std::vector<rclcpp::Parameter> & pa
 
 void path_follower_base::declare_parameters()
 {
-  params.declare_param("chi_infty", .5);
-  params.declare_param("k_path", 0.05);
-  params.declare_param("k_orbit", 4.0);
+  params.declare_double("chi_infty", .5);
+  params.declare_double("k_path", 0.05);
+  params.declare_double("k_orbit", 4.0);
 }
 
 } // namespace rosplane

rosplane/src/path_manager_example.cpp

@@ -564,10 +564,10 @@ void path_manager_example::dubinsParameters(const waypoint_s start_node, const w
 
 void path_manager_example::declare_parameters()
 {
-  params.declare_param("R_min", 25.0);
-  params.declare_param("orbit_last", false);
-  params.declare_param("default_altitude", 50.0);
-  params.declare_param("default_airspeed", 15.0);
+  params.declare_double("R_min", 25.0);
+  params.declare_bool("orbit_last", false);
+  params.declare_double("default_altitude", 50.0);
+  params.declare_double("default_airspeed", 15.0);
 }
 
 int path_manager_example::orbit_direction(float pn, float pe, float chi, float c_n, float c_e)