test_costmap_3d.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2016-2018, the neonavigation authors
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 copyright holder 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 
30 #include <cstddef>
31 #include <string>
32 #include <algorithm>
33 
34 #include <gtest/gtest.h>
35 
36 #include <ros/ros.h>
37 #include <nav_msgs/OccupancyGrid.h>
39 
40 const std::string footprint_str(
41  "<value><array><data>"
42  " <value><array><data>"
43  " <value><double>1.5</double></value>"
44  " <value><double>0.0</double></value>"
45  " </data></array></value>"
46  " <value><array><data>"
47  " <value><double>-0.5</double></value>"
48  " <value><double>-0.5</double></value>"
49  " </data></array></value>"
50  " <value><array><data>"
51  " <value><double>-0.5</double></value>"
52  " <value><double>0.5</double></value>"
53  " </data></array></value>"
54  "</data></array></value>");
55 // This footprint with resolution of 0.1 means:
56 // 0 0 0
57 // 1 1 0 <--x
58 // 0 0 0
59 const char temp_dir[4][2] =
60  {
61  // x, y which must be occupied in the template
62  -1, 0,
63  0, -1,
64  1, 0,
65  0, 1
66  };
67 
68 TEST(Costmap3dLayerFootprint, CSpaceTemplate)
69 {
71 
72  // Settings: 4 angular grids, no expand/spread
73  cm.setAngleResolution(4);
74  cm.setExpansion(0.0, 0.0);
76 
77  // Set example footprint
78  int footprint_offset = 0;
79  XmlRpc::XmlRpcValue footprint_xml;
80  ASSERT_TRUE(footprint_xml.fromXml(footprint_str, &footprint_offset));
81  cm.setFootprint(costmap_cspace::Polygon(footprint_xml));
82 
83  // Check local footprint
84  const costmap_cspace::Polygon polygon = cm.getFootprint();
85  ASSERT_EQ(polygon.v.size(), 3u + 1u);
86  ASSERT_EQ(polygon.v[0][0], 1.5);
87  ASSERT_EQ(polygon.v[0][1], 0.0);
88  ASSERT_EQ(polygon.v[1][0], -0.5);
89  ASSERT_EQ(polygon.v[1][1], -0.5);
90  ASSERT_EQ(polygon.v[2][0], -0.5);
91  ASSERT_EQ(polygon.v[2][1], 0.5);
92  // Last point is same as the first point
93  ASSERT_EQ(polygon.v[3][0], 1.5);
94  ASSERT_EQ(polygon.v[3][1], 0.0);
95  ASSERT_EQ(cm.getFootprintRadius(), 1.5);
96 
97  // Generate CSpace pattern around the robot
98  costmap_cspace_msgs::MapMetaData3D map_info;
99  map_info.width = 3;
100  map_info.height = 3;
101  map_info.angle = 4;
102  map_info.linear_resolution = 1.0;
103  map_info.angular_resolution = M_PI / 2.0;
104  map_info.origin.orientation.w = 1.0;
105 
106  cm.setMapMetaData(map_info);
107 
108  ASSERT_EQ(cm.getRangeMax(), static_cast<int>(ceilf(1.5 / 1.0)));
109 
110  const costmap_cspace::CSpace3Cache& temp = cm.getTemplate();
111  // Check template size
112  int x, y, a;
113  int cx, cy, ca;
114  temp.getSize(x, y, a);
115  temp.getCenter(cx, cy, ca);
116  ASSERT_EQ(x, 2 * 2 + 1);
117  ASSERT_EQ(y, 2 * 2 + 1);
118  ASSERT_EQ(a, 4);
119  ASSERT_EQ(cx, 2);
120  ASSERT_EQ(cy, 2);
121  ASSERT_EQ(ca, 0);
122 
123  // Check generated template
124  for (int k = -ca; k < a - ca; ++k)
125  {
126  for (int j = -cy; j < y - cy; ++j)
127  {
128  for (int i = -cx; i < x - cx; ++i)
129  {
130  if (i == 0 && j == 0)
131  {
132  ASSERT_EQ(temp.e(i, j, k), 100);
133  }
134  else if (i == temp_dir[k + ca][0] && j == temp_dir[k + ca][1])
135  {
136  ASSERT_EQ(temp.e(i, j, k), 100);
137  }
138  else
139  {
140  ASSERT_EQ(temp.e(i, j, k), 0);
141  }
142  // std::cout << std::setfill(' ') << std::setw(3) << static_cast<int>(temp.e(i, j, k)) << " ";
143  }
144  // std::cout << std::endl;
145  }
146  // std::cout << "----" << std::endl;
147  }
148 }
149 
150 TEST(Costmap3dLayerPlain, CSpaceTemplate)
151 {
153 
154  // Settings: 4 angular grids, no expand/spread
155  cm.setAngleResolution(4);
156  cm.setExpansion(0.0, 0.0);
158 
159  // Generate CSpace pattern around the robot
160  costmap_cspace_msgs::MapMetaData3D map_info;
161  map_info.width = 1;
162  map_info.height = 1;
163  map_info.angle = 4;
164  map_info.linear_resolution = 1.0;
165  map_info.angular_resolution = M_PI / 2.0;
166  map_info.origin.orientation.w = 1.0;
167 
168  cm.setMapMetaData(map_info);
169 
170  ASSERT_EQ(cm.getRangeMax(), 0);
171 
172  const costmap_cspace::CSpace3Cache& temp = cm.getTemplate();
173  // Check template size
174  int x, y, a;
175  int cx, cy, ca;
176  temp.getSize(x, y, a);
177  temp.getCenter(cx, cy, ca);
178  ASSERT_EQ(x, 1);
179  ASSERT_EQ(y, 1);
180  ASSERT_EQ(a, 4);
181  ASSERT_EQ(cx, 0);
182  ASSERT_EQ(cy, 0);
183  ASSERT_EQ(ca, 0);
184 
185  // Check generated template
186  for (int k = -ca; k < a - ca; ++k)
187  {
188  ASSERT_EQ(temp.e(0, 0, k), 100);
189  }
190 }
191 
192 TEST(Costmap3dLayerFootprint, CSpaceGenerate)
193 {
195 
196  // Set example footprint
197  int footprint_offset = 0;
198  XmlRpc::XmlRpcValue footprint_xml;
199  footprint_xml.fromXml(footprint_str, &footprint_offset);
200  cm.setFootprint(costmap_cspace::Polygon(footprint_xml));
201 
202  // Settings: 4 angular grids, no expand/spread
203  cm.setAngleResolution(4);
204  cm.setExpansion(0.0, 0.0);
206 
207  // Generate sample map
208  nav_msgs::OccupancyGrid::Ptr map(new nav_msgs::OccupancyGrid);
209  map->info.width = 7;
210  map->info.height = 7;
211  map->info.resolution = 1.0;
212  map->info.origin.orientation.w = 1.0;
213  map->data.resize(map->info.width * map->info.height);
214 
215  // Apply empty map
216  cm.setBaseMap(map);
217 
218  for (int k = 0; k < cm.getAngularGrid(); ++k)
219  {
220  for (size_t j = 0; j < map->info.height; ++j)
221  {
222  for (size_t i = 0; i < map->info.width; ++i)
223  {
224  const int cost = cm.getMapOverlay()->getCost(i, j, k);
225  // All grid must be 0
226  ASSERT_EQ(cost, 0);
227  // std::cout << std::setfill(' ') << std::setw(3) << cost << " ";
228  }
229  // std::cout << std::endl;
230  }
231  // std::cout << "----" << std::endl;
232  }
233 
234  // std::cout << "========" << std::endl;
235  for (auto& g : map->data)
236  {
237  g = 100;
238  }
239  cm.setBaseMap(map);
240  for (int k = 0; k < cm.getAngularGrid(); ++k)
241  {
242  for (size_t j = 0; j < map->info.height; ++j)
243  {
244  for (size_t i = 0; i < map->info.width; ++i)
245  {
246  const int cost = cm.getMapOverlay()->getCost(i, j, k);
247  // All grid must be 100
248  ASSERT_EQ(cost, 100);
249  // std::cout << std::setfill(' ') << std::setw(3) << cost << " ";
250  }
251  // std::cout << std::endl;
252  }
253  // std::cout << "----" << std::endl;
254  }
255 
256  // C shape wall in the map
257  for (auto& g : map->data)
258  {
259  g = 0;
260  }
261  for (size_t i = map->info.width / 2 - 2; i < map->info.width / 2 + 2; ++i)
262  {
263  map->data[i + (map->info.height / 2 - 2) * map->info.width] = 100;
264  map->data[i + (map->info.height / 2 + 2) * map->info.width] = 100;
265  }
266  for (size_t i = map->info.height / 2 - 2; i < map->info.height / 2 + 2; ++i)
267  {
268  map->data[(map->info.width / 2 - 2) + i * map->info.width] = 100;
269  }
270  cm.setBaseMap(map);
271  for (int k = 0; k < cm.getAngularGrid(); ++k)
272  {
273  for (size_t j = 0; j < map->info.height; ++j)
274  {
275  for (size_t i = 0; i < map->info.width; ++i)
276  {
277  const int cost = cm.getMapOverlay()->getCost(i, j, k);
278 
279  // Offset according to the template shape
280  int cost_offset = 0;
281  const int i_offset = static_cast<int>(i) - temp_dir[k][0];
282  const int j_offset = static_cast<int>(j) - temp_dir[k][1];
283  if (static_cast<size_t>(i_offset) < map->info.width &&
284  static_cast<size_t>(j_offset) < map->info.height)
285  {
286  cost_offset = map->data[i_offset + j_offset * map->info.width];
287  }
288  if (map->data[i + j * map->info.width] == 100 || cost_offset == 100)
289  {
290  ASSERT_EQ(cost, 100);
291  }
292  else
293  {
294  ASSERT_EQ(cost, 0);
295  }
296  // std::cout << std::setfill(' ') << std::setw(3) << cost << " ";
297  }
298  // std::cout << std::endl;
299  }
300  // std::cout << "----" << std::endl;
301  }
302 }
303 
304 TEST(Costmap3dLayerFootprint, CSpaceExpandSpread)
305 {
307 
308  // Set example footprint
309  int footprint_offset = 0;
310  XmlRpc::XmlRpcValue footprint_xml;
311  footprint_xml.fromXml(footprint_str, &footprint_offset);
312  cm.setFootprint(costmap_cspace::Polygon(footprint_xml));
313 
314  // Settings: 4 angular grids, expand 1.0, spread 2.0
315  const float expand = 1.0;
316  const float spread = 2.0;
317  cm.setAngleResolution(4);
318  cm.setExpansion(expand, spread);
320 
321  // Generate sample map
322  nav_msgs::OccupancyGrid::Ptr map(new nav_msgs::OccupancyGrid);
323  map->info.width = 9;
324  map->info.height = 9;
325  map->info.resolution = 1.0;
326  map->info.origin.orientation.w = 1.0;
327  map->data.resize(map->info.width * map->info.height);
328 
329  const int max_cost = 80;
330  map->data[map->info.width / 2 + (map->info.height / 2) * map->info.width] = max_cost;
331  cm.setBaseMap(map);
332 
333  for (int k = 0; k < cm.getAngularGrid(); ++k)
334  {
335  const int i_center = map->info.width / 2;
336  const int j_center = map->info.height / 2;
337  const int i_center2 = map->info.width / 2 + temp_dir[k][0];
338  const int j_center2 = map->info.height / 2 + temp_dir[k][1];
339 
340  for (size_t j = 0; j < map->info.height; ++j)
341  {
342  for (size_t i = 0; i < map->info.width; ++i)
343  {
344  const int cost = cm.getMapOverlay()->getCost(i, j, k);
345  const float dist1 = hypotf(static_cast<int>(i) - i_center, static_cast<int>(j) - j_center);
346  const float dist2 = hypotf(static_cast<int>(i) - i_center2, static_cast<int>(j) - j_center2);
347  const float dist = std::min(dist1, dist2);
348  if (dist <= expand)
349  {
350  // Inside expand range must be max_cost
351  EXPECT_EQ(cost, max_cost);
352  }
353  else if (dist <= expand + spread)
354  {
355  // Between expand and spread must be intermidiate value
356  EXPECT_NE(cost, 0);
357  EXPECT_NE(cost, 100);
358  }
359  else if (dist > expand + spread + 1)
360  {
361  // Outside must be zero
362  // Since the template is calculated by the precised footprint not by the grid,
363  // tolerance of test (+1) is needed.
364  EXPECT_EQ(cost, 0);
365  }
366  // std::cout << std::setfill(' ') << std::setw(3) << cost << " ";
367  }
368  // std::cout << std::endl;
369  }
370  // std::cout << "----" << std::endl;
371  }
372 }
373 
374 TEST(Costmap3dLayerFootprint, CSpaceOverwrite)
375 {
378 
379  // Set example footprint
380  int footprint_offset = 0;
381  XmlRpc::XmlRpcValue footprint_xml;
382  footprint_xml.fromXml(footprint_str, &footprint_offset);
383  costmap_cspace::Polygon footprint(footprint_xml);
384  cm_ref.setFootprint(footprint);
385  cm_base.setFootprint(footprint);
386 
387  // Settings: 4 angular grids, no expand/spread
390  cm->setExpansion(0.0, 0.0);
391  cm->setFootprint(footprint);
394  cm_over->setExpansion(0.0, 0.0);
395  cm_over->setFootprint(footprint);
396  auto cm_output = cms.addLayer<costmap_cspace::Costmap3dLayerOutput>();
397 
398  cm_ref.setAngleResolution(4);
399  cm_ref.setExpansion(0.0, 0.0);
401  cm_base.setAngleResolution(4);
402  cm_base.setExpansion(0.0, 0.0);
404 
405  // Generate two sample maps
406  nav_msgs::OccupancyGrid::Ptr map(new nav_msgs::OccupancyGrid);
407  map->info.width = 9;
408  map->info.height = 9;
409  map->info.resolution = 1.0;
410  map->info.origin.orientation.w = 1.0;
411  map->data.resize(map->info.width * map->info.height);
412 
413  nav_msgs::OccupancyGrid::Ptr map2(new nav_msgs::OccupancyGrid);
414  *map2 = *map;
415 
416  const int num_points_base_map = 2;
417  const int points_base_map[num_points_base_map][2] =
418  {
419  2, 3,
420  4, 4
421  };
422  for (int i = 0; i < num_points_base_map; ++i)
423  {
424  map->data[points_base_map[i][0] + points_base_map[i][1] * map->info.width] = 100;
425  }
426 
427  const int num_points_local_map = 3;
428  const int points_local_map[num_points_local_map][2] =
429  {
430  3, 4,
431  5, 3,
432  4, 4
433  };
434  for (int i = 0; i < num_points_local_map; ++i)
435  {
436  map2->data[points_local_map[i][0] + points_local_map[i][1] * map->info.width] = 100;
437  }
438 
439  // Apply base map
440  cm->setBaseMap(map);
441 
442  // Overlay local map
443  costmap_cspace_msgs::CSpace3DUpdate::Ptr updated(new costmap_cspace_msgs::CSpace3DUpdate);
444  auto cb = [&updated](
446  const costmap_cspace_msgs::CSpace3DUpdate::Ptr& update) -> bool
447  {
448  updated = update;
449  return true;
450  };
451  cm_output->setHandler(cb);
452  cm_over->processMapOverlay(map2);
453 
454  // In this case, updated map must have same size as the base map. Check it.
455  ASSERT_EQ(updated->x, 0u);
456  ASSERT_EQ(updated->y, 0u);
457  ASSERT_EQ(updated->yaw, 0u);
458  ASSERT_EQ(updated->width, map->info.width);
459  ASSERT_EQ(updated->height, map->info.height);
460  ASSERT_EQ(updated->angle, static_cast<size_t>(cm_over->getAngularGrid()));
461 
462  // Generate reference local and base cspace map
463  cm_ref.setBaseMap(map2);
464  cm_base.setBaseMap(map);
465 
466  // Compare to confirm MAX mode
467  // note: boundary of the local map is not completely overwritten for keeping the spread effect.
468  for (int k = 0; k < cm_over->getAngularGrid(); ++k)
469  {
470  for (size_t j = cm_over->getRangeMax(); j < map->info.height - cm_over->getRangeMax(); ++j)
471  {
472  for (size_t i = cm_over->getRangeMax(); i < map->info.width - cm_over->getRangeMax(); ++i)
473  {
474  const size_t addr = ((k * map->info.height + j) * map->info.width) + i;
475  ROS_ASSERT(addr < updated->data.size());
476  const int cost = updated->data[addr];
477  const int cost_ref = cm_ref.getMapOverlay()->getCost(i, j, k);
478 
479  ASSERT_EQ(cost, cost_ref);
480 
481  // std::cout << std::setfill(' ') << std::setw(3) << cost << " ";
482  }
483  /*std::cout << " | ";
484  for (int i = cm_over->getRangeMax(); i < map->info.width - cm_over->getRangeMax(); ++i)
485  {
486  const int cost_ref = cm_ref.getMapOverlay()->getCost(i, j, k);
487  std::cout << std::setfill(' ') << std::setw(3) << cost_ref << " ";
488  }
489  std::cout << std::endl;
490  */
491  }
492  // std::cout << "----" << std::endl;
493  }
494  // Set MAX mode and check
495  cm_over->setAngleResolution(4);
496  cm_over->setExpansion(0.0, 0.0);
497  cm_over->setOverlayMode(costmap_cspace::MapOverlayMode::MAX);
498  costmap_cspace_msgs::CSpace3DUpdate::Ptr updated_max(new costmap_cspace_msgs::CSpace3DUpdate);
499  auto cb_max = [&updated_max](
501  const costmap_cspace_msgs::CSpace3DUpdate::Ptr& update) -> bool
502  {
503  updated_max = update;
504  return true;
505  };
506  cm_output->setHandler(cb_max);
507  cm_over->processMapOverlay(map2);
508 
509  ASSERT_EQ(updated_max->x, 0u);
510  ASSERT_EQ(updated_max->y, 0u);
511  ASSERT_EQ(updated_max->yaw, 0u);
512  ASSERT_EQ(updated_max->width, map->info.width);
513  ASSERT_EQ(updated_max->height, map->info.height);
514  ASSERT_EQ(updated_max->angle, static_cast<size_t>(cm_over->getAngularGrid()));
515 
516  for (int k = 0; k < cm_over->getAngularGrid(); ++k)
517  {
518  for (int j = cm_over->getRangeMax(); j < static_cast<int>(map->info.height) - cm_over->getRangeMax(); ++j)
519  {
520  for (int i = cm_over->getRangeMax(); i < static_cast<int>(map->info.width) - cm_over->getRangeMax(); ++i)
521  {
522  const size_t addr = ((k * map->info.height + j) * map->info.width) + i;
523  ROS_ASSERT(addr < updated_max->data.size());
524  const int cost = updated_max->data[addr];
525  const int cost_ref = cm_ref.getMapOverlay()->getCost(i, j, k);
526  const int cost_base = cm_base.getMapOverlay()->getCost(i, j, k);
527  const int cost_max = std::max(cost_ref, cost_base);
528 
529  ASSERT_EQ(cost, cost_max);
530 
531  // std::cout << std::setfill(' ') << std::setw(3) << cost << " ";
532  }
533  /*
534  std::cout << " | ";
535  for (int i = cm_over->getRangeMax(); i < map->info.width - cm_over->getRangeMax(); ++i)
536  {
537  const int cost_ref = cm_ref.getMapOverlay()->getCost(i, j, k);
538  std::cout << std::setfill(' ') << std::setw(3) << cost_ref << " ";
539  }
540  std::cout << " | ";
541  for (int i = cm_over->getRangeMax(); i < map->info.width - cm_over->getRangeMax(); ++i)
542  {
543  const int cost_base = cm_base.getMapOverlay()->getCost(i, j, k);
544  std::cout << std::setfill(' ') << std::setw(3) << cost_base << " ";
545  }
546  std::cout << std::endl;
547  */
548  }
549  // std::cout << "----" << std::endl;
550  }
551 }
552 
553 TEST(Costmap3dLayerFootprint, CSpaceOverlayMove)
554 {
555  // Set example footprint
556  int footprint_offset = 0;
557  XmlRpc::XmlRpcValue footprint_xml;
558  footprint_xml.fromXml(footprint_str, &footprint_offset);
559  costmap_cspace::Polygon footprint(footprint_xml);
560 
561  // Settings: 4 angular grids, no expand/spread
564  cm->setExpansion(0.0, 0.0);
565  cm->setFootprint(footprint);
568  cm_over->setExpansion(0.0, 0.0);
569  cm_over->setFootprint(footprint);
570 
571  // Generate sample map
572  nav_msgs::OccupancyGrid::Ptr map(new nav_msgs::OccupancyGrid);
573  map->info.width = 5;
574  map->info.height = 5;
575  map->info.resolution = 1.0;
576  map->info.origin.orientation.w = 1.0;
577  map->data.resize(map->info.width * map->info.height);
578 
579  const int max_cost = 100;
580  map->data[map->info.width / 2 + (map->info.height / 2) * map->info.width] = max_cost;
581  cm->setBaseMap(map);
582 
583  // Generate local sample map
584  nav_msgs::OccupancyGrid::Ptr map2(new nav_msgs::OccupancyGrid);
585  *map2 = *map;
586 
587  for (int xp = -1; xp <= 1; ++xp)
588  {
589  for (int yp = -1; yp <= 1; ++yp)
590  {
591  map2->info.origin.position.x = map2->info.resolution * xp;
592  map2->info.origin.position.y = map2->info.resolution * yp;
593  /*
594  std::cout << "=== origin: ("
595  << map2->info.origin.position.x << ", " << map2->info.origin.position.y
596  << ")" << std::endl;
597  */
598  cm_over->processMapOverlay(map2);
599  for (int k = 0; k < cm_over->getAngularGrid(); ++k)
600  {
601  const size_t i_center = map->info.width / 2;
602  const size_t j_center = map->info.height / 2;
603  const size_t i_center2 = map->info.width / 2 + temp_dir[k][0];
604  const size_t j_center2 = map->info.height / 2 + temp_dir[k][1];
605 
606  for (size_t j = 0; j < map->info.height; ++j)
607  {
608  for (size_t i = 0; i < map->info.width; ++i)
609  {
610  const int cost = cm_over->getMapOverlay()->getCost(i, j, k);
611 
612  if ((i == i_center && j == j_center) ||
613  (i == i_center2 && j == j_center2) ||
614  (i == i_center + xp && j == j_center + yp) ||
615  (i == i_center2 + xp && j == j_center2 + yp))
616  {
617  ASSERT_EQ(cost, max_cost);
618  }
619  else
620  {
621  ASSERT_EQ(cost, 0);
622  }
623  // std::cout << std::setfill(' ') << std::setw(3) << cost << " ";
624  }
625  // std::cout << std::endl;
626  }
627  // std::cout << "----" << std::endl;
628  }
629  }
630  }
631 }
632 
633 int main(int argc, char** argv)
634 {
635  testing::InitGoogleTest(&argc, argv);
636 
637  return RUN_ALL_TESTS();
638 }
void setMapMetaData(const costmap_cspace_msgs::MapMetaData3D &info)
Definition: footprint.h:115
const char temp_dir[4][2]
void setExpansion(const float linear_expand, const float linear_spread)
Definition: footprint.h:77
const std::string footprint_str("<value><array><data>"" <value><array><data>"" <value><double>1.5</double></value>"" <value><double>0.0</double></value>"" </data></array></value>"" <value><array><data>"" <value><double>-0.5</double></value>"" <value><double>-0.5</double></value>"" </data></array></value>"" <value><array><data>"" <value><double>-0.5</double></value>"" <value><double>0.5</double></value>"" </data></array></value>""</data></array></value>")
void getCenter(int &x, int &y, int &a) const
Definition: cspace3_cache.h:91
const CSpace3Cache & getTemplate() const
Definition: footprint.h:111
void setOverlayMode(const MapOverlayMode overlay_mode)
Definition: base.h:243
TFSIMD_FORCE_INLINE const tfScalar & y() const
std::vector< Vec > v
Definition: polygon.h:89
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
void setBaseMap(const nav_msgs::OccupancyGrid::ConstPtr &base_map)
Definition: base.h:254
T::Ptr addLayer(const MapOverlayMode overlay_mode=MapOverlayMode::MAX)
Definition: costmap_3d.h:78
TFSIMD_FORCE_INLINE const tfScalar & x() const
void setAngleResolution(const int ang_resolution)
Definition: base.h:238
int main(int argc, char **argv)
bool fromXml(std::string const &valueXml, int *offset)
void getSize(int &x, int &y, int &a) const
Definition: cspace3_cache.h:85
std::shared_ptr< CSpace3DMsg > Ptr
Definition: base.h:50
TEST(Costmap3dLayerFootprint, CSpaceTemplate)
#define ROS_ASSERT(cond)
void setFootprint(const Polygon footprint)
Definition: footprint.h:89
char & e(const int &x, const int &y, const int &yaw)
Definition: cspace3_cache.h:71
CSpace3DMsg::Ptr getMapOverlay()
Definition: base.h:333


costmap_cspace
Author(s): Atsushi Watanabe
autogenerated on Tue Jul 9 2019 04:59:48