From 066b2a8f25d1ff6b68a62433068900571e78625f Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 15 Jan 2025 15:13:44 -0800 Subject: [PATCH] Remove redundant collision checks in the Smac Planners to optimize performance (#4857) * initial prototype to resolve smac planner issue Signed-off-by: Steve Macenski * fix test Signed-off-by: Steve Macenski * initial prototype for coarse to fine checking for smac: incomplete Signed-off-by: Steve Macenski * completed initial prototype; for testing and benchmarking now Signed-off-by: Steve Macenski * fix typo Signed-off-by: Steve Macenski * adding bounds checking for coarse Signed-off-by: Steve Macenski * fix test Signed-off-by: Steve Macenski * remove coarse to fine checks Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski Signed-off-by: RBT22 --- .../include/nav2_smac_planner/a_star.hpp | 2 ++ .../nav2_smac_planner/collision_checker.hpp | 3 +-- .../include/nav2_smac_planner/node_2d.hpp | 1 + .../include/nav2_smac_planner/node_hybrid.hpp | 6 +++++- .../include/nav2_smac_planner/node_lattice.hpp | 1 + nav2_smac_planner/src/a_star.cpp | 9 ++++++++- nav2_smac_planner/src/node_2d.cpp | 12 ++++++++---- nav2_smac_planner/src/node_hybrid.cpp | 15 +++++++++------ nav2_smac_planner/src/node_lattice.cpp | 16 ++++++++++++++-- nav2_smac_planner/test/test_a_star.cpp | 4 ++-- 10 files changed, 51 insertions(+), 18 deletions(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 2184dbf4477..8b127960cc3 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -240,6 +240,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 diff --git a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp index 0f31860f5a9..656e069f7e9 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp @@ -113,7 +113,6 @@ class GridCollisionChecker */ std::shared_ptr 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 oriented_footprints_; nav2_costmap_2d::Footprint unoriented_footprint_; float center_cost_; - bool footprint_is_radius_; + bool footprint_is_radius_{false}; std::vector angles_; float possible_collision_cost_{-1}; rclcpp::Logger logger_{rclcpp::get_logger("SmacPlannerCollisionChecker")}; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp index b18ba0043ea..1bd0993a8eb 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp @@ -273,6 +273,7 @@ class Node2D uint64_t _index; bool _was_visited; bool _is_queued; + bool _in_collision{false}; }; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index 7d2684d33d0..ee3a4bf231c 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -301,9 +301,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 @@ -488,6 +491,7 @@ class NodeHybrid bool _was_visited; unsigned int _motion_primitive_index; TurnDirection _turn_dir; + bool _is_node_valid{false}; }; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index fe5af4efec7..824b435447d 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -427,6 +427,7 @@ class NodeLattice bool _was_visited; MotionPrimitive * _motion_primitive; bool _backwards; + bool _is_node_valid{false}; }; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index aa95567123e..52b94db14af 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -326,7 +326,8 @@ bool AStarAlgorithm::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::getHeuristicCost(const NodePtr & node) return heuristic; } +template +bool AStarAlgorithm::onVisitationCheckNode(const NodePtr & current_node) +{ + return current_node->wasVisited(); +} + template void AStarAlgorithm::clearQueue() { diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp index e0e23840aef..bf64ae29e3e 100644 --- a/nav2_smac_planner/src/node_2d.cpp +++ b/nav2_smac_planner/src/node_2d.cpp @@ -31,7 +31,8 @@ Node2D::Node2D(const uint64_t index) _accumulated_cost(std::numeric_limits::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::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) diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index abbdc6f712b..0eaef5fe6be 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -350,7 +350,8 @@ NodeHybrid::NodeHybrid(const uint64_t index) _accumulated_cost(std::numeric_limits::max()), _index(index), _was_visited(false), - _motion_primitive_index(std::numeric_limits::max()) + _motion_primitive_index(std::numeric_limits::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) diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index 6c794b9a987..dcbc33f197d 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -194,7 +194,8 @@ NodeLattice::NodeLattice(const uint64_t index) _index(index), _was_visited(false), _motion_primitive(nullptr), - _backwards(false) + _backwards(false), + _is_node_valid(false) { } @@ -214,6 +215,7 @@ void NodeLattice::reset() pose.theta = 0.0f; _motion_primitive = nullptr; _backwards = false; + _is_node_valid = false; } bool NodeLattice::isNodeValid( @@ -222,6 +224,11 @@ 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(); @@ -229,6 +236,8 @@ bool NodeLattice::isNodeValid( 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; } @@ -270,6 +279,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()); @@ -278,7 +289,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) diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 32ff9158251..ef09202dbbb 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/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); }