From e36ba62c13e00d7e3b2ef1e1e4c14ab26edcbda5 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Mon, 17 Jun 2024 13:02:56 -0600 Subject: [PATCH 1/3] Renamed declare function names to include datatype I found that when using the declare_param function with the string datatype, unless the value was explicitly instantiated as a string (std::string("value") instead of "value"), the value would get cast to a bool --- rosplane/include/param_manager.hpp | 6 +- rosplane/src/controller_base.cpp | 10 ++-- rosplane/src/controller_state_machine.cpp | 4 +- rosplane/src/controller_successive_loop.cpp | 66 ++++++++++----------- rosplane/src/controller_total_energy.cpp | 18 +++--- rosplane/src/estimator_base.cpp | 4 +- rosplane/src/estimator_example.cpp | 18 +++--- rosplane/src/param_manager.cpp | 16 ++--- rosplane/src/path_follower_base.cpp | 6 +- rosplane/src/path_manager_example.cpp | 8 +-- 10 files changed, 78 insertions(+), 78 deletions(-) diff --git a/rosplane/include/param_manager.hpp b/rosplane/include/param_manager.hpp index 5f7e62b..c57f0df 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 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..6f8dc1d 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; @@ -46,7 +46,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 +59,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 +72,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 +85,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 +120,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) From d42d840d81c6164bc010f3044dd37c89dd2ef73c Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Mon, 17 Jun 2024 13:03:17 -0600 Subject: [PATCH 2/3] 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 From cafa745de516fdd0aecacda6708eec8abb20785e Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Mon, 17 Jun 2024 13:16:43 -0600 Subject: [PATCH 3/3] renamed set functions to include datatypes in name --- rosplane/include/param_manager.hpp | 6 +++--- rosplane/src/param_manager.cpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rosplane/include/param_manager.hpp b/rosplane/include/param_manager.hpp index 27d4279..de70e11 100644 --- a/rosplane/include/param_manager.hpp +++ b/rosplane/include/param_manager.hpp @@ -55,10 +55,10 @@ class param_manager * 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_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_parameter(std::string param_name, std::string 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/param_manager.cpp b/rosplane/src/param_manager.cpp index 2b7ac29..2001b6b 100644 --- a/rosplane/src/param_manager.cpp +++ b/rosplane/src/param_manager.cpp @@ -37,7 +37,7 @@ 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) +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()) @@ -52,7 +52,7 @@ void param_manager::set_parameter(std::string param_name, double value) container_node_->set_parameter(rclcpp::Parameter(param_name, value)); } -void param_manager::set_parameter(std::string param_name, bool 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()) @@ -82,7 +82,7 @@ void param_manager::set_int(std::string param_name, int64_t value) container_node_->set_parameter(rclcpp::Parameter(param_name, value)); } -void param_manager::set_parameter(std::string param_name, std::string 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())