obstacle_tests.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
35 #include <costmap_2d/costmap_2d.h>
39 #include <set>
40 #include <gtest/gtest.h>
41 #include <tf/transform_listener.h>
42 
43 using namespace costmap_2d;
44 
45 /*
46  * For reference, the static map looks like this:
47  *
48  * 0 0 0 0 0 0 0 254 254 254
49  *
50  * 0 0 0 0 0 0 0 254 254 254
51  *
52  * 0 0 0 254 254 254 0 0 0 0
53  *
54  * 0 0 0 0 0 0 0 0 0 0
55  *
56  * 0 0 0 0 0 0 0 0 0 0
57  *
58  * 0 0 0 0 254 0 0 254 254 254
59  *
60  * 0 0 0 0 254 0 0 254 254 254
61  *
62  * 0 0 0 0 0 0 0 254 254 254
63  *
64  * 0 0 0 0 0 0 0 0 0 0
65  *
66  * 0 0 0 0 0 0 0 0 0 0
67  *
68  * upper left is 0,0, lower right is 9,9
69  */
70 
74 TEST(costmap, testRaytracing){
76 
77  LayeredCostmap layers("frame", false, false); // Not rolling window, not tracking unknown
78  addStaticLayer(layers, tf); // This adds the static map
79  ObstacleLayer* olayer = addObstacleLayer(layers, tf);
80 
81  // Add a point at 0, 0, 0
82  addObservation(olayer, 0.0, 0.0, MAX_Z/2, 0, 0, MAX_Z/2);
83 
84  // This actually puts the LETHAL (254) point in the costmap at (0,0)
85  layers.updateMap(0,0,0); // 0, 0, 0 is robot pose
86  //printMap(*(layers.getCostmap()));
87 
88  int lethal_count = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE);
89 
90  // We expect just one obstacle to be added (20 in static map)
91  ASSERT_EQ(lethal_count, 21);
92 }
93 
97 TEST(costmap, testRaytracing2){
99  LayeredCostmap layers("frame", false, false);
100  addStaticLayer(layers, tf);
101  ObstacleLayer* olayer = addObstacleLayer(layers, tf);
102 
103  //If we print map now, it is 10x10 all value 0
104  //printMap(*(layers.getCostmap()));
105 
106  // Update will fill in the costmap with the static map
107  layers.updateMap(0,0,0);
108 
109  //If we print the map now, we get the static map
110  //printMap(*(layers.getCostmap()));
111 
112  // Static map has 20 LETHAL cells (see diagram above)
113  int obs_before = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE);
114  ASSERT_EQ(obs_before, 20);
115 
116  // The sensor origin will be <0,0>. So if we add an obstacle at 9,9,
117  // we would expect cells <0, 0> thru <8, 8> to be traced through
118  // however the static map is not cleared by obstacle layer
119  addObservation(olayer, 9.5, 9.5, MAX_Z/2, 0.5, 0.5, MAX_Z/2);
120  layers.updateMap(0, 0, 0);
121 
122  // If we print map now, we have static map + <9,9> is LETHAL
123  //printMap(*(layers.getCostmap()));
124  int obs_after = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE);
125 
126  // Change from previous test:
127  // No obstacles from the static map will be cleared, so the
128  // net change is +1.
129  ASSERT_EQ(obs_after, obs_before + 1);
130 
131  // Fill in the diagonal, <7,7> and <9,9> already filled in, <0,0> is robot
132  for(int i = 0; i < olayer->getSizeInCellsY(); ++i)
133  {
134  olayer->setCost(i, i, LETHAL_OBSTACLE);
135  }
136  // This will updateBounds, which will raytrace the static observation added
137  // above, thus clearing out the diagonal again!
138  layers.updateMap(0, 0, 0);
139 
140  // Map now has diagonal except <0,0> filled with LETHAL (254)
141  //printMap(*(layers.getCostmap()));
142  int with_static = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE);
143 
144  // Should thus be the same
145  ASSERT_EQ(with_static, obs_after);
146  // If 21 are filled, 79 should be free
147  ASSERT_EQ(79, countValues(*(layers.getCostmap()), costmap_2d::FREE_SPACE));
148 }
149 
153 TEST(costmap, testWaveInterference){
155 
156  // Start with an empty map, no rolling window, tracking unknown
157  LayeredCostmap layers("frame", false, true);
158  layers.resizeMap(10, 10, 1, 0, 0);
159  ObstacleLayer* olayer = addObstacleLayer(layers, tf);
160 
161  // If we print map now, it is 10x10, all cells are 255 (NO_INFORMATION)
162  //printMap(*(layers.getCostmap()));
163 
164  // Lay out 3 obstacles in a line - along the diagonal, separated by a cell.
165  addObservation(olayer, 3.0, 3.0, MAX_Z);
166  addObservation(olayer, 5.0, 5.0, MAX_Z);
167  addObservation(olayer, 7.0, 7.0, MAX_Z);
168  layers.updateMap(0,0,0);
169 
170  Costmap2D* costmap = layers.getCostmap();
171  // 3 obstacle cells are filled, <1,1>,<2,2>,<4,4> and <6,6> are now free
172  // <0,0> is footprint and is free
173  //printMap(*costmap);
174  ASSERT_EQ(3, countValues(*costmap, costmap_2d::LETHAL_OBSTACLE));
175  ASSERT_EQ(92, countValues(*costmap, costmap_2d::NO_INFORMATION));
176  ASSERT_EQ(5, countValues(*costmap, costmap_2d::FREE_SPACE));
177 }
178 
182 TEST(costmap, testZThreshold){
184  // Start with an empty map
185  LayeredCostmap layers("frame", false, true);
186  layers.resizeMap(10, 10, 1, 0, 0);
187 
188  ObstacleLayer* olayer = addObstacleLayer(layers, tf);
189 
190  // A point cloud with 2 points falling in a cell with a non-lethal cost
191  addObservation(olayer, 0.0, 5.0, 0.4);
192  addObservation(olayer, 1.0, 5.0, 2.2);
193 
194  layers.updateMap(0,0,0);
195 
196  Costmap2D* costmap = layers.getCostmap();
197  ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 1);
198 }
199 
200 
204 TEST(costmap, testDynamicObstacles){
206  LayeredCostmap layers("frame", false, false);
207  addStaticLayer(layers, tf);
208 
209  ObstacleLayer* olayer = addObstacleLayer(layers, tf);
210 
211  // Add a point cloud and verify its insertion. There should be only one new one
212  addObservation(olayer, 0.0, 0.0);
213  addObservation(olayer, 0.0, 0.0);
214  addObservation(olayer, 0.0, 0.0);
215 
216  layers.updateMap(0,0,0);
217 
218  Costmap2D* costmap = layers.getCostmap();
219  // Should now have 1 insertion and no deletions
220  ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 21);
221 
222  // Repeating the call - we should see no insertions or deletions
223  ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 21);
224 }
225 
226 
230 TEST(costmap, testMultipleAdditions){
232  LayeredCostmap layers("frame", false, false);
233  addStaticLayer(layers, tf);
234 
235  ObstacleLayer* olayer = addObstacleLayer(layers, tf);
236 
237  // A point cloud with one point that falls within an existing obstacle
238  addObservation(olayer, 9.5, 0.0);
239  layers.updateMap(0,0,0);
240  Costmap2D* costmap = layers.getCostmap();
241  //printMap(*costmap);
242 
243  ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 20);
244 
245 }
246 
247 
248 int main(int argc, char** argv){
249  ros::init(argc, argv, "obstacle_tests");
250  testing::InitGoogleTest(&argc, argv);
251  return RUN_ALL_TESTS();
252 }
void setCost(unsigned int mx, unsigned int my, unsigned char cost)
Set the cost of a cell in the costmap.
Definition: costmap_2d.cpp:197
unsigned int countValues(costmap_2d::Costmap2D &costmap, unsigned char value, bool equal=true)
const double MAX_Z(1.0)
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y, bool size_locked=false)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
costmap_2d::ObstacleLayer * addObstacleLayer(costmap_2d::LayeredCostmap &layers, tf::TransformListener &tf)
void addStaticLayer(costmap_2d::LayeredCostmap &layers, tf::TransformListener &tf)
static const unsigned char FREE_SPACE
Definition: cost_values.h:45
TEST(costmap, testRaytracing)
unsigned int getSizeInCellsY() const
Accessor for the y size of the costmap in cells.
Definition: costmap_2d.cpp:431
static const unsigned char LETHAL_OBSTACLE
Definition: cost_values.h:43
static const unsigned char NO_INFORMATION
Definition: cost_values.h:42
void addObservation(costmap_2d::ObstacleLayer *olayer, double x, double y, double z=0.0, double ox=0.0, double oy=0.0, double oz=MAX_Z)
A 2D costmap provides a mapping between points in the world and their associated "costs".
Definition: costmap_2d.h:60
Instantiates different layer plugins and aggregates them into one score.
void updateMap(double robot_x, double robot_y, double robot_yaw)
Update the underlying costmap with new data. If you want to update the map outside of the update loop...
int main(int argc, char **argv)


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:42