diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 5a89a608a4..0396664f8e 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -103,12 +103,12 @@ planner_server: GridBased: plugin: "nav2_smac_planner/SmacPlannerHybrid" - tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters, for 2D node + tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters downsample_costmap: false # whether or not to downsample the map downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) allow_unknown: false # allow traveling in unknown space max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance, 2D only + max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance max_planning_time: 3.5 # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning. motion_model_for_search: "DUBIN" # For Hybrid Dubin, Redds-Shepp cost_travel_multiplier: 2.0 # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index 51f6608b5f..c782fe41e0 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -111,6 +111,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner unsigned int _angle_quantizations; bool _allow_unknown; int _max_iterations; + int _max_on_approach_iterations; SearchInfo _search_info; double _max_planning_time; double _lookup_table_size; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp index c34204b18e..f5207cd45c 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp @@ -106,6 +106,7 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner SearchInfo _search_info; bool _allow_unknown; int _max_iterations; + int _max_on_approach_iterations; float _tolerance; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; double _max_planning_time; diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index b6a56271a9..06bc0520d4 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -326,6 +326,11 @@ bool AStarAlgorithm::createPath( } } + if (_best_heuristic_node.first < getToleranceHeuristic()) { + // If we run out of serach options, return the path that is closest, if within tolerance. + return _graph.at(_best_heuristic_node.second).backtracePath(path); + } + return false; } diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 279af1804c..ee614c7b94 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -80,12 +80,18 @@ void SmacPlannerHybrid::configure( _angle_bin_size = 2.0 * M_PI / angle_quantizations; _angle_quantizations = static_cast(angle_quantizations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".tolerance", rclcpp::ParameterValue(0.25)); + _tolerance = static_cast(node->get_parameter(name + ".tolerance").as_double()); nav2_util::declare_parameter_if_not_declared( node, name + ".allow_unknown", rclcpp::ParameterValue(true)); node->get_parameter(name + ".allow_unknown", _allow_unknown); nav2_util::declare_parameter_if_not_declared( node, name + ".max_iterations", rclcpp::ParameterValue(1000000)); node->get_parameter(name + ".max_iterations", _max_iterations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000)); + node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations); nav2_util::declare_parameter_if_not_declared( node, name + ".smooth_path", rclcpp::ParameterValue(true)); node->get_parameter(name + ".smooth_path", smooth_path); @@ -139,6 +145,13 @@ void SmacPlannerHybrid::configure( _motion_model_for_search.c_str()); } + if (_max_on_approach_iterations <= 0) { + RCLCPP_INFO( + _logger, "On approach iteration selected as <= 0, " + "disabling tolerance and on approach iterations."); + _max_on_approach_iterations = std::numeric_limits::max(); + } + if (_max_iterations <= 0) { RCLCPP_INFO( _logger, "maximum iteration selected as <= 0, " @@ -180,7 +193,7 @@ void SmacPlannerHybrid::configure( _a_star->initialize( _allow_unknown, _max_iterations, - std::numeric_limits::max(), + _max_on_approach_iterations, _max_planning_time, _lookup_table_dim, _angle_quantizations); @@ -205,10 +218,11 @@ void SmacPlannerHybrid::configure( RCLCPP_INFO( _logger, "Configured plugin %s of type SmacPlannerHybrid with " - "maximum iterations %i, and %s. Using motion model: %s.", - _name.c_str(), _max_iterations, + "maximum iterations %i, max on approach iterations %i, and %s. Tolerance %.2f." + "Using motion model: %s.", + _name.c_str(), _max_iterations, _max_on_approach_iterations, _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", - toString(_motion_model).c_str()); + _tolerance, toString(_motion_model).c_str()); } void SmacPlannerHybrid::activate() @@ -315,7 +329,9 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( int num_iterations = 0; std::string error; try { - if (!_a_star->createPath(path, num_iterations, 0.0)) { + if (!_a_star->createPath( + path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) + { if (num_iterations < _a_star->getMaxIterations()) { error = std::string("no valid path found"); } else { @@ -392,6 +408,8 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para if (name == _name + ".max_planning_time") { reinit_a_star = true; _max_planning_time = parameter.as_double(); + } else if (name == _name + ".tolerance") { + _tolerance = static_cast(parameter.as_double()); } else if (name == _name + ".lookup_table_size") { reinit_a_star = true; _lookup_table_size = parameter.as_double(); @@ -452,6 +470,15 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para "disabling maximum iterations."); _max_iterations = std::numeric_limits::max(); } + } else if (name == _name + ".max_on_approach_iterations") { + reinit_a_star = true; + _max_on_approach_iterations = parameter.as_int(); + if (_max_on_approach_iterations <= 0) { + RCLCPP_INFO( + _logger, "On approach iteration selected as <= 0, " + "disabling tolerance and on approach iterations."); + _max_on_approach_iterations = std::numeric_limits::max(); + } } else if (name == _name + ".angle_quantization_bins") { reinit_collision_checker = true; reinit_a_star = true; @@ -504,7 +531,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para _a_star->initialize( _allow_unknown, _max_iterations, - std::numeric_limits::max(), + _max_on_approach_iterations, _max_planning_time, _lookup_table_dim, _angle_quantizations); diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 09a3591928..b7e22ee1f9 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -65,12 +65,18 @@ void SmacPlannerLattice::configure( double analytic_expansion_max_length_m; bool smooth_path; + nav2_util::declare_parameter_if_not_declared( + node, name + ".tolerance", rclcpp::ParameterValue(0.25)); + _tolerance = static_cast(node->get_parameter(name + ".tolerance").as_double()); nav2_util::declare_parameter_if_not_declared( node, name + ".allow_unknown", rclcpp::ParameterValue(true)); node->get_parameter(name + ".allow_unknown", _allow_unknown); nav2_util::declare_parameter_if_not_declared( node, name + ".max_iterations", rclcpp::ParameterValue(1000000)); node->get_parameter(name + ".max_iterations", _max_iterations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000)); + node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations); nav2_util::declare_parameter_if_not_declared( node, name + ".smooth_path", rclcpp::ParameterValue(true)); node->get_parameter(name + ".smooth_path", smooth_path); @@ -126,6 +132,13 @@ void SmacPlannerLattice::configure( _metadata.min_turning_radius / (_costmap->getResolution()); _motion_model = MotionModel::STATE_LATTICE; + if (_max_on_approach_iterations <= 0) { + RCLCPP_INFO( + _logger, "On approach iteration selected as <= 0, " + "disabling tolerance and on approach iterations."); + _max_on_approach_iterations = std::numeric_limits::max(); + } + if (_max_iterations <= 0) { RCLCPP_INFO( _logger, "maximum iteration selected as <= 0, " @@ -167,7 +180,7 @@ void SmacPlannerLattice::configure( _a_star->initialize( _allow_unknown, _max_iterations, - std::numeric_limits::max(), + _max_on_approach_iterations, _max_planning_time, lookup_table_dim, _metadata.number_of_headings); @@ -182,11 +195,11 @@ void SmacPlannerLattice::configure( RCLCPP_INFO( _logger, "Configured plugin %s of type SmacPlannerLattice with " - "maximum iterations %i, " - "and %s. Using motion model: %s. State lattice file: %s.", - _name.c_str(), _max_iterations, + "maximum iterations %i, max on approach iterations %i, " + "and %s. Tolerance %.2f. Using motion model: %s. State lattice file: %s.", + _name.c_str(), _max_iterations, _max_on_approach_iterations, _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", - toString(_motion_model).c_str(), _search_info.lattice_filepath.c_str()); + _tolerance, toString(_motion_model).c_str(), _search_info.lattice_filepath.c_str()); } void SmacPlannerLattice::activate() @@ -262,7 +275,9 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( int num_iterations = 0; std::string error; try { - if (!_a_star->createPath(path, num_iterations, 0 /*no tolerance*/)) { + if (!_a_star->createPath( + path, num_iterations, _tolerance / static_cast(_costmap->getResolution()))) + { if (num_iterations < _a_star->getMaxIterations()) { error = std::string("no valid path found"); } else { @@ -349,6 +364,8 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par if (name == _name + ".max_planning_time") { reinit_a_star = true; _max_planning_time = parameter.as_double(); + } else if (name == _name + ".tolerance") { + _tolerance = static_cast(parameter.as_double()); } else if (name == _name + ".lookup_table_size") { reinit_a_star = true; _lookup_table_size = parameter.as_double(); @@ -403,6 +420,15 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par _max_iterations = std::numeric_limits::max(); } } + } else if (name == _name + ".max_on_approach_iterations") { + reinit_a_star = true; + _max_on_approach_iterations = parameter.as_int(); + if (_max_on_approach_iterations <= 0) { + RCLCPP_INFO( + _logger, "On approach iteration selected as <= 0, " + "disabling tolerance and on approach iterations."); + _max_on_approach_iterations = std::numeric_limits::max(); + } } else if (type == ParameterType::PARAMETER_STRING) { if (name == _name + ".lattice_filepath") { reinit_a_star = true; @@ -453,7 +479,7 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par _a_star->initialize( _allow_unknown, _max_iterations, - std::numeric_limits::max(), + _max_on_approach_iterations, _max_planning_time, lookup_table_dim, _metadata.number_of_headings); diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp index 3eb077cfb0..3be1a9e897 100644 --- a/nav2_smac_planner/test/test_smac_hybrid.cpp +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -110,12 +110,14 @@ TEST(SmacTest, test_smac_se2_reconfigure) rclcpp::Parameter("test.change_penalty", 1.0), rclcpp::Parameter("test.non_straight_penalty", 2.0), rclcpp::Parameter("test.cost_penalty", 2.0), + rclcpp::Parameter("test.tolerance", 0.2), rclcpp::Parameter("test.retrospective_penalty", 0.2), rclcpp::Parameter("test.analytic_expansion_ratio", 4.0), rclcpp::Parameter("test.max_planning_time", 10.0), rclcpp::Parameter("test.lookup_table_size", 30.0), rclcpp::Parameter("test.smooth_path", false), rclcpp::Parameter("test.analytic_expansion_max_length", 42.0), + rclcpp::Parameter("test.max_on_approach_iterations", 42), rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP"))}); rclcpp::spin_until_future_complete( @@ -134,11 +136,13 @@ TEST(SmacTest, test_smac_se2_reconfigure) EXPECT_EQ(nodeSE2->get_parameter("test.non_straight_penalty").as_double(), 2.0); EXPECT_EQ(nodeSE2->get_parameter("test.cost_penalty").as_double(), 2.0); EXPECT_EQ(nodeSE2->get_parameter("test.retrospective_penalty").as_double(), 0.2); + EXPECT_EQ(nodeSE2->get_parameter("test.tolerance").as_double(), 0.2); EXPECT_EQ(nodeSE2->get_parameter("test.analytic_expansion_ratio").as_double(), 4.0); EXPECT_EQ(nodeSE2->get_parameter("test.smooth_path").as_bool(), false); EXPECT_EQ(nodeSE2->get_parameter("test.max_planning_time").as_double(), 10.0); EXPECT_EQ(nodeSE2->get_parameter("test.lookup_table_size").as_double(), 30.0); EXPECT_EQ(nodeSE2->get_parameter("test.analytic_expansion_max_length").as_double(), 42.0); + EXPECT_EQ(nodeSE2->get_parameter("test.max_on_approach_iterations").as_int(), 42); EXPECT_EQ( nodeSE2->get_parameter("test.motion_model_for_search").as_string(), std::string("REEDS_SHEPP")); diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp index ff813f08c1..9ce99a46b1 100644 --- a/nav2_smac_planner/test/test_smac_lattice.cpp +++ b/nav2_smac_planner/test/test_smac_lattice.cpp @@ -124,7 +124,9 @@ TEST(SmacTest, test_smac_lattice_reconfigure) rclcpp::Parameter("test.lookup_table_size", 30.0), rclcpp::Parameter("test.smooth_path", false), rclcpp::Parameter("test.analytic_expansion_max_length", 42.0), + rclcpp::Parameter("test.tolerance", 42.0), rclcpp::Parameter("test.rotation_penalty", 42.0), + rclcpp::Parameter("test.max_on_approach_iterations", 42), rclcpp::Parameter("test.allow_reverse_expansion", true)}); try {