Skip to content

Change to use supercover #322

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

Open
wants to merge 7 commits into
base: master
Choose a base branch
from
Open

Change to use supercover #322

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
050da88
Fix service zero tolerance numerical issue (#1)
renan028 Sep 13, 2023
242e494
solution sanity check
renan028 Sep 13, 2023
43d89d1
change to angle_max_step_size
renan028 Sep 15, 2023
e71d2d6
fix conversion
renan028 Sep 18, 2023
567f2c8
Merge branch 'magazino:master' into master
renan028 Oct 24, 2023
73599ed
change to use supercover
renan028 Jul 20, 2023
8812e78
fix test for new footprint
renan028 Oct 24, 2023
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 mbf_costmap_nav/src/mbf_costmap_nav/footprint_helper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -314,7 +314,7 @@ std::vector<Cell> FootprintHelper::getFootprintCells(double x, double y, double
return clearAndReturn(footprint_cells);
}

getLineCells(x0, x1, y0, y1, footprint_cells);
supercover(x0, x1, y0, y1, footprint_cells);
}

//we need to close the loop, so we also have to raytrace from the last pt to first pt
Expand All @@ -329,7 +329,7 @@ std::vector<Cell> FootprintHelper::getFootprintCells(double x, double y, double
return clearAndReturn(footprint_cells);
}

getLineCells(x0, x1, y0, y1, footprint_cells);
supercover(x0, x1, y0, y1, footprint_cells);

if(fill) {
getFillCells(footprint_cells);
Expand Down
74 changes: 36 additions & 38 deletions mbf_costmap_nav/test/free_pose_search_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -247,8 +247,7 @@ TEST_F(SearchHelperTest, getFootprintState)
EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(4.6, 4.6, M_PI)).state, state);
EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(4.5, 4.5, 0)).state, state);
EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(4.4, 4.4, 0)).state, SearchState::FREE);
EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(4.5, 4.5, M_PI_4)).state,
SearchState::FREE);
EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(4.5, 4.5, M_PI_4)).state, state);
EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(3, 5, 0)).state, SearchState::FREE);
};

Expand Down Expand Up @@ -281,8 +280,7 @@ TEST_F(SearchHelperTest, findValidOrientation)
std::vector<geometry_msgs::Point> footprint = { toPoint(-0.5, -0.5), toPoint(0.5, -0.5), toPoint(0.5, 0.5),
toPoint(-0.5, 0.5) };
auto sol = FreePoseSearch::findValidOrientation(costmap, footprint, toPose2D(4.6, 4.6, 0), { M_PI_4 / 2, M_PI }, viz);
EXPECT_EQ(sol.search_state.state, SearchState::FREE);
EXPECT_NEAR(sol.pose.theta, M_PI_4 / 2, 1e-6);
EXPECT_EQ(sol.search_state.state, SearchState::LETHAL);

sol = FreePoseSearch::findValidOrientation(costmap, footprint, toPose2D(4.6, 4.6, 0), { M_PI, 2 * M_PI }, viz);
EXPECT_EQ(sol.search_state.state, SearchState::LETHAL);
Expand Down Expand Up @@ -324,8 +322,8 @@ TEST_F(SearchHelperTest, search)
0.5 | 0 0 0 0 0 0 0 254 254 254
1.5 | 0 0 0 0 0 0 0 254 254 254
2.5 | 0 0 0 254 254 254 0 0 0 0
3.5 | 0 0 0 0 0 0 0 0 0 0
4.5 | 0 0 0 0 0 0 x 0 0 0
3.5 | 0 0 0 0 0 0 0 x 0 0
4.5 | 0 0 0 0 0 0 0 0 0 0
5.5 | 0 0 0 0 254 G 0 254 254 254
6.5 | 0 0 0 0 254 0 0 254 254 254
7.5 | 0 0 0 0 0 0 0 254 254 254
Expand All @@ -338,13 +336,13 @@ TEST_F(SearchHelperTest, search)

auto sol = sh.search();
EXPECT_EQ(sol.search_state.state, SearchState::FREE);
EXPECT_NEAR(sol.pose.x, 6.5, 1e-6);
EXPECT_NEAR(sol.pose.y, 4.5, 1e-6);
EXPECT_NEAR(sol.pose.theta, -M_PI_4, 1e-6);
EXPECT_NEAR(sol.pose.x, 7.5, 1e-6);
EXPECT_NEAR(sol.pose.y, 3.5, 1e-6);
EXPECT_NEAR(sol.pose.theta, 0, 1e-6);

addObstacle(cm, 6.5, 4.5);
addObstacle(cm, 7.5, 3.5);
map.header.stamp = ros::Time::now();
map.data[cm.getCostmap()->getIndex(6, 4)] = 100;
map.data[cm.getCostmap()->getIndex(7, 3)] = 100;
map_pub.publish(map);

/*
Expand All @@ -353,24 +351,24 @@ TEST_F(SearchHelperTest, search)
0.5 | 0 0 0 0 0 0 0 254 254 254
1.5 | 0 0 0 0 0 0 0 254 254 254
2.5 | 0 0 0 254 254 254 0 0 0 0
3.5 | 0 0 0 0 254 0 0 0 0 0
4.5 | 0 0 0 0 0 0 254 0 0 0
5.5 | 0 0 0 0 254 G 0 254 254 254
3.5 | 0 0 0 0 254 0 0 254 0 0
4.5 | 0 0 0 0 0 0 0 0 0 0
5.5 | 0 0 x 0 254 G 0 254 254 254
6.5 | 0 0 0 0 254 0 0 254 254 254
7.5 | 0 0 0 0 0 x 0 254 254 254
7.5 | 0 0 0 0 0 0 0 254 254 254
8.5 | 0 0 0 0 0 0 0 0 0 0
9.5 | 0 0 0 0 0 0 0 0 0 0
*/

