00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <gtest/gtest.h>
00038 #include <ros/ros.h>
00039 #include <tf/transform_listener.h>
00040 #include <costmap_2d/costmap_2d_ros.h>
00041 #include <costmap_2d/cost_values.h>
00042
00043 namespace costmap_2d {
00044
00045 class CostmapTester : public testing::Test {
00046 public:
00047 CostmapTester(tf::TransformListener& tf);
00048 void checkConsistentCosts();
00049 void compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y);
00050 void compareCells(costmap_2d::Costmap2D& costmap,
00051 unsigned int x, unsigned int y, unsigned int nx, unsigned int ny);
00052 virtual void TestBody(){}
00053
00054 private:
00055 costmap_2d::Costmap2DROS costmap_ros_;
00056 };
00057
00058 CostmapTester::CostmapTester(tf::TransformListener& tf): costmap_ros_("test_costmap", tf){}
00059
00060 void CostmapTester::checkConsistentCosts(){
00061 costmap_2d::Costmap2D* costmap = costmap_ros_.getCostmap();
00062
00063
00064 costmap->saveMap("costmap_test.pgm");
00065
00066
00067 for(unsigned int i = 0; i < costmap->getSizeInCellsX(); ++i){
00068 for(unsigned int j = 0; j < costmap->getSizeInCellsY(); ++j){
00069 compareCellToNeighbors(*costmap, i, j);
00070 }
00071 }
00072 }
00073
00074 void CostmapTester::compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y){
00075
00076 for(int offset_x = -1; offset_x <= 1; ++offset_x){
00077 for(int offset_y = -1; offset_y <= 1; ++offset_y){
00078 int nx = x + offset_x;
00079 int ny = y + offset_y;
00080
00081
00082 if(nx >= 0 && nx < (int)costmap.getSizeInCellsX() && ny >=0 && ny < (int)costmap.getSizeInCellsY()){
00083 compareCells(costmap, x, y, nx, ny);
00084 }
00085 }
00086 }
00087 }
00088
00089
00090 void CostmapTester::compareCells(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y, unsigned int nx, unsigned int ny){
00091 double cell_distance = hypot(static_cast<int>(x-nx), static_cast<int>(y-ny));
00092
00093 unsigned char cell_cost = costmap.getCost(x, y);
00094 unsigned char neighbor_cost = costmap.getCost(nx, ny);
00095
00096 if(cell_cost == costmap_2d::LETHAL_OBSTACLE){
00097
00098 unsigned char expected_lowest_cost = 0;
00099 EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (cell_distance > 0 && neighbor_cost == costmap_2d::FREE_SPACE));
00100 }
00101 else if(cell_cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
00102
00103 double furthest_valid_distance = 0;
00104 unsigned char expected_lowest_cost = 0;
00105 if(neighbor_cost < expected_lowest_cost){
00106 ROS_ERROR("Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f",
00107 x, y, cell_cost, nx, ny, neighbor_cost, expected_lowest_cost, cell_distance, furthest_valid_distance);
00108 ROS_ERROR("Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny);
00109 costmap.saveMap("failing_costmap.pgm");
00110 }
00111 EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (furthest_valid_distance > 0&& neighbor_cost == costmap_2d::FREE_SPACE));
00112 }
00113 }
00114 };
00115
00116 costmap_2d::CostmapTester* map_tester = NULL;
00117
00118 TEST(CostmapTester, checkConsistentCosts){
00119 map_tester->checkConsistentCosts();
00120 }
00121
00122 void testCallback(const ros::TimerEvent& e){
00123 int test_result = RUN_ALL_TESTS();
00124 ROS_INFO("gtest return value: %d", test_result);
00125 ros::shutdown();
00126 }
00127
00128 int main(int argc, char** argv){
00129 ros::init(argc, argv, "costmap_tester_node");
00130 testing::InitGoogleTest(&argc, argv);
00131
00132 ros::NodeHandle n;
00133 ros::NodeHandle private_nh("~");
00134
00135 tf::TransformListener tf(ros::Duration(10));
00136 map_tester = new costmap_2d::CostmapTester(tf);
00137
00138 double wait_time;
00139 private_nh.param("wait_time", wait_time, 30.0);
00140 ros::Timer timer = n.createTimer(ros::Duration(wait_time), testCallback);
00141
00142 ros::spin();
00143
00144 return(0);
00145 }