test_costmap_3d.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2016-2018, the neonavigation authors
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the copyright holder nor the names of its 
00014  *       contributors may be used to endorse or promote products derived from 
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <cstddef>
00031 #include <string>
00032 #include <algorithm>
00033 
00034 #include <gtest/gtest.h>
00035 
00036 #include <ros/ros.h>
00037 #include <nav_msgs/OccupancyGrid.h>
00038 #include <costmap_cspace/costmap_3d.h>
00039 
00040 const std::string footprint_str(
00041     "<value><array><data>"
00042     "  <value><array><data>"
00043     "    <value><double>1.5</double></value>"
00044     "    <value><double>0.0</double></value>"
00045     "  </data></array></value>"
00046     "  <value><array><data>"
00047     "    <value><double>-0.5</double></value>"
00048     "    <value><double>-0.5</double></value>"
00049     "  </data></array></value>"
00050     "  <value><array><data>"
00051     "    <value><double>-0.5</double></value>"
00052     "    <value><double>0.5</double></value>"
00053     "  </data></array></value>"
00054     "</data></array></value>");
00055 // This footprint with resolution of 0.1 means:
00056 //  0 0 0
00057 //  1 1 0  <--x
00058 //  0 0 0
00059 const char temp_dir[4][2] =
00060     {
00061       // x, y which must be occupied in the template
00062       -1, 0,
00063       0, -1,
00064       1, 0,
00065       0, 1
00066     };
00067 
00068 TEST(Costmap3dLayerFootprint, CSpaceTemplate)
00069 {
00070   costmap_cspace::Costmap3dLayerFootprint cm;
00071 
00072   // Settings: 4 angular grids, no expand/spread
00073   cm.setAngleResolution(4);
00074   cm.setExpansion(0.0, 0.0);
00075   cm.setOverlayMode(costmap_cspace::MapOverlayMode::MAX);
00076 
00077   // Set example footprint
00078   int footprint_offset = 0;
00079   XmlRpc::XmlRpcValue footprint_xml;
00080   ASSERT_TRUE(footprint_xml.fromXml(footprint_str, &footprint_offset));
00081   cm.setFootprint(costmap_cspace::Polygon(footprint_xml));
00082 
00083   // Check local footprint
00084   const costmap_cspace::Polygon polygon = cm.getFootprint();
00085   ASSERT_EQ(polygon.v.size(), 3u + 1u);
00086   ASSERT_EQ(polygon.v[0][0], 1.5);
00087   ASSERT_EQ(polygon.v[0][1], 0.0);
00088   ASSERT_EQ(polygon.v[1][0], -0.5);
00089   ASSERT_EQ(polygon.v[1][1], -0.5);
00090   ASSERT_EQ(polygon.v[2][0], -0.5);
00091   ASSERT_EQ(polygon.v[2][1], 0.5);
00092   // Last point is same as the first point
00093   ASSERT_EQ(polygon.v[3][0], 1.5);
00094   ASSERT_EQ(polygon.v[3][1], 0.0);
00095   ASSERT_EQ(cm.getFootprintRadius(), 1.5);
00096 
00097   // Generate CSpace pattern around the robot
00098   costmap_cspace_msgs::MapMetaData3D map_info;
00099   map_info.width = 3;
00100   map_info.height = 3;
00101   map_info.angle = 4;
00102   map_info.linear_resolution = 1.0;
00103   map_info.angular_resolution = M_PI / 2.0;
00104   map_info.origin.orientation.w = 1.0;
00105 
00106   cm.setMapMetaData(map_info);
00107 
00108   ASSERT_EQ(cm.getRangeMax(), static_cast<int>(ceilf(1.5 / 1.0)));
00109 
00110   const costmap_cspace::CSpace3Cache& temp = cm.getTemplate();
00111   // Check template size
00112   int x, y, a;
00113   int cx, cy, ca;
00114   temp.getSize(x, y, a);
00115   temp.getCenter(cx, cy, ca);
00116   ASSERT_EQ(x, 2 * 2 + 1);
00117   ASSERT_EQ(y, 2 * 2 + 1);
00118   ASSERT_EQ(a, 4);
00119   ASSERT_EQ(cx, 2);
00120   ASSERT_EQ(cy, 2);
00121   ASSERT_EQ(ca, 0);
00122 
00123   // Check generated template
00124   for (int k = -ca; k < a - ca; ++k)
00125   {
00126     for (int j = -cy; j < y - cy; ++j)
00127     {
00128       for (int i = -cx; i < x - cx; ++i)
00129       {
00130         if (i == 0 && j == 0)
00131         {
00132           ASSERT_EQ(temp.e(i, j, k), 100);
00133         }
00134         else if (i == temp_dir[k + ca][0] && j == temp_dir[k + ca][1])
00135         {
00136           ASSERT_EQ(temp.e(i, j, k), 100);
00137         }
00138         else
00139         {
00140           ASSERT_EQ(temp.e(i, j, k), 0);
00141         }
00142         // std::cout << std::setfill(' ') << std::setw(3) << static_cast<int>(temp.e(i, j, k)) << " ";
00143       }
00144       // std::cout << std::endl;
00145     }
00146     // std::cout << "----" << std::endl;
00147   }
00148 }
00149 
00150 TEST(Costmap3dLayerPlain, CSpaceTemplate)
00151 {
00152   costmap_cspace::Costmap3dLayerPlain cm;
00153 
00154   // Settings: 4 angular grids, no expand/spread
00155   cm.setAngleResolution(4);
00156   cm.setExpansion(0.0, 0.0);
00157   cm.setOverlayMode(costmap_cspace::MapOverlayMode::MAX);
00158 
00159   // Generate CSpace pattern around the robot
00160   costmap_cspace_msgs::MapMetaData3D map_info;
00161   map_info.width = 1;
00162   map_info.height = 1;
00163   map_info.angle = 4;
00164   map_info.linear_resolution = 1.0;
00165   map_info.angular_resolution = M_PI / 2.0;
00166   map_info.origin.orientation.w = 1.0;
00167 
00168   cm.setMapMetaData(map_info);
00169 
00170   ASSERT_EQ(cm.getRangeMax(), 0);
00171 
00172   const costmap_cspace::CSpace3Cache& temp = cm.getTemplate();
00173   // Check template size
00174   int x, y, a;
00175   int cx, cy, ca;
00176   temp.getSize(x, y, a);
00177   temp.getCenter(cx, cy, ca);
00178   ASSERT_EQ(x, 1);
00179   ASSERT_EQ(y, 1);
00180   ASSERT_EQ(a, 4);
00181   ASSERT_EQ(cx, 0);
00182   ASSERT_EQ(cy, 0);
00183   ASSERT_EQ(ca, 0);
00184 
00185   // Check generated template
00186   for (int k = -ca; k < a - ca; ++k)
00187   {
00188     ASSERT_EQ(temp.e(0, 0, k), 100);
00189   }
00190 }
00191 
00192 TEST(Costmap3dLayerFootprint, CSpaceGenerate)
00193 {
00194   costmap_cspace::Costmap3dLayerFootprint cm;
00195 
00196   // Set example footprint
00197   int footprint_offset = 0;
00198   XmlRpc::XmlRpcValue footprint_xml;
00199   footprint_xml.fromXml(footprint_str, &footprint_offset);
00200   cm.setFootprint(costmap_cspace::Polygon(footprint_xml));
00201 
00202   // Settings: 4 angular grids, no expand/spread
00203   cm.setAngleResolution(4);
00204   cm.setExpansion(0.0, 0.0);
00205   cm.setOverlayMode(costmap_cspace::MapOverlayMode::MAX);
00206 
00207   // Generate sample map
00208   nav_msgs::OccupancyGrid::Ptr map(new nav_msgs::OccupancyGrid);
00209   map->info.width = 7;
00210   map->info.height = 7;
00211   map->info.resolution = 1.0;
00212   map->info.origin.orientation.w = 1.0;
00213   map->data.resize(map->info.width * map->info.height);
00214 
00215   // Apply empty map
00216   cm.setBaseMap(map);
00217 
00218   for (int k = 0; k < cm.getAngularGrid(); ++k)
00219   {
00220     for (size_t j = 0; j < map->info.height; ++j)
00221     {
00222       for (size_t i = 0; i < map->info.width; ++i)
00223       {
00224         const int cost = cm.getMapOverlay()->getCost(i, j, k);
00225         // All grid must be 0
00226         ASSERT_EQ(cost, 0);
00227         // std::cout << std::setfill(' ') << std::setw(3) << cost << " ";
00228       }
00229       // std::cout << std::endl;
00230     }
00231     // std::cout << "----" << std::endl;
00232   }
00233 
00234   // std::cout << "========" << std::endl;
00235   for (auto& g : map->data)
00236   {
00237     g = 100;
00238   }
00239   cm.setBaseMap(map);
00240   for (int k = 0; k < cm.getAngularGrid(); ++k)
00241   {
00242     for (size_t j = 0; j < map->info.height; ++j)
00243     {
00244       for (size_t i = 0; i < map->info.width; ++i)
00245       {
00246         const int cost = cm.getMapOverlay()->getCost(i, j, k);
00247         // All grid must be 100
00248         ASSERT_EQ(cost, 100);
00249         // std::cout << std::setfill(' ') << std::setw(3) << cost << " ";
00250       }
00251       // std::cout << std::endl;
00252     }
00253     // std::cout << "----" << std::endl;
00254   }
00255 
00256   // C shape wall in the map
00257   for (auto& g : map->data)
00258   {
00259     g = 0;
00260   }
00261   for (size_t i = map->info.width / 2 - 2; i < map->info.width / 2 + 2; ++i)
00262   {
00263     map->data[i + (map->info.height / 2 - 2) * map->info.width] = 100;
00264     map->data[i + (map->info.height / 2 + 2) * map->info.width] = 100;
00265   }
00266   for (size_t i = map->info.height / 2 - 2; i < map->info.height / 2 + 2; ++i)
00267   {
00268     map->data[(map->info.width / 2 - 2) + i * map->info.width] = 100;
00269   }
00270   cm.setBaseMap(map);
00271   for (int k = 0; k < cm.getAngularGrid(); ++k)
00272   {
00273     for (size_t j = 0; j < map->info.height; ++j)
00274     {
00275       for (size_t i = 0; i < map->info.width; ++i)
00276       {
00277         const int cost = cm.getMapOverlay()->getCost(i, j, k);
00278 
00279         // Offset according to the template shape
00280         int cost_offset = 0;
00281         const int i_offset = static_cast<int>(i) - temp_dir[k][0];
00282         const int j_offset = static_cast<int>(j) - temp_dir[k][1];
00283         if (static_cast<size_t>(i_offset) < map->info.width &&
00284             static_cast<size_t>(j_offset) < map->info.height)
00285         {
00286           cost_offset = map->data[i_offset + j_offset * map->info.width];
00287         }
00288         if (map->data[i + j * map->info.width] == 100 || cost_offset == 100)
00289         {
00290           ASSERT_EQ(cost, 100);
00291         }
00292         else
00293         {
00294           ASSERT_EQ(cost, 0);
00295         }
00296         // std::cout << std::setfill(' ') << std::setw(3) << cost << " ";
00297       }
00298       // std::cout << std::endl;
00299     }
00300     // std::cout << "----" << std::endl;
00301   }
00302 }
00303 
00304 TEST(Costmap3dLayerFootprint, CSpaceExpandSpread)
00305 {
00306   costmap_cspace::Costmap3dLayerFootprint cm;
00307 
00308   // Set example footprint
00309   int footprint_offset = 0;
00310   XmlRpc::XmlRpcValue footprint_xml;
00311   footprint_xml.fromXml(footprint_str, &footprint_offset);
00312   cm.setFootprint(costmap_cspace::Polygon(footprint_xml));
00313 
00314   // Settings: 4 angular grids, expand 1.0, spread 2.0
00315   const float expand = 1.0;
00316   const float spread = 2.0;
00317   cm.setAngleResolution(4);
00318   cm.setExpansion(expand, spread);
00319   cm.setOverlayMode(costmap_cspace::MapOverlayMode::MAX);
00320 
00321   // Generate sample map
00322   nav_msgs::OccupancyGrid::Ptr map(new nav_msgs::OccupancyGrid);
00323   map->info.width = 9;
00324   map->info.height = 9;
00325   map->info.resolution = 1.0;
00326   map->info.origin.orientation.w = 1.0;
00327   map->data.resize(map->info.width * map->info.height);
00328 
00329   const int max_cost = 80;
00330   map->data[map->info.width / 2 + (map->info.height / 2) * map->info.width] = max_cost;
00331   cm.setBaseMap(map);
00332 
00333   for (int k = 0; k < cm.getAngularGrid(); ++k)
00334   {
00335     const int i_center = map->info.width / 2;
00336     const int j_center = map->info.height / 2;
00337     const int i_center2 = map->info.width / 2 + temp_dir[k][0];
00338     const int j_center2 = map->info.height / 2 + temp_dir[k][1];
00339 
00340     for (size_t j = 0; j < map->info.height; ++j)
00341     {
00342       for (size_t i = 0; i < map->info.width; ++i)
00343       {
00344         const int cost = cm.getMapOverlay()->getCost(i, j, k);
00345         const float dist1 = hypotf(static_cast<int>(i) - i_center, static_cast<int>(j) - j_center);
00346         const float dist2 = hypotf(static_cast<int>(i) - i_center2, static_cast<int>(j) - j_center2);
00347         const float dist = std::min(dist1, dist2);
00348         if (dist <= expand)
00349         {
00350           // Inside expand range must be max_cost
00351           EXPECT_EQ(cost, max_cost);
00352         }
00353         else if (dist <= expand + spread)
00354         {
00355           // Between expand and spread must be intermidiate value
00356           EXPECT_NE(cost, 0);
00357           EXPECT_NE(cost, 100);
00358         }
00359         else if (dist > expand + spread + 1)
00360         {
00361           // Outside must be zero
00362           // Since the template is calculated by the precised footprint not by the grid,
00363           // tolerance of test (+1) is needed.
00364           EXPECT_EQ(cost, 0);
00365         }
00366         // std::cout << std::setfill(' ') << std::setw(3) << cost << " ";
00367       }
00368       // std::cout << std::endl;
00369     }
00370     // std::cout << "----" << std::endl;
00371   }
00372 }
00373 
00374 TEST(Costmap3dLayerFootprint, CSpaceOverwrite)
00375 {
00376   costmap_cspace::Costmap3dLayerFootprint cm_ref;
00377   costmap_cspace::Costmap3dLayerFootprint cm_base;
00378 
00379   // Set example footprint
00380   int footprint_offset = 0;
00381   XmlRpc::XmlRpcValue footprint_xml;
00382   footprint_xml.fromXml(footprint_str, &footprint_offset);
00383   costmap_cspace::Polygon footprint(footprint_xml);
00384   cm_ref.setFootprint(footprint);
00385   cm_base.setFootprint(footprint);
00386 
00387   // Settings: 4 angular grids, no expand/spread
00388   costmap_cspace::Costmap3d cms(4);
00389   auto cm = cms.addRootLayer<costmap_cspace::Costmap3dLayerFootprint>();
00390   cm->setExpansion(0.0, 0.0);
00391   cm->setFootprint(footprint);
00392   auto cm_over = cms.addLayer<costmap_cspace::Costmap3dLayerFootprint>(
00393       costmap_cspace::MapOverlayMode::OVERWRITE);
00394   cm_over->setExpansion(0.0, 0.0);
00395   cm_over->setFootprint(footprint);
00396   auto cm_output = cms.addLayer<costmap_cspace::Costmap3dLayerOutput>();
00397 
00398   cm_ref.setAngleResolution(4);
00399   cm_ref.setExpansion(0.0, 0.0);
00400   cm_ref.setOverlayMode(costmap_cspace::MapOverlayMode::OVERWRITE);
00401   cm_base.setAngleResolution(4);
00402   cm_base.setExpansion(0.0, 0.0);
00403   cm_base.setOverlayMode(costmap_cspace::MapOverlayMode::OVERWRITE);
00404 
00405   // Generate two sample maps
00406   nav_msgs::OccupancyGrid::Ptr map(new nav_msgs::OccupancyGrid);
00407   map->info.width = 9;
00408   map->info.height = 9;
00409   map->info.resolution = 1.0;
00410   map->info.origin.orientation.w = 1.0;
00411   map->data.resize(map->info.width * map->info.height);
00412 
00413   nav_msgs::OccupancyGrid::Ptr map2(new nav_msgs::OccupancyGrid);
00414   *map2 = *map;
00415 
00416   const int num_points_base_map = 2;
00417   const int points_base_map[num_points_base_map][2] =
00418       {
00419         2, 3,
00420         4, 4
00421       };
00422   for (int i = 0; i < num_points_base_map; ++i)
00423   {
00424     map->data[points_base_map[i][0] + points_base_map[i][1] * map->info.width] = 100;
00425   }
00426 
00427   const int num_points_local_map = 3;
00428   const int points_local_map[num_points_local_map][2] =
00429       {
00430         3, 4,
00431         5, 3,
00432         4, 4
00433       };
00434   for (int i = 0; i < num_points_local_map; ++i)
00435   {
00436     map2->data[points_local_map[i][0] + points_local_map[i][1] * map->info.width] = 100;
00437   }
00438 
00439   // Apply base map
00440   cm->setBaseMap(map);
00441 
00442   // Overlay local map
00443   costmap_cspace_msgs::CSpace3DUpdate::Ptr updated(new costmap_cspace_msgs::CSpace3DUpdate);
00444   auto cb = [&updated](
00445       const costmap_cspace::CSpace3DMsg::Ptr map,
00446       const costmap_cspace_msgs::CSpace3DUpdate::Ptr& update) -> bool
00447   {
00448     updated = update;
00449     return true;
00450   };
00451   cm_output->setHandler(cb);
00452   cm_over->processMapOverlay(map2);
00453 
00454   // In this case, updated map must have same size as the base map. Check it.
00455   ASSERT_EQ(updated->x, 0u);
00456   ASSERT_EQ(updated->y, 0u);
00457   ASSERT_EQ(updated->yaw, 0u);
00458   ASSERT_EQ(updated->width, map->info.width);
00459   ASSERT_EQ(updated->height, map->info.height);
00460   ASSERT_EQ(updated->angle, static_cast<size_t>(cm_over->getAngularGrid()));
00461 
00462   // Generate reference local and base cspace map
00463   cm_ref.setBaseMap(map2);
00464   cm_base.setBaseMap(map);
00465 
00466   // Compare to confirm MAX mode
00467   // note: boundary of the local map is not completely overwritten for keeping the spread effect.
00468   for (int k = 0; k < cm_over->getAngularGrid(); ++k)
00469   {
00470     for (size_t j = cm_over->getRangeMax(); j < map->info.height - cm_over->getRangeMax(); ++j)
00471     {
00472       for (size_t i = cm_over->getRangeMax(); i < map->info.width - cm_over->getRangeMax(); ++i)
00473       {
00474         const size_t addr = ((k * map->info.height + j) * map->info.width) + i;
00475         ROS_ASSERT(addr < updated->data.size());
00476         const int cost = updated->data[addr];
00477         const int cost_ref = cm_ref.getMapOverlay()->getCost(i, j, k);
00478 
00479         ASSERT_EQ(cost, cost_ref);
00480 
00481         // std::cout << std::setfill(' ') << std::setw(3) << cost << " ";
00482       }
00483       /*std::cout << "  |  ";
00484       for (int i = cm_over->getRangeMax(); i < map->info.width - cm_over->getRangeMax(); ++i)
00485       {
00486         const int cost_ref = cm_ref.getMapOverlay()->getCost(i, j, k);
00487         std::cout << std::setfill(' ') << std::setw(3) << cost_ref << " ";
00488       }
00489       std::cout << std::endl;
00490       */
00491     }
00492     // std::cout << "----" << std::endl;
00493   }
00494   // Set MAX mode and check
00495   cm_over->setAngleResolution(4);
00496   cm_over->setExpansion(0.0, 0.0);
00497   cm_over->setOverlayMode(costmap_cspace::MapOverlayMode::MAX);
00498   costmap_cspace_msgs::CSpace3DUpdate::Ptr updated_max(new costmap_cspace_msgs::CSpace3DUpdate);
00499   auto cb_max = [&updated_max](
00500       const costmap_cspace::CSpace3DMsg::Ptr map,
00501       const costmap_cspace_msgs::CSpace3DUpdate::Ptr& update) -> bool
00502   {
00503     updated_max = update;
00504     return true;
00505   };
00506   cm_output->setHandler(cb_max);
00507   cm_over->processMapOverlay(map2);
00508 
00509   ASSERT_EQ(updated_max->x, 0u);
00510   ASSERT_EQ(updated_max->y, 0u);
00511   ASSERT_EQ(updated_max->yaw, 0u);
00512   ASSERT_EQ(updated_max->width, map->info.width);
00513   ASSERT_EQ(updated_max->height, map->info.height);
00514   ASSERT_EQ(updated_max->angle, static_cast<size_t>(cm_over->getAngularGrid()));
00515 
00516   for (int k = 0; k < cm_over->getAngularGrid(); ++k)
00517   {
00518     for (int j = cm_over->getRangeMax(); j < static_cast<int>(map->info.height) - cm_over->getRangeMax(); ++j)
00519     {
00520       for (int i = cm_over->getRangeMax(); i < static_cast<int>(map->info.width) - cm_over->getRangeMax(); ++i)
00521       {
00522         const size_t addr = ((k * map->info.height + j) * map->info.width) + i;
00523         ROS_ASSERT(addr < updated_max->data.size());
00524         const int cost = updated_max->data[addr];
00525         const int cost_ref = cm_ref.getMapOverlay()->getCost(i, j, k);
00526         const int cost_base = cm_base.getMapOverlay()->getCost(i, j, k);
00527         const int cost_max = std::max(cost_ref, cost_base);
00528 
00529         ASSERT_EQ(cost, cost_max);
00530 
00531         // std::cout << std::setfill(' ') << std::setw(3) << cost << " ";
00532       }
00533       /*
00534       std::cout << "  |  ";
00535       for (int i = cm_over->getRangeMax(); i < map->info.width - cm_over->getRangeMax(); ++i)
00536       {
00537         const int cost_ref = cm_ref.getMapOverlay()->getCost(i, j, k);
00538         std::cout << std::setfill(' ') << std::setw(3) << cost_ref << " ";
00539       }
00540       std::cout << "  |  ";
00541       for (int i = cm_over->getRangeMax(); i < map->info.width - cm_over->getRangeMax(); ++i)
00542       {
00543         const int cost_base = cm_base.getMapOverlay()->getCost(i, j, k);
00544         std::cout << std::setfill(' ') << std::setw(3) << cost_base << " ";
00545       }
00546       std::cout << std::endl;
00547       */
00548     }
00549     // std::cout << "----" << std::endl;
00550   }
00551 }
00552 
00553 TEST(Costmap3dLayerFootprint, CSpaceOverlayMove)
00554 {
00555   // Set example footprint
00556   int footprint_offset = 0;
00557   XmlRpc::XmlRpcValue footprint_xml;
00558   footprint_xml.fromXml(footprint_str, &footprint_offset);
00559   costmap_cspace::Polygon footprint(footprint_xml);
00560 
00561   // Settings: 4 angular grids, no expand/spread
00562   costmap_cspace::Costmap3d cms(4);
00563   auto cm = cms.addRootLayer<costmap_cspace::Costmap3dLayerFootprint>();
00564   cm->setExpansion(0.0, 0.0);
00565   cm->setFootprint(footprint);
00566   auto cm_over = cms.addLayer<costmap_cspace::Costmap3dLayerFootprint>(
00567       costmap_cspace::MapOverlayMode::MAX);
00568   cm_over->setExpansion(0.0, 0.0);
00569   cm_over->setFootprint(footprint);
00570 
00571   // Generate sample map
00572   nav_msgs::OccupancyGrid::Ptr map(new nav_msgs::OccupancyGrid);
00573   map->info.width = 5;
00574   map->info.height = 5;
00575   map->info.resolution = 1.0;
00576   map->info.origin.orientation.w = 1.0;
00577   map->data.resize(map->info.width * map->info.height);
00578 
00579   const int max_cost = 100;
00580   map->data[map->info.width / 2 + (map->info.height / 2) * map->info.width] = max_cost;
00581   cm->setBaseMap(map);
00582 
00583   // Generate local sample map
00584   nav_msgs::OccupancyGrid::Ptr map2(new nav_msgs::OccupancyGrid);
00585   *map2 = *map;
00586 
00587   for (int xp = -1; xp <= 1; ++xp)
00588   {
00589     for (int yp = -1; yp <= 1; ++yp)
00590     {
00591       map2->info.origin.position.x = map2->info.resolution * xp;
00592       map2->info.origin.position.y = map2->info.resolution * yp;
00593       /*
00594       std::cout << "=== origin: ("
00595                 << map2->info.origin.position.x << ", " << map2->info.origin.position.y
00596                 << ")" << std::endl;
00597       */
00598       cm_over->processMapOverlay(map2);
00599       for (int k = 0; k < cm_over->getAngularGrid(); ++k)
00600       {
00601         const size_t i_center = map->info.width / 2;
00602         const size_t j_center = map->info.height / 2;
00603         const size_t i_center2 = map->info.width / 2 + temp_dir[k][0];
00604         const size_t j_center2 = map->info.height / 2 + temp_dir[k][1];
00605 
00606         for (size_t j = 0; j < map->info.height; ++j)
00607         {
00608           for (size_t i = 0; i < map->info.width; ++i)
00609           {
00610             const int cost = cm_over->getMapOverlay()->getCost(i, j, k);
00611 
00612             if ((i == i_center && j == j_center) ||
00613                 (i == i_center2 && j == j_center2) ||
00614                 (i == i_center + xp && j == j_center + yp) ||
00615                 (i == i_center2 + xp && j == j_center2 + yp))
00616             {
00617               ASSERT_EQ(cost, max_cost);
00618             }
00619             else
00620             {
00621               ASSERT_EQ(cost, 0);
00622             }
00623             // std::cout << std::setfill(' ') << std::setw(3) << cost << " ";
00624           }
00625           // std::cout << std::endl;
00626         }
00627         // std::cout << "----" << std::endl;
00628       }
00629     }
00630   }
00631 }
00632 
00633 int main(int argc, char** argv)
00634 {
00635   testing::InitGoogleTest(&argc, argv);
00636 
00637   return RUN_ALL_TESTS();
00638 }


costmap_cspace
Author(s): Atsushi Watanabe
autogenerated on Sat Jun 22 2019 20:07:13