Skip to content

Commit cad2fa1

Browse files
renan028renan028
committed
change to use supercover
1 parent cc5351a commit cad2fa1

File tree

2 files changed

+40
-41
lines changed

2 files changed

+40
-41
lines changed

‎mbf_costmap_nav/src/mbf_costmap_nav/footprint_helper.cpp

+2-2
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

+38-39
Original file line numberDiff line numberDiff line change
@@ -246,8 +246,7 @@ TEST_F(SearchHelperTest, getFootprintState)
246246
EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(4.6, 4.6, M_PI)).state, state);
247247
EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(4.5, 4.5, 0)).state, state);
248248
EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(4.4, 4.4, 0)).state, SearchState::FREE);
249-
EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(4.5, 4.5, M_PI_4)).state,
250-
SearchState::FREE);
249+
EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(4.5, 4.5, M_PI_4)).state, state);
251250
EXPECT_EQ(FreePoseSearch::getFootprintState(costmap, footprint, toPose2D(3, 5, 0)).state, SearchState::FREE);
252251
};
253252

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

286284
sol = FreePoseSearch::findValidOrientation(costmap, footprint, toPose2D(4.6, 4.6, 0), { M_PI, 2 * M_PI }, viz);
287285
EXPECT_EQ(sol.search_state.state, SearchState::LETHAL);
@@ -323,8 +321,8 @@ TEST_F(SearchHelperTest, search)
323321
0.5 | 0 0 0 0 0 0 0 254 254 254
324322
1.5 | 0 0 0 0 0 0 0 254 254 254
325323
2.5 | 0 0 0 254 254 254 0 0 0 0
326-
3.5 | 0 0 0 0 0 0 0 0 0 0
327-
4.5 | 0 0 0 0 0 0 x 0 0 0
324+
3.5 | 0 0 0 0 0 0 0 x 0 0
325+
4.5 | 0 0 0 0 0 0 0 0 0 0
328326
5.5 | 0 0 0 0 254 G 0 254 254 254
329327
6.5 | 0 0 0 0 254 0 0 254 254 254
330328
7.5 | 0 0 0 0 0 0 0 254 254 254
@@ -337,13 +335,13 @@ TEST_F(SearchHelperTest, search)
337335

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

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

349347
/*
@@ -352,24 +350,24 @@ TEST_F(SearchHelperTest, search)
352350
0.5 | 0 0 0 0 0 0 0 254 254 254
353351
1.5 | 0 0 0 0 0 0 0 254 254 254
354352
2.5 | 0 0 0 254 254 254 0 0 0 0
355-
3.5 | 0 0 0 0 254 0 0 0 0 0
356-
4.5 | 0 0 0 0 0 0 254 0 0 0
357-
5.5 | 0 0 0 0 254 G 0 254 254 254
353+
3.5 | 0 0 0 0 254 0 0 254 0 0
354+
4.5 | 0 0 0 0 0 0 0 0 0 0
355+
5.5 | 0 0 x 0 254 G 0 254 254 254
358356
6.5 | 0 0 0 0 254 0 0 254 254 254
359-
7.5 | 0 0 0 0 0 x 0 254 254 254
357+
7.5 | 0 0 0 0 0 0 0 254 254 254
360358
8.5 | 0 0 0 0 0 0 0 0 0 0
361359
9.5 | 0 0 0 0 0 0 0 0 0 0
362360
*/
363361

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

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

