costmap_2d
module_tests.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, 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>
36 #include <costmap_2d/observation_buffer.h>
37 #include <set>
38 #include <gtest/gtest.h>
39 
40 using namespace costmap_2d;
41 
42 const unsigned char MAP_10_BY_10_CHAR[] = {
43  0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
44  0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
45  0, 0, 0, 0, 0, 0, 0, 200, 200, 200,
46  0, 0, 0, 0, 100, 0, 0, 200, 200, 200,
47  0, 0, 0, 0, 100, 0, 0, 200, 200, 200,
48  70, 70, 0, 0, 0, 0, 0, 0, 0, 0,
49  0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
50  0, 0, 0, 200, 200, 200, 0, 0, 0, 0,
51  0, 0, 0, 0, 0, 0, 0, 255, 255, 255,
52  0, 0, 0, 0, 0, 0, 0, 255, 255, 255
53 };
54 
55 const unsigned char MAP_5_BY_5_CHAR[] = {
56  0, 0, 0, 0, 0,
57  0, 0, 0, 0, 0,
58  0, 0, 0, 0, 0,
59  0, 0, 0, 0, 0,
60  0, 0, 0, 0, 0,
61 };
62 
63 std::vector<unsigned char> MAP_5_BY_5;
64 std::vector<unsigned char> MAP_10_BY_10;
65 std::vector<unsigned char> EMPTY_10_BY_10;
66 std::vector<unsigned char> EMPTY_100_BY_100;
67 
68 const unsigned int GRID_WIDTH(10);
69 const unsigned int GRID_HEIGHT(10);
70 const double RESOLUTION(1);
71 const double WINDOW_LENGTH(10);
72 const unsigned char THRESHOLD(100);
73 const double MAX_Z(1.0);
74 const double RAYTRACE_RANGE(20.0);
75 const double OBSTACLE_RANGE(20.0);
76 const double ROBOT_RADIUS(1.0);
77 
78 bool find(const std::vector<unsigned int>& l, unsigned int n){
79  for(std::vector<unsigned int>::const_iterator it = l.begin(); it != l.end(); ++it){
80  if(*it == n)
81  return true;
82  }
83 
84  return false;
85 }
86 
90 TEST(costmap, testResetForStaticMap){
91  // Define a static map with a large object in the center
92  std::vector<unsigned char> staticMap;
93  for(unsigned int i=0; i<10; i++){
94  for(unsigned int j=0; j<10; j++){
95  staticMap.push_back(costmap_2d::LETHAL_OBSTACLE);
96  }
97  }
98 
99  // Allocate the cost map, with a inflation to 3 cells all around
100  Costmap2D map(10, 10, RESOLUTION, 0.0, 0.0, 3, 3, 3, OBSTACLE_RANGE, MAX_Z, RAYTRACE_RANGE, 25, staticMap, THRESHOLD);
101 
102  // Populate the cost map with a wall around the perimeter. Free space should clear out the room.
103  pcl::PointCloud<pcl::PointXYZ> cloud;
104  cloud.points.resize(40);
105 
106  // Left wall
107  unsigned int ind = 0;
108  for (unsigned int i = 0; i < 10; i++){
109  // Left
110  cloud.points[ind].x = 0;
111  cloud.points[ind].y = i;
112  cloud.points[ind].z = MAX_Z;
113  ind++;
114 
115  // Top
116  cloud.points[ind].x = i;
117  cloud.points[ind].y = 0;
118  cloud.points[ind].z = MAX_Z;
119  ind++;
120 
121  // Right
122  cloud.points[ind].x = 9;
123  cloud.points[ind].y = i;
124  cloud.points[ind].z = MAX_Z;
125  ind++;
126 
127  // Bottom
128  cloud.points[ind].x = i;
129  cloud.points[ind].y = 9;
130  cloud.points[ind].z = MAX_Z;
131  ind++;
132  }
133 
134  double wx = 5.0, wy = 5.0;
135  geometry_msgs::Point p;
136  p.x = wx;
137  p.y = wy;
138  p.z = MAX_Z;
139  Observation obs(p, cloud, OBSTACLE_RANGE, RAYTRACE_RANGE);
140  std::vector<Observation> obsBuf;
141  obsBuf.push_back(obs);
142 
143  // Update the cost map for this observation
144  map.updateWorld(wx, wy, obsBuf, obsBuf);
145 
146  // Verify that we now have only 36 cells with lethal cost, thus provong that we have correctly cleared
147  // free space
148  int hitCount = 0;
149  for(unsigned int i=0; i < 10; ++i){
150  for(unsigned int j=0; j < 10; ++j){
151  if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){
152  hitCount++;
153  }
154  }
155  }
156  ASSERT_EQ(hitCount, 36);
157 
158  // Veriy that we have 64 non-leathal
159  hitCount = 0;
160  for(unsigned int i=0; i < 10; ++i){
161  for(unsigned int j=0; j < 10; ++j){
162  if(map.getCost(i, j) != costmap_2d::LETHAL_OBSTACLE)
163  hitCount++;
164  }
165  }
166  ASSERT_EQ(hitCount, 64);
167 
168  // Now if we reset the cost map, we should have our map go back to being completely occupied
169  map.resetMapOutsideWindow(wx, wy, 0.0, 0.0);
170 
171  //We should now go back to everything being occupied
172  hitCount = 0;
173  for(unsigned int i=0; i < 10; ++i){
174  for(unsigned int j=0; j < 10; ++j){
175  if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE)
176  hitCount++;
177  }
178  }
179  ASSERT_EQ(hitCount, 100);
180 
181 }
182 
186 TEST(costmap, testCostFunctionCorrectness){
187  Costmap2D map(100, 100, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS * 5.0, ROBOT_RADIUS * 8.0, ROBOT_RADIUS * 10.5,
188  100.0, MAX_Z, 100.0, 25, EMPTY_100_BY_100, THRESHOLD);
189 
190  // Verify that the circumscribed cost lower bound is as expected: based on the cost function.
191  unsigned char c = map.computeCost((ROBOT_RADIUS * 8.0 / RESOLUTION));
192  ASSERT_EQ(map.getCircumscribedCost(), c);
193 
194  // Add a point in the center
195  pcl::PointCloud<pcl::PointXYZ> cloud;
196  cloud.points.resize(1);
197  cloud.points[0].x = 50;
198  cloud.points[0].y = 50;
199  cloud.points[0].z = MAX_Z;
200 
201  geometry_msgs::Point p;
202  p.x = 0.0;
203  p.y = 0.0;
204  p.z = MAX_Z;
205 
206  Observation obs(p, cloud, 100.0, 100.0);
207  std::vector<Observation> obsBuf;
208  obsBuf.push_back(obs);
209 
210  map.updateWorld(0, 0, obsBuf, obsBuf);
211 
212  for(unsigned int i = 0; i <= (unsigned int)ceil(ROBOT_RADIUS * 5.0); i++){
213  // To the right
214  ASSERT_EQ(map.getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
215  ASSERT_EQ(map.getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
216  // To the left
217  ASSERT_EQ(map.getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
218  ASSERT_EQ(map.getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
219  // Down
220  ASSERT_EQ(map.getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
221  ASSERT_EQ(map.getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
222  // Up
223  ASSERT_EQ(map.getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
224  ASSERT_EQ(map.getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
225  }
226 
227  // Verify the normalized cost attenuates as expected
228  for(unsigned int i = (unsigned int)(ceil(ROBOT_RADIUS * 5.0) + 1); i <= (unsigned int)ceil(ROBOT_RADIUS * 10.5); i++){
229  unsigned char expectedValue = map.computeCost(i / RESOLUTION);
230  ASSERT_EQ(map.getCost(50 + i, 50), expectedValue);
231  }
232 
233  // Update with no hits. Should clear (revert to the static map
234  map.resetMapOutsideWindow(0, 0, 0.0, 0.0);
235  cloud.points.resize(0);
236 
237  p.x = 0.0;
238  p.y = 0.0;
239  p.z = MAX_Z;
240 
241  Observation obs2(p, cloud, 100.0, 100.0);
242  std::vector<Observation> obsBuf2;
243  obsBuf2.push_back(obs2);
244 
245  map.updateWorld(0, 0, obsBuf2, obsBuf2);
246 
247  for(unsigned int i = 0; i < 100; i++)
248  for(unsigned int j = 0; j < 100; j++)
249  ASSERT_EQ(map.getCost(i, j), costmap_2d::FREE_SPACE);
250 }
251 
252 char printableCost( unsigned char cost )
253 {
254  switch( cost )
255  {
256  case NO_INFORMATION: return '?';
257  case LETHAL_OBSTACLE: return 'L';
258  case INSCRIBED_INFLATED_OBSTACLE: return 'I';
259  case FREE_SPACE: return '.';
260  default: return '0' + (unsigned char) (10 * cost / 255);
261  }
262 }
263 
267 TEST(costmap, testWaveInterference){
268  // Start with an empty map
269  Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS * 2, ROBOT_RADIUS * 3.01,
270  10.0, MAX_Z * 2, 10.0, 1, EMPTY_10_BY_10, THRESHOLD);
271 
272  // Lay out 3 obstacles in a line - along the diagonal, separated by a cell.
273  pcl::PointCloud<pcl::PointXYZ> cloud;
274  cloud.points.resize(3);
275  cloud.points[0].x = 3;
276  cloud.points[0].y = 3;
277  cloud.points[0].z = MAX_Z;
278  cloud.points[1].x = 5;
279  cloud.points[1].y = 5;
280  cloud.points[1].z = MAX_Z;
281  cloud.points[2].x = 7;
282  cloud.points[2].y = 7;
283  cloud.points[2].z = MAX_Z;
284 
285  geometry_msgs::Point p;
286  p.x = 0.0;
287  p.y = 0.0;
288  p.z = MAX_Z;
289 
290  Observation obs(p, cloud, 100.0, 100.0);
291  std::vector<Observation> obsBuf;
292  obsBuf.push_back(obs);
293 
294  map.updateWorld(0, 0, obsBuf, obsBuf);
295 
296  int update_count = 0;
297 
298  // Expect to see a union of obstacles
299  printf("map:\n");
300  for(unsigned int i = 0; i < 10; ++i){
301  for(unsigned int j = 0; j < 10; ++j){
302  if(map.getCost(i, j) != costmap_2d::FREE_SPACE){
303  update_count++;
304  }
305  printf("%c", printableCost( map.getCost( i, j )));
306  }
307  printf("\n");
308  }
309 
310  ASSERT_EQ(update_count, 79);
311 }
312 
314 TEST(costmap, testWindowCopy){
315  Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
316  10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
317 
318  /*
319  for(unsigned int i = 0; i < 10; ++i){
320  for(unsigned int j = 0; j < 10; ++j){
321  printf("%3d ", map.getCost(i, j));
322  }
323  printf("\n");
324  }
325  printf("\n");
326  */
327 
328  Costmap2D windowCopy;
329 
330  //first test that if we try to make a window that is too big, things fail
331  windowCopy.copyCostmapWindow(map, 2.0, 2.0, 6.0, 12.0);
332  ASSERT_EQ(windowCopy.getSizeInCellsX(), (unsigned int)0);
333  ASSERT_EQ(windowCopy.getSizeInCellsY(), (unsigned int)0);
334 
335  //Next, test that trying to make a map window itself fails
336  map.copyCostmapWindow(map, 2.0, 2.0, 6.0, 6.0);
337  ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)10);
338  ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)10);
339 
340  //Next, test that legal input generates the result that we expect
341  windowCopy.copyCostmapWindow(map, 2.0, 2.0, 6.0, 6.0);
342  ASSERT_EQ(windowCopy.getSizeInCellsX(), (unsigned int)6);
343  ASSERT_EQ(windowCopy.getSizeInCellsY(), (unsigned int)6);
344 
345  //check that we actually get the windo that we expect
346  for(unsigned int i = 0; i < windowCopy.getSizeInCellsX(); ++i){
347  for(unsigned int j = 0; j < windowCopy.getSizeInCellsY(); ++j){
348  ASSERT_EQ(windowCopy.getCost(i, j), map.getCost(i + 2, j + 2));
349  //printf("%3d ", windowCopy.getCost(i, j));
350  }
351  //printf("\n");
352  }
353 
354 }
355 
356 //test for updating costmaps with static data
357 TEST(costmap, testFullyContainedStaticMapUpdate){
358  Costmap2D map(5, 5, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
359  10.0, MAX_Z, 10.0, 25, MAP_5_BY_5, THRESHOLD);
360 
361  Costmap2D static_map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
362  10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
363 
364  map.updateStaticMapWindow(0, 0, 10, 10, MAP_10_BY_10);
365 
366  for(unsigned int i = 0; i < map.getSizeInCellsX(); ++i){
367  for(unsigned int j = 0; j < map.getSizeInCellsY(); ++j){
368  ASSERT_EQ(map.getCost(i, j), static_map.getCost(i, j));
369  }
370  }
371 }
372 
373 TEST(costmap, testOverlapStaticMapUpdate){
374  Costmap2D map(5, 5, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
375  10.0, MAX_Z, 10.0, 25, MAP_5_BY_5, THRESHOLD);
376 
377  Costmap2D static_map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
378  10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
379 
380  map.updateStaticMapWindow(-10, -10, 10, 10, MAP_10_BY_10);
381 
382  ASSERT_FLOAT_EQ(map.getOriginX(), -10);
383  ASSERT_FLOAT_EQ(map.getOriginX(), -10);
384  ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)15);
385  ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)15);
386  for(unsigned int i = 0; i < 10; ++i){
387  for(unsigned int j = 0; j < 10; ++j){
388  ASSERT_EQ(map.getCost(i, j), static_map.getCost(i, j));
389  }
390  }
391 
392  std::vector<unsigned char> blank(100);
393 
394  //check to make sure that inflation on updates are being done correctly
395  map.updateStaticMapWindow(-10, -10, 10, 10, blank);
396 
397  for(unsigned int i = 0; i < map.getSizeInCellsX(); ++i){
398  for(unsigned int j = 0; j < map.getSizeInCellsY(); ++j){
399  ASSERT_EQ(map.getCost(i, j), 0);
400  }
401  }
402 
403  std::vector<unsigned char> fully_contained(25);
404  fully_contained[0] = 254;
405  fully_contained[4] = 254;
406  fully_contained[5] = 254;
407  fully_contained[9] = 254;
408 
409  Costmap2D small_static_map(5, 5, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
410  10.0, MAX_Z, 10.0, 25, fully_contained, THRESHOLD);
411 
412  map.updateStaticMapWindow(0, 0, 5, 5, fully_contained);
413 
414  ASSERT_FLOAT_EQ(map.getOriginX(), -10);
415  ASSERT_FLOAT_EQ(map.getOriginX(), -10);
416  ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)15);
417  ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)15);
418  for(unsigned int j = 0; j < 5; ++j){
419  for(unsigned int i = 0; i < 5; ++i){
420  ASSERT_EQ(map.getCost(i + 10, j + 10), small_static_map.getCost(i, j));
421  }
422  }
423 
424 }
425 
429 TEST(costmap, testRaytracing){
430  Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
431  10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
432 
433  // Add a point cloud, should not affect the map
434  pcl::PointCloud<pcl::PointXYZ> cloud;
435  cloud.points.resize(1);
436  cloud.points[0].x = 0;
437  cloud.points[0].y = 0;
438  cloud.points[0].z = MAX_Z;
439 
440  geometry_msgs::Point p;
441  p.x = 0.0;
442  p.y = 0.0;
443  p.z = MAX_Z;
444 
445  Observation obs(p, cloud, 100.0, 100.0);
446  std::vector<Observation> obsBuf;
447  obsBuf.push_back(obs);
448 
449  map.updateWorld(0, 0, obsBuf, obsBuf);
450 
451  int lethal_count = 0;
452 
453  for(unsigned int i = 0; i < 10; ++i){
454  for(unsigned int j = 0; j < 10; ++j){
455  if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){
456  lethal_count++;
457  }
458  }
459  }
460 
461  //we expect just one obstacle to be added
462  ASSERT_EQ(lethal_count, 21);
463 }
464 
465 TEST(costmap, testAdjacentToObstacleCanStillMove){
466  Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, 2.1, 3.1, 4.1,
467  10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
468  pcl::PointCloud<pcl::PointXYZ> cloud;
469  cloud.points.resize(1);
470  cloud.points[0].x = 0;
471  cloud.points[0].y = 0;
472  cloud.points[0].z = MAX_Z;
473 
474  geometry_msgs::Point p;
475  p.x = 0.0;
476  p.y = 0.0;
477  p.z = MAX_Z;
478 
479  Observation obs(p, cloud, 100.0, 100.0);
480  std::vector<Observation> obsBuf;
481  obsBuf.push_back(obs);
482 
483  map.updateWorld(9, 9, obsBuf, obsBuf);
484 
485  EXPECT_EQ( LETHAL_OBSTACLE, map.getCost( 0, 0 ));
486  EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, map.getCost( 1, 0 ));
487  EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, map.getCost( 2, 0 ));
488  EXPECT_TRUE( INSCRIBED_INFLATED_OBSTACLE > map.getCost( 3, 0 ));
489  EXPECT_TRUE( INSCRIBED_INFLATED_OBSTACLE > map.getCost( 2, 1 ));
490  EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, map.getCost( 1, 1 ));
491 }
492 
493 TEST(costmap, testInflationShouldNotCreateUnknowns){
494  Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, 2.1, 3.1, 4.1,
495  10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
496  pcl::PointCloud<pcl::PointXYZ> cloud;
497  cloud.points.resize(1);
498  cloud.points[0].x = 0;
499  cloud.points[0].y = 0;
500  cloud.points[0].z = MAX_Z;
501 
502  geometry_msgs::Point p;
503  p.x = 0.0;
504  p.y = 0.0;
505  p.z = MAX_Z;
506 
507  Observation obs(p, cloud, 100.0, 100.0);
508  std::vector<Observation> obsBuf;
509  obsBuf.push_back(obs);
510 
511  map.updateWorld(9, 9, obsBuf, obsBuf);
512 
513  int unknown_count = 0;
514 
515  for(unsigned int i = 0; i < 10; ++i){
516  for(unsigned int j = 0; j < 10; ++j){
517  if(map.getCost(i, j) == costmap_2d::NO_INFORMATION){
518  unknown_count++;
519  }
520  }
521  }
522  EXPECT_EQ( 0, unknown_count );
523 }
524 
525 unsigned int worldToIndex(Costmap2D& map, double wx, double wy){
526  unsigned int mx, my;
527  map.worldToMap(wx, wy, mx, my);
528  return map.getIndex(mx, my);
529 }
530 
531 void indexToWorld(Costmap2D& map, unsigned int index, double& wx, double& wy){
532  unsigned int mx, my;
533  map.indexToCells(index, mx, my);
534  map.mapToWorld(mx, my, wx, wy);
535 }
536 
537 TEST(costmap, testStaticMap){
538  Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
539  10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
540 
541  ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)10);
542  ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)10);
543 
544  // Verify that obstacles correctly identified from the static map.
545  std::vector<unsigned int> occupiedCells;
546 
547  for(unsigned int i = 0; i < 10; ++i){
548  for(unsigned int j = 0; j < 10; ++j){
549  if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){
550  occupiedCells.push_back(map.getIndex(i, j));
551  }
552  }
553  }
554 
555  ASSERT_EQ(occupiedCells.size(), (unsigned int)20);
556 
557  // Iterate over all id's and verify that they are present according to their
558  for(std::vector<unsigned int>::const_iterator it = occupiedCells.begin(); it != occupiedCells.end(); ++it){
559  unsigned int ind = *it;
560  unsigned int x, y;
561  map.indexToCells(ind, x, y);
562  ASSERT_EQ(find(occupiedCells, map.getIndex(x, y)), true);
563  ASSERT_EQ(MAP_10_BY_10[ind] >= 100, true);
564  ASSERT_EQ(map.getCost(x, y) >= 100, true);
565  }
566 
567  // Block of 200
568  ASSERT_EQ(find(occupiedCells, map.getIndex(7, 2)), true);
569  ASSERT_EQ(find(occupiedCells, map.getIndex(8, 2)), true);
570  ASSERT_EQ(find(occupiedCells, map.getIndex(9, 2)), true);
571  ASSERT_EQ(find(occupiedCells, map.getIndex(7, 3)), true);
572  ASSERT_EQ(find(occupiedCells, map.getIndex(8, 3)), true);
573  ASSERT_EQ(find(occupiedCells, map.getIndex(9, 3)), true);
574  ASSERT_EQ(find(occupiedCells, map.getIndex(7, 4)), true);
575  ASSERT_EQ(find(occupiedCells, map.getIndex(8, 4)), true);
576  ASSERT_EQ(find(occupiedCells, map.getIndex(9, 4)), true);
577 
578  // Block of 100
579  ASSERT_EQ(find(occupiedCells, map.getIndex(4, 3)), true);
580  ASSERT_EQ(find(occupiedCells, map.getIndex(4, 4)), true);
581 
582  // Block of 200
583  ASSERT_EQ(find(occupiedCells, map.getIndex(3, 7)), true);
584  ASSERT_EQ(find(occupiedCells, map.getIndex(4, 7)), true);
585  ASSERT_EQ(find(occupiedCells, map.getIndex(5, 7)), true);
586 
587 
588  // Verify Coordinate Transformations, ROW MAJOR ORDER
589  ASSERT_EQ(worldToIndex(map, 0.0, 0.0), (unsigned int)0);
590  ASSERT_EQ(worldToIndex(map, 0.0, 0.99), (unsigned int)0);
591  ASSERT_EQ(worldToIndex(map, 0.0, 1.0), (unsigned int)10);
592  ASSERT_EQ(worldToIndex(map, 1.0, 0.99), (unsigned int)1);
593  ASSERT_EQ(worldToIndex(map, 9.99, 9.99), (unsigned int)99);
594  ASSERT_EQ(worldToIndex(map, 8.2, 3.4), (unsigned int)38);
595 
596  // Ensure we hit the middle of the cell for world co-ordinates
597  double wx, wy;
598  indexToWorld(map, 99, wx, wy);
599  ASSERT_EQ(wx, 9.5);
600  ASSERT_EQ(wy, 9.5);
601 }
602 
603 
608 TEST(costmap, testDynamicObstacles){
609  Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
610  10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
611 
612  // Add a point cloud and verify its insertion. There should be only one new one
613  pcl::PointCloud<pcl::PointXYZ> cloud;
614  cloud.points.resize(3);
615  cloud.points[0].x = 0;
616  cloud.points[0].y = 0;
617  cloud.points[1].x = 0;
618  cloud.points[1].y = 0;
619  cloud.points[2].x = 0;
620  cloud.points[2].y = 0;
621 
622  geometry_msgs::Point p;
623  p.x = 0.0;
624  p.y = 0.0;
625  p.z = MAX_Z;
626 
627  Observation obs(p, cloud, 100.0, 100.0);
628  std::vector<Observation> obsBuf;
629  obsBuf.push_back(obs);
630 
631  map.updateWorld(0, 0, obsBuf, obsBuf);
632 
633  std::vector<unsigned int> ids;
634 
635  for(unsigned int i = 0; i < 10; ++i){
636  for(unsigned int j = 0; j < 10; ++j){
637  if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){
638  ids.push_back(map.getIndex(i, j));
639  }
640  }
641  }
642 
643  // Should now have 1 insertion and no deletions
644  ASSERT_EQ(ids.size(), (unsigned int)21);
645 
646  // Repeating the call - we should see no insertions or deletions
647  map.updateWorld(0, 0, obsBuf, obsBuf);
648  ASSERT_EQ(ids.size(), (unsigned int)21);
649 }
650 
654 TEST(costmap, testMultipleAdditions){
655  Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
656  10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
657 
658  // A point cloud with one point that falls within an existing obstacle
659  pcl::PointCloud<pcl::PointXYZ> cloud;
660  cloud.points.resize(1);
661  cloud.points[0].x = 7;
662  cloud.points[0].y = 2;
663 
664  geometry_msgs::Point p;
665  p.x = 0.0;
666  p.y = 0.0;
667  p.z = MAX_Z;
668 
669  Observation obs(p, cloud, 100.0, 100.0);
670  std::vector<Observation> obsBuf;
671  obsBuf.push_back(obs);
672 
673  map.updateWorld(0, 0, obsBuf, obsBuf);
674 
675  std::vector<unsigned int> ids;
676 
677  for(unsigned int i = 0; i < 10; ++i){
678  for(unsigned int j = 0; j < 10; ++j){
679  if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){
680  ids.push_back(map.getIndex(i, j));
681  }
682  }
683  }
684 
685  ASSERT_EQ(ids.size(), (unsigned int)20);
686 }
687 
691 TEST(costmap, testZThreshold){
692  Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
693  10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
694 
695  // A point cloud with 2 points falling in a cell with a non-lethal cost
696  pcl::PointCloud<pcl::PointXYZ> c0;
697  c0.points.resize(2);
698  c0.points[0].x = 0;
699  c0.points[0].y = 5;
700  c0.points[0].z = 0.4;
701  c0.points[1].x = 1;
702  c0.points[1].y = 5;
703  c0.points[1].z = 1.2;
704 
705  geometry_msgs::Point p;
706  p.x = 0.0;
707  p.y = 0.0;
708  p.z = MAX_Z;
709 
710  Observation obs(p, c0, 100.0, 100.0);
711  std::vector<Observation> obsBuf;
712  obsBuf.push_back(obs);
713 
714  map.updateWorld(0, 0, obsBuf, obsBuf);
715 
716  std::vector<unsigned int> ids;
717 
718  for(unsigned int i = 0; i < 10; ++i){
719  for(unsigned int j = 0; j < 10; ++j){
720  if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){
721  ids.push_back(map.getIndex(i, j));
722  }
723  }
724  }
725 
726  ASSERT_EQ(ids.size(), (unsigned int)21);
727 }
728 
733 TEST(costmap, testInflation){
734  Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
735  10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
736 
737  // Verify that obstacles correctly identified
738  std::vector<unsigned int> occupiedCells;
739 
740  for(unsigned int i = 0; i < 10; ++i){
741  for(unsigned int j = 0; j < 10; ++j){
742  if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
743  occupiedCells.push_back(map.getIndex(i, j));
744  }
745  }
746  }
747 
748  // There should be no duplicates
749  std::set<unsigned int> setOfCells;
750  for(unsigned int i=0;i<occupiedCells.size(); i++)
751  setOfCells.insert(i);
752 
753  ASSERT_EQ(setOfCells.size(), occupiedCells.size());
754  ASSERT_EQ(setOfCells.size(), (unsigned int)48);
755 
756  // Iterate over all id's and verify they are obstacles
757  for(std::vector<unsigned int>::const_iterator it = occupiedCells.begin(); it != occupiedCells.end(); ++it){
758  unsigned int ind = *it;
759  unsigned int x, y;
760  map.indexToCells(ind, x, y);
761  ASSERT_EQ(find(occupiedCells, map.getIndex(x, y)), true);
762  ASSERT_EQ(map.getCost(x, y) == costmap_2d::LETHAL_OBSTACLE || map.getCost(x, y) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
763  }
764 
765  // Set an obstacle at the origin and observe insertions for it and its neighbors
766  pcl::PointCloud<pcl::PointXYZ> c0;
767  c0.points.resize(1);
768  c0.points[0].x = 0;
769  c0.points[0].y = 0;
770  c0.points[0].z = 0.4;
771 
772  geometry_msgs::Point p;
773  p.x = 0.0;
774  p.y = 0.0;
775  p.z = MAX_Z;
776 
777  Observation obs(p, c0, 100.0, 100.0);
778  std::vector<Observation> obsBuf, empty;
779  obsBuf.push_back(obs);
780 
781  map.updateWorld(0, 0, obsBuf, empty);
782 
783  occupiedCells.clear();
784  for(unsigned int i = 0; i < 10; ++i){
785  for(unsigned int j = 0; j < 10; ++j){
786  if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
787  occupiedCells.push_back(map.getIndex(i, j));
788  }
789  }
790  }
791 
792  // It and its 2 neighbors makes 3 obstacles
793  ASSERT_EQ(occupiedCells.size(), (unsigned int)51);
794 
795  // @todo Rewrite
796  // Add an obstacle at <2,0> which will inflate and refresh to of the other inflated cells
797  pcl::PointCloud<pcl::PointXYZ> c1;
798  c1.points.resize(1);
799  c1.points[0].x = 2;
800  c1.points[0].y = 0;
801  c1.points[0].z = 0.0;
802 
803  geometry_msgs::Point p1;
804  p1.x = 0.0;
805  p1.y = 0.0;
806  p1.z = MAX_Z;
807 
808  Observation obs1(p1, c1, 100.0, 100.0);
809  std::vector<Observation> obsBuf1;
810  obsBuf1.push_back(obs1);
811 
812  map.updateWorld(0, 0, obsBuf1, empty);
813 
814  occupiedCells.clear();
815  for(unsigned int i = 0; i < 10; ++i){
816  for(unsigned int j = 0; j < 10; ++j){
817  if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
818  occupiedCells.push_back(map.getIndex(i, j));
819  }
820  }
821  }
822 
823  // Now we expect insertions for it, and 2 more neighbors, but not all 5. Free space will propagate from
824  // the origin to the target, clearing the point at <0, 0>, but not over-writing the inflation of the obstacle
825  // at <0, 1>
826  ASSERT_EQ(occupiedCells.size(), (unsigned int)54);
827 
828 
829  // Add an obstacle at <1, 9>. This will inflate obstacles around it
830  pcl::PointCloud<pcl::PointXYZ> c2;
831  c2.points.resize(1);
832  c2.points[0].x = 1;
833  c2.points[0].y = 9;
834  c2.points[0].z = 0.0;
835 
836  geometry_msgs::Point p2;
837  p2.x = 0.0;
838  p2.y = 0.0;
839  p2.z = MAX_Z;
840 
841  Observation obs2(p2, c2, 100.0, 100.0);
842  std::vector<Observation> obsBuf2;
843  obsBuf2.push_back(obs2);
844 
845  map.updateWorld(0, 0, obsBuf2, empty);
846 
847  ASSERT_EQ(map.getCost(1, 9), costmap_2d::LETHAL_OBSTACLE);
848  ASSERT_EQ(map.getCost(0, 9), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
849  ASSERT_EQ(map.getCost(2, 9), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
850 
851  // Add an obstacle and verify that it over-writes its inflated status
852  pcl::PointCloud<pcl::PointXYZ> c3;
853  c3.points.resize(1);
854  c3.points[0].x = 0;
855  c3.points[0].y = 9;
856  c3.points[0].z = 0.0;
857 
858  geometry_msgs::Point p3;
859  p3.x = 0.0;
860  p3.y = 0.0;
861  p3.z = MAX_Z;
862 
863  Observation obs3(p3, c3, 100.0, 100.0);
864  std::vector<Observation> obsBuf3;
865  obsBuf3.push_back(obs3);
866 
867  map.updateWorld(0, 0, obsBuf3, empty);
868 
869  ASSERT_EQ(map.getCost(0, 9), costmap_2d::LETHAL_OBSTACLE);
870 }
871 
875 TEST(costmap, testInflation2){
876  Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
877  10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
878 
879  // Creat a small L-Shape all at once
880  pcl::PointCloud<pcl::PointXYZ> c0;
881  c0.points.resize(3);
882  c0.points[0].x = 1;
883  c0.points[0].y = 1;
884  c0.points[0].z = MAX_Z;
885  c0.points[1].x = 1;
886  c0.points[1].y = 2;
887  c0.points[1].z = MAX_Z;
888  c0.points[2].x = 2;
889  c0.points[2].y = 2;
890  c0.points[2].z = MAX_Z;
891 
892  geometry_msgs::Point p;
893  p.x = 0.0;
894  p.y = 0.0;
895  p.z = MAX_Z;
896 
897  Observation obs(p, c0, 100.0, 100.0);
898  std::vector<Observation> obsBuf;
899  obsBuf.push_back(obs);
900 
901  map.updateWorld(0, 0, obsBuf, obsBuf);
902 
903  ASSERT_EQ(map.getCost(3, 2), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
904  ASSERT_EQ(map.getCost(3, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
905 }
906 
910 TEST(costmap, testInflation3){
911  std::vector<unsigned char> mapData;
912  for(unsigned int i=0; i< GRID_WIDTH; i++){
913  for(unsigned int j = 0; j < GRID_HEIGHT; j++){
914  mapData.push_back(0);
915  }
916  }
917 
918  Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS * 2, ROBOT_RADIUS * 3,
919  10.0, MAX_Z, 10.0, 1, mapData, THRESHOLD);
920 
921  // There should be no occupied cells
922  std::vector<unsigned int> ids;
923 
924  for(unsigned int i = 0; i < 10; ++i){
925  for(unsigned int j = 0; j < 10; ++j){
926  if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
927  ids.push_back(map.getIndex(i, j));
928  }
929  }
930  }
931 
932  ASSERT_EQ(ids.size(), (unsigned int)0);
933 
934  // Add an obstacle at 5,5
935  pcl::PointCloud<pcl::PointXYZ> c0;
936  c0.points.resize(1);
937  c0.points[0].x = 5;
938  c0.points[0].y = 5;
939  c0.points[0].z = MAX_Z;
940 
941  geometry_msgs::Point p;
942  p.x = 0.0;
943  p.y = 0.0;
944  p.z = MAX_Z;
945 
946  Observation obs(p, c0, 100.0, 100.0);
947  std::vector<Observation> obsBuf;
948  obsBuf.push_back(obs);
949 
950  map.updateWorld(0, 0, obsBuf, obsBuf);
951 
952  for(unsigned int i = 0; i < 10; ++i){
953  for(unsigned int j = 0; j < 10; ++j){
954  if(map.getCost(i, j) != costmap_2d::FREE_SPACE){
955  ids.push_back(map.getIndex(i, j));
956  }
957  }
958  }
959 
960  ASSERT_EQ(ids.size(), (unsigned int)29);
961 
962  ids.clear();
963  for(unsigned int i = 0; i < 10; ++i){
964  for(unsigned int j = 0; j < 10; ++j){
965  if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
966  ids.push_back(map.getIndex(i, j));
967  }
968  }
969  }
970 
971  ASSERT_EQ(ids.size(), (unsigned int)5);
972 
973  // Update again - should see no change
974  map.updateWorld(0, 0, obsBuf, obsBuf);
975 
976  ids.clear();
977  for(unsigned int i = 0; i < 10; ++i){
978  for(unsigned int j = 0; j < 10; ++j){
979  if(map.getCost(i, j) != costmap_2d::FREE_SPACE){
980  ids.push_back(map.getIndex(i, j));
981  }
982  }
983  }
984 
985  ASSERT_EQ(ids.size(), (unsigned int)29);
986 }
987 
992 TEST(costmap, testRaytracing2){
993  Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
994  100.0, MAX_Z, 100.0, 1, MAP_10_BY_10, THRESHOLD);
995 
996  // The sensor origin will be <0,0>. So if we add an obstacle at 9,9, we would expect cells
997  // <0, 0> thru <8, 8> to be traced through
998  pcl::PointCloud<pcl::PointXYZ> c0;
999  c0.points.resize(1);
1000  c0.points[0].x = 9.5;
1001  c0.points[0].y = 9.5;
1002  c0.points[0].z = MAX_Z;
1003 
1004  geometry_msgs::Point p;
1005  p.x = 0.5;
1006  p.y = 0.5;
1007  p.z = MAX_Z;
1008 
1009  Observation obs(p, c0, 100.0, 100.0);
1010  std::vector<Observation> obsBuf;
1011  obsBuf.push_back(obs);
1012 
1013  std::vector<unsigned int> obstacles;
1014 
1015  for(unsigned int i = 0; i < 10; ++i){
1016  for(unsigned int j = 0; j < 10; ++j){
1017  if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){
1018  obstacles.push_back(map.getIndex(i, j));
1019  }
1020  }
1021  }
1022 
1023  unsigned int obs_before = obstacles.size();
1024 
1025  map.updateWorld(0, 0, obsBuf, obsBuf);
1026 
1027  obstacles.clear();
1028  for(unsigned int i = 0; i < 10; ++i){
1029  for(unsigned int j = 0; j < 10; ++j){
1030  if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){
1031  obstacles.push_back(map.getIndex(i, j));
1032  }
1033  }
1034  }
1035 
1036  //Two obstacles shoulb be removed from the map by raytracing
1037  ASSERT_EQ(obstacles.size(), obs_before - 2);
1038 
1039 
1040  // many cells will have been switched to free space along the diagonal except
1041  // for those inflated in the update.. tests that inflation happens properly
1042  // after raytracing
1043  unsigned char test[10]= {0, 0, 0, 253, 253, 0, 0, 253, 253, 254};
1044  for(unsigned int i=0; i < 10; i++)
1045  ASSERT_EQ(map.getCost(i, i), test[i]);
1046 }
1047 
1054 TEST(costmap, testTrickyPropagation){
1055  const unsigned char MAP_HALL_CHAR[10 * 10] = {
1056  0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
1057  254, 0, 0, 0, 0, 0, 0, 0, 0, 0,
1058  0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
1059  0, 0, 0, 254, 0, 0, 0, 0, 0, 0,
1060  0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
1061  0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
1062  0, 0, 0, 0, 0, 254, 0, 0, 0, 0,
1063  0, 0, 0, 0, 0, 254, 0, 0, 0, 0,
1064  0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
1065  0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
1066  };
1067  std::vector<unsigned char> MAP_HALL;
1068  for (int i = 0; i < 10 * 10; i++) {
1069  MAP_HALL.push_back(MAP_HALL_CHAR[i]);
1070  }
1071 
1072  Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
1073  100.0, MAX_Z, 100.0, 1, MAP_HALL, THRESHOLD);
1074 
1075 
1076  //Add a dynamic obstacle
1077  pcl::PointCloud<pcl::PointXYZ> c2;
1078  c2.points.resize(3);
1079  //Dynamic obstacle that raytaces.
1080  c2.points[0].x = 7.0;
1081  c2.points[0].y = 8.0;
1082  c2.points[0].z = 1.0;
1083  //Dynamic obstacle that should not be raytraced the
1084  //first update, but should on the second.
1085  c2.points[1].x = 3.0;
1086  c2.points[1].y = 4.0;
1087  c2.points[1].z = 1.0;
1088  //Dynamic obstacle that should not be erased.
1089  c2.points[2].x = 6.0;
1090  c2.points[2].y = 3.0;
1091  c2.points[2].z = 1.0;
1092 
1093  geometry_msgs::Point p2;
1094  p2.x = 0.5;
1095  p2.y = 0.5;
1096  p2.z = MAX_Z;
1097 
1098  Observation obs2(p2, c2, 100.0, 100.0);
1099  std::vector<Observation> obsBuf2;
1100  obsBuf2.push_back(obs2);
1101 
1102  map.updateWorld(0, 0, obsBuf2, obsBuf2);
1103 
1104  const unsigned char MAP_HALL_CHAR_TEST[10 * 10] = {
1105  253, 254, 253, 0, 0, 0, 0, 0, 0, 0,
1106  0, 253, 0, 0, 0, 0, 0, 0, 0, 0,
1107  0, 0, 0, 0, 253, 0, 0, 0, 0, 0,
1108  0, 0, 0, 253, 254, 253, 0, 0, 0, 0,
1109  0, 0, 0, 0, 253, 0, 0, 253, 0, 0,
1110  0, 0, 0, 253, 0, 0, 253, 254, 253, 0,
1111  0, 0, 253, 254, 253, 0, 0, 253, 253, 0,
1112  0, 0, 0, 253, 0, 0, 0, 253, 254, 253,
1113  0, 0, 0, 0, 0, 0, 0, 0, 253, 0,
1114  0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
1115  };
1116 
1117 
1118  for (int i = 0; i < 10 * 10; i++) {
1119  ASSERT_EQ(map.getCost(i / 10, i % 10), MAP_HALL_CHAR_TEST[i]);
1120  }
1121 
1122  pcl::PointCloud<pcl::PointXYZ> c;
1123  c.points.resize(1);
1124  //Dynamic obstacle that raytaces the one at (3.0, 4.0).
1125  c.points[0].x = 4.0;
1126  c.points[0].y = 5.0;
1127  c.points[0].z = 1.0;
1128 
1129  geometry_msgs::Point p3;
1130  p3.x = 0.5;
1131  p3.y = 0.5;
1132  p3.z = MAX_Z;
1133 
1134  Observation obs3(p3, c, 100.0, 100.0);
1135  std::vector<Observation> obsBuf3;
1136  obsBuf3.push_back(obs3);
1137 
1138  map.updateWorld(0, 0, obsBuf3, obsBuf3);
1139 
1140  const unsigned char MAP_HALL_CHAR_TEST2[10 * 10] = {
1141  253, 254, 253, 0, 0, 0, 0, 0, 0, 0,
1142  0, 253, 0, 0, 0, 0, 0, 0, 0, 0,
1143  0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
1144  0, 0, 0, 0, 0, 253, 0, 0, 0, 0,
1145  0, 0, 0, 0, 253, 254, 253, 253, 0, 0,
1146  0, 0, 0, 253, 0, 253, 253, 254, 253, 0,
1147  0, 0, 253, 254, 253, 0, 0, 253, 253, 0,
1148  0, 0, 0, 253, 0, 0, 0, 253, 254, 253,
1149  0, 0, 0, 0, 0, 0, 0, 0, 253, 0,
1150  0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
1151  };
1152 
1153 
1154  for (int i = 0; i < 10 * 10; i++) {
1155  ASSERT_EQ(map.getCost(i / 10, i % 10), MAP_HALL_CHAR_TEST2[i]);
1156  }
1157 }
1158 
1159 
1160 
1161 int main(int argc, char** argv){
1162  for(unsigned int i = 0; i< GRID_WIDTH * GRID_HEIGHT; i++){
1163  EMPTY_10_BY_10.push_back(0);
1164  MAP_10_BY_10.push_back(MAP_10_BY_10_CHAR[i]);
1165  }
1166 
1167  for(unsigned int i = 0; i< 5 * 5; i++){
1168  MAP_5_BY_5.push_back(MAP_10_BY_10_CHAR[i]);
1169  }
1170 
1171  for(unsigned int i = 0; i< 100 * 100; i++)
1172  EMPTY_100_BY_100.push_back(0);
1173 
1174  testing::InitGoogleTest(&argc, argv);
1175  return RUN_ALL_TESTS();
1176 }
TEST
TEST(costmap, testResetForStaticMap)
Definition: module_tests.cpp:90
MAX_Z
const double MAX_Z(1.0)
costmap_2d::Costmap2D::worldToMap
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const
Convert from world coordinates to map coordinates.
Definition: costmap_2d.cpp:224
std::vector
std::vector::size
T size(T... args)
costmap_2d::INSCRIBED_INFLATED_OBSTACLE
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
Definition: cost_values.h:79
OBSTACLE_RANGE
const double OBSTACLE_RANGE(20.0)
costmap_2d::NO_INFORMATION
static const unsigned char NO_INFORMATION
Definition: cost_values.h:77
MAP_10_BY_10
std::vector< unsigned char > MAP_10_BY_10
Definition: module_tests.cpp:64
costmap_2d::Costmap2D
A 2D costmap provides a mapping between points in the world and their associated "costs".
Definition: costmap_2d.h:96
costmap_2d::Costmap2D::mapToWorld
void mapToWorld(unsigned int mx, unsigned int my, double &wx, double &wy) const
Convert from map coordinates to world coordinates.
Definition: costmap_2d.cpp:218
main
int main(int argc, char **argv)
Definition: module_tests.cpp:1161
costmap_2d::Costmap2D::getOriginX
double getOriginX() const
Accessor for the x origin of the costmap.
Definition: costmap_2d.cpp:471
MAP_5_BY_5_CHAR
const unsigned char MAP_5_BY_5_CHAR[]
Definition: module_tests.cpp:55
std::vector::clear
T clear(T... args)
costmap_2d.h
MAP_10_BY_10_CHAR
const unsigned char MAP_10_BY_10_CHAR[]
Definition: module_tests.cpp:42
std::vector::push_back
T push_back(T... args)
find
bool find(const std::vector< unsigned int > &l, unsigned int n)
Definition: module_tests.cpp:78
observation_buffer.h
indexToWorld
void indexToWorld(Costmap2D &map, unsigned int index, double &wx, double &wy)
Definition: module_tests.cpp:531
THRESHOLD
const unsigned char THRESHOLD(100)
worldToIndex
unsigned int worldToIndex(Costmap2D &map, double wx, double wy)
Definition: module_tests.cpp:525
WINDOW_LENGTH
const double WINDOW_LENGTH(10)
costmap_2d::Costmap2D::indexToCells
void indexToCells(unsigned int index, unsigned int &mx, unsigned int &my) const
Given an index... compute the associated map coordinates.
Definition: costmap_2d.h:226
costmap_2d::Costmap2D::getSizeInCellsY
unsigned int getSizeInCellsY() const
Accessor for the y size of the costmap in cells.
Definition: costmap_2d.cpp:456
costmap_2d::LETHAL_OBSTACLE
static const unsigned char LETHAL_OBSTACLE
Definition: cost_values.h:78
RAYTRACE_RANGE
const double RAYTRACE_RANGE(20.0)
RESOLUTION
const double RESOLUTION(1)
costmap_2d::Costmap2D::copyCostmapWindow
bool copyCostmapWindow(const Costmap2D &map, double win_origin_x, double win_origin_y, double win_size_x, double win_size_y)
Turn this costmap into a copy of a window of a costmap passed in.
Definition: costmap_2d.cpp:108
MAP_5_BY_5
std::vector< unsigned char > MAP_5_BY_5
Definition: module_tests.cpp:63
costmap_2d::Observation
Stores an observation in terms of a point cloud and the origin of the source.
Definition: observation.h:46
std::vector::begin
T begin(T... args)
std::set::insert
T insert(T... args)
GRID_WIDTH
const unsigned int GRID_WIDTH(10)
EMPTY_10_BY_10
std::vector< unsigned char > EMPTY_10_BY_10
Definition: module_tests.cpp:65
costmap_2d::Costmap2D::getIndex
unsigned int getIndex(unsigned int mx, unsigned int my) const
Given two map coordinates... compute the associated index.
Definition: costmap_2d.h:215
std::vector::end
T end(T... args)
costmap_2d::FREE_SPACE
static const unsigned char FREE_SPACE
Definition: cost_values.h:80
printableCost
char printableCost(unsigned char cost)
Definition: module_tests.cpp:252
costmap_2d::Costmap2D::getSizeInCellsX
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
Definition: costmap_2d.cpp:451
ROBOT_RADIUS
const double ROBOT_RADIUS(1.0)
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:202
EMPTY_100_BY_100
std::vector< unsigned char > EMPTY_100_BY_100
Definition: module_tests.cpp:66
costmap_2d
Definition: array_parser.h:37
set
GRID_HEIGHT
const unsigned int GRID_HEIGHT(10)
Generated on Sat Apr 19 2025 04:34:08 for costmap_2d by   doxygen 1.8.17