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 #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   //get a copy of the costmap contained by our ros wrapper
00064   costmap->saveMap("costmap_test.pgm");
00065 
00066   //loop through the costmap and check for any unexpected drop-offs in costs
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   //we'll compare the cost of this cell with that of its eight neighbors to see if they're reasonable
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       //check to make sure that the neighbor cell is a legal one
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 //for all lethal and inscribed costs, we'll make sure that their neighbors have the cost values we'd expect
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     //if the cell is a lethal obstacle, then we know that all its neighbors should have equal or slighlty less cost
00098     unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(cell_distance);
00099     EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (cell_distance > 0 /*costmap.cell_inflation_radius_*/ && neighbor_cost == costmap_2d::FREE_SPACE));
00100   }
00101   else if(cell_cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
00102     //the furthest valid distance from an obstacle is the inscribed radius plus the cell distance away
00103     double furthest_valid_distance = 0; // ################costmap.cell_inscribed_radius_ + cell_distance + 1;
00104     unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(furthest_valid_distance);
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/* costmap.cell_inflation_radius_ */&& 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 }


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:06:55