Skip to content

Fix Smac Planner confined collision checker #4055

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 3 commits into from
Jan 20, 2024
Merged

Fix Smac Planner confined collision checker #4055

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
8891a53
Update collision_checker.cpp
SteveMacenski Jan 17, 2024
1f3745f
Fix tests
SteveMacenski Jan 17, 2024
743a09d
Update test_a_star.cpp
SteveMacenski Jan 19, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions nav2_smac_planner/src/collision_checker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down Expand Up @@ -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;
Expand Down
8 changes: 4 additions & 4 deletions nav2_smac_planner/test/test_a_star.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand Down Expand Up @@ -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);
}
Expand Down