sol = sh.search();
EXPECT_EQ(sol.search_state.state, SearchState::FREE);
EXPECT_NEAR(sol.pose.x, 5.5, 1e-6);
EXPECT_NEAR(sol.pose.y, 7.5, 1e-6);
EXPECT_NEAR(sol.pose.theta, M_PI_4, 1e-6);
EXPECT_NEAR(sol.pose.x, 2.5, 1e-6);
EXPECT_NEAR(sol.pose.y, 5.5, 1e-6);
EXPECT_NEAR(sol.pose.theta, M_PI_2, 1e-6);

addObstacle(cm, 5.5, 7.5);
addObstacle(cm, 2.5, 5.5);
map.header.stamp = ros::Time::now();
map.data[cm.getCostmap()->getIndex(5, 7)] = 100;
map.data[cm.getCostmap()->getIndex(2, 5)] = 100;
map_pub.publish(map);

/*
Expand All @@ -379,24 +377,24 @@ TEST_F(SearchHelperTest, search)
0.5 | 0 0 0 0 0 0 0 254 254 254
1.5 | 0 0 0 0 0 0 0 254 254 254
2.5 | 0 0 0 254 254 254 0 0 0 0
3.5 | 0 0 0 0 254 0 0 0 0 0
4.5 | 0 0 0 x 0 0 254 0 0 0
5.5 | 0 0 0 0 254 G 0 254 254 254
3.5 | 0 0 0 0 254 0 0 254 0 0
4.5 | 0 0 0 0 0 0 0 0 0 0
5.5 | 0 0 254 0 254 G 0 254 254 254
6.5 | 0 0 0 0 254 0 0 254 254 254
7.5 | 0 0 0 0 0 254 0 254 254 254
8.5 | 0 0 0 0 0 0 0 0 0 0
7.5 | 0 0 0 0 0 0 0 254 254 254
8.5 | 0 0 0 0 0 x 0 0 0 0
9.5 | 0 0 0 0 0 0 0 0 0 0
*/

sol = sh.search();
EXPECT_EQ(sol.search_state.state, SearchState::FREE);
EXPECT_NEAR(sol.pose.x, 3.5, 1e-6);
EXPECT_NEAR(sol.pose.y, 4.5, 1e-6);
EXPECT_NEAR(sol.pose.theta, 3 * M_PI_4, 1e-6);
EXPECT_NEAR(sol.pose.x, 5.5, 1e-6);
EXPECT_NEAR(sol.pose.y, 8.5, 1e-6);
EXPECT_NEAR(sol.pose.theta, M_PI, 1e-6);

addObstacle(cm, 3.5, 4.5);
addObstacle(cm, 5.5, 8.5);
map.header.stamp = ros::Time::now();
map.data[cm.getCostmap()->getIndex(3, 4)] = 100;
map.data[cm.getCostmap()->getIndex(5, 8)] = 100;
map_pub.publish(map);

/*
Expand All @@ -405,18 +403,18 @@ TEST_F(SearchHelperTest, search)
0.5 | 0 0 0 0 0 0 0 254 254 254
1.5 | 0 0 0 0 0 0 0 254 254 254
2.5 | 0 0 0 254 254 254 0 0 0 0
3.5 | 0 0 0 0 254 0 0 0 0 0
4.5 | 0 0 0 254 0 0 254 0 0 0
5.5 | 0 0 0 0 254 G 0 254 254 254
3.5 | 0 0 0 0 254 0 0 254 0 0
4.5 | 0 0 0 0 0 0 0 0 0 0
5.5 | 0 0 254 0 254 G 0 254 254 254
6.5 | 0 0 0 0 254 0 0 254 254 254
7.5 | 0 0 0 x 0 254 0 254 254 254
8.5 | 0 0 0 0 0 0 0 0 0 0
7.5 | 0 0 x 0 0 0 0 254 254 254
8.5 | 0 0 0 0 0 254 0 0 0 0
9.5 | 0 0 0 0 0 0 0 0 0 0
*/

sol = sh.search();
EXPECT_EQ(sol.search_state.state, SearchState::FREE);
EXPECT_NEAR(sol.pose.x, 3.5, 1e-6);
EXPECT_NEAR(sol.pose.x, 2.5, 1e-6);
EXPECT_NEAR(sol.pose.y, 7.5, 1e-6);
EXPECT_NEAR(sol.pose.theta, M_PI_4, 1e-6);

Expand All @@ -426,8 +424,8 @@ TEST_F(SearchHelperTest, search)
sol = sh2.search();
EXPECT_EQ(sol.search_state.state, SearchState::FREE);
EXPECT_NEAR(sol.pose.x, 1.5, 1e-6);
EXPECT_NEAR(sol.pose.y, 4.5, 1e-6);
EXPECT_NEAR(sol.pose.theta, M_PI_4, 1e-6);
EXPECT_NEAR(sol.pose.y, 3.5, 1e-6);
EXPECT_NEAR(sol.pose.theta, -M_PI_2, 1e-6);
}

TEST_F(SearchHelperTest, service_test)
Expand Down