costmap_tester.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2009, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Willow Garage, Inc. nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author: Eitan Marder-Eppstein
36 *********************************************************************/
37 #include <gtest/gtest.h>
38 #include <ros/ros.h>
40 #include <costmap_2d/cost_values.h>
42 
43 namespace costmap_2d {
44 
45 class CostmapTester : public testing::Test {
46  public:
48  void checkConsistentCosts();
49  void compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y);
50  void compareCells(costmap_2d::Costmap2D& costmap,
51  unsigned int x, unsigned int y, unsigned int nx, unsigned int ny);
52  virtual void TestBody(){}
53 
54  private:
56 };
57 
58 CostmapTester::CostmapTester(tf2_ros::Buffer& tf): costmap_ros_("test_costmap", tf){}
59 
60 void CostmapTester::checkConsistentCosts(){
61  costmap_2d::Costmap2D* costmap = costmap_ros_.getCostmap();
62 
63  //get a copy of the costmap contained by our ros wrapper
64  costmap->saveMap("costmap_test.pgm");
65 
66  //loop through the costmap and check for any unexpected drop-offs in costs
67  for(unsigned int i = 0; i < costmap->getSizeInCellsX(); ++i){
68  for(unsigned int j = 0; j < costmap->getSizeInCellsY(); ++j){
69  compareCellToNeighbors(*costmap, i, j);
70  }
71  }
72 }
73 
74 void CostmapTester::compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y){
75  //we'll compare the cost of this cell with that of its eight neighbors to see if they're reasonable
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;
80 
81  //check to make sure that the neighbor cell is a legal one
82  if(nx >= 0 && nx < (int)costmap.getSizeInCellsX() && ny >=0 && ny < (int)costmap.getSizeInCellsY()){
83  compareCells(costmap, x, y, nx, ny);
84  }
85  }
86  }
87 }
88 
89 //for all lethal and inscribed costs, we'll make sure that their neighbors have the cost values we'd expect
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));
92 
93  unsigned char cell_cost = costmap.getCost(x, y);
94  unsigned char neighbor_cost = costmap.getCost(nx, ny);
95 
96  if(cell_cost == costmap_2d::LETHAL_OBSTACLE){
97  //if the cell is a lethal obstacle, then we know that all its neighbors should have equal or slighlty less cost
98  unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(cell_distance);
99  EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (cell_distance > 0 /*costmap.cell_inflation_radius_*/ && neighbor_cost == costmap_2d::FREE_SPACE));
100  }
101  else if(cell_cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
102  //the furthest valid distance from an obstacle is the inscribed radius plus the cell distance away
103  double furthest_valid_distance = 0; // ################costmap.cell_inscribed_radius_ + cell_distance + 1;
104  unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(furthest_valid_distance);
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);
109  costmap.saveMap("failing_costmap.pgm");
110  }
111  EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (furthest_valid_distance > 0/* costmap.cell_inflation_radius_ */&& neighbor_cost == costmap_2d::FREE_SPACE));
112  }
113 }
114 };
115 
117 
118 TEST(CostmapTester, checkConsistentCosts){
120 }
121 
123  int test_result = RUN_ALL_TESTS();
124  ROS_INFO("gtest return value: %d", test_result);
126 }
127 
128 int main(int argc, char** argv){
129  ros::init(argc, argv, "costmap_tester_node");
130  testing::InitGoogleTest(&argc, argv);
131 
132  ros::NodeHandle n;
133  ros::NodeHandle private_nh("~");
134 
138 
139  double wait_time;
140  private_nh.param("wait_time", wait_time, 30.0);
141  ros::Timer timer = n.createTimer(ros::Duration(wait_time), testCallback);
142 
143  ros::spin();
144 
145  return(0);
146 }
costmap_2d::CostmapTester
Definition: costmap_tester.cpp:80
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
main
int main(int argc, char **argv)
Definition: costmap_tester.cpp:128
ros.h
costmap_2d::INSCRIBED_INFLATED_OBSTACLE
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
Definition: cost_values.h:79
ros::shutdown
ROSCPP_DECL void shutdown()
costmap_2d::Costmap2D
A 2D costmap provides a mapping between points in the world and their associated "costs".
Definition: costmap_2d.h:96
TEST
TEST(CostmapTester, checkConsistentCosts)
Definition: costmap_tester.cpp:118
tf2_ros::TransformListener
costmap_2d::CostmapTester::compareCells
void compareCells(costmap_2d::Costmap2D &costmap, unsigned int x, unsigned int y, unsigned int nx, unsigned int ny)
Definition: costmap_tester.cpp:125
costmap_2d::Costmap2D::saveMap
bool saveMap(std::string file_name)
Save the costmap out to a pgm file.
Definition: costmap_2d.cpp:465
costmap_2d::Costmap2D::getSizeInCellsY
unsigned int getSizeInCellsY() const
Accessor for the y size of the costmap in cells.
Definition: costmap_2d.cpp:435
costmap_2d::LETHAL_OBSTACLE
static const unsigned char LETHAL_OBSTACLE
Definition: cost_values.h:78
tf2_ros::Buffer
ros::TimerEvent
costmap_2d::CostmapTester::CostmapTester
CostmapTester(tf2_ros::Buffer &tf)
Definition: costmap_tester.cpp:93
testCallback
void testCallback(const ros::TimerEvent &e)
Definition: costmap_tester.cpp:122
transform_listener.h
costmap_2d::CostmapTester::compareCellToNeighbors
void compareCellToNeighbors(costmap_2d::Costmap2D &costmap, unsigned int x, unsigned int y)
Definition: costmap_tester.cpp:109
cost_values.h
costmap_2d::CostmapTester::costmap_ros_
costmap_2d::Costmap2DROS costmap_ros_
Definition: costmap_tester.cpp:125
ROS_ERROR
#define ROS_ERROR(...)
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
ros::spin
ROSCPP_DECL void spin()
tf
costmap_2d::FREE_SPACE
static const unsigned char FREE_SPACE
Definition: cost_values.h:80
map_tester
costmap_2d::CostmapTester * map_tester
Definition: costmap_tester.cpp:116
costmap_2d::Costmap2D::getSizeInCellsX
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
Definition: costmap_2d.cpp:430
ROS_INFO
#define ROS_INFO(...)
costmap_2d::CostmapTester::checkConsistentCosts
void checkConsistentCosts()
Definition: costmap_tester.cpp:95
ros::NodeHandle::createTimer
Timer createTimer(Duration period, const TimerCallback &callback, bool oneshot=false, bool autostart=true) const
costmap_2d::CostmapTester::TestBody
virtual void TestBody()
Definition: costmap_tester.cpp:122
costmap_2d::Costmap2D::getCost
unsigned char getCost(unsigned int mx, unsigned int my) const
Get the cost of a cell in the costmap.
Definition: costmap_2d.cpp:192
costmap_2d
Definition: array_parser.h:37
ros::Duration
ros::Timer
costmap_2d::Costmap2DROS
A ROS wrapper for a 2D Costmap. Handles subscribing to topics that provide observations about obstacl...
Definition: costmap_2d_ros.h:74
ros::NodeHandle
costmap_2d_ros.h


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Mon Mar 6 2023 03:50:17