DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Duong update

Browse Source
This commit is contained in:
HiepLM
2026-01-12 15:48:43 +07:00
parent 9d3d31a4f9
commit 81e7874274
14 changed files with 219 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
test/static_layer_test.cpp
View File

@@ -3,7 +3,6 @@
#include <gtest/gtest.h>
#include <tf3/buffer_core.h>
#include <robot_geometry_msgs/PoseWithCovariance.h>
#include <robot/console.h>
namespace robot_costmap_2d {
@@ -68,9 +67,9 @@ void CostmapTester::compareCells(robot_costmap_2d::Costmap2D& costmap, unsigned
double furthest_valid_distance = 0; // ################costmap.cell_inscribed_radius_ + cell_distance + 1;
unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(furthest_valid_distance);
if(neighbor_cost < expected_lowest_cost){
robot::log_error("Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f",
printf("Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f",
x, y, cell_cost, nx, ny, neighbor_cost, expected_lowest_cost, cell_distance, furthest_valid_distance);
robot::log_error("Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny);
printf("Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny);
costmap.saveMap("failing_costmap.pgm");
}
EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (furthest_valid_distance > 0/* costmap.cell_inflation_radius_ */&& neighbor_cost == robot_costmap_2d::FREE_SPACE));