occupancy_grid_utils: occupancy_grid_utils: test_grid

00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
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 Willow Garage, Inc. 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 <occupancy_grid_utils/ray_tracer.h>
00031 #include <occupancy_grid_utils/exceptions.h>
00032 #include <occupancy_grid_utils/shortest_path.h>
00033 #include <occupancy_grid_utils/combine_grids.h>
00034 #include <occupancy_grid_utils/geometry.h>
00035 #include <gtest/gtest.h>
00036 #include <tf/transform_datatypes.h>
00037 #include <ros/ros.h>
00038 #include <iostream>
00039 #include <boost/foreach.hpp>
00040 #include <boost/bind.hpp>
00041 #include <boost/assign.hpp>
00042 #include <cmath>
00043 
00044 namespace gm=geometry_msgs;
00045 namespace nm=nav_msgs;
00046 namespace gu=occupancy_grid_utils;
00047 namespace sm=sensor_msgs;
00048 
00049 using std::ostream;
00050 using gu::Cell;
00051 using std::abs;
00052 using boost::bind;
00053 using std::vector;
00054 using std::set;
00055 using boost::assign::operator+=;
00056 using std::operator<<;
00057 
00058 const double PI = 3.14159265;
00059 
00060 const double TOL=1e-6;
00061 
00062 
00063 typedef vector<Cell> Path;
00064 typedef set<Cell> Cells;
00065 typedef boost::shared_ptr<nm::OccupancyGrid> GridPtr;
00066 typedef boost::shared_ptr<nm::OccupancyGrid const> GridConstPtr;
00067 
00068 bool samePath (const Path& p1, const Path& p2)
00069 {
00070   if (p1.size()!=p2.size())
00071     return false;
00072   for (unsigned i=0; i<p1.size(); i++)
00073     if (!(p1[i]==p2[i]))
00074       return false;
00075   return true;
00076 }
00077 
00078 void setOccupied (nm::OccupancyGrid* g, const unsigned x, const unsigned y)
00079 {
00080   g->data[gu::cellIndex(g->info, Cell(x, y))] = gu::OCCUPIED;;
00081 }
00082 
00083 void setOccupied (GridPtr g, const unsigned x, const unsigned y)
00084 {
00085   setOccupied(g.get(), x, y);
00086 }
00087 
00088 template <class T>
00089 bool equalSets (const set<T>& s1, const set<T>& s2)
00090 {
00091   BOOST_FOREACH (const T& x, s1) 
00092   {
00093     if (s2.find(x)==s2.end())
00094       return false;
00095   }
00096   BOOST_FOREACH (const T& x, s2)
00097   {
00098     if (s1.find(x)==s1.end())
00099       return false;
00100   }
00101   return true;
00102 }
00103 
00104 bool approxEqual (const double x, const double y)
00105 {
00106   return abs(x-y)<TOL;
00107 }
00108 
00109 
00110 namespace geometry_msgs
00111 {
00112 
00113 bool operator== (const Polygon& p1, const Polygon& p2)
00114 {
00115   if (p1.points.size() != p2.points.size())
00116     return false;
00117   for (unsigned i=0; i<p1.points.size(); i++) 
00118     if (!approxEqual(p1.points[i].x, p2.points[i].x) ||
00119         !approxEqual(p1.points[i].y, p2.points[i].y) || 
00120         !approxEqual(p1.points[i].z, p2.points[i].z))
00121       return false;
00122   return true;  
00123 }
00124 
00125 } // namespace geometry_msgs
00126 
00127 
00128 template <class T>
00129 ostream& operator<< (ostream& str, const vector<T>& s)
00130 {
00131   str << "(";
00132   BOOST_FOREACH (const T& x, s) 
00133     str << x << " ";
00134   str << ")";
00135   return str;
00136 }
00137 
00138 template <class T>
00139 ostream& operator<< (ostream& str, const set<T>& s)
00140 {
00141   str << "{";
00142   std::ostream_iterator<T> iter(str, ", ");
00143   copy(s.begin(), s.end(), iter);
00144   str << "}";
00145   return str;
00146 }
00147 
00148 
00149 ostream& operator<< (ostream& str, const gm::Point& p)
00150 {
00151   str << "(" << p.x << ", " << p.y << ")";
00152   return str;
00153 }
00154 
00155 gm::Point makePoint (const double x, const double y)
00156 {
00157   gm::Point p;
00158   p.x = x;
00159   p.y = y;
00160   return p;
00161 }
00162 
00163 gm::Point32 makePoint32 (const double x, const double y)
00164 {
00165   gm::Point32 p;
00166   p.x = x;
00167   p.y = y;
00168   return p;
00169 }
00170 
00171 double norm (const double x, const double y)
00172 {
00173   return sqrt(x*x + y*y);
00174 }
00175 
00176 struct CloseTo
00177 {
00178   CloseTo(double res) : res(res) {}
00179   bool operator() (const gm::Point& p, const gm::Point& p2) 
00180   {
00181     const double dx=p2.x-p.x;
00182     const double dy=p2.y-p.y;
00183     return norm(dx, dy) < res*sqrt(2);
00184   }
00185   const double res;
00186 };
00187 
00188 double angle (const double x1, const double y1, const double x2, const double y2)
00189 {
00190   const double ip = x1*x2 + y1*y2;
00191   return acos(ip/(norm(x1, y1)*norm(x2, y2)));
00192 }
00193 
00194 gm::Pose makePose (const double x, const double y, const double theta)
00195 {
00196   gm::Pose p;
00197   p.position.x = x;
00198   p.position.y = y;
00199   p.orientation = tf::createQuaternionMsgFromYaw(theta);
00200   return p;
00201 }
00202 
00203 float dist (const gu::DistanceField& d, int x, int y)
00204 {
00205   return d[gu::Cell(x,y)];
00206 }
00207 
00208 
00209 TEST(GridUtils, CoordinateConversions)
00210 {
00211   // Set up info for a map at (2, -1) that is rotated 45 degrees, with resolution 0.1
00212   nm::MapMetaData info;
00213   info.resolution = 0.1;
00214   info.origin = makePose(2, -1, PI/4);
00215   info.height = 50;
00216   info.width = 80;
00217 
00218   // Check conversions
00219   EXPECT_EQ (804u, gu::cellIndex(info, Cell(4, 10)));
00220   EXPECT_EQ (Cell(8, 1), gu::pointCell(info, makePoint(2.5, -0.35)));
00221   EXPECT_EQ (88u, gu::pointIndex(info, makePoint(2.5, -0.35)));
00222   EXPECT_EQ (Cell(-8, 7), gu::pointCell(info, makePoint(1, -1)));
00223   EXPECT_THROW (gu::pointIndex(info, makePoint(1, -1)), gu::CellOutOfBoundsException);
00224   EXPECT_THROW (gu::cellIndex(info, Cell(100, 100)), gu::CellOutOfBoundsException);
00225 
00226   // Cell polygon 
00227   const double side=sqrt(2)/2;
00228   gm::Polygon expected;
00229   expected.points.resize(4);
00230   expected.points[0].x = 2 + .1*side;
00231   expected.points[0].y = -1 + .3*side;
00232   expected.points[1].x = 2;
00233   expected.points[1].y = -1 + .4*side;
00234   expected.points[2].x = 2 + .1*side;
00235   expected.points[2].y = -1 + .5*side;
00236   expected.points[3].x = 2 + .2*side;
00237   expected.points[3].y = -1 + .4*side;
00238   EXPECT_EQ(expected, cellPolygon(info, Cell(2, 1)));
00239   
00240   
00241 }
00242 
00243 
00244   
00245 TEST(GridUtils, DistanceField)
00246 {
00247   nm::OccupancyGrid g;
00248   g.info.height=5;
00249   g.info.width=6;
00250   g.data.resize(30);
00251   g.info.resolution=0.5;
00252   setOccupied(&g, 3, 1);
00253   setOccupied(&g, 0, 4);
00254   setOccupied(&g, 4, 3);
00255   
00256   gu::DistanceField d = gu::distanceField(g);
00257   EXPECT_EQ(2, dist(d, 0, 0));
00258   EXPECT_EQ(1.5, dist(d, 1, 0));
00259   EXPECT_EQ(1, dist(d, 2, 0));
00260   EXPECT_EQ(.5, dist(d, 3, 0));
00261   EXPECT_EQ(1, dist(d, 4, 0));
00262   EXPECT_EQ(0, dist(d, 3, 1));
00263   EXPECT_EQ(0, dist(d, 0, 4));
00264   EXPECT_EQ(1.5, dist(d, 1, 2));
00265   EXPECT_EQ(1, dist(d, 1, 3));
00266   EXPECT_EQ(1, dist(d, 2, 2));
00267   EXPECT_EQ(1, dist(d, 2, 3));
00268   EXPECT_EQ(.5, dist(d, 4, 4));
00269 
00270 }
00271 gm::Point lastPoint (const nm::MapMetaData& info, const gu::RayTraceIterRange& r)
00272 {
00273   boost::optional<gm::Point> last_point;
00274   BOOST_FOREACH (const Cell& c, r) {
00275     last_point = gu::cellCenter(info, c);
00276   }
00277   ROS_ASSERT(last_point);
00278   return *last_point;
00279 }
00280 
00281 TEST(GridUtils, RayTrace)
00282 {
00283   const double res=0.2;
00284   nm::MapMetaData info;
00285   info.resolution = res;
00286   info.origin = makePose(3, 1, PI/2);
00287   info.height = 100;
00288   info.width = 50;
00289   
00290   gu::RayTraceIterRange r = gu::rayTrace(info, makePoint(1, 2), makePoint(2, 5));
00291   gm::Point last_point = lastPoint(info, r);
00292 
00293   EXPECT_PRED2 (CloseTo(res), makePoint(2, 5), last_point);
00294   EXPECT_PRED2 (CloseTo(res), makePoint(1, 2), gu::cellCenter(info, *(r.first)));
00295   EXPECT_EQ (16, std::distance(r.first, r.second));
00296 
00297   // Try again with x and y flipped
00298   nm::MapMetaData info2=info;
00299   info2.origin = makePose(1, 3, PI/-2);
00300   info.height = 50;
00301   info.width = 100;
00302 
00303   gu::RayTraceIterRange r2 = gu::rayTrace(info2, makePoint(2, 1), makePoint(5, 2));
00304   gm::Point lp2 = lastPoint(info2, r2);
00305   
00306   EXPECT_PRED2 (CloseTo(res), makePoint(5, 2), lp2);
00307   EXPECT_PRED2 (CloseTo(res), makePoint(2, 1), gu::cellCenter(info2, *(r2.first)));
00308   EXPECT_EQ (16, std::distance(r2.first, r2.second));
00309 
00310 
00311   // Out of bounds points
00312   EXPECT_THROW(gu::rayTrace(info, makePoint(1, 2), makePoint(4, 2)), gu::PointOutOfBoundsException);
00313   gu::RayTraceIterRange r3 = gu::rayTrace(info, makePoint(1, 2), makePoint(4, 2), true);
00314   gm::Point lp3 = lastPoint(info, r3);
00315   EXPECT_PRED2 (CloseTo(res), makePoint(3, 2), lp3);
00316   EXPECT_PRED2 (CloseTo(res), makePoint(1, 2), gu::cellCenter(info, *(r.first)));
00317   EXPECT_EQ (10, std::distance(r3.first, r3.second));
00318   EXPECT_THROW(gu::rayTrace(info, makePoint(4, 2), makePoint(1, 2), true), gu::PointOutOfBoundsException);
00319 }
00320 
00321 
00322 TEST(GridUtils, GridOverlay)
00323 {
00324   // Grid at origin with resolution 0.1
00325   const double res=10.0;
00326   nm::MapMetaData info;
00327   info.resolution = res;
00328   info.origin = makePose(0, 0, 0);
00329   info.height = 4;
00330   info.width = 4;
00331   
00332   // Create clouds
00333   typedef vector<gm::Point32> PointVec;
00334   typedef boost::shared_ptr<gu::LocalizedCloud const> CloudConstPtr;
00335   typedef boost::shared_ptr<gu::LocalizedCloud> CloudPtr;
00336 
00337   CloudPtr c1(new gu::LocalizedCloud());
00338   c1->sensor_pose = makePose(.001, .001, 0.0);
00339   c1->header.frame_id = "foo";
00340   c1->cloud.points = PointVec(1);
00341   CloudPtr c2(new gu::LocalizedCloud());
00342   c2->sensor_pose = makePose(25.0, 5.0, PI/2);
00343   c2->header.frame_id = "foo";
00344   c2->cloud.points = PointVec(4);
00345   c1->cloud.points[0] = makePoint32(20, 20);
00346   c2->cloud.points[0] = makePoint32(20, 0);
00347   c2->cloud.points[1] = makePoint32(45, 35);
00348   c2->cloud.points[2] = makePoint32(0, 1000);
00349   c2->cloud.points[3] = makePoint32(-1, -1);
00350 
00351   // Overlay two clouds
00352   nm::OccupancyGrid fake_grid;
00353   fake_grid.info = info;
00354   gu::OverlayClouds o1 = gu::createCloudOverlay(fake_grid, "foo", .5, 200);
00355   gu::addCloud(&o1, c1);
00356   gu::addCloud(&o1, c2);
00357   GridConstPtr grid = gu::getGrid(o1);
00358   EXPECT_EQ (gu::UNKNOWN, grid->data[0]);
00359   EXPECT_EQ (gu::UNKNOWN, grid->data[1]);
00360   EXPECT_EQ (gu::UNOCCUPIED, grid->data[2]);
00361   EXPECT_EQ (gu::UNKNOWN, grid->data[3]);
00362   EXPECT_EQ (gu::UNKNOWN, grid->data[4]);
00363   EXPECT_EQ (gu::UNOCCUPIED, grid->data[5]);
00364   EXPECT_EQ (gu::UNKNOWN, grid->data[6]);
00365   EXPECT_EQ (gu::UNKNOWN, grid->data[9]);
00366   EXPECT_EQ (gu::OCCUPIED, grid->data[10]);
00367 
00368   // Vary parameters
00369   gu::OverlayClouds o2 = gu::createCloudOverlay(fake_grid, "foo", .1, 2000);
00370   gu::addCloud(&o2, c2);
00371   gu::addCloud(&o2, c1);
00372   GridConstPtr grid2 = gu::getGrid(o2);
00373   EXPECT_EQ (gu::UNOCCUPIED, grid2->data[0]);
00374   EXPECT_EQ (gu::UNKNOWN, grid2->data[1]);
00375   EXPECT_EQ (gu::OCCUPIED, grid2->data[2]);
00376   EXPECT_EQ (gu::UNKNOWN, grid2->data[3]);
00377   EXPECT_EQ (gu::UNKNOWN, grid2->data[4]);
00378   EXPECT_EQ (gu::UNOCCUPIED, grid2->data[5]);
00379   EXPECT_EQ (gu::UNKNOWN, grid2->data[6]);
00380   EXPECT_EQ (gu::UNKNOWN, grid2->data[9]);
00381   EXPECT_EQ (gu::OCCUPIED, grid2->data[10]);
00382   EXPECT_EQ (gu::UNKNOWN, grid2->data[15]);
00383 
00384   // Add another cloud
00385   gu::addCloud(&o1, c1);
00386   GridConstPtr grid3 = gu::getGrid(o1);
00387   EXPECT_EQ (gu::UNOCCUPIED, grid3->data[0]);
00388   EXPECT_EQ (gu::UNKNOWN, grid3->data[1]);
00389   EXPECT_EQ (gu::UNOCCUPIED, grid3->data[2]);
00390 
00391   // Subtract the cloud we just added and check that it's the same as grid 1
00392   gu::removeCloud(&o1, c1);
00393   GridConstPtr grid4 = gu::getGrid(o1);
00394   for (unsigned i=0; i<16; i++)
00395     EXPECT_EQ (grid->data[i], grid4->data[i]);
00396 
00397   // Check header
00398   EXPECT_EQ(grid4->header.frame_id, "foo");
00399 }
00400 
00401 
00402 TEST(GridUtils, SimulateScan)
00403 {
00404   nm::MapMetaData info;
00405   info.resolution = 0.2;
00406   info.origin = makePose(1, 1, 0);
00407   info.height = 20;
00408   info.width = 20;
00409 
00410   GridPtr grid(new nm::OccupancyGrid());
00411   grid->info = info;
00412   grid->data.resize(400);
00413   setOccupied(grid, 1, 6);
00414   setOccupied(grid, 3, 4);
00415   setOccupied(grid, 3, 7);
00416   setOccupied(grid, 4, 4);
00417   setOccupied(grid, 4, 6);
00418   setOccupied(grid, 5, 4);
00419   setOccupied(grid, 7, 8);
00420 
00421   sm::LaserScan scan_info;
00422   scan_info.angle_min = -PI/2;
00423   scan_info.angle_max = PI/2;
00424   scan_info.angle_increment = PI/4;
00425   scan_info.range_max = 1.0;
00426 
00427   const gm::Pose sensor_pose = makePose(1.7, 1.9, PI/2);
00428 
00429   sm::LaserScan::ConstPtr scan =
00430     gu::simulateRangeScan(*grid, sensor_pose, scan_info);
00431 
00432   ASSERT_EQ(scan->ranges.size(), 5);
00433   EXPECT_FLOAT_EQ(scan->ranges[0], 0.2);
00434   EXPECT_TRUE(scan->ranges[1] > 1.0);
00435   EXPECT_FLOAT_EQ(scan->ranges[2], 0.6);
00436   EXPECT_FLOAT_EQ(scan->ranges[3], 0.4*sqrt(2));
00437   EXPECT_TRUE(scan->ranges[4] > 1.0);
00438 }
00439 
00440 
00441 TEST(GridUtils, ShortestPath)
00442 {
00443   nm::MapMetaData info;
00444   info.resolution = 0.1;
00445   info.origin = makePose(0, 0, 0);
00446   info.height = 5;
00447   info.width = 5;
00448 
00449   GridPtr grid(new nm::OccupancyGrid());
00450   grid->info = info;
00451   grid->data.resize(25);
00452   setOccupied(grid, 1, 3);
00453   setOccupied(grid, 2, 2);
00454   setOccupied(grid, 2, 1);
00455   setOccupied(grid, 3, 2);
00456   setOccupied(grid, 3, 3);
00457   setOccupied(grid, 3, 4);
00458   setOccupied(grid, 4, 2);
00459 
00460   Cell c1(0, 1);
00461   Cell c2(1, 4);
00462   Cell c3(2, 3);
00463   Cell c4(3, 1);
00464   Cell c5(4, 2);
00465   Cell c6(4, 3);
00466 
00467   const gu::ResultPtr res=singleSourceShortestPaths(*grid, c1);
00468   
00469   EXPECT_PRED2(approxEqual, 0.1*(2+sqrt(2)), *distanceTo(res, c2));
00470   EXPECT_PRED2(approxEqual, 0.1*(2*sqrt(2)), *distanceTo(res, c3));
00471   EXPECT_PRED2(approxEqual, 0.1*(1+2*sqrt(2)), *distanceTo(res, c4));
00472   EXPECT_TRUE(!distanceTo(res, c5));
00473   EXPECT_TRUE(!distanceTo(res, c6));
00474 
00475   Path p12, p13, p14;
00476   p12 += Cell(0, 1), Cell(0, 2), Cell(0, 3), Cell(1, 4);
00477   p13 += Cell(0, 1), Cell(1, 2), Cell(2, 3);
00478   p14 += Cell(0, 1), Cell(1, 1), Cell(2, 0), Cell(3, 1);
00479   
00480   EXPECT_PRED2(samePath, p12, *extractPath(res, c2));
00481   EXPECT_PRED2(samePath, p13, *extractPath(res, c3));
00482   EXPECT_PRED2(samePath, p14, *extractPath(res, c4));
00483   EXPECT_TRUE(!extractPath(res, c5));
00484   EXPECT_TRUE(!extractPath(res, c6));
00485 
00486   // Conversion to and from ros messages
00487   const gu::NavigationFunction msg = gu::shortestPathResultToMessage(res);
00488   const gu::ResultPtr res2 = gu::shortestPathResultFromMessage(msg);
00489   
00490   EXPECT_PRED2(approxEqual, 0.1*(2+sqrt(2)), *distanceTo(res2, c2));
00491   EXPECT_PRED2(approxEqual, 0.1*(2*sqrt(2)), *distanceTo(res2, c3));
00492   EXPECT_PRED2(approxEqual, 0.1*(1+2*sqrt(2)), *distanceTo(res2, c4));
00493   EXPECT_TRUE(!distanceTo(res2, c5));
00494   EXPECT_TRUE(!distanceTo(res2, c6));
00495 
00496   EXPECT_PRED2(samePath, p12, *extractPath(res2, c2));
00497   EXPECT_PRED2(samePath, p13, *extractPath(res2, c3));
00498   EXPECT_PRED2(samePath, p14, *extractPath(res2, c4));
00499   EXPECT_TRUE(!extractPath(res2, c5));
00500   EXPECT_TRUE(!extractPath(res2, c6));
00501 
00502 
00503   // Termination conditions
00504   const gu::TerminationCondition t1(0.4, false);
00505   const gu::ResultPtr res3 = gu::singleSourceShortestPaths(*grid, c4, t1);
00506   EXPECT_PRED2(approxEqual, 0.1*(1+2*sqrt(2)), *distanceTo(res3, c1));
00507   EXPECT_TRUE(!distanceTo(res3, c2));
00508   EXPECT_TRUE(!distanceTo(res3, c3));
00509 
00510   gu::Cells goals;
00511   goals += c3, c6;
00512   const gu::TerminationCondition t2(goals);
00513   const gu::ResultPtr res4 = gu::singleSourceShortestPaths(*grid, c4, t2);
00514   EXPECT_PRED2(approxEqual, 0.1*(1+3*sqrt(2)), *distanceTo(res4, c3));
00515   EXPECT_PRED2(approxEqual, 0.1*(1+2*sqrt(2)), *distanceTo(res4, c1));
00516   EXPECT_TRUE(!distanceTo(res4, c6));
00517 
00518   const gu::TerminationCondition t3(goals, 0.4, false);
00519   const gu::ResultPtr res5 = gu::singleSourceShortestPaths(*grid, c4, t3);
00520   EXPECT_PRED2(approxEqual, 0.1*(1+2*sqrt(2)), *distanceTo(res5, c1));
00521   EXPECT_TRUE(!distanceTo(res5, c2));
00522   EXPECT_TRUE(!distanceTo(res5, c3));
00523 
00524 
00525   // A*
00526   boost::optional<gu::AStarResult> res6 = shortestPathAStar(*grid, c1, c4);
00527   EXPECT_FLOAT_EQ (res6->second, 0.5);
00528   EXPECT_EQ(6u, res6->first.size());
00529 
00530   boost::optional<gu::AStarResult> res7 = shortestPathAStar(*grid, c1, c1);
00531   EXPECT_FLOAT_EQ (res7->second, 0);
00532   EXPECT_EQ(1u, res7->first.size());
00533 
00534   boost::optional<gu::AStarResult> res8 = shortestPathAStar(*grid, c1, c5);
00535   EXPECT_TRUE (!res8);
00536 
00537   boost::optional<gu::AStarResult> res9 = shortestPathAStar(*grid, c1, c6);
00538   EXPECT_TRUE (!res9);
00539   
00540 }
00541 
00542 int val (const nm::OccupancyGrid& g, const gu::coord_t x, const gu::coord_t y)
00543 {
00544   const gu::Cell c(x, y);
00545   ROS_ASSERT (gu::withinBounds(g.info, c));
00546   return g.data[cellIndex(g.info, c)];
00547 }
00548 
00549 void setVal (nm::OccupancyGrid* g, const gu::coord_t x, const gu::coord_t y, const int v)
00550 {
00551   const gu::Cell c(x, y);
00552   ROS_ASSERT (gu::withinBounds(g->info, c));
00553   g->data[cellIndex(g->info, c)]=v;
00554 }
00555 
00556 
00557 TEST(GridUtils, InflateObstacles)
00558 {
00559   GridPtr g(new nm::OccupancyGrid());
00560   g->info.origin = makePose(0, 0, 0);
00561   g->info.resolution = 0.5;
00562   g->info.width = 10;
00563   g->info.height = 11;
00564   g->data.resize(g->info.width*g->info.height);
00565   fill(g->data.begin(), g->data.end(), -1);
00566   
00567   setVal(g.get(), 2, 2, 0);
00568   for (unsigned x=7; x<10; x++) {
00569     for (unsigned y=7; y<11; y++) {
00570       setVal(g.get(), x, y, 0);
00571     }
00572   }
00573     
00574   setVal(g.get(), 3, 6, 50);
00575   setVal(g.get(), 3, 7, 100);
00576   setVal(g.get(), 3, 4, 0);
00577 
00578   GridPtr g2 = gu::inflateObstacles(*g, 0.7, false);
00579 
00580   EXPECT_EQ (100, val(*g2, 3, 6));
00581   EXPECT_EQ (50, val(*g2, 3, 4));
00582   EXPECT_EQ (100, val(*g2, 3, 5));
00583   EXPECT_EQ (-1, val(*g2, 1, 3));
00584   EXPECT_EQ (-1, val(*g2, 7, 7));
00585   EXPECT_EQ (0, val(*g2, 9, 9));
00586   
00587 
00588 }
00589 
00590 bool pred (const Cell& c)
00591 {
00592   if (c.x==2 || c.y==2)
00593     return false;
00594   if (((c.x==1) || (c.x==3)) &&
00595       ((c.y==1) || (c.y==3)))
00596     return false;
00597   return true;
00598 }
00599 
00600 bool not_pred (const Cell& c)
00601 {
00602   return !pred(c);
00603 }
00604 
00605 TEST(GridUtils, Geometry)
00606 {
00607   nm::MapMetaData info;
00608   info.origin = makePose(.1,.1,0);
00609   info.resolution = .10;
00610   info.height = 5;
00611   info.width = 5;
00612 
00613   gm::Polygon poly;
00614   poly.points.push_back(makePoint32(.41, .61));
00615   poly.points.push_back(makePoint32(.43, .45));
00616   poly.points.push_back(makePoint32(.51, .41));
00617   poly.points.push_back(makePoint32(.8, .46));
00618   poly.points.push_back(makePoint32(.78, .63));
00619   
00620   const Cells cells = gu::cellsInConvexPolygon(info, poly);
00621   Cells expected;
00622   expected.insert(Cell(4,4));
00623   expected.insert(Cell(4,3));
00624   expected.insert(Cell(3,4));
00625   expected.insert(Cell(3,3));
00626   EXPECT_EQ(expected, cells);
00627   
00628 
00629   Cells cells2 = gu::tileCells(info, 0.2, pred);
00630   Cells expected2;
00631   expected2.insert(Cell(0,0));
00632   expected2.insert(Cell(0,3));
00633   expected2.insert(Cell(3,0));
00634   expected2.insert(Cell(3,4));
00635   EXPECT_EQ(expected2, cells2);
00636   
00637   Cells cells3 = gu::tileCells(info, 0.2, not_pred);
00638   Cells expected3;
00639   expected3.insert(Cell(0,2));
00640   expected3.insert(Cell(2,0));
00641   expected3.insert(Cell(2,3));
00642   expected3.insert(Cell(4,2));
00643   EXPECT_EQ(expected3, cells3);
00644 }
00645 
00646 
00647 TEST(GridUtils, CombineGrids)
00648 {
00649   GridPtr g1(new nm::OccupancyGrid());
00650   GridPtr g2(new nm::OccupancyGrid());
00651   
00652   g1->info.origin = makePose(2, 1, 0);
00653   g2->info.origin = makePose(3, 0, PI/4);
00654   g1->info.resolution = 0.5;
00655   g2->info.resolution = 0.5;
00656   g1->info.height = 4;
00657   g1->info.width = 6;
00658   g2->info.height = 4;
00659   g2->info.width = 4;
00660   g1->data.resize(24);
00661   g2->data.resize(16);
00662 
00663   fill(g1->data.begin(), g1->data.end(), -1);
00664   fill(g2->data.begin(), g2->data.end(), -1);
00665 
00666   setVal(g2.get(), 1, 0, 0);
00667   setVal(g2.get(), 0, 3, 50);
00668   setVal(g2.get(), 2, 0, 42);
00669   setVal(g2.get(), 0, 2, 11);
00670   setVal(g2.get(), 3, 2, 0);
00671   setVal(g2.get(), 3, 0, 110);
00672   
00673   setVal(g1.get(), 5, 3, 100);
00674   setVal(g1.get(), 3, 0, 24);
00675   setVal(g1.get(), 0, 0, 66);
00676   setVal(g1.get(), 4, 0, 90);
00677 
00678 
00679   
00680   vector<GridConstPtr> grids;
00681   grids += g1, g2;
00682   GridPtr combined = gu::combineGrids(grids, g1->info.resolution/2.0);
00683   
00684   EXPECT_PRED2(approxEqual, 3-sqrt(2), combined->info.origin.position.x);
00685   EXPECT_PRED2(approxEqual, 0, combined->info.origin.position.y);
00686   EXPECT_EQ(0.25, combined->info.resolution);
00687   EXPECT_EQ(round((2+sqrt(2))/0.25), combined->info.width);
00688   EXPECT_EQ(12u, combined->info.height);
00689 
00690   // Note there are rounding issues that mean that some of the values below
00691   // could theoretically go either way
00692   EXPECT_EQ(-1, val(*combined, 11, 2));
00693   EXPECT_EQ(-1, val(*combined, 2, 0));
00694   EXPECT_EQ(-1, val(*combined, 5, 0));
00695   EXPECT_EQ(0, val(*combined, 7, 2));
00696   EXPECT_EQ(66, val(*combined, 1, 6));
00697   EXPECT_EQ(100, val(*combined, 11, 11));
00698   EXPECT_EQ(-1, val(*combined, 2, 11));
00699   EXPECT_EQ(11, val(*combined, 3, 2));
00700   EXPECT_EQ(42, val(*combined, 7, 4));
00701   EXPECT_EQ(66, val(*combined, 2, 4));
00702   EXPECT_EQ(0, val(*combined, 6, 8));
00703   EXPECT_EQ(90, val(*combined, 10, 5));
00704   
00705   
00706 }
00707 
00708 
00709 
00710 
00711 
00712 int main (int argc, char** argv)
00713 {
00714   ros::Time::init();
00715   testing::InitGoogleTest(&argc, argv);
00716   return RUN_ALL_TESTS();
00717 }
test_grid_utils.cpp
