costmap_tester.cpp
Go to the documentation of this file.
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 }


costmap_2d
Author(s): Eitan Marder-Eppstein
autogenerated on Sat Dec 28 2013 17:13:40