34 #include <gtest/gtest.h> 37 #include <nav_msgs/OccupancyGrid.h> 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>");
68 TEST(Costmap3dLayerFootprint, CSpaceTemplate)
78 int footprint_offset = 0;
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);
93 ASSERT_EQ(polygon.
v[3][0], 1.5);
94 ASSERT_EQ(polygon.
v[3][1], 0.0);
98 costmap_cspace_msgs::MapMetaData3D map_info;
102 map_info.linear_resolution = 1.0;
103 map_info.angular_resolution = M_PI / 2.0;
104 map_info.origin.orientation.w = 1.0;
108 ASSERT_EQ(cm.
getRangeMax(),
static_cast<int>(ceilf(1.5 / 1.0)));
116 ASSERT_EQ(x, 2 * 2 + 1);
117 ASSERT_EQ(y, 2 * 2 + 1);
124 for (
int k = -ca; k < a - ca; ++k)
126 for (
int j = -cy; j < y - cy; ++j)
128 for (
int i = -cx; i < x - cx; ++i)
130 if (i == 0 && j == 0)
132 ASSERT_EQ(temp.
e(i, j, k), 100);
136 ASSERT_EQ(temp.
e(i, j, k), 100);
140 ASSERT_EQ(temp.
e(i, j, k), 0);
150 TEST(Costmap3dLayerPlain, CSpaceTemplate)
160 costmap_cspace_msgs::MapMetaData3D map_info;
164 map_info.linear_resolution = 1.0;
165 map_info.angular_resolution = M_PI / 2.0;
166 map_info.origin.orientation.w = 1.0;
176 temp.getSize(x, y, a);
177 temp.getCenter(cx, cy, ca);
186 for (
int k = -ca; k < a - ca; ++k)
188 ASSERT_EQ(temp.e(0, 0, k), 100);
192 TEST(Costmap3dLayerFootprint, CSpaceGenerate)
197 int footprint_offset = 0;
208 nav_msgs::OccupancyGrid::Ptr map(
new nav_msgs::OccupancyGrid);
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);
220 for (
size_t j = 0; j < map->info.height; ++j)
222 for (
size_t i = 0; i < map->info.width; ++i)
235 for (
auto& g : map->data)
242 for (
size_t j = 0; j < map->info.height; ++j)
244 for (
size_t i = 0; i < map->info.width; ++i)
248 ASSERT_EQ(cost, 100);
257 for (
auto& g : map->data)
261 for (
size_t i = map->info.width / 2 - 2; i < map->info.width / 2 + 2; ++i)
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;
266 for (
size_t i = map->info.height / 2 - 2; i < map->info.height / 2 + 2; ++i)
268 map->data[(map->info.width / 2 - 2) + i * map->info.width] = 100;
273 for (
size_t j = 0; j < map->info.height; ++j)
275 for (
size_t i = 0; i < map->info.width; ++i)
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)
286 cost_offset = map->data[i_offset + j_offset * map->info.width];
288 if (map->data[i + j * map->info.width] == 100 || cost_offset == 100)
290 ASSERT_EQ(cost, 100);
304 TEST(Costmap3dLayerFootprint, CSpaceExpandSpread)
309 int footprint_offset = 0;
315 const float expand = 1.0;
316 const float spread = 2.0;
322 nav_msgs::OccupancyGrid::Ptr map(
new nav_msgs::OccupancyGrid);
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);
329 const int max_cost = 80;
330 map->data[map->info.width / 2 + (map->info.height / 2) * map->info.width] = max_cost;
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];
340 for (
size_t j = 0; j < map->info.height; ++j)
342 for (
size_t i = 0; i < map->info.width; ++i)
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);
351 EXPECT_EQ(cost, max_cost);
353 else if (dist <= expand + spread)
357 EXPECT_NE(cost, 100);
359 else if (dist > expand + spread + 1)
374 TEST(Costmap3dLayerFootprint, CSpaceOverwrite)
380 int footprint_offset = 0;
391 cm->setFootprint(footprint);
395 cm_over->setFootprint(footprint);
406 nav_msgs::OccupancyGrid::Ptr map(
new nav_msgs::OccupancyGrid);
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);
413 nav_msgs::OccupancyGrid::Ptr map2(
new nav_msgs::OccupancyGrid);
416 const int num_points_base_map = 2;
417 const int points_base_map[num_points_base_map][2] =
422 for (
int i = 0; i < num_points_base_map; ++i)
424 map->data[points_base_map[i][0] + points_base_map[i][1] * map->info.width] = 100;
427 const int num_points_local_map = 3;
428 const int points_local_map[num_points_local_map][2] =
434 for (
int i = 0; i < num_points_local_map; ++i)
436 map2->data[points_local_map[i][0] + points_local_map[i][1] * map->info.width] = 100;
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 451 cm_output->setHandler(cb);
452 cm_over->processMapOverlay(map2);
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()));
468 for (
int k = 0; k < cm_over->getAngularGrid(); ++k)
470 for (
size_t j = cm_over->getRangeMax(); j < map->info.height - cm_over->getRangeMax(); ++j)
472 for (
size_t i = cm_over->getRangeMax(); i < map->info.width - cm_over->getRangeMax(); ++i)
474 const size_t addr = ((k * map->info.height + j) * map->info.width) + i;
476 const int cost = updated->data[addr];
477 const int cost_ref = cm_ref.
getMapOverlay()->getCost(i, j, k);
479 ASSERT_EQ(cost, cost_ref);
495 cm_over->setAngleResolution(4);
496 cm_over->setExpansion(0.0, 0.0);
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 506 cm_output->setHandler(cb_max);
507 cm_over->processMapOverlay(map2);
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()));
516 for (
int k = 0; k < cm_over->getAngularGrid(); ++k)
518 for (
int j = cm_over->getRangeMax(); j < static_cast<int>(map->info.height) - cm_over->getRangeMax(); ++j)
520 for (
int i = cm_over->getRangeMax(); i < static_cast<int>(map->info.width) - cm_over->getRangeMax(); ++i)
522 const size_t addr = ((k * map->info.height + j) * map->info.width) + i;
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);
529 ASSERT_EQ(cost, cost_max);
553 TEST(Costmap3dLayerFootprint, CSpaceOverlayMove)
556 int footprint_offset = 0;
565 cm->setFootprint(footprint);
569 cm_over->setFootprint(footprint);
572 nav_msgs::OccupancyGrid::Ptr map(
new nav_msgs::OccupancyGrid);
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);
579 const int max_cost = 100;
580 map->data[map->info.width / 2 + (map->info.height / 2) * map->info.width] = max_cost;
584 nav_msgs::OccupancyGrid::Ptr map2(
new nav_msgs::OccupancyGrid);
587 for (
int xp = -1; xp <= 1; ++xp)
589 for (
int yp = -1; yp <= 1; ++yp)
591 map2->info.origin.position.x = map2->info.resolution * xp;
592 map2->info.origin.position.y = map2->info.resolution * yp;
598 cm_over->processMapOverlay(map2);
599 for (
int k = 0; k < cm_over->getAngularGrid(); ++k)
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];
606 for (
size_t j = 0; j < map->info.height; ++j)
608 for (
size_t i = 0; i < map->info.width; ++i)
610 const int cost = cm_over->getMapOverlay()->getCost(i, j, k);
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))
617 ASSERT_EQ(cost, max_cost);
633 int main(
int argc,
char** argv)
635 testing::InitGoogleTest(&argc, argv);
637 return RUN_ALL_TESTS();
const char temp_dir[4][2]
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
void setOverlayMode(const MapOverlayMode overlay_mode)
TFSIMD_FORCE_INLINE const tfScalar & y() const
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
void setBaseMap(const nav_msgs::OccupancyGrid::ConstPtr &base_map)
T::Ptr addLayer(const MapOverlayMode overlay_mode=MapOverlayMode::MAX)
TFSIMD_FORCE_INLINE const tfScalar & x() const
void setAngleResolution(const int ang_resolution)
int main(int argc, char **argv)
bool fromXml(std::string const &valueXml, int *offset)
void getSize(int &x, int &y, int &a) const
std::shared_ptr< CSpace3DMsg > Ptr
TEST(Costmap3dLayerFootprint, CSpaceTemplate)
char & e(const int &x, const int &y, const int &yaw)
int getAngularGrid() const
CSpace3DMsg::Ptr getMapOverlay()