From b705c13ed22122ed8a730114c38602bfd007f0ea Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 14 Jan 2025 13:54:47 -0800 Subject: [PATCH] Fixes smac planner non-circular footprint search issue (backport #4851) (#4852) * Fixes smac planner non-circular footprint search issue (#4851) * initial prototype to resolve smac planner issue Signed-off-by: Steve Macenski * fix test Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski (cherry picked from commit bc9f2bc9c1134d4ff3b2df2f7cd6f130c96c3f52) # Conflicts: # nav2_smac_planner/src/collision_checker.cpp # nav2_smac_planner/src/node_hybrid.cpp * fix mergify issue Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski Co-authored-by: Steve Macenski --- .../nav2_smac_planner/collision_checker.hpp | 2 +- .../include/nav2_smac_planner/node_hybrid.hpp | 10 ------ nav2_smac_planner/src/collision_checker.cpp | 36 +++++++++---------- nav2_smac_planner/src/node_hybrid.cpp | 34 +----------------- .../test/test_collision_checker.cpp | 30 ++++++++++------ 5 files changed, 38 insertions(+), 74 deletions(-) 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 205c3f0471a..36057581240 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp @@ -127,7 +127,7 @@ class GridCollisionChecker std::shared_ptr costmap_ros_; std::vector oriented_footprints_; nav2_costmap_2d::Footprint unoriented_footprint_; - float footprint_cost_; + float center_cost_; bool footprint_is_radius_; std::vector angles_; float possible_collision_cost_{-1}; 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 b67ed978ec8..da1c2186d30 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -441,14 +441,6 @@ class NodeHybrid const unsigned int & start_x, const unsigned int & start_y, const unsigned int & goal_x, const unsigned int & goal_y); - /** - * @brief Using the inflation layer, find the footprint's adjusted cost - * if the robot is non-circular - * @param cost Cost to adjust - * @return float Cost adjusted - */ - static float adjustedFootprintCost(const float & cost); - /** * @brief Retrieve all valid neighbors of a node. * @param validity_checker Functor for state validity checking @@ -476,7 +468,6 @@ class NodeHybrid */ static void destroyStaticAssets() { - inflation_layer.reset(); costmap_ros.reset(); } @@ -491,7 +482,6 @@ class NodeHybrid static ObstacleHeuristicQueue obstacle_heuristic_queue; static std::shared_ptr costmap_ros; - static std::shared_ptr inflation_layer; // Dubin / Reeds-Shepp lookup and size for dereferencing static LookupTable dist_heuristic_lookup_table; static float size_lookup; diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index a44acc518d2..5becfa17c3a 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -115,32 +115,31 @@ bool GridCollisionChecker::inCollision( } // Assumes setFootprint already set - double wx, wy; - costmap_->mapToWorld(static_cast(x), static_cast(y), wx, wy); + center_cost_ = static_cast(costmap_->getCost( + static_cast(x + 0.5f), static_cast(y + 0.5f))); if (!footprint_is_radius_) { // if footprint, then we check for the footprint's points, but first see // if the robot is even potentially in an inscribed collision - footprint_cost_ = static_cast(costmap_->getCost( - static_cast(x + 0.5f), static_cast(y + 0.5f))); - - if (footprint_cost_ < possible_collision_cost_ && possible_collision_cost_ > 0.0f) { + if (center_cost_ < possible_collision_cost_ && possible_collision_cost_ > 0.0f) { return false; } // If its inscribed, in collision, or unknown in the middle, // no need to even check the footprint, its invalid - if (footprint_cost_ == UNKNOWN && !traverse_unknown) { + if (center_cost_ == UNKNOWN && !traverse_unknown) { return true; } - if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) { + if (center_cost_ == INSCRIBED || center_cost_ == OCCUPIED) { return true; } // if possible inscribed, need to check actual footprint pose. // Use precomputed oriented footprints are done on initialization, // offset by translation value to collision check + double wx, wy; + costmap_->mapToWorld(static_cast(x), static_cast(y), wx, wy); geometry_msgs::msg::Point new_pt; const nav2_costmap_2d::Footprint & oriented_footprint = oriented_footprints_[angle_bin]; nav2_costmap_2d::Footprint current_footprint; @@ -151,25 +150,22 @@ bool GridCollisionChecker::inCollision( current_footprint.push_back(new_pt); } - footprint_cost_ = static_cast(footprintCost(current_footprint)); + float footprint_cost = static_cast(footprintCost(current_footprint)); - if (footprint_cost_ == UNKNOWN && traverse_unknown) { + if (footprint_cost == UNKNOWN && traverse_unknown) { return false; } // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= OCCUPIED; + return footprint_cost >= OCCUPIED; } else { // if radius, then we can check the center of the cost assuming inflation is used - footprint_cost_ = static_cast(costmap_->getCost( - static_cast(x + 0.5f), static_cast(y + 0.5f))); - - if (footprint_cost_ == UNKNOWN && traverse_unknown) { + if (center_cost_ == UNKNOWN && traverse_unknown) { return false; } // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= INSCRIBED; + return center_cost_ >= INSCRIBED; } } @@ -177,19 +173,19 @@ bool GridCollisionChecker::inCollision( const unsigned int & i, const bool & traverse_unknown) { - footprint_cost_ = costmap_->getCost(i); - if (footprint_cost_ == UNKNOWN && traverse_unknown) { + center_cost_ = costmap_->getCost(i); + if (center_cost_ == UNKNOWN && traverse_unknown) { return false; } // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= INSCRIBED; + return center_cost_ >= INSCRIBED; } float GridCollisionChecker::getCost() { // Assumes inCollision called prior - return static_cast(footprint_cost_); + return static_cast(center_cost_); } bool GridCollisionChecker::outsideRange(const unsigned int & max, const float & value) diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 1d3d36d520b..1e93a7457ce 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -41,7 +41,6 @@ HybridMotionTable NodeHybrid::motion_table; float NodeHybrid::size_lookup = 25; LookupTable NodeHybrid::dist_heuristic_lookup_table; std::shared_ptr NodeHybrid::costmap_ros = nullptr; -std::shared_ptr NodeHybrid::inflation_layer = nullptr; ObstacleHeuristicQueue NodeHybrid::obstacle_heuristic_queue; @@ -492,7 +491,6 @@ void NodeHybrid::resetObstacleHeuristic( // erosion of path quality after even modest smoothing. The error would be no more // than 0.05 * normalized cost. Since this is just a search prior, there's no loss in generality costmap_ros = costmap_ros_i; - inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer(costmap_ros); auto costmap = costmap_ros->getCostmap(); // Clear lookup table @@ -539,29 +537,6 @@ void NodeHybrid::resetObstacleHeuristic( obstacle_heuristic_lookup_table[goal_index] = -0.00001f; } -float NodeHybrid::adjustedFootprintCost(const float & cost) -{ - if (!inflation_layer) { - return cost; - } - - const auto layered_costmap = costmap_ros->getLayeredCostmap(); - const float scale_factor = inflation_layer->getCostScalingFactor(); - const float min_radius = layered_costmap->getInscribedRadius(); - float dist_to_obj = (scale_factor * min_radius - log(cost) + log(253.0f)) / scale_factor; - - // Subtract minimum radius for edge cost - dist_to_obj -= min_radius; - if (dist_to_obj < 0.0f) { - dist_to_obj = 0.0f; - } - - // Compute cost at this value - return static_cast( - inflation_layer->computeCost(dist_to_obj / layered_costmap->getCostmap()->getResolution())); -} - - float NodeHybrid::getObstacleHeuristic( const Coordinates & node_coords, const Coordinates & goal_coords, @@ -569,7 +544,6 @@ float NodeHybrid::getObstacleHeuristic( { // If already expanded, return the cost auto costmap = costmap_ros->getCostmap(); - const bool is_circular = costmap_ros->getUseRadius(); unsigned int size_x = 0u; unsigned int size_y = 0u; if (motion_table.downsample_obstacle_heuristic) { @@ -671,13 +645,7 @@ float NodeHybrid::getObstacleHeuristic( cost = static_cast(costmap->getCost(new_idx)); } - if (!is_circular) { - // Adjust cost value if using SE2 footprint checks - cost = adjustedFootprintCost(cost); - if (cost >= OCCUPIED) { - continue; - } - } else if (cost >= INSCRIBED) { + if (cost >= INSCRIBED) { continue; } diff --git a/nav2_smac_planner/test/test_collision_checker.cpp b/nav2_smac_planner/test/test_collision_checker.cpp index dd4e032a51e..a793479df64 100644 --- a/nav2_smac_planner/test/test_collision_checker.cpp +++ b/nav2_smac_planner/test/test_collision_checker.cpp @@ -153,17 +153,22 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement) nav2_smac_planner::GridCollisionChecker collision_checker(costmap_ros, 72, node); collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0); - collision_checker.inCollision(50, 50, 0.0, false); + EXPECT_FALSE(collision_checker.inCollision(50, 50, 0.0, false)); float cost = collision_checker.getCost(); EXPECT_NEAR(cost, 128.0, 0.001); - collision_checker.inCollision(50, 49, 0.0, false); + EXPECT_TRUE(collision_checker.inCollision(50, 49, 0.0, false)); float up_value = collision_checker.getCost(); - EXPECT_NEAR(up_value, 254.0, 0.001); + EXPECT_NEAR(up_value, 128.0, 0.001); // center cost - collision_checker.inCollision(50, 52, 0.0, false); + EXPECT_TRUE(collision_checker.inCollision(50, 52, 0.0, false)); float down_value = collision_checker.getCost(); - EXPECT_NEAR(down_value, 254.0, 0.001); + EXPECT_NEAR(down_value, 128.0, 0.001); // center cost + + EXPECT_TRUE(collision_checker.inCollision(11, 11, 0.0, false)); + float other_value = collision_checker.getCost(); + EXPECT_NEAR(other_value, 254.0, 0.001); // center cost + delete costmap_; } @@ -200,16 +205,21 @@ TEST(collision_footprint, test_point_and_line_cost) nav2_smac_planner::GridCollisionChecker collision_checker(costmap_ros, 72, node); collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0); - collision_checker.inCollision(50, 50, 0.0, false); + EXPECT_FALSE(collision_checker.inCollision(50, 50, 0.0, false)); float value = collision_checker.getCost(); EXPECT_NEAR(value, 128.0, 0.001); - collision_checker.inCollision(49, 50, 0.0, false); + EXPECT_TRUE(collision_checker.inCollision(49, 50, 0.0, false)); float left_value = collision_checker.getCost(); - EXPECT_NEAR(left_value, 254.0, 0.001); + EXPECT_NEAR(left_value, 128.0, 0.001); // center cost - collision_checker.inCollision(52, 50, 0.0, false); + EXPECT_TRUE(collision_checker.inCollision(52, 50, 0.0, false)); float right_value = collision_checker.getCost(); - EXPECT_NEAR(right_value, 254.0, 0.001); + EXPECT_NEAR(right_value, 128.0, 0.001); // center cost + + EXPECT_TRUE(collision_checker.inCollision(39, 60, 0.0, false)); + float other_value = collision_checker.getCost(); + EXPECT_NEAR(other_value, 254.0, 0.001); // center cost + delete costmap_; }