Adding Smac Planner Tolerances for Feasible Planners

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*.

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;

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<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;
  double _max_planning_time;

nav2_smac_planner/src/a_star.cpp

@@ -326,6 +326,11 @@ bool AStarAlgorithm<NodeT>::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;
 }
 

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<unsigned int>(angle_quantizations);
 
+ nav2_util::declare_parameter_if_not_declared(
+ node, name + ".tolerance", rclcpp::ParameterValue(0.25));
+ _tolerance = static_cast<float>(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<int>::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<int>::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<float>(costmap->getResolution())))
+ {
  if (num_iterations < _a_star->getMaxIterations()) {
  error = std::string("no valid path found");
  } else {
@@ -392,6 +408,8 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> 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<float>(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<rclcpp::Parameter> para
  "disabling maximum iterations.");
  _max_iterations = std::numeric_limits<int>::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<int>::max();
+ }
  } else if (name == _name + ".angle_quantization_bins") {
  reinit_collision_checker = true;
  reinit_a_star = true;
@@ -504,7 +531,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
  _a_star->initialize(
  _allow_unknown,
  _max_iterations,
- std::numeric_limits<int>::max(),
+ _max_on_approach_iterations,
  _max_planning_time,
  _lookup_table_dim,
  _angle_quantizations);

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<float>(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<int>::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<int>::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<float>(_costmap->getResolution())))
+ {
  if (num_iterations < _a_star->getMaxIterations()) {
  error = std::string("no valid path found");
  } else {
@@ -349,6 +364,8 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector<rclcpp::Parameter> 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<float>(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<rclcpp::Parameter> par
  _max_iterations = std::numeric_limits<int>::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<int>::max();
+ }
  } else if (type == ParameterType::PARAMETER_STRING) {
  if (name == _name + ".lattice_filepath") {
  reinit_a_star = true;
@@ -453,7 +479,7 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector<rclcpp::Parameter> par
  _a_star->initialize(
  _allow_unknown,
  _max_iterations,
- std::numeric_limits<int>::max(),
+ _max_on_approach_iterations,
  _max_planning_time,
  lookup_table_dim,
  _metadata.number_of_headings);

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"));

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 {