$search
00001 /********************************************************************* 00002 * 00003 * Software License Agreement (BSD License) 00004 * 00005 * Copyright (c) 2009, Willow Garage, Inc. 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of Willow Garage, Inc. nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 * Author: Eitan Marder-Eppstein 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 00042 namespace costmap_2d { 00043 00044 class CostmapTester : public testing::Test { 00045 public: 00046 CostmapTester(tf::TransformListener& tf); 00047 void checkConsistentCosts(); 00048 void compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y); 00049 void compareCells(costmap_2d::Costmap2D& costmap, 00050 unsigned int x, unsigned int y, unsigned int nx, unsigned int ny); 00051 virtual void TestBody(){} 00052 00053 private: 00054 costmap_2d::Costmap2DROS costmap_ros_; 00055 }; 00056 00057 CostmapTester::CostmapTester(tf::TransformListener& tf): costmap_ros_("test_costmap", tf){} 00058 00059 void CostmapTester::checkConsistentCosts(){ 00060 costmap_2d::Costmap2D costmap; 00061 00062 //get a copy of the costmap contained by our ros wrapper 00063 costmap_ros_.getCostmapCopy(costmap); 00064 00065 costmap.saveMap("costmap_test.pgm"); 00066 00067 //loop through the costmap and check for any unexpected drop-offs in costs 00068 for(unsigned int i = 0; i < costmap.getSizeInCellsX(); ++i){ 00069 for(unsigned int j = 0; j < costmap.getSizeInCellsY(); ++j){ 00070 compareCellToNeighbors(costmap, i, j); 00071 } 00072 } 00073 } 00074 00075 void CostmapTester::compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y){ 00076 //we'll compare the cost of this cell with that of its eight neighbors to see if they're reasonable 00077 for(int offset_x = -1; offset_x <= 1; ++offset_x){ 00078 for(int offset_y = -1; offset_y <= 1; ++offset_y){ 00079 int nx = x + offset_x; 00080 int ny = y + offset_y; 00081 00082 //check to make sure that the neighbor cell is a legal one 00083 if(nx >= 0 && nx < (int)costmap.getSizeInCellsX() && ny >=0 && ny < (int)costmap.getSizeInCellsY()){ 00084 compareCells(costmap, x, y, nx, ny); 00085 } 00086 } 00087 } 00088 } 00089 00090 //for all lethal and inscribed costs, we'll make sure that their neighbors have the cost values we'd expect 00091 void CostmapTester::compareCells(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y, unsigned int nx, unsigned int ny){ 00092 double cell_distance = sqrt((x - nx) * (x - nx) + (y - ny) * (y - ny)); 00093 00094 unsigned char cell_cost = costmap.getCost(x, y); 00095 unsigned char neighbor_cost = costmap.getCost(nx, ny); 00096 00097 if(cell_cost == costmap_2d::LETHAL_OBSTACLE){ 00098 //if the cell is a lethal obstacle, then we know that all its neighbors should have equal or slighlty less cost 00099 unsigned char expected_lowest_cost = costmap.computeCost(cell_distance); 00100 EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (cell_distance > costmap.cell_inflation_radius_ && neighbor_cost == costmap_2d::FREE_SPACE)); 00101 } 00102 else if(cell_cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ 00103 //the furthest valid distance from an obstacle is the inscribed radius plus the cell distance away 00104 double furthest_valid_distance = costmap.cell_inscribed_radius_ + cell_distance + 1; 00105 unsigned char expected_lowest_cost = costmap.computeCost(furthest_valid_distance); 00106 if(neighbor_cost < expected_lowest_cost){ 00107 ROS_ERROR("Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f", 00108 x, y, cell_cost, nx, ny, neighbor_cost, expected_lowest_cost, cell_distance, furthest_valid_distance); 00109 ROS_ERROR("Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny); 00110 costmap.saveMap("failing_costmap.pgm"); 00111 } 00112 EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (furthest_valid_distance > costmap.cell_inflation_radius_ && neighbor_cost == costmap_2d::FREE_SPACE)); 00113 } 00114 } 00115 }; 00116 00117 costmap_2d::CostmapTester* map_tester = NULL; 00118 00119 TEST(CostmapTester, checkConsistentCosts){ 00120 map_tester->checkConsistentCosts(); 00121 } 00122 00123 void testCallback(const ros::TimerEvent& e){ 00124 int test_result = RUN_ALL_TESTS(); 00125 ROS_INFO("gtest return value: %d", test_result); 00126 ros::shutdown(); 00127 } 00128 00129 int main(int argc, char** argv){ 00130 ros::init(argc, argv, "costmap_tester_node"); 00131 testing::InitGoogleTest(&argc, argv); 00132 00133 ros::NodeHandle n; 00134 ros::NodeHandle private_nh("~"); 00135 00136 tf::TransformListener tf(ros::Duration(10)); 00137 map_tester = new costmap_2d::CostmapTester(tf); 00138 00139 double wait_time; 00140 private_nh.param("wait_time", wait_time, 30.0); 00141 ros::Timer timer = n.createTimer(ros::Duration(wait_time), testCallback); 00142 00143 ros::spin(); 00144 00145 return(0); 00146 }