37 #include <gtest/gtest.h>
41 #include <tf2_ros/transform_listener.h>
45 class CostmapTester :
public testing::Test {
51 unsigned int x,
unsigned int y,
unsigned int nx,
unsigned int ny);
60 void CostmapTester::checkConsistentCosts(){
64 costmap->
saveMap(
"costmap_test.pgm");
69 compareCellToNeighbors(*costmap, i, j);
74 void CostmapTester::compareCellToNeighbors(
costmap_2d::Costmap2D& costmap,
unsigned int x,
unsigned int y){
76 for(
int offset_x = -1; offset_x <= 1; ++offset_x){
77 for(
int offset_y = -1; offset_y <= 1; ++offset_y){
78 int nx = x + offset_x;
79 int ny = y + offset_y;
83 compareCells(costmap, x, y, nx, ny);
90 void CostmapTester::compareCells(
costmap_2d::Costmap2D& costmap,
unsigned int x,
unsigned int y,
unsigned int nx,
unsigned int ny){
91 double cell_distance =
hypot(
static_cast<int>(x-nx),
static_cast<int>(y-ny));
93 unsigned char cell_cost = costmap.
getCost(x, y);
94 unsigned char neighbor_cost = costmap.
getCost(nx, ny);
98 unsigned char expected_lowest_cost = 0;
99 EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (cell_distance > 0 && neighbor_cost ==
costmap_2d::FREE_SPACE));
103 double furthest_valid_distance = 0;
104 unsigned char expected_lowest_cost = 0;
105 if(neighbor_cost < expected_lowest_cost){
106 ROS_ERROR(
"Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f",
107 x, y, cell_cost, nx, ny, neighbor_cost, expected_lowest_cost, cell_distance, furthest_valid_distance);
108 ROS_ERROR(
"Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny);
111 EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (furthest_valid_distance > 0&& neighbor_cost ==
costmap_2d::FREE_SPACE));
118 TEST(CostmapTester, checkConsistentCosts){
123 int test_result = RUN_ALL_TESTS();
124 ROS_INFO(
"gtest return value: %d", test_result);
128 int main(
int argc,
char** argv){
129 ros::init(argc, argv,
"costmap_tester_node");
130 testing::InitGoogleTest(&argc, argv);
133 ros::NodeHandle private_nh(
"~");
135 tf2_ros::Buffer tf(ros::Duration(10));
136 tf2_ros::TransformListener tfl(tf);
140 private_nh.param(
"wait_time", wait_time, 30.0);
141 ros::Timer timer = n.createTimer(ros::Duration(wait_time),
testCallback);