From 6a540f197da316bafb34182ed92f06f7a6622b1b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Sat, 20 Jan 2024 11:59:35 +0000 Subject: [PATCH] Fix Smac Planner confined collision checker (#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski * Fix tests Signed-off-by: Steve Macenski * Update test_a_star.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- nav2_smac_planner/src/collision_checker.cpp | 4 ++-- nav2_smac_planner/test/test_a_star.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 20d288809d..328a88f9db 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -107,7 +107,7 @@ bool GridCollisionChecker::inCollision( // 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_ = costmap_->getCost( - static_cast(x), static_cast(y)); + static_cast(x + 0.5), static_cast(y + 0.5)); if (footprint_cost_ < possible_inscribed_cost_) { if (possible_inscribed_cost_ > 0) { @@ -157,7 +157,7 @@ bool GridCollisionChecker::inCollision( } else { // if radius, then we can check the center of the cost assuming inflation is used footprint_cost_ = costmap_->getCost( - static_cast(x), static_cast(y)); + static_cast(x + 0.5), static_cast(y + 0.5)); if (footprint_cost_ == UNKNOWN && traverse_unknown) { return false; diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index f048cc5550..730193f802 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -164,8 +164,8 @@ TEST(AStarTest, test_a_star_se2) EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, expansions.get())); // check path is the right size and collision free - EXPECT_EQ(num_it, 3186); - EXPECT_EQ(path.size(), 64u); + EXPECT_EQ(num_it, 3146); + EXPECT_EQ(path.size(), 63u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } @@ -229,8 +229,8 @@ TEST(AStarTest, test_a_star_lattice) EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); // check path is the right size and collision free - EXPECT_EQ(num_it, 26); - EXPECT_GT(path.size(), 47u); + EXPECT_EQ(num_it, 22); + EXPECT_GT(path.size(), 45u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); }