diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/options.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/options.hpp index 6c1f3fb33e4..17a7971daed 100644 --- a/nav2_constrained_smoother/include/nav2_constrained_smoother/options.hpp +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/options.hpp @@ -49,34 +49,19 @@ struct SmootherParams std::string local_name = name + std::string("."); // Smoother params - double minimum_turning_radius; - nav2::declare_parameter_if_not_declared( - node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.4)); - node->get_parameter(name + ".minimum_turning_radius", minimum_turning_radius); + double minimum_turning_radius = node->declare_or_get_parameter( + name + ".minimum_turning_radius", 0.4); max_curvature = 1.0f / minimum_turning_radius; - nav2::declare_parameter_if_not_declared( - node, local_name + "w_curve", rclcpp::ParameterValue(30.0)); - node->get_parameter(local_name + "w_curve", curvature_weight); - nav2::declare_parameter_if_not_declared( - node, local_name + "w_cost", rclcpp::ParameterValue(0.015)); - node->get_parameter(local_name + "w_cost", costmap_weight); - double cost_cusp_multiplier; - nav2::declare_parameter_if_not_declared( - node, local_name + "w_cost_cusp_multiplier", rclcpp::ParameterValue(3.0)); - node->get_parameter(local_name + "w_cost_cusp_multiplier", cost_cusp_multiplier); + curvature_weight = node->declare_or_get_parameter(local_name + "w_curve", 30.0); + costmap_weight = node->declare_or_get_parameter(local_name + "w_cost", 0.015); + double cost_cusp_multiplier = node->declare_or_get_parameter( + local_name + "w_cost_cusp_multiplier", 3.0); cusp_costmap_weight = costmap_weight * cost_cusp_multiplier; - nav2::declare_parameter_if_not_declared( - node, local_name + "cusp_zone_length", rclcpp::ParameterValue(2.5)); - node->get_parameter(local_name + "cusp_zone_length", cusp_zone_length); - nav2::declare_parameter_if_not_declared( - node, local_name + "w_dist", rclcpp::ParameterValue(0.0)); - node->get_parameter(local_name + "w_dist", distance_weight); - nav2::declare_parameter_if_not_declared( - node, local_name + "w_smooth", rclcpp::ParameterValue(2000000.0)); - node->get_parameter(local_name + "w_smooth", smooth_weight); - nav2::declare_parameter_if_not_declared( - node, local_name + "cost_check_points", rclcpp::ParameterValue(std::vector())); - node->get_parameter(local_name + "cost_check_points", cost_check_points); + cusp_zone_length = node->declare_or_get_parameter(local_name + "cusp_zone_length", 2.5); + distance_weight = node->declare_or_get_parameter(local_name + "w_dist", 0.0); + smooth_weight = node->declare_or_get_parameter(local_name + "w_smooth", 2000000.0); + cost_check_points = node->declare_or_get_parameter( + local_name + "cost_check_points", std::vector()); if (cost_check_points.size() % 3 != 0) { RCLCPP_ERROR( rclcpp::get_logger( @@ -93,21 +78,15 @@ struct SmootherParams for (size_t i = 2u; i < cost_check_points.size(); i += 3) { cost_check_points[i] /= check_point_weights_sum; } - nav2::declare_parameter_if_not_declared( - node, local_name + "path_downsampling_factor", rclcpp::ParameterValue(1)); - node->get_parameter(local_name + "path_downsampling_factor", path_downsampling_factor); - nav2::declare_parameter_if_not_declared( - node, local_name + "path_upsampling_factor", rclcpp::ParameterValue(1)); - node->get_parameter(local_name + "path_upsampling_factor", path_upsampling_factor); - nav2::declare_parameter_if_not_declared( - node, local_name + "reversing_enabled", rclcpp::ParameterValue(true)); - node->get_parameter(local_name + "reversing_enabled", reversing_enabled); - nav2::declare_parameter_if_not_declared( - node, local_name + "keep_goal_orientation", rclcpp::ParameterValue(true)); - node->get_parameter(local_name + "keep_goal_orientation", keep_goal_orientation); - nav2::declare_parameter_if_not_declared( - node, local_name + "keep_start_orientation", rclcpp::ParameterValue(true)); - node->get_parameter(local_name + "keep_start_orientation", keep_start_orientation); + path_downsampling_factor = node->declare_or_get_parameter( + local_name + "path_downsampling_factor", 1); + path_upsampling_factor = node->declare_or_get_parameter( + local_name + "path_upsampling_factor", 1); + reversing_enabled = node->declare_or_get_parameter(local_name + "reversing_enabled", true); + keep_goal_orientation = node->declare_or_get_parameter( + local_name + "keep_goal_orientation", true); + keep_start_orientation = node->declare_or_get_parameter( + local_name + "keep_start_orientation", true); } double smooth_weight{0.0}; @@ -151,9 +130,8 @@ struct OptimizerParams std::string local_name = name + std::string(".optimizer."); // Optimizer params - nav2::declare_parameter_if_not_declared( - node, local_name + "linear_solver_type", rclcpp::ParameterValue("SPARSE_NORMAL_CHOLESKY")); - node->get_parameter(local_name + "linear_solver_type", linear_solver_type); + linear_solver_type = node->declare_or_get_parameter( + local_name + "linear_solver_type", std::string("SPARSE_NORMAL_CHOLESKY")); if (solver_types.find(linear_solver_type) == solver_types.end()) { std::stringstream valid_types_str; for (auto type = solver_types.begin(); type != solver_types.end(); type++) { @@ -167,21 +145,11 @@ struct OptimizerParams "Invalid linear_solver_type. Valid values are %s", valid_types_str.str().c_str()); throw std::runtime_error("Invalid parameter: linear_solver_type"); } - nav2::declare_parameter_if_not_declared( - node, local_name + "param_tol", rclcpp::ParameterValue(1e-15)); - node->get_parameter(local_name + "param_tol", param_tol); - nav2::declare_parameter_if_not_declared( - node, local_name + "fn_tol", rclcpp::ParameterValue(1e-7)); - node->get_parameter(local_name + "fn_tol", fn_tol); - nav2::declare_parameter_if_not_declared( - node, local_name + "gradient_tol", rclcpp::ParameterValue(1e-10)); - node->get_parameter(local_name + "gradient_tol", gradient_tol); - nav2::declare_parameter_if_not_declared( - node, local_name + "max_iterations", rclcpp::ParameterValue(100)); - node->get_parameter(local_name + "max_iterations", max_iterations); - nav2::declare_parameter_if_not_declared( - node, local_name + "debug_optimizer", rclcpp::ParameterValue(false)); - node->get_parameter(local_name + "debug_optimizer", debug); + param_tol = node->declare_or_get_parameter(local_name + "param_tol", 1e-15); + fn_tol = node->declare_or_get_parameter(local_name + "fn_tol", 1e-7); + gradient_tol = node->declare_or_get_parameter(local_name + "gradient_tol", 1e-10); + max_iterations = node->declare_or_get_parameter(local_name + "max_iterations", 100); + debug = node->declare_or_get_parameter(local_name + "debug_optimizer", false); } const std::map solver_types = { diff --git a/nav2_constrained_smoother/src/constrained_smoother.cpp b/nav2_constrained_smoother/src/constrained_smoother.cpp index 03ab36ec3ce..5f96cf6d9d2 100644 --- a/nav2_constrained_smoother/src/constrained_smoother.cpp +++ b/nav2_constrained_smoother/src/constrained_smoother.cpp @@ -32,7 +32,6 @@ #include "tf2/utils.hpp" -using nav2::declare_parameter_if_not_declared; using nav2_util::geometry_utils::euclidean_distance; using namespace nav2_costmap_2d; // NOLINT