diff --git a/nav2_graceful_controller/src/parameter_handler.cpp b/nav2_graceful_controller/src/parameter_handler.cpp index 2b9cfdfbca..78d9c45ace 100644 --- a/nav2_graceful_controller/src/parameter_handler.cpp +++ b/nav2_graceful_controller/src/parameter_handler.cpp @@ -24,7 +24,6 @@ namespace nav2_graceful_controller { -using nav2::declare_parameter_if_not_declared; using rcl_interfaces::msg::ParameterType; ParameterHandler::ParameterHandler( @@ -33,49 +32,15 @@ ParameterHandler::ParameterHandler( : nav2_util::ParameterHandler(node, logger) { plugin_name_ = plugin_name; - declare_parameter_if_not_declared( - node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".min_lookahead", rclcpp::ParameterValue(0.25)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_lookahead", rclcpp::ParameterValue(1.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_robot_pose_search_dist", - rclcpp::ParameterValue(costmap_size_x / 2.0)); - declare_parameter_if_not_declared(node, plugin_name_ + ".k_phi", rclcpp::ParameterValue(2.0)); - declare_parameter_if_not_declared(node, plugin_name_ + ".k_delta", rclcpp::ParameterValue(1.0)); - declare_parameter_if_not_declared(node, plugin_name_ + ".beta", rclcpp::ParameterValue(0.4)); - declare_parameter_if_not_declared(node, plugin_name_ + ".lambda", rclcpp::ParameterValue(2.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".v_linear_min", rclcpp::ParameterValue(0.1)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".v_linear_max", rclcpp::ParameterValue(0.5)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".v_angular_max", rclcpp::ParameterValue(1.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".v_angular_min_in_place", rclcpp::ParameterValue(0.25)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".slowdown_radius", rclcpp::ParameterValue(1.5)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".initial_rotation", rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".initial_rotation_tolerance", rclcpp::ParameterValue(0.75)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".prefer_final_rotation", rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".rotation_scaling_factor", rclcpp::ParameterValue(0.5)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".allow_backward", rclcpp::ParameterValue(false)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".in_place_collision_resolution", rclcpp::ParameterValue(0.1)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_collision_detection", rclcpp::ParameterValue(true)); - node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance); - node->get_parameter(plugin_name_ + ".min_lookahead", params_.min_lookahead); - node->get_parameter(plugin_name_ + ".max_lookahead", params_.max_lookahead); - node->get_parameter( - plugin_name_ + ".max_robot_pose_search_dist", params_.max_robot_pose_search_dist); + params_.transform_tolerance = node->declare_or_get_parameter( + plugin_name_ + ".transform_tolerance", 0.1); + params_.min_lookahead = node->declare_or_get_parameter( + plugin_name_ + ".min_lookahead", 0.25); + params_.max_lookahead = node->declare_or_get_parameter( + plugin_name_ + ".max_lookahead", 1.0); + params_.max_robot_pose_search_dist = node->declare_or_get_parameter( + plugin_name_ + ".max_robot_pose_search_dist", costmap_size_x / 2.0); if (params_.max_robot_pose_search_dist < 0.0) { RCLCPP_WARN( logger_, "Max robot search distance is negative, setting to max to search" @@ -83,29 +48,38 @@ ParameterHandler::ParameterHandler( params_.max_robot_pose_search_dist = std::numeric_limits::max(); } - node->get_parameter(plugin_name_ + ".k_phi", params_.k_phi); - node->get_parameter(plugin_name_ + ".k_delta", params_.k_delta); - node->get_parameter(plugin_name_ + ".beta", params_.beta); - node->get_parameter(plugin_name_ + ".lambda", params_.lambda); - node->get_parameter(plugin_name_ + ".v_linear_min", params_.v_linear_min); - node->get_parameter(plugin_name_ + ".v_linear_max", params_.v_linear_max); - params_.v_linear_max_initial = params_.v_linear_max; - node->get_parameter(plugin_name_ + ".v_angular_max", params_.v_angular_max); - params_.v_angular_max_initial = params_.v_angular_max; - node->get_parameter( - plugin_name_ + ".v_angular_min_in_place", params_.v_angular_min_in_place); - node->get_parameter(plugin_name_ + ".slowdown_radius", params_.slowdown_radius); - node->get_parameter(plugin_name_ + ".initial_rotation", params_.initial_rotation); - node->get_parameter( - plugin_name_ + ".initial_rotation_tolerance", params_.initial_rotation_tolerance); - node->get_parameter(plugin_name_ + ".prefer_final_rotation", params_.prefer_final_rotation); - node->get_parameter(plugin_name_ + ".rotation_scaling_factor", params_.rotation_scaling_factor); - node->get_parameter(plugin_name_ + ".allow_backward", params_.allow_backward); - node->get_parameter( - plugin_name_ + ".in_place_collision_resolution", params_.in_place_collision_resolution); - node->get_parameter( - plugin_name_ + ".use_collision_detection", params_.use_collision_detection); - + params_.k_phi = node->declare_or_get_parameter( + plugin_name_ + ".k_phi", 2.0); + params_.k_delta = node->declare_or_get_parameter( + plugin_name_ + ".k_delta", 1.0); + params_.beta = node->declare_or_get_parameter( + plugin_name_ + ".beta", 0.4); + params_.lambda = node->declare_or_get_parameter( + plugin_name_ + ".lambda", 2.0); + params_.v_linear_min = node->declare_or_get_parameter( + plugin_name_ + ".v_linear_min", 0.1); + params_.v_linear_max = node->declare_or_get_parameter( + plugin_name_ + ".v_linear_max", 0.5); + params_.v_angular_max = node->declare_or_get_parameter( + plugin_name_ + ".v_angular_max", 1.0); + params_.v_angular_min_in_place = node->declare_or_get_parameter( + plugin_name_ + ".v_angular_min_in_place", 0.25); + params_.slowdown_radius = node->declare_or_get_parameter( + plugin_name_ + ".slowdown_radius", 1.5); + params_.initial_rotation = node->declare_or_get_parameter( + plugin_name_ + ".initial_rotation", true); + params_.initial_rotation_tolerance = node->declare_or_get_parameter( + plugin_name_ + ".initial_rotation_tolerance", 0.75); + params_.prefer_final_rotation = node->declare_or_get_parameter( + plugin_name_ + ".prefer_final_rotation", true); + params_.rotation_scaling_factor = node->declare_or_get_parameter( + plugin_name_ + ".rotation_scaling_factor", 0.5); + params_.allow_backward = node->declare_or_get_parameter( + plugin_name_ + ".allow_backward", false); + params_.in_place_collision_resolution = node->declare_or_get_parameter( + plugin_name_ + ".in_place_collision_resolution", 0.1); + params_.use_collision_detection = node->declare_or_get_parameter( + plugin_name_ + ".use_collision_detection", true); if (params_.initial_rotation && params_.allow_backward) { RCLCPP_WARN( logger_, "Initial rotation and allow backward parameters are both true, "