Smac Planner 2D changes (#3083)

* removing 4-connected option from smac; fixing 2D heuristic and traversal function from other planner's changes

* fix name of variable since no longer a neighborhood

* partial test updates

* ported unit tests fully

* revert to no costmap downsampling

nav2_smac_planner/README.md

@@ -3,7 +3,7 @@
 The SmacPlanner is a plugin for the Nav2 Planner server. It includes currently 3 distinct plugins:
 - `SmacPlannerHybrid`: a highly optimized fully reconfigurable Hybrid-A* implementation supporting Dubin and Reeds-Shepp models (legged, ackermann and car models).
  - `SmacPlannerLattice`: a highly optimized fully reconfigurable State Lattice implementation supporting configurable minimum control sets, with provided control sets for Ackermann, Legged, Differential and Omnidirectional models.
-- `SmacPlanner2D`: a highly optimized fully reconfigurable grid-based A* implementation supporting Moore and Von Neumann models.
+- `SmacPlanner2D`: a highly optimized fully reconfigurable grid-based A* implementation supporting 8-connected neighborhood models.
 
 It also introduces the following basic building blocks:
 - `CostmapDownsampler`: A library to take in a costmap object and downsample it to another resolution.
@@ -111,7 +111,7 @@ planner_server:
       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_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"    # 2D Moore, Von Neumann; Hybrid Dubin, Redds-Shepp; State Lattice set internally
+      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*.
       angle_quantization_bins: 64         # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search)
       analytic_expansion_ratio: 3.5       # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach.

nav2_smac_planner/include/nav2_smac_planner/constants.hpp

@@ -22,20 +22,17 @@ namespace nav2_smac_planner
 enum class MotionModel
 {
   UNKNOWN = 0,
-  VON_NEUMANN = 1,
-  MOORE = 2,
-  DUBIN = 3,
-  REEDS_SHEPP = 4,
-  STATE_LATTICE = 5,
+  TWOD = 1,
+  DUBIN = 2,
+  REEDS_SHEPP = 3,
+  STATE_LATTICE = 4,
 };
 
 inline std::string toString(const MotionModel & n)
 {
   switch (n) {
-    case MotionModel::VON_NEUMANN:
-      return "Von Neumann";
-    case MotionModel::MOORE:
-      return "Moore";
+    case MotionModel::TWOD:
+      return "2D";
     case MotionModel::DUBIN:
       return "Dubin";
     case MotionModel::REEDS_SHEPP:
@@ -49,10 +46,8 @@ inline std::string toString(const MotionModel & n)
 
 inline MotionModel fromString(const std::string & n)
 {
-  if (n == "VON_NEUMANN") {
-    return MotionModel::VON_NEUMANN;
-  } else if (n == "MOORE") {
-    return MotionModel::MOORE;
+  if (n == "2D") {
+    return MotionModel::TWOD;
   } else if (n == "DUBIN") {
     return MotionModel::DUBIN;
   } else if (n == "REEDS_SHEPP") {

nav2_smac_planner/src/node_2d.cpp

@@ -72,9 +72,10 @@ float Node2D::getTraversalCost(const NodePtr & child)
 
   // If a diagonal move, travel cost is sqrt(2) not 1.0.
   if ((dx * dx + dy * dy) > 1.05) {
-    return sqrt_2 + cost_travel_multiplier * normalized_cost;
+    return sqrt_2 * (1.0 + cost_travel_multiplier * normalized_cost);
   }
 
+  // Length = 1.0
   return 1.0 + cost_travel_multiplier * normalized_cost;
 }
 
@@ -85,34 +86,24 @@ float Node2D::getHeuristicCost(
 {
   // Using Moore distance as it more accurately represents the distances
   // even a Van Neumann neighborhood robot can navigate.
-  return fabs(goal_coordinates.x - node_coords.x) +
-         fabs(goal_coordinates.y - node_coords.y);
+  return hypotf(goal_coordinates.x - node_coords.x, goal_coordinates.y - node_coords.y);
 }
 
 void Node2D::initMotionModel(
-  const MotionModel & neighborhood,
+  const MotionModel & motion_model,
   unsigned int & x_size_uint,
   unsigned int & /*size_y*/,
   unsigned int & /*num_angle_quantization*/,
   SearchInfo & search_info)
 {
+  if (motion_model != MotionModel::TWOD) {
+    throw std::runtime_error("Invalid motion model for 2D node.");
+  }
+
   int x_size = static_cast<int>(x_size_uint);
   cost_travel_multiplier = search_info.cost_penalty;
-  switch (neighborhood) {
-    case MotionModel::UNKNOWN:
-      throw std::runtime_error("Unknown neighborhood type selected.");
-    case MotionModel::VON_NEUMANN:
-      _neighbors_grid_offsets = {-1, +1, -x_size, +x_size};
-      break;
-    case MotionModel::MOORE:
-      _neighbors_grid_offsets = {-1, +1, -x_size, +x_size, -x_size - 1,
-        -x_size + 1, +x_size - 1, +x_size + 1};
-      break;
-    default:
-      throw std::runtime_error(
-              "Invalid neighborhood type selected. "
-              "Von-Neumann and Moore are valid for Node2D.");
-  }
+  _neighbors_grid_offsets = {-1, +1, -x_size, +x_size, -x_size - 1,
+    -x_size + 1, +x_size - 1, +x_size + 1};
 }
 
 void Node2D::getNeighbors(

nav2_smac_planner/src/smac_planner_2d.cpp

@@ -91,17 +91,7 @@ void SmacPlanner2D::configure(
     node, name + ".max_planning_time", rclcpp::ParameterValue(2.0));
   node->get_parameter(name + ".max_planning_time", _max_planning_time);
 
-  nav2_util::declare_parameter_if_not_declared(
-    node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("MOORE")));
-  node->get_parameter(name + ".motion_model_for_search", _motion_model_for_search);
-  _motion_model = fromString(_motion_model_for_search);
-  if (_motion_model == MotionModel::UNKNOWN) {
-    RCLCPP_WARN(
-      _logger,
-      "Unable to get MotionModel search type. Given '%s', "
-      "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.",
-      _motion_model_for_search.c_str());
-  }
+  _motion_model = MotionModel::TWOD;
 
   if (_max_on_approach_iterations <= 0) {
     RCLCPP_INFO(
@@ -154,10 +144,9 @@ void SmacPlanner2D::configure(
   RCLCPP_INFO(
     _logger, "Configured plugin %s of type SmacPlanner2D with "
     "tolerance %.2f, maximum iterations %i, "
-    "max on approach iterations %i, and %s. Using motion model: %s.",
+    "max on approach iterations %i, and %s.",
     _name.c_str(), _tolerance, _max_iterations, _max_on_approach_iterations,
-    _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal",
-    toString(_motion_model).c_str());
+    _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal");
 }
 
 void SmacPlanner2D::activate()
@@ -392,18 +381,6 @@ SmacPlanner2D::dynamicParametersCallback(std::vector<rclcpp::Parameter> paramete
           _max_on_approach_iterations = std::numeric_limits<int>::max();
         }
       }
-    } else if (type == ParameterType::PARAMETER_STRING) {
-      if (name == _name + ".motion_model_for_search") {
-        reinit_a_star = true;
-        _motion_model = fromString(parameter.as_string());
-        if (_motion_model == MotionModel::UNKNOWN) {
-          RCLCPP_WARN(
-            _logger,
-            "Unable to get MotionModel search type. Given '%s', "
-            "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.",
-            _motion_model_for_search.c_str());
-        }
-      }
     }
   }
 

nav2_smac_planner/test/test_a_star.cpp

@@ -41,7 +41,7 @@ TEST(AStarTest, test_a_star_2d)
 {
   nav2_smac_planner::SearchInfo info;
   nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::Node2D> a_star(
-    nav2_smac_planner::MotionModel::MOORE, info);
+    nav2_smac_planner::MotionModel::TWOD, info);
   int max_iterations = 10000;
   float tolerance = 0.0;
   float some_tolerance = 20.0;
@@ -69,7 +69,7 @@ TEST(AStarTest, test_a_star_2d)
   a_star.setGoal(80u, 80u, 0);
   nav2_smac_planner::Node2D::CoordinateVector path;
   EXPECT_TRUE(a_star.createPath(path, num_it, tolerance));
-  EXPECT_EQ(num_it, 102);
+  EXPECT_EQ(num_it, 2414);
 
   // check path is the right size and collision free
   EXPECT_EQ(path.size(), 81u);
@@ -84,7 +84,7 @@ TEST(AStarTest, test_a_star_2d)
   path.clear();
   // failure cases with invalid inputs
   nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::Node2D> a_star_2(
-    nav2_smac_planner::MotionModel::VON_NEUMANN, info);
+    nav2_smac_planner::MotionModel::TWOD, info);
   a_star_2.initialize(false, max_iterations, it_on_approach, max_planning_time, 0, 1);
   num_it = 0;
   EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error);
@@ -104,7 +104,7 @@ TEST(AStarTest, test_a_star_2d)
   a_star_2.setStart(20, 20, 0);  // valid
   a_star_2.setGoal(50, 50, 0);  // invalid
   EXPECT_TRUE(a_star_2.createPath(path, num_it, some_tolerance));
-  EXPECT_EQ(path.size(), 42u);
+  EXPECT_EQ(path.size(), 20u);
   for (unsigned int i = 0; i != path.size(); i++) {
     EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0);
   }
@@ -286,19 +286,16 @@ TEST(AStarTest, test_constants)
 {
   nav2_smac_planner::MotionModel mm = nav2_smac_planner::MotionModel::UNKNOWN;  // unknown
   EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Unknown"));
-  mm = nav2_smac_planner::MotionModel::VON_NEUMANN;  // vonneumann
-  EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Von Neumann"));
-  mm = nav2_smac_planner::MotionModel::MOORE;  // moore
-  EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Moore"));
+  mm = nav2_smac_planner::MotionModel::TWOD;  // 2d
+  EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("2D"));
   mm = nav2_smac_planner::MotionModel::DUBIN;  // dubin
   EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Dubin"));
   mm = nav2_smac_planner::MotionModel::REEDS_SHEPP;  // reeds-shepp
   EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Reeds-Shepp"));
 
   EXPECT_EQ(
     nav2_smac_planner::fromString(
-      "VON_NEUMANN"), nav2_smac_planner::MotionModel::VON_NEUMANN);
-  EXPECT_EQ(nav2_smac_planner::fromString("MOORE"), nav2_smac_planner::MotionModel::MOORE);
+      "2D"), nav2_smac_planner::MotionModel::TWOD);
   EXPECT_EQ(nav2_smac_planner::fromString("DUBIN"), nav2_smac_planner::MotionModel::DUBIN);
   EXPECT_EQ(
     nav2_smac_planner::fromString(

nav2_smac_planner/test/test_node2d.cpp

@@ -50,7 +50,7 @@ TEST(Node2DTest, test_node_2d)
   info.cost_penalty = 1.0;
   unsigned int size = 10;
   nav2_smac_planner::Node2D::initMotionModel(
-    nav2_smac_planner::MotionModel::MOORE, size, size, size, info);
+    nav2_smac_planner::MotionModel::TWOD, size, size, size, info);
 
   // test reset
   testA.reset();
@@ -68,7 +68,7 @@ TEST(Node2DTest, test_node_2d)
   // check heuristic cost computation
   nav2_smac_planner::Node2D::Coordinates A(0.0, 0.0);
   nav2_smac_planner::Node2D::Coordinates B(10.0, 5.0);
-  EXPECT_NEAR(testB.getHeuristicCost(A, B, nullptr), 15., 0.01);
+  EXPECT_NEAR(testB.getHeuristicCost(A, B, nullptr), 11.18, 0.02);
 
   // check operator== works on index
   unsigned char costC = '2';
@@ -106,18 +106,9 @@ TEST(Node2DTest, test_node_2d_neighbors)
   unsigned int size_y = 10u;
   unsigned int quant = 0u;
   // test neighborhood computation
-  nav2_smac_planner::Node2D::initMotionModel(
-    nav2_smac_planner::MotionModel::VON_NEUMANN, size_x,
-    size_y, quant, info);
-  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets.size(), 4u);
-  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[0], -1);
-  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[1], 1);
-  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[2], -10);
-  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[3], 10);
-
   size_x = 100u;
   nav2_smac_planner::Node2D::initMotionModel(
-    nav2_smac_planner::MotionModel::MOORE, size_x, size_y,
+    nav2_smac_planner::MotionModel::TWOD, size_x, size_y,
     quant, info);
   EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets.size(), 8u);
   EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[0], -1);

nav2_smac_planner/test/test_smac_2d.cpp

@@ -108,7 +108,6 @@ TEST(SmacTest, test_smac_2d_reconfigure) {
       rclcpp::Parameter("test.downsampling_factor", 2),
       rclcpp::Parameter("test.max_iterations", -1),
       rclcpp::Parameter("test.max_on_approach_iterations", -1),
-      rclcpp::Parameter("test.motion_model_for_search", "UNKNOWN"),
       rclcpp::Parameter("test.use_final_approach_orientation", false)});
 
   rclcpp::spin_until_future_complete(
@@ -128,9 +127,6 @@ TEST(SmacTest, test_smac_2d_reconfigure) {
   EXPECT_EQ(
     node2D->get_parameter("test.max_on_approach_iterations").as_int(),
     -1);
-  EXPECT_EQ(
-    node2D->get_parameter("test.motion_model_for_search").as_string(),
-    "UNKNOWN");
 
   results = rec_param->set_parameters_atomically(
     {rclcpp::Parameter("test.downsample_costmap", true)});