diff --git a/rosplane/include/param_manager.hpp b/rosplane/include/param_manager.hpp index 5f7e62b..de70e11 100644 --- a/rosplane/include/param_manager.hpp +++ b/rosplane/include/param_manager.hpp @@ -38,10 +38,10 @@ 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 @@ -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_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. diff --git a/rosplane/src/controller_base.cpp b/rosplane/src/controller_base.cpp index 2b894f0..6c944af 100644 --- a/rosplane/src/controller_base.cpp +++ b/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); } diff --git a/rosplane/src/controller_state_machine.cpp b/rosplane/src/controller_state_machine.cpp index fd3838f..aa6a73d 100644 --- a/rosplane/src/controller_state_machine.cpp +++ b/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 \ No newline at end of file diff --git a/rosplane/src/controller_successive_loop.cpp b/rosplane/src/controller_successive_loop.cpp index dcfc023..1f3c8ac 100644 --- a/rosplane/src/controller_successive_loop.cpp +++ b/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 diff --git a/rosplane/src/controller_total_energy.cpp b/rosplane/src/controller_total_energy.cpp index cc1bdd9..7db9271 100644 --- a/rosplane/src/controller_total_energy.cpp +++ b/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 diff --git a/rosplane/src/estimator_base.cpp b/rosplane/src/estimator_base.cpp index 75c54a0..7a0a762 100644 --- a/rosplane/src/estimator_base.cpp +++ b/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 diff --git a/rosplane/src/estimator_example.cpp b/rosplane/src/estimator_example.cpp index 5df0280..5634236 100644 --- a/rosplane/src/estimator_example.cpp +++ b/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 diff --git a/rosplane/src/param_manager.cpp b/rosplane/src/param_manager.cpp index add0950..2001b6b 100644 --- a/rosplane/src/param_manager.cpp +++ b/rosplane/src/param_manager.cpp @@ -5,7 +5,7 @@ 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; @@ -13,7 +13,7 @@ void param_manager::declare_param(std::string param_name, double value) 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,7 +29,7 @@ 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; @@ -37,6 +37,66 @@ void param_manager::declare_param(std::string param_name, std::string value) 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_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) diff --git a/rosplane/src/path_follower_base.cpp b/rosplane/src/path_follower_base.cpp index f957dcd..a0b219f 100644 --- a/rosplane/src/path_follower_base.cpp +++ b/rosplane/src/path_follower_base.cpp @@ -113,9 +113,9 @@ path_follower_base::parametersCallback(const std::vector & 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 diff --git a/rosplane/src/path_manager_example.cpp b/rosplane/src/path_manager_example.cpp index 7e9cc10..302cfd5 100644 --- a/rosplane/src/path_manager_example.cpp +++ b/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)