Fix Smac Planner confined collision checker (#4055)

* Update collision_checker.cpp Signed-off-by: Steve Macenski <[email protected]> * Fix tests Signed-off-by: Steve Macenski <[email protected]> * Update test_a_star.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]>
ros-navigation · Jan 20, 2024 · 6a540f1 · 6a540f1
1 parent a8a0c3a
commit 6a540f1
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 6 deletions.
diff --git 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<unsigned int>(x), static_cast<unsigned int>(y));
+      static_cast<unsigned int>(x + 0.5), static_cast<unsigned int>(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<unsigned int>(x), static_cast<unsigned int>(y));
+      static_cast<unsigned int>(x + 0.5), static_cast<unsigned int>(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
@@ -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);
   }