Remove redundant collision checks in the Smac Planners to optimize performance (#4857) (#4858)

* initial prototype to resolve smac planner issue

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* fix test

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* initial prototype for coarse to fine checking for smac: incomplete

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* completed initial prototype; for testing and benchmarking now

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* fix typo

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* adding bounds checking for coarse

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* fix test

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* remove coarse to fine checks

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

---------

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
(cherry picked from commit 5ff8cc7)

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

Author: mergify[bot]

nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

@@ -242,6 +242,8 @@ class AStarAlgorithm
    */
   inline void clearGraph();
 
+  inline bool onVisitationCheckNode(const NodePtr & node);
+
   /**
    * @brief Populate a debug log of expansions for Hybrid-A* for visualization
    * @param node Node expanded

nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp

@@ -113,7 +113,6 @@ class GridCollisionChecker
    */
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> getCostmapROS() {return costmap_ros_;}
 
-private:
   /**
    * @brief Check if value outside the range
    * @param min Minimum value of the range
@@ -128,7 +127,7 @@ class GridCollisionChecker
   std::vector<nav2_costmap_2d::Footprint> oriented_footprints_;
   nav2_costmap_2d::Footprint unoriented_footprint_;
   float center_cost_;
-  bool footprint_is_radius_;
+  bool footprint_is_radius_{false};
   std::vector<float> angles_;
   float possible_collision_cost_{-1};
   rclcpp::Logger logger_{rclcpp::get_logger("SmacPlannerCollisionChecker")};

nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp

@@ -277,6 +277,7 @@ class Node2D
   uint64_t _index;
   bool _was_visited;
   bool _is_queued;
+  bool _in_collision{false};
 };
 
 }  // namespace nav2_smac_planner

nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp

@@ -306,9 +306,12 @@ class NodeHybrid
   /**
    * @brief Check if this node is valid
    * @param traverse_unknown If we can explore unknown nodes on the graph
+   * @param collision_checker: Collision checker object
    * @return whether this node is valid and collision free
    */
-  bool isNodeValid(const bool & traverse_unknown, GridCollisionChecker * collision_checker);
+  bool isNodeValid(
+    const bool & traverse_unknown,
+    GridCollisionChecker * collision_checker);
 
   /**
    * @brief Get traversal cost of parent node to child node
@@ -493,6 +496,7 @@ class NodeHybrid
   bool _was_visited;
   unsigned int _motion_primitive_index;
   TurnDirection _turn_dir;
+  bool _is_node_valid{false};
 };
 
 }  // namespace nav2_smac_planner

nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp

@@ -436,6 +436,7 @@ class NodeLattice
   bool _was_visited;
   MotionPrimitive * _motion_primitive;
   bool _backwards;
+  bool _is_node_valid{false};
 };
 
 }  // namespace nav2_smac_planner

nav2_smac_planner/src/a_star.cpp

@@ -326,7 +326,8 @@ bool AStarAlgorithm<NodeT>::createPath(
 
     // We allow for nodes to be queued multiple times in case
     // shorter paths result in it, but we can visit only once
-    if (current_node->wasVisited()) {
+    // Also a chance to perform last-checks necessary.
+    if (onVisitationCheckNode(current_node)) {
       continue;
     }
 
@@ -435,6 +436,12 @@ float AStarAlgorithm<NodeT>::getHeuristicCost(const NodePtr & node)
   return heuristic;
 }
 
+template<typename NodeT>
+bool AStarAlgorithm<NodeT>::onVisitationCheckNode(const NodePtr & current_node)
+{
+  return current_node->wasVisited();
+}
+
 template<typename NodeT>
 void AStarAlgorithm<NodeT>::clearQueue()
 {

nav2_smac_planner/src/node_2d.cpp

@@ -31,7 +31,8 @@ Node2D::Node2D(const uint64_t index)
   _accumulated_cost(std::numeric_limits<float>::max()),
   _index(index),
   _was_visited(false),
-  _is_queued(false)
+  _is_queued(false),
+  _in_collision(false)
 {
 }
 
@@ -47,18 +48,21 @@ void Node2D::reset()
   _accumulated_cost = std::numeric_limits<float>::max();
   _was_visited = false;
   _is_queued = false;
+  _in_collision = false;
 }
 
 bool Node2D::isNodeValid(
   const bool & traverse_unknown,
   GridCollisionChecker * collision_checker)
 {
-  if (collision_checker->inCollision(this->getIndex(), traverse_unknown)) {
-    return false;
+  // Already found, we can return the result
+  if (!std::isnan(_cell_cost)) {
+    return !_in_collision;
   }
 
+  _in_collision = collision_checker->inCollision(this->getIndex(), traverse_unknown);
   _cell_cost = collision_checker->getCost();
-  return true;
+  return !_in_collision;
 }
 
 float Node2D::getTraversalCost(const NodePtr & child)

nav2_smac_planner/src/node_hybrid.cpp

@@ -350,7 +350,8 @@ NodeHybrid::NodeHybrid(const uint64_t index)
   _accumulated_cost(std::numeric_limits<float>::max()),
   _index(index),
   _was_visited(false),
-  _motion_primitive_index(std::numeric_limits<unsigned int>::max())
+  _motion_primitive_index(std::numeric_limits<unsigned int>::max()),
+  _is_node_valid(false)
 {
 }
 
@@ -369,20 +370,22 @@ void NodeHybrid::reset()
   pose.x = 0.0f;
   pose.y = 0.0f;
   pose.theta = 0.0f;
+  _is_node_valid = false;
 }
 
 bool NodeHybrid::isNodeValid(
   const bool & traverse_unknown,
   GridCollisionChecker * collision_checker)
 {
-  if (collision_checker->inCollision(
-      this->pose.x, this->pose.y, this->pose.theta /*bin number*/, traverse_unknown))
-  {
-    return false;
+  // Already found, we can return the result
+  if (!std::isnan(_cell_cost)) {
+    return _is_node_valid;
   }
 
+  _is_node_valid = !collision_checker->inCollision(
+    this->pose.x, this->pose.y, this->pose.theta /*bin number*/, traverse_unknown);
   _cell_cost = collision_checker->getCost();
-  return true;
+  return _is_node_valid;
 }
 
 float NodeHybrid::getTraversalCost(const NodePtr & child)

