From 8702ba6c7b1ab00b9b79f8806ac3a0f422a0b15b Mon Sep 17 00:00:00 2001 From: Renan Salles Date: Thu, 20 Jul 2023 17:12:16 +0900 Subject: [PATCH 1/2] change to use supercover --- .../src/mbf_costmap_nav/footprint_helper.cpp | 4 +- .../test/free_pose_search_test.cpp | 76 +++++++++---------- 2 files changed, 39 insertions(+), 41 deletions(-) diff --git a/mbf_costmap_nav/src/mbf_costmap_nav/footprint_helper.cpp b/mbf_costmap_nav/src/mbf_costmap_nav/footprint_helper.cpp index 88700d25..cddc000d 100644 --- a/mbf_costmap_nav/src/mbf_costmap_nav/footprint_helper.cpp +++ b/mbf_costmap_nav/src/mbf_costmap_nav/footprint_helper.cpp @@ -314,7 +314,7 @@ std::vector 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 @@ -329,7 +329,7 @@ std::vector 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); diff --git a/mbf_costmap_nav/test/free_pose_search_test.cpp b/mbf_costmap_nav/test/free_pose_search_test.cpp index e797a167..8494eb40 100644 --- a/mbf_costmap_nav/test/free_pose_search_test.cpp +++ b/mbf_costmap_nav/test/free_pose_search_test.cpp @@ -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); }; @@ -281,8 +280,7 @@ TEST_F(SearchHelperTest, findValidOrientation) std::vector 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); @@ -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 @@ -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); /* @@ -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); /* @@ -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); /* @@ -405,20 +403,20 @@ 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 0 0 0 0 0 254 254 254 + 8.5 | 0 0 0 x 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.y, 7.5, 1e-6); - EXPECT_NEAR(sol.pose.theta, M_PI_4, 1e-6); + EXPECT_NEAR(sol.pose.y, 8.5, 1e-6); + EXPECT_NEAR(sol.pose.theta, -3.0 / 4 * M_PI, 1e-6); // Non-zero orientation SearchConfig config2{ M_PI_4, M_PI, 5.0, false, 0.0, toPose2D(1.5, 4.5, M_PI_4) }; @@ -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, 3.0 / 2 * M_PI, 1e-6); } TEST_F(SearchHelperTest, service_test) From 34c6a45a73f8e39ce14e92106b1cc478efb08881 Mon Sep 17 00:00:00 2001 From: Renan Salles Date: Tue, 24 Oct 2023 11:47:42 +0900 Subject: [PATCH 2/2] fix test for new footprint --- mbf_costmap_nav/test/free_pose_search_test.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/mbf_costmap_nav/test/free_pose_search_test.cpp b/mbf_costmap_nav/test/free_pose_search_test.cpp index 8494eb40..57d07a67 100644 --- a/mbf_costmap_nav/test/free_pose_search_test.cpp +++ b/mbf_costmap_nav/test/free_pose_search_test.cpp @@ -407,16 +407,16 @@ TEST_F(SearchHelperTest, search) 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 0 0 254 254 254 - 8.5 | 0 0 0 x 0 254 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.y, 8.5, 1e-6); - EXPECT_NEAR(sol.pose.theta, -3.0 / 4 * M_PI, 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); // Non-zero orientation SearchConfig config2{ M_PI_4, M_PI, 5.0, false, 0.0, toPose2D(1.5, 4.5, M_PI_4) }; @@ -425,7 +425,7 @@ TEST_F(SearchHelperTest, search) EXPECT_EQ(sol.search_state.state, SearchState::FREE); EXPECT_NEAR(sol.pose.x, 1.5, 1e-6); EXPECT_NEAR(sol.pose.y, 3.5, 1e-6); - EXPECT_NEAR(sol.pose.theta, 3.0 / 2 * M_PI, 1e-6); + EXPECT_NEAR(sol.pose.theta, -M_PI_2, 1e-6); } TEST_F(SearchHelperTest, service_test)