Skip to content

Commit 8a0a828

Browse files
Nisarg236renan028
Nisarg236
and
renan028
authored
Change to use supercover (#18)
[AB#98018](https://dev.azure.com/rapyuta-robotics/AMR/_workitems/edit/98018) [AB#96427](https://dev.azure.com/rapyuta-robotics/sootballs/_workitems/edit/96427) Thanks to the original pr from [Renan](https://github.com/renan028): naturerobots#322 --------- Co-authored-by: Renan Salles <renan028@gmail.com>
1 parent 68dfb71 commit 8a0a828

2 files changed

Lines changed: 38 additions & 40 deletions

File tree

‎mbf_costmap_nav/src/mbf_costmap_nav/footprint_helper.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -314,7 +314,7 @@ std::vector<Cell> FootprintHelper::getFootprintCells(double x, double y, double
314314
return clearAndReturn(footprint_cells);
315315
}
316316

317-
getLineCells(x0, x1, y0, y1, footprint_cells);
317+
supercover(x0, x1, y0, y1, footprint_cells);
318318
}
319319

320320
//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<Cell> FootprintHelper::getFootprintCells(double x, double y, double
329329
return clearAndReturn(footprint_cells);
330330
}
331331

332-
getLineCells(x0, x1, y0, y1, footprint_cells);
332+
supercover(x0, x1, y0, y1, footprint_cells);
333333

334334
if(fill) {
335335
getFillCells(footprint_cells);

‎mbf_costmap_nav/test/free_pose_search_test.cpp‎

Lines changed: 36 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -247,8 +247,7 @@ TEST_F(SearchHelperTest, getFootprintState)
247247
EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(4.6, 4.6, M_PI)).state, state);
248248
EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(4.5, 4.5, 0)).state, state);
249249
EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(4.4, 4.4, 0)).state, SearchState::FREE);
250-
EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(4.5, 4.5, M_PI_4)).state,
251-
SearchState::FREE);
250+
EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(4.5, 4.5, M_PI_4)).state, state);
252251
EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(3, 5, 0)).state, SearchState::FREE);
253252
};
254253

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

287285
sol = FreePoseSearch::findValidOrientation(costmap, footprint, toPose2D(4.6, 4.6, 0), { M_PI, 2 * M_PI }, viz);
288286
EXPECT_EQ(sol.search_state.state, SearchState::LETHAL);
@@ -324,8 +322,8 @@ TEST_F(SearchHelperTest, search)
324322
0.5 | 0 0 0 0 0 0 0 254 254 254
325323
1.5 | 0 0 0 0 0 0 0 254 254 254
326324
2.5 | 0 0 0 254 254 254 0 0 0 0
327-
3.5 | 0 0 0 0 0 0 0 0 0 0
328-
4.5 | 0 0 0 0 0 0 x 0 0 0
325+
3.5 | 0 0 0 0 0 0 0 x 0 0
326+
4.5 | 0 0 0 0 0 0 0 0 0 0
329327
5.5 | 0 0 0 0 254 G 0 254 254 254
330328
6.5 | 0 0 0 0 254 0 0 254 254 254
331329
7.5 | 0 0 0 0 0 0 0 254 254 254
@@ -338,13 +336,13 @@ TEST_F(SearchHelperTest, search)
338336

339337
auto sol = sh.search();
340338
EXPECT_EQ(sol.search_state.state, SearchState::FREE);
341-
EXPECT_NEAR(sol.pose.x, 6.5, 1e-6);
342-
EXPECT_NEAR(sol.pose.y, 4.5, 1e-6);
343-
EXPECT_NEAR(sol.pose.theta, -M_PI_4, 1e-6);
339+
EXPECT_NEAR(sol.pose.x, 7.5, 1e-6);
340+
EXPECT_NEAR(sol.pose.y, 3.5, 1e-6);
341+
EXPECT_NEAR(sol.pose.theta, 0, 1e-6);
344342

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

350348
/*
@@ -353,24 +351,24 @@ TEST_F(SearchHelperTest, search)
353351
0.5 | 0 0 0 0 0 0 0 254 254 254
354352
1.5 | 0 0 0 0 0 0 0 254 254 254
355353
2.5 | 0 0 0 254 254 254 0 0 0 0
356-
3.5 | 0 0 0 0 254 0 0 0 0 0
357-
4.5 | 0 0 0 0 0 0 254 0 0 0
358-
5.5 | 0 0 0 0 254 G 0 254 254 254
354+
3.5 | 0 0 0 0 254 0 0 254 0 0
355+
4.5 | 0 0 0 0 0 0 0 0 0 0
356+
5.5 | 0 0 x 0 254 G 0 254 254 254
359357
6.5 | 0 0 0 0 254 0 0 254 254 254
360-
7.5 | 0 0 0 0 0 x 0 254 254 254
358+
7.5 | 0 0 0 0 0 0 0 254 254 254
361359
8.5 | 0 0 0 0 0 0 0 0 0 0
362360
9.5 | 0 0 0 0 0 0 0 0 0 0
363361
*/
364362

365363
sol = sh.search();
366364
EXPECT_EQ(sol.search_state.state, SearchState::FREE);
367-
EXPECT_NEAR(sol.pose.x, 5.5, 1e-6);
368-
EXPECT_NEAR(sol.pose.y, 7.5, 1e-6);
369-
EXPECT_NEAR(sol.pose.theta, M_PI_4, 1e-6);
365+
EXPECT_NEAR(sol.pose.x, 2.5, 1e-6);
366+
EXPECT_NEAR(sol.pose.y, 5.5, 1e-6);
367+
EXPECT_NEAR(sol.pose.theta, M_PI_2, 1e-6);
370368

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

376374
/*
@@ -379,24 +377,24 @@ TEST_F(SearchHelperTest, search)
379377
0.5 | 0 0 0 0 0 0 0 254 254 254
380378
1.5 | 0 0 0 0 0 0 0 254 254 254
381379
2.5 | 0 0 0 254 254 254 0 0 0 0
382-
3.5 | 0 0 0 0 254 0 0 0 0 0
383-
4.5 | 0 0 0 x 0 0 254 0 0 0
384-
5.5 | 0 0 0 0 254 G 0 254 254 254
380+
3.5 | 0 0 0 0 254 0 0 254 0 0
381+
4.5 | 0 0 0 0 0 0 0 0 0 0
382+
5.5 | 0 0 254 0 254 G 0 254 254 254
385383
6.5 | 0 0 0 0 254 0 0 254 254 254
386-
7.5 | 0 0 0 0 0 254 0 254 254 254
387-
8.5 | 0 0 0 0 0 0 0 0 0 0
384+
7.5 | 0 0 0 0 0 0 0 254 254 254
385+
8.5 | 0 0 0 0 0 x 0 0 0 0
388386
9.5 | 0 0 0 0 0 0 0 0 0 0
389387
*/
390388

391389
sol = sh.search();
392390
EXPECT_EQ(sol.search_state.state, SearchState::FREE);
393-
EXPECT_NEAR(sol.pose.x, 3.5, 1e-6);
394-
EXPECT_NEAR(sol.pose.y, 4.5, 1e-6);
395-
EXPECT_NEAR(sol.pose.theta, 3 * M_PI_4, 1e-6);
391+
EXPECT_NEAR(sol.pose.x, 5.5, 1e-6);
392+
EXPECT_NEAR(sol.pose.y, 8.5, 1e-6);
393+
EXPECT_NEAR(sol.pose.theta, M_PI, 1e-6);
396394

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

402400
/*
@@ -405,18 +403,18 @@ TEST_F(SearchHelperTest, search)
405403
0.5 | 0 0 0 0 0 0 0 254 254 254
406404
1.5 | 0 0 0 0 0 0 0 254 254 254
407405
2.5 | 0 0 0 254 254 254 0 0 0 0
408-
3.5 | 0 0 0 0 254 0 0 0 0 0
409-
4.5 | 0 0 0 254 0 0 254 0 0 0
410-
5.5 | 0 0 0 0 254 G 0 254 254 254
406+
3.5 | 0 0 0 0 254 0 0 254 0 0
407+
4.5 | 0 0 0 0 0 0 0 0 0 0
408+
5.5 | 0 0 254 0 254 G 0 254 254 254
411409
6.5 | 0 0 0 0 254 0 0 254 254 254
412-
7.5 | 0 0 0 x 0 254 0 254 254 254
413-
8.5 | 0 0 0 0 0 0 0 0 0 0
410+
7.5 | 0 0 x 0 0 0 0 254 254 254
411+
8.5 | 0 0 0 0 0 254 0 0 0 0
414412
9.5 | 0 0 0 0 0 0 0 0 0 0
415413
*/
416414

417415
sol = sh.search();
418416
EXPECT_EQ(sol.search_state.state, SearchState::FREE);
419-
EXPECT_NEAR(sol.pose.x, 3.5, 1e-6);
417+
EXPECT_NEAR(sol.pose.x, 2.5, 1e-6);
420418
EXPECT_NEAR(sol.pose.y, 7.5, 1e-6);
421419
EXPECT_NEAR(sol.pose.theta, M_PI_4, 1e-6);
422420

@@ -426,8 +424,8 @@ TEST_F(SearchHelperTest, search)
426424
sol = sh2.search();
427425
EXPECT_EQ(sol.search_state.state, SearchState::FREE);
428426
EXPECT_NEAR(sol.pose.x, 1.5, 1e-6);
429-
EXPECT_NEAR(sol.pose.y, 4.5, 1e-6);
430-
EXPECT_NEAR(sol.pose.theta, M_PI_4, 1e-6);
427+
EXPECT_NEAR(sol.pose.y, 3.5, 1e-6);
428+
EXPECT_NEAR(sol.pose.theta, -M_PI_2, 1e-6);
431429
}
432430

433431
TEST_F(SearchHelperTest, service_test)

0 commit comments

Comments
 (0)