nav2_smac_planner/src/node_lattice.cpp

@@ -193,7 +193,8 @@ NodeLattice::NodeLattice(const uint64_t index)
   _index(index),
   _was_visited(false),
   _motion_primitive(nullptr),
-  _backwards(false)
+  _backwards(false),
+  _is_node_valid(false)
 {
 }
 
@@ -213,6 +214,7 @@ void NodeLattice::reset()
   pose.theta = 0.0f;
   _motion_primitive = nullptr;
   _backwards = false;
+  _is_node_valid = false;
 }
 
 bool NodeLattice::isNodeValid(
@@ -221,13 +223,20 @@ bool NodeLattice::isNodeValid(
   MotionPrimitive * motion_primitive,
   bool is_backwards)
 {
+  // Already found, we can return the result
+  if (!std::isnan(_cell_cost)) {
+    return _is_node_valid;
+  }
+
   // Check primitive end pose
   // Convert grid quantization of primitives to radians, then collision checker quantization
   static const double bin_size = 2.0 * M_PI / collision_checker->getPrecomputedAngles().size();
   const double & angle = motion_table.getAngleFromBin(this->pose.theta) / bin_size;
   if (collision_checker->inCollision(
       this->pose.x, this->pose.y, angle /*bin in collision checker*/, traverse_unknown))
   {
+    _is_node_valid = false;
+    _cell_cost = collision_checker->getCost();
     return false;
   }
 
@@ -269,6 +278,8 @@ bool NodeLattice::isNodeValid(
             prim_pose._theta / bin_size /*bin in collision checker*/,
             traverse_unknown))
         {
+          _is_node_valid = false;
+          _cell_cost = std::max(max_cell_cost, collision_checker->getCost());
           return false;
         }
         max_cell_cost = std::max(max_cell_cost, collision_checker->getCost());
@@ -277,7 +288,8 @@ bool NodeLattice::isNodeValid(
   }
 
   _cell_cost = max_cell_cost;
-  return true;
+  _is_node_valid = true;
+  return _is_node_valid;
 }
 
 float NodeLattice::getTraversalCost(const NodePtr & child)

nav2_smac_planner/test/test_a_star.cpp

@@ -201,8 +201,8 @@ TEST(AStarTest, test_a_star_se2)
   EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get()));
 
   // check path is the right size and collision free
-  EXPECT_EQ(num_it, 3146);
-  EXPECT_EQ(path.size(), 63u);
+  EXPECT_GT(num_it, 2000);
+  EXPECT_NEAR(path.size(), 63u, 2u);
   for (unsigned int i = 0; i != path.size(); i++) {
     EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0);
   }