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

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