375373
/*
@@ -378,24 +376,24 @@ TEST_F(SearchHelperTest, search)
378376
0.5 | 0 0 0 0 0 0 0 254 254 254
379377
1.5 | 0 0 0 0 0 0 0 254 254 254
380378
2.5 | 0 0 0 254 254 254 0 0 0 0
381-
3.5 | 0 0 0 0 254 0 0 0 0 0
382-
4.5 | 0 0 0 x 0 0 254 0 0 0
383-
5.5 | 0 0 0 0 254 G 0 254 254 254
379+
3.5 | 0 0 0 0 254 0 0 254 0 0
380+
4.5 | 0 0 0 0 0 0 0 0 0 0
381+
5.5 | 0 0 254 0 254 G 0 254 254 254
384382
6.5 | 0 0 0 0 254 0 0 254 254 254
385-
7.5 | 0 0 0 0 0 254 0 254 254 254
386-
8.5 | 0 0 0 0 0 0 0 0 0 0
383+
7.5 | 0 0 0 0 0 0 0 254 254 254
384+
8.5 | 0 0 0 0 0 x 0 0 0 0
387385
9.5 | 0 0 0 0 0 0 0 0 0 0
388386
*/
389387

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

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

401399
/*
@@ -404,29 +402,29 @@ TEST_F(SearchHelperTest, search)
404402
0.5 | 0 0 0 0 0 0 0 254 254 254
405403
1.5 | 0 0 0 0 0 0 0 254 254 254
406404
2.5 | 0 0 0 254 254 254 0 0 0 0
407-
3.5 | 0 0 0 0 254 0 0 0 0 0
408-
4.5 | 0 0 0 254 0 0 254 0 0 0
409-
5.5 | 0 0 0 0 254 G 0 254 254 254
405+
3.5 | 0 0 0 0 254 0 0 254 0 0
406+
4.5 | 0 0 0 0 0 0 0 0 0 0
407+
5.5 | 0 0 254 0 254 G 0 254 254 254
410408
6.5 | 0 0 0 0 254 0 0 254 254 254
411-
7.5 | 0 0 0 x 0 254 0 254 254 254
412-
8.5 | 0 0 0 0 0 0 0 0 0 0
409+
7.5 | 0 0 0 0 0 0 0 254 254 254
410+
8.5 | 0 0 0 x 0 254 0 0 0 0
413411
9.5 | 0 0 0 0 0 0 0 0 0 0
414412
*/
415413

416414
sol = sh.search();
417415
EXPECT_EQ(sol.search_state.state, SearchState::FREE);
418416
EXPECT_NEAR(sol.pose.x, 3.5, 1e-6);
419-
EXPECT_NEAR(sol.pose.y, 7.5, 1e-6);
420-
EXPECT_NEAR(sol.pose.theta, M_PI_4, 1e-6);
417+
EXPECT_NEAR(sol.pose.y, 8.5, 1e-6);
418+
EXPECT_NEAR(sol.pose.theta, -3.0 / 4 * M_PI, 1e-6);
421419

422420
// Non-zero orientation
423421
SearchConfig config2{ M_PI_4, M_PI, 5.0, false, 0.0, toPose2D(1.5, 4.5, M_PI_4) };
424422
FreePoseSearch sh2(cm, config2, std::nullopt, viz);
425423
sol = sh2.search();
426424
EXPECT_EQ(sol.search_state.state, SearchState::FREE);
427425
EXPECT_NEAR(sol.pose.x, 1.5, 1e-6);
428-
EXPECT_NEAR(sol.pose.y, 4.5, 1e-6);
429-
EXPECT_NEAR(sol.pose.theta, M_PI_4, 1e-6);
426+
EXPECT_NEAR(sol.pose.y, 3.5, 1e-6);
427+
EXPECT_NEAR(sol.pose.theta, 3.0 / 2 * M_PI, 1e-6);
430428
}
431429

432430
TEST_F(SearchHelperTest, service_test)
@@ -488,6 +486,7 @@ TEST_F(SearchHelperTest, enforce_bounds)
488486
EXPECT_NEAR(sol.pose.y, 0.5, 1e-6);
489487
EXPECT_NEAR(sol.pose.theta, 0, 1e-6);
490488
}
489+
491490
} // namespace mbf_costmap_nav::test
492491

493492
int main(int argc, char** argv)

0 commit comments

Comments
 (0)