PolyOps.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2007, 2010 David Li, Patrick Beeson, Bartley Gillen,
00003  *                           Jack O'Quin
00004  *
00005  *  License: Modified BSD Software License Agreement
00006  * 
00007  *  $Id: PolyOps.cc 1665 2011-08-17 15:11:43Z ruifei0713 $
00008  */
00009 
00019 #include <float.h>
00020 #include <assert.h>
00021 #include <limits>
00022 #include <iostream>
00023 
00024 #include <art/epsilon.h>
00025 
00026 #include <art_map/coordinates.h>
00027 #include <art_map/euclidean_distance.h>
00028 #include <art_map/PolyOps.h>
00029 
00030 // for turning on extremely verbose driver logging:
00031 //#define EXTREME_DEBUG 1
00032 
00033 PolyOps::PolyOps()
00034 {
00035   //constructor
00036 }
00037 
00038 PolyOps::~PolyOps()
00039 {
00040   //destructor
00041 }
00042 
00043 
00044 bool PolyOps::pointInPoly_ratio(float x, float y, const poly& p, float ratio)
00045 {
00046   
00047   // this is an unrolled version of the standard point-in-polygon algorithm
00048 
00049   float diff=(1-ratio)/2;
00050 
00051   MapXY p1=p.p1;
00052   MapXY p2=p.p2;
00053   MapXY p3=p.p3;
00054   MapXY p4=p.p4;
00055 
00056   poly newpoly;
00057         
00058   newpoly.p1.x=p1.x+(p4.x-p1.x)*diff;
00059   newpoly.p4.x=p1.x+(p4.x-p1.x)*(1-diff);
00060   newpoly.p1.y=p1.y+(p4.y-p1.y)*diff;
00061   newpoly.p4.y=p1.y+(p4.y-p1.y)*(1-diff);       
00062 
00063   newpoly.p2.x=p2.x+(p3.x-p2.x)*diff;
00064   newpoly.p3.x=p2.x+(p3.x-p2.x)*(1-diff);
00065   newpoly.p2.y=p2.y+(p3.y-p2.y)*diff;
00066   newpoly.p3.y=p2.y+(p3.y-p2.y)*(1-diff);       
00067         
00068   bool value=pointInPoly(x,y,newpoly);
00069 
00070 #if 0
00071   if (value) {
00072     ROS_DEBUG("Point in small poly: original poly: [%f,%f; %f,%f;%f,%f;%f,%f]"
00073               "   new poly: [%f,%f; %f,%f;%f,%f;%f,%f]   point: [%f,%f]",
00074               p.x1,p.y1,p.x2,p.y2,p.x3,p.y3,p.x4,p.y4,
00075               newpoly.x1,newpoly.y1,newpoly.x2,newpoly.y2,
00076               newpoly.x3,newpoly.y3,newpoly.x4,newpoly.y4,x,y);
00077   }
00078 #endif
00079 
00080   return value;
00081 
00082 }
00083 
00084 // return index of polygon containing location (x, y)
00085 int PolyOps::getContainingPoly(const std::vector<poly> &polys,
00086                                float x, float y)
00087 {
00088   //assert(polys.size() > 0);
00089   int pindex;
00090   pindex= getClosestPoly(polys, x, y);
00091   
00092   if (pindex < 0)
00093     {
00094       ROS_DEBUG("no polygon contains point (%.3f, %.3f)", x, y);
00095       return -1;                        // no match
00096     }
00097 
00098   poly closest=polys.at(pindex);
00099 
00100   // make sure the polygon (nearly) contains this way-point
00101   if (getShortestDistToPoly(x, y, closest) < Epsilon::distance)
00102     {
00103       return pindex;
00104     }
00105 
00106   ROS_DEBUG("no polygon contains point (%.3f, %.3f)", x, y);
00107   return -1;                    // no match
00108 }
00109 
00110 /*
00111   int PolyOps::getContainingPoly(const std::vector<poly>& polys, 
00112   float x, float y)
00113   {
00114 
00115   poly p;
00116 
00117   for (int i = 0; (unsigned)i < polys.size(); i++)
00118   {
00119   p = polys.at(i);
00120   if (pointInPoly(x, y, p))
00121   return i;
00122   }
00123         
00124   return -1;
00125   }
00126 */
00127 
00128 // if the point lies within the given polygon, the returned distance
00129 // is 0 otherwise, the shortest distance to any edge/vertex of the
00130 // given polygon is returned
00131 float PolyOps::getShortestDistToPoly(float x, float y, const poly& p)
00132 {
00133   float dist = 0;
00134   float d;
00135 
00136   // first check if point lies inside polygon - if so, return 0
00137   if (pointInPoly(x, y, p))
00138     return dist;
00139 
00140   // return minimum of distance to all vertices and of distance to
00141   // projections that fall on edges
00142         
00143   dist = shortestDistToLineSegment(x, y, p.p1.x, p.p1.y, p.p2.x, p.p2.y);
00144 
00145   if ( (d = shortestDistToLineSegment(x, y, p.p2.x, p.p2.y, p.p3.x, p.p3.y)) 
00146        < dist)
00147     dist = d;
00148         
00149   if ( (d = shortestDistToLineSegment(x, y, p.p3.x, p.p3.y, p.p4.x, p.p4.y)) 
00150        < dist)
00151     dist = d;
00152 
00153   if ( (d = shortestDistToLineSegment(x, y, p.p4.x, p.p4.y, p.p1.x, p.p1.y)) 
00154        < dist)
00155     dist = d;
00156         
00157   return dist;
00158 }
00159 
00160 // if the point lies within a polygon, that polygon is returned.
00161 // otherwise, the nearest polygon from the list is returned index of
00162 // winning poly within list is stored in index
00163 int PolyOps::getClosestPoly(const std::vector<poly>& polys, float x, 
00164                             float y)
00165 {
00166   poly p;
00167   float d;
00168   int index = -1;
00169 
00170   float min_dist = std::numeric_limits<float>::max();
00171 
00172   for (int i = 0; (unsigned)i < polys.size(); i++)
00173     {
00174       p = polys.at(i);
00175       d = getShortestDistToPoly(x, y, p);
00176 
00177       if (Epsilon::equal(d,0)) // point is inside polygon
00178         {
00179           index = i;
00180           return index;
00181         }
00182 
00183       if (i == 0) // initialize the min values
00184         {
00185           min_dist = d;
00186           index = 0;
00187         }
00188                 
00189       if (d < min_dist) // new minimum
00190         {
00191           min_dist = d;
00192           index = i;
00193         }
00194     }
00195 
00196   return index;
00197 }
00198 
00199 // if the point lies within a non-transtion polygon, that polygon is returned.
00200 // otherwise, the nearest non-transition polygon from the list is returned.
00201 // index of winning non-transition poly within list is stored in index.
00202 int PolyOps::getClosestNonTransPoly(const std::vector<poly>& polys, float x, 
00203                             float y)
00204 {
00205   poly p;
00206   float d;
00207   int index = -1;
00208 
00209   float min_dist = std::numeric_limits<float>::max();
00210 
00211   for (int i = 0; (unsigned)i < polys.size(); i++)
00212     {
00213       p = polys.at(i);
00214       if (p.is_transition)
00215         continue;
00216       d = getShortestDistToPoly(x, y, p);
00217 
00218       if (Epsilon::equal(d,0)) // point is inside polygon
00219         {
00220           index = i;
00221           return index;
00222         }
00223 
00224       if (i == 0) // initialize the min values
00225         {
00226           min_dist = d;
00227           index = 0;
00228         }
00229                 
00230       if (d < min_dist) // new minimum
00231         {
00232           min_dist = d;
00233           index = i;
00234         }
00235     }
00236 
00237   return index;
00238 }
00239 
00240 #if 0 //TODO
00241 // Returns index of closest polygon if within given epsilon, -1 otherwise
00242 int PolyOps::getClosestPolyEpsilon(const std::vector<poly>& polys,
00243                 const player_pose2d_t& pose, const float epsilon)
00244 {
00245     int index = getClosestPoly(polys, pose);
00246     if (index < 0)
00247       return -1;
00248     if (pointNearPoly(pose, polys.at(index), epsilon))
00249       return index;
00250     else
00251       return -1;
00252 }
00253 
00254 // Returns index of closest non-transition polygon if within given epsilon,
00255 // -1 otherwise.
00256 int PolyOps::getClosestNonTransPolyEpsilon(const std::vector<poly>& polys,
00257                 const player_pose2d_t& pose, const float epsilon) 
00258 {
00259     int index = getClosestNonTransPoly(polys, pose);
00260     if (index < 0)
00261       return -1;
00262     if (pointNearPoly(pose, polys.at(index), epsilon))
00263       return index;
00264     else
00265       return -1;
00266 }
00267 #endif
00268 
00269 MapXY PolyOps::getPolyEdgeMidpoint(const poly& p)
00270 {
00271   MapXY r;
00272   r.x = (p.p2.x + p.p3.x)/2;
00273   r.y = (p.p2.y + p.p3.y)/2;
00274   return r;
00275 }
00276 
00277 MapXY PolyOps::centerpoint(const poly& p)
00278 {
00279   return midpoint(midpoint(p.p1, p.p3) , midpoint(p.p2, p.p4));
00280 }
00281 
00282 float PolyOps::getLength(const poly& p)
00283 {
00284   MapXY back=midpoint(p.p1, p.p4);
00285   MapXY front=midpoint(p.p2, p.p3);
00286   float d1 = distance(back.x,back.y,p.midpoint.x,p.midpoint.y);
00287   float d2 = distance(front.x,front.y,p.midpoint.x,p.midpoint.y);
00288   return d1+d2;
00289 }
00290 
00291 std::vector<poly> 
00292 PolyOps::getPolysBetweenPoints(const std::vector<poly>& polys,
00293                                float x1, float y1, 
00294                                float x2, float y2)
00295 {
00296   std::vector<poly> retPolys;
00297   bool foundFirstPoint = false;
00298 
00299   /*    
00300         for (int i = 0; (unsigned)i < polys.size(); i++)
00301         {
00302         if(!foundFirstPoint)
00303         {
00304         if(pointInPoly(x1, y1, polys.at(i)))
00305         foundFirstPoint = true;
00306         }
00307         else
00308         {
00309         if(pointInPoly(x2, y2, polys.at(i)))
00310         break;
00311                                 
00312         retPolys.push_back(polys.at(i));
00313         }
00314         }
00315   */
00316 
00317   // find polygon corresponding to first point
00318   int i;
00319   for (i = 0; (unsigned)i < polys.size(); i++)
00320     {
00321       if(pointInPoly(x1, y1, polys.at(i)))
00322         {
00323           foundFirstPoint = true;
00324           break;
00325         }
00326     }
00327         
00328   if (!foundFirstPoint) // first point not in poly - find nearest poly
00329     {   
00330       poly p;
00331       i= getClosestPoly(polys, x1, y1);
00332       if (i<0)
00333         return retPolys;
00334       p=polys.at(i);
00335     }
00336 
00337   for ( ; (unsigned)i < polys.size(); i++)
00338     {
00339       if(pointInPoly(x2, y2, polys.at(i)))
00340         break;
00341                                 
00342       retPolys.push_back(polys.at(i));
00343     }
00344         
00345   return retPolys;
00346 }
00347 
00348 // copy from_polys polygons to to_polygons after nearest to point
00349 void PolyOps::getRemainingPolys(const std::vector<poly> &from_polys,
00350                                 std::vector<poly> &to_polys,
00351                                 const MapXY &point)
00352 {
00353   bool found_point = false;
00354 
00355   // find polygon corresponding to point
00356   unsigned i;
00357   for (i = 0; i < from_polys.size(); i++)
00358     {
00359       if(pointInPoly(point, from_polys.at(i)))
00360         {
00361           found_point = true;
00362           break;
00363         }
00364     }
00365         
00366   if (!found_point)
00367     {
00368       // point not in from_polys - find the nearest
00369       poly p;
00370       int closest = getClosestPoly(from_polys, point);
00371       if (closest < 0)
00372         {
00373           // none found, return empty vector
00374           to_polys.clear();
00375           return;
00376         }
00377       i = closest;
00378     }
00379 
00380   CollectPolys(from_polys, to_polys, i);
00381 }
00382 
00383 std::vector<MapXY> PolyOps::getPointsFromPolys(const std::vector<poly>& polys)
00384 {
00385   std::vector<MapXY> retPoints;
00386 
00387   for (int i = 0; (unsigned)i < polys.size(); i++)
00388     {
00389       retPoints.push_back(getPolyEdgeMidpoint(polys.at(i)));
00390     }
00391 
00392   return retPoints;
00393 }
00394 
00395 float PolyOps::distance(float x1, float y1, float x2, float y2)
00396 {
00397   return Euclidean::DistanceTo(x1,y1,x2,y2);
00398 }
00399 
00400 float PolyOps::shortestDistToLineSegment(float x, float y, 
00401                                          float line_x1, float line_y1, 
00402                                          float line_x2, float line_y2)
00403 {
00404   float dist1, dist2;
00405   dist1=dist2=0;
00406 
00407   Euclidean::DistanceFromLine(x,y,line_x1,line_y1,line_x2,line_y2,dist1,dist2);
00408   return dist1;
00409 
00410   /*
00411     float A = x - line_x1;
00412     float B = y - line_y1;
00413     float C = line_x2 - line_x1;
00414     float D = line_y2 - line_y1;
00415 
00416     float param = (A * C + B * D) / (C * C + D * D); // (dot product)/
00417     // (distance
00418     // squared)
00419 
00420     float x_on_line, y_on_line;
00421 
00422     if(param < 0)
00423     {
00424     x_on_line = line_x1;
00425     y_on_line = line_y1;
00426     }
00427     else if(param > 1)
00428     {
00429     x_on_line = line_x2;
00430     y_on_line = line_y2;
00431     }
00432     else
00433     {
00434     x_on_line = line_x1 + param * C;
00435     y_on_line = line_y1 + param * D;
00436     }
00437 
00438     return distance(x, y , x_on_line, y_on_line);
00439   */
00440 }
00441 
00442 int PolyOps::get_waypoint_index(const std::vector<poly> &polys,
00443                                 const ElementID& waypoint)
00444 {
00445   
00446   for (unsigned i=0; i<polys.size(); i++)
00447     if (polys.at(i).start_way==waypoint &&
00448         polys.at(i).end_way==waypoint)
00449       return i;
00450 
00451   return -1;                    // no match
00452 }
00453 
00454 int PolyOps::getPolyWayPt(const std::vector<poly> &polys,
00455                                 const ElementID& waypoint) {
00456 
00457   for (unsigned i=0; i<polys.size(); i++)
00458     if (polys.at(i).start_way==waypoint)
00459       return i;
00460 
00461   return -1;                    // no match
00462 }
00463 
00464 // add from_polys polygons to to_polys matching from_id and to_id
00465 //
00466 // Adds all polygons between from_id and to_id, and also the one
00467 // containing the to_id way-point, but not the one containing from_id.
00468 
00469 void PolyOps::add_polys_for_waypts(const std::vector <poly> &from_polys,
00470                                    std::vector <poly> &to_polys,
00471                                    ElementID from_id, ElementID to_id)
00472 {
00473   if (from_id != to_id)
00474     {
00475       for (unsigned i = 0; i < from_polys.size(); ++i)
00476         if (match_waypt_poly(from_polys.at(i), from_id, to_id))
00477           {
00478             to_polys.push_back(from_polys.at(i));
00479 #define EXTREME_DEBUG 1
00480 #ifdef EXTREME_DEBUG
00481             ROS_DEBUG("adding start, end waypoints %s, %s, poly_id = %d",
00482                       to_polys.back().start_way.name().str,
00483                       to_polys.back().end_way.name().str,
00484                       to_polys.back().poly_id);
00485 #endif
00486           }
00487     }
00488 
00489   for (unsigned i = 0; i < from_polys.size(); ++i)
00490     if (match_waypt_poly(from_polys.at(i), to_id))
00491       {
00492         to_polys.push_back(from_polys.at(i));
00493 #ifdef EXTREME_DEBUG
00494         ROS_DEBUG("adding start, end waypoints %s, %s, poly_id = %d",
00495                   to_polys.back().start_way.name().str,
00496                   to_polys.back().end_way.name().str,
00497                   to_polys.back().poly_id);
00498 #endif
00499         break;
00500       }
00501 }
00502 
00503 // add from_polys polygons matching segment and lane to to_polys
00504 void PolyOps::AddTransitionPolys(const std::vector <poly> &from_polys,
00505                                  std::vector <poly> &to_polys,
00506                                  WayPointNode way0, WayPointNode way1)
00507 {
00508   for (unsigned i = 0; i < from_polys.size(); ++i)
00509     {
00510       if (MatchTransitionPoly(from_polys.at(i), way0, way1))
00511         {
00512           to_polys.push_back(from_polys.at(i));
00513         }
00514     }
00515 }
00516   
00517 // add from_polys polygons matching segment and lane of waypt id to
00518 // to_polys
00519 void PolyOps::AddLanePolys(const std::vector <poly> &from_polys,
00520                            std::vector <poly> &to_polys, ElementID id)
00521 {
00522   for (unsigned i = 0; i < from_polys.size(); ++i)
00523     {
00524       if (LanePoly(from_polys.at(i), id))
00525         {
00526           to_polys.push_back(from_polys.at(i));
00527         }
00528     }
00529 }
00530 
00531 // add from_polys polygons matching segment and lane of waypt to
00532 // to_polys
00533 void PolyOps::AddLanePolys(const std::vector <poly> &from_polys,
00534                            std::vector <poly> &to_polys, WayPointNode waypt)
00535 {
00536   AddLanePolys(from_polys, to_polys, waypt.id);
00537 }
00538 
00539 // add from_polys polygons matching segment and lane to to_polys
00540 // in either direction (reverse if direction < 0)
00541 void PolyOps::AddLanePolysEither(const std::vector <poly> &from_polys,
00542                                  std::vector <poly> &to_polys, WayPointNode waypt,
00543                                  int direction)
00544 {
00545   if (direction >= 0)
00546     AddLanePolys(from_polys, to_polys, waypt);
00547   else
00548     AddReverseLanePolys(from_polys, to_polys, waypt);
00549 }
00550   
00551 // add from_polys polygons matching segment and lane to to_polys,
00552 // searching the list in the reverse direction
00553 void PolyOps::AddReverseLanePolys(const std::vector <poly> &from_polys,
00554                                   std::vector <poly> &to_polys, ElementID id)
00555 {
00556   // use int compares so the loop terminates
00557   int last = from_polys.size() - 1;
00558   ROS_DEBUG("scanning polygons from %d back to 0", last);
00559   for (int i = last; i >= 0; --i)
00560     {
00561       //fprintf(stderr, "poly ID = %d\n", from_polys.at(i).poly_id);
00562       if (LanePoly(from_polys.at(i), id))
00563         {
00564           to_polys.push_back(from_polys.at(i));
00565         }
00566     }
00567 }
00568 
00569 // add from_polys polygons matching segment and lane to to_polys,
00570 // searching the list in the reverse direction
00571 void PolyOps::AddReverseLanePolys(const std::vector <poly> &from_polys,
00572                                   std::vector <poly> &to_polys, WayPointNode waypt)
00573 {
00574   AddReverseLanePolys(from_polys, to_polys, waypt.id);
00575 }
00576   
00577 // Collect all polygons of from_poly from start to end from to_polys.
00578 void PolyOps::CollectPolys(const std::vector<poly> &from_polys,
00579                            std::vector<poly> &to_polys,
00580                            unsigned start, unsigned end)
00581 {
00582   to_polys.clear();
00583   for (unsigned i = start; i < end; ++i)
00584     to_polys.push_back(from_polys.at(i));
00585 }
00586 void PolyOps::CollectPolys(const std::vector<poly> &from_polys,
00587                            std::vector<poly> &to_polys,
00588                            unsigned start)
00589 {
00590   CollectPolys(from_polys, to_polys, start, from_polys.size());
00591 }
00592 
00593 // Convert ArtLanes message into polygon list
00594 void PolyOps::GetPolys(const art_msgs::ArtLanes &lanes, poly_list_t &polyList)
00595 {
00596   polyList.resize(lanes.polygons.size());
00597   for (unsigned i = 0; i < lanes.polygons.size(); i++) {
00598     polyList[i] = poly(lanes.polygons[i]);
00599   }
00600 }
00601 
00602 void PolyOps::GetLanes(poly_list_t &polyList, art_msgs::ArtLanes &lanes)
00603 {
00604   lanes.polygons.resize(polyList.size());
00605   for (unsigned i = 0; i < polyList.size(); i++) {
00606     polyList[i].toMsg(lanes.polygons[i]);
00607   }
00608 }
00609 
00610 // true if curPoly is in the specified segment and lane
00611 bool PolyOps::LanePoly(const poly &curPoly, WayPointNode waypt)
00612 {
00613   return LanePoly(curPoly, waypt.id);
00614 }
00615 
00616 // true if curPoly is in the specified segment and lane
00617 bool PolyOps::LanePoly(const poly &curPoly, ElementID id)
00618 {
00619   return (curPoly.start_way.seg == id.seg
00620           && curPoly.start_way.lane == id.lane
00621           && curPoly.end_way.seg == id.seg
00622           && curPoly.end_way.lane == id.lane
00623           && !curPoly.is_transition);
00624 }
00625 
00626 // return true if curPoly is an transition polygon leading from way0 to way1
00627 bool PolyOps::MatchTransitionPoly(const poly& curPoly, 
00628                                   const WayPointNode& way0, 
00629                                   const WayPointNode& way1)
00630 {
00631   return (curPoly.start_way.same_lane(way0.id) &&
00632           curPoly.end_way.same_lane(way1.id) &&
00633           curPoly.is_transition);
00634 }
00635 
00636 float PolyOps::PolyHeading(const poly& curPoly)
00637 {
00638   using Coordinates::bearing;
00639   float left_heading =  bearing(curPoly.p1,curPoly.p2);
00640   float right_heading = bearing(curPoly.p4, curPoly.p3);
00641   using Coordinates::normalize;
00642   return normalize(right_heading
00643                    + normalize(left_heading - right_heading) / 2.0f);
00644 }
00645 
00646 int PolyOps::getStartingPoly(const MapPose &pose,
00647                              const std::vector<poly>& polygons,
00648                              float distance,
00649                              float min_heading) 
00650 {
00651   using Coordinates::normalize;
00652 
00653   // See if already in a polygon
00654   int index = getContainingPoly(polygons, pose.map);
00655   if (index >=0
00656       && (fabs(normalize(polygons.at(index).heading - pose.yaw))
00657           < min_heading))
00658     {
00659       return index;
00660     }
00661   
00662   // Get closest polygon 
00663   index = getClosestPoly(polygons, pose);
00664   if (index < 0)
00665     {
00666       ROS_WARN_STREAM("none of " << polygons.size()
00667                       << " polygons near starting pose");
00668       return index;
00669     }
00670   else if (fabs(normalize(polygons.at(index).heading - pose.yaw))
00671            < min_heading)
00672     {
00673       ROS_DEBUG_STREAM("closest polygon["  << index
00674                        << "] has suitable heading");
00675       return index;
00676     }
00677   
00678   // Find closest segment
00679   segment_id_t segment = polygons.at(index).start_way.seg;
00680   ROS_INFO_STREAM("closest polygon["  << index
00681                   << "] in segment " << segment);
00682 
00683   // Find all lanes that share same segment with closest polygons
00684   std::vector<lane_id_t> lanes;
00685 
00686   for (uint i=0; i<polygons.size(); i++)
00687     if (polygons.at(i).start_way.seg ==
00688         segment) {
00689       bool lane_found=false;
00690       for (uint j=0;j<lanes.size();j++)
00691         if (lanes.at(j)==polygons.at(i).start_way.lane) {
00692           lane_found=true;
00693           break;
00694         }
00695       if (!lane_found)
00696         lanes.push_back(polygons.at(i).start_way.lane);
00697     }
00698   
00699 
00700   // Find closest polygons in every lane found above and mark one with
00701   // heading closest to vehcile's heading
00702   float mindist = FLT_MAX;
00703   poly minpoly = polygons.at(0);
00704   bool foundd = false;
00705   for (uint i=0; i<lanes.size(); i++)
00706     {
00707       ElementID point(segment,lanes.at(i),0);
00708       WayPointNode waypt;
00709       waypt.id=point;
00710 
00711       std::vector<poly> lane_polys;
00712       AddLanePolys(polygons, lane_polys, waypt);
00713       int newind = getClosestPoly(lane_polys, pose);
00714       if (newind < 0)
00715         continue;
00716       float tempdist =
00717         fabs(normalize(lane_polys.at(newind).heading - pose.yaw));
00718       if (tempdist < mindist)
00719         {
00720           mindist = tempdist;
00721           minpoly = lane_polys.at(newind);
00722           foundd=true;
00723         }
00724     }
00725 
00726   if (!foundd)
00727     {
00728       ROS_WARN_STREAM("no lane within distance " << distance);
00729       return -1;
00730     }
00731 
00732   // Return min polygon 
00733   for (uint i=0; i< polygons.size(); i++)
00734     if (minpoly.poly_id == polygons.at(i).poly_id)
00735       return i;
00736 
00737   ROS_WARN("no min polygon found");
00738   return -1;
00739 }
00740 
00741 #if 0 //TODO
00742 ElementID
00743 PolyOps::updateLaneLocation(const std::vector<poly>& polygons,
00744                             const MapPose& pose,
00745                             const WayPointNode& waypt1,
00746                             const WayPointNode& waypt2)
00747 {
00748   
00749   std::vector<poly> lanes_polys;
00750 
00751   for (uint i=0; i< polygons.size(); i++)
00752     if ((polygons.at(i).start_way.seg == waypt1.id.seg &&
00753          polygons.at(i).start_way.lane == waypt1.id.lane && 
00754          polygons.at(i).start_way.pt == waypt1.id.pt)
00755         ||
00756         (polygons.at(i).start_way.seg == waypt2.id.seg &&
00757          polygons.at(i).start_way.lane == waypt2.id.lane && 
00758          polygons.at(i).start_way.pt == waypt2.id.pt))
00759       lanes_polys.push_back(polygons.at(i));
00760   
00761   int index=getClosestPoly(lanes_polys,pose.px,pose.py);
00762 
00763   if (index >= 0)
00764     return lanes_polys.at(index).start_way;
00765 
00766   return waypt1.id;
00767 
00768 }
00769 #endif
00770 
00771 float PolyOps::length_between_polygons(const std::vector<poly>& polygons,
00772                                        int index1,
00773                                        int index2) 
00774 {
00775   float length = 0;
00776 
00777   index1=std::max(0,index1);
00778   index1=std::min(int(polygons.size()),index1);
00779   
00780   index2=std::max(0,index2);
00781   index2=std::min(int(polygons.size()),index2);
00782   
00783   for(int i = index1+1; i < index2; i++)
00784     length += polygons.at(i).length;
00785   return length;
00786 }
00787 
00788 //Finds the distance between the midpoints of two polygons
00789 //float distanceBetweenPolygons(const std::vector<poly>& polygons,
00790 //                            poly from,
00791 //                            poly to){
00792 // return 
00793 //    distanceAlongLane(polygons,
00794 //                    from.midpoint,
00795 //                    to.midpoint);
00796 //}
00797 
00798 // Requires that the polygons are in the order to follow, and all in
00799 // the same lane
00800 float PolyOps::distanceAlongLane(const std::vector<poly>& polygons,
00801                                  const MapXY& from,
00802                                  const MapXY& to)
00803 {
00804   return (specialDistanceAlongLane(polygons, from, to)).first;
00805 }
00806 
00807 //Required by observers. 
00808 // Returns the projection of start point on to the lane
00809 // and the distance along the lane to the 'to' point
00810 std::pair<float, MapXY> 
00811 PolyOps::specialDistanceAlongLane(const std::vector<poly>& polygons,
00812                                   const MapXY& from,
00813                                   const MapXY& to)
00814 {
00815   //Check if all Polygons are in the same lane
00816   int index1=getClosestPoly(polygons, from);
00817   int index2=getClosestPoly(polygons, to);
00818 
00819   if (index1 == -1 || index2 == -1) {
00820     std::pair<float, MapXY> return_value(0, MapXY());
00821     return return_value;
00822   }
00823 
00824   poly poly_start = polygons.at(index1);
00825   poly poly_end = polygons.at(index2);
00826 
00827   MapXY start_bisect1=midpoint(poly_start.p1, poly_start.p4);
00828   MapXY start_bisect2=midpoint(poly_start.p2, poly_start.p3);
00829 
00830   MapXY end_bisect1=midpoint(poly_end.p1, poly_end.p4);
00831   MapXY end_bisect2=midpoint(poly_end.p2, poly_end.p3);
00832 
00833   MapXY start_point=GetClosestPointToLine(start_bisect1,start_bisect2,
00834                                           from, true);
00835 
00836   MapXY end_point=GetClosestPointToLine(end_bisect1,end_bisect2,
00837                                         to, true);
00838 
00839   float distance_start, distance_end, polygon_length;
00840   float tmp;
00841   if (index1 < index2)                  // target is ahead?
00842     {
00843       Euclidean::DistanceFromLine(start_point, poly_start.p2, poly_start.p3,
00844                                   distance_start, tmp);
00845       Euclidean::DistanceFromLine(end_point, poly_end.p1, poly_end.p4,
00846                                   distance_end, tmp);
00847       polygon_length = length_between_polygons(polygons, index1, index2);
00848     }
00849   else if (index1 > index2)             // target is behind?
00850     {
00851       Euclidean::DistanceFromLine(start_point, poly_start.p1, poly_start.p4,
00852                                   distance_start, tmp);
00853       Euclidean::DistanceFromLine(end_point, poly_end.p2, poly_end.p3,
00854                                   distance_end, tmp);
00855       polygon_length = length_between_polygons(polygons, index2, index1);
00856     }
00857   else                                  // target in the same polygon
00858     {
00859       Euclidean::DistanceFromLine(start_point, poly_start.p1, poly_start.p4,
00860                                   distance_start, tmp);
00861       distance_start = -distance_start;
00862       Euclidean::DistanceFromLine(end_point, poly_end.p1, poly_end.p4,
00863                                   distance_end, tmp);
00864       polygon_length = 0;
00865     }
00866 
00867   float distance_total = distance_start + polygon_length + distance_end;
00868   
00869   if (index1 > index2)
00870     distance_total = -distance_total;
00871 
00872 #ifdef EXTREME_DEBUG
00873   ROS_DEBUG("distance_total to (%f, %f) %f )", to.x, to.y, distance_total);
00874   ROS_DEBUG("polygon_length to (%f, %f) %f )", to.x, to.y, polygon_length);
00875 #endif
00876 
00877   std::pair<float, MapXY> return_value(distance_total, start_point);
00878   return return_value;
00879 }
00880 
00881 int PolyOps::index_of_downstream_poly(const std::vector<poly>& polygons,
00882                                       int start_index,
00883                                       float distance) 
00884 {
00885   if(start_index < 0 || start_index >= (int)polygons.size())
00886     return -1;
00887 
00888   if(distance <= 0)
00889     return start_index;
00890 
00891   int index = start_index;
00892   float length_so_far = polygons.at(index).length;
00893   while(distance > length_so_far) {
00894     if(index == (int)polygons.size() - 1)
00895       return index;
00896     index++;
00897     length_so_far += polygons.at(index).length;
00898   }
00899   return index;
00900 }
00901 
00902 MapXY PolyOps::midpoint(const MapXY& p1, const MapXY& p2) 
00903 {
00904   MapXY mid((p1.x+p2.x)/2.0, (p1.y+p2.y)/2.0);
00905   return mid;
00906 }
00907 
00908 float PolyOps::avgLengthOfPolySides(const poly& p) 
00909 {
00910   float s12 = Euclidean::DistanceTo(p.p1, p.p2);
00911   float s23 = Euclidean::DistanceTo(p.p2, p.p3);
00912   float s34 = Euclidean::DistanceTo(p.p3, p.p4);
00913   float s41 = Euclidean::DistanceTo(p.p4, p.p1);
00914   return (s12 + s23 + s34 + s41) / 4;
00915 }
00916 
00917 // Return a Set of unique lane IDs corresponding to the polys in the list
00918 std::set<ElementID> PolyOps::getPolyLaneIds(const std::vector<poly>& polys) 
00919 {
00920   std::set<ElementID> lane_ids;
00921   lane_ids.clear();   // ensure it's empty
00922   for (uint i = 0; i < polys.size(); ++i) {
00923     lane_ids.insert(ElementID(polys.at(i).start_way.seg,
00924                               polys.at(i).start_way.lane, 0));
00925   }
00926   return lane_ids;
00927 }
00928 
00929 #if 0 //TODO
00930 // Return a unique lane ID corresponding to the polys/dir given
00931 // (uses transition polygons to determine closest lanes)
00932 // input:  a) neighborhood polygons
00933 //         b) relative flag (used for determining direction)
00934 //              0 relative to lane heading
00935 //              1 relative to pose paramater
00936 //         c) direction (relative to relative flag)
00937 //             +1 for getting left lane ID
00938 //              0 for getting current lane ID
00939 //             -1 for getting right lane ID
00940 //         d) pose
00941 //         e) epsilon in which our closest poly to make observations
00942 //              from must be within
00943 ElementID PolyOps::getPolyLaneIdDir(const std::vector<poly>& polys,
00944                                     const int relative,
00945                                     const int direction,
00946                                     const player_pose2d_t& pose,
00947                                     const float poly_epsilon) 
00948 {
00949   int cur_poly_index = getClosestPolyEpsilon(polys, pose, poly_epsilon);
00950   if (cur_poly_index == -1) {
00951     ROS_DEBUG("PolyOps::getPolyLaneIdDir: No poly found");
00952     return ElementID(-1,-1,-1);
00953   }
00954 
00955   int adj_poly_index[2];
00956   poly my_cur_poly = polys.at(cur_poly_index);
00957 
00958   if (direction == 0)   // if we want current lane id, return here
00959     return ElementID(my_cur_poly.start_way.seg, my_cur_poly.start_way.lane, 0);
00960 
00961   // generate adjacent lane IDs
00962   ElementID adj_lane[2];
00963   adj_lane[0] = polys.at(cur_poly_index).start_way;
00964   adj_lane[1] = polys.at(cur_poly_index).start_way;
00965   adj_lane[0].lane--;
00966   adj_lane[1].lane++;
00967   poly_list_t adj_lane_polys[2];                // adjacent lanes in segment
00968 
00969   // iterate through the adjacent lanes
00970   for (unsigned i = 0; i < 2; ++i) {
00971     adj_lane[i].pt = 0;         // lane ID, not way-point
00972     adj_lane_polys[i].clear();
00973 
00974     if (adj_lane[i].lane > 0) {  // lane ID in valid range?
00975 
00976       // get adjacent lane polys
00977       AddLanePolys(polys, adj_lane_polys[i], adj_lane[i]);
00978       if (adj_lane_polys[i].size() == 0 ) // make sure we found polys
00979         continue;
00980 
00981       // get index of closest poly in adjacent lane
00982       adj_poly_index[i] = getClosestPoly(adj_lane_polys[i],
00983                                          my_cur_poly.midpoint);
00984       if (adj_poly_index[i] == -1)
00985         continue;
00986       poly my_adj_poly = adj_lane_polys[i].at(adj_poly_index[i]);
00987 
00988       // find relative direction of poly
00989       MapPose poly_pose(my_cur_poly.midpoint, 0.0);
00990       if (relative == 0)      // relative to lane heading
00991         poly_pose.yaw = my_cur_poly.heading;
00992       else if (relative == 1) // relative to pose heading
00993         poly_pose.yaw = pose.pa;
00994       float theta = Coordinates::bearing(poly_pose, my_adj_poly.midpoint);
00995 
00996       if ( (theta > 0 && direction == +1) ||  // is lane left?
00997            (theta < 0 && direction == -1)  )   // is lane right?
00998         return ElementID(my_adj_poly.start_way.seg,
00999                          my_adj_poly.start_way.lane, 0);
01000       else;  // boundary case(0, +-M_PI): don't consider left/right
01001 
01002     } //end if valid lane
01003   } //end for
01004 
01005   ROS_DEBUG("Error: lane must not exist this direction");
01006   return ElementID();                 // error: no lane this direction
01007 }
01008 
01009 // Return a unique lane ID corresponding to the polys/dir given
01010 // (does NOT use transition polygons to determine closest lanes)
01011 // input:  a) neighborhood polygons
01012 //         b) relative flag (used for determining direction)
01013 //              0 relative to lane heading
01014 //              1 relative to pose paramater
01015 //         c) direction (relative to relative flag)
01016 //             +1 for getting left lane ID
01017 //              0 for getting current lane ID
01018 //             -1 for getting right lane ID
01019 //         d) pose
01020 //         e) epsilon in which our closest poly to make observations
01021 //              from must be within
01022 ElementID PolyOps::getNonTransPolyLaneIdDir(const poly_list_t& polys,
01023                                     const int relative,
01024                                     const int direction,
01025                                     const player_pose2d_t& pose,
01026                                     const float poly_epsilon) 
01027 {
01028   int cur_poly_index = getClosestNonTransPolyEpsilon(polys, pose, poly_epsilon);
01029   if (cur_poly_index == -1) {
01030     ROS_DEBUG("PolyOps::getNonTransPolyLaneIdDir: No poly found");
01031     return ElementID(-1,-1,-1);
01032   }
01033 
01034   int adj_poly_index[2];
01035   poly my_cur_poly = polys.at(cur_poly_index);
01036 
01037   if (direction == 0)   // if we want current lane id, return here
01038     return ElementID(my_cur_poly.start_way.seg, my_cur_poly.start_way.lane, 0);
01039 
01040   // generate adjacent lane IDs
01041   ElementID adj_lane[2];
01042   adj_lane[0] = polys.at(cur_poly_index).start_way;
01043   adj_lane[1] = polys.at(cur_poly_index).start_way;
01044   adj_lane[0].lane--;
01045   adj_lane[1].lane++;
01046   poly_list_t adj_lane_polys[2];                // adjacent lanes in segment
01047 
01048   // iterate through the adjacent lanes
01049   for (unsigned i = 0; i < 2; ++i) {
01050     adj_lane[i].pt = 0;         // lane ID, not way-point
01051     adj_lane_polys[i].clear();
01052 
01053     if (adj_lane[i].lane > 0) {  // lane ID in valid range?
01054 
01055       // get adjacent lane polys
01056       AddLanePolys(polys, adj_lane_polys[i], adj_lane[i]);
01057       if (adj_lane_polys[i].size() == 0 ) // make sure we found polys
01058         continue;
01059 
01060       // get index of closest poly in adjacent lane
01061       adj_poly_index[i] = getClosestNonTransPoly(adj_lane_polys[i],
01062                       my_cur_poly.midpoint);
01063       if (adj_poly_index[i] == -1)
01064         continue;
01065       poly my_adj_poly = adj_lane_polys[i].at(adj_poly_index[i]);
01066 
01067       // find relative direction of poly
01068       MapPose poly_pose(my_cur_poly.midpoint, 0.0);
01069       if (relative == 0)      // relative to lane heading
01070         poly_pose.yaw = my_cur_poly.heading;
01071       else if (relative == 1) // relative to pose heading
01072         poly_pose.yaw = pose.pa;
01073       float theta = Coordinates::bearing(poly_pose, my_adj_poly.midpoint);
01074 
01075       if ( (theta > 0 && direction == +1) ||  // is lane left?
01076            (theta < 0 && direction == -1)  )   // is lane right?
01077         return ElementID(my_adj_poly.start_way.seg,
01078                          my_adj_poly.start_way.lane, 0);
01079       else;  // boundary case(0, +-M_PI): don't consider left/right
01080 
01081     } //end if valid lane
01082   } //end for
01083   ROS_DEBUG("Error: lane must not exist this direction");
01084   return ElementID(-1,-1,-1);  // error: no lane this direction
01085 
01086 }
01087 #endif
01088 
01089 // Return the polygons in lane corresponding to the polys/dir given
01090 // input:  a) neighborhood polygons
01091 //         b) empty destination polygon vector
01092 //         c) relative flag (used for determining direction)
01093 //              0 relative to lane heading
01094 //              1 relative to pose paramater
01095 //         d) direction (relative to relative flag)
01096 //             +1 for getting left lane
01097 //              0 for getting current lane
01098 //             -1 for getting right lane
01099 //         e) pose
01100 void PolyOps::getLaneDir(const std::vector<poly>& polys,
01101                          std::vector<poly>& to_polys,
01102                          const int relative,
01103                          const int direction,
01104                          const MapPose &pose) 
01105 {
01106   // Clear this out here in case this function returns early.
01107   to_polys.clear();
01108 
01109   int cur_poly_index = getClosestPoly(polys, pose);
01110   if (cur_poly_index == -1) {
01111     ROS_DEBUG("PolyOps::getLaneDir: No poly found");
01112     return;
01113   }
01114   int adj_poly_index[2];
01115   poly my_cur_poly = polys.at(cur_poly_index);
01116 
01117   if (direction == 0) {  // if we want current lane id, return here
01118     AddLanePolys(polys, to_polys, my_cur_poly.start_way);
01119     return;
01120   }
01121 
01122   // generate adjacent lane IDs
01123   ElementID adj_lane[2];
01124   adj_lane[0] = polys.at(cur_poly_index).start_way;
01125   adj_lane[1] = polys.at(cur_poly_index).start_way;
01126   adj_lane[0].lane--;
01127   adj_lane[1].lane++;
01128   poly_list_t adj_lane_polys[2];                // adjacent lanes in segment
01129 
01130   // iterate through the adjacent lanes
01131   for (unsigned i = 0; i < 2; ++i) {
01132     adj_lane[i].pt = 0;         // lane ID, not way-point
01133     adj_lane_polys[i].clear();
01134 
01135     if (adj_lane[i].lane > 0) {  // lane ID in valid range?
01136       
01137       // get adjacent lane polys
01138       AddLanePolys(polys, adj_lane_polys[i], adj_lane[i]);
01139       if (adj_lane_polys[i].size() == 0 ) // make sure we found polys
01140         continue;
01141 
01142       // get index of closest poly in adjacent lane
01143       adj_poly_index[i] = getClosestPoly(adj_lane_polys[i],
01144                                          my_cur_poly.midpoint);
01145       if (adj_poly_index[i] == -1)
01146         continue;
01147       poly my_adj_poly = adj_lane_polys[i].at(adj_poly_index[i]);
01148 
01149       // find relative direction of poly
01150       MapPose poly_pose(my_cur_poly.midpoint, 0.0);
01151       if (relative == 0)      // relative to lane heading
01152         poly_pose.yaw = my_cur_poly.heading;
01153       else if (relative == 1) // relative to pose heading
01154         poly_pose.yaw = pose.yaw;
01155       float theta = Coordinates::bearing(poly_pose, my_adj_poly.midpoint);
01156 
01157       if ( (theta > 0 && direction == +1) ||    // is lane left?
01158            (theta < 0 && direction == -1)  )   // is lane right?
01159         {
01160           CollectPolys(adj_lane_polys[i], to_polys, 0);
01161           return;
01162         }
01163       // else boundary case(0, +-M_PI): don't consider left/right
01164 
01165     } //end if valid lane
01166   } //end for
01167   ROS_DEBUG("Error: lane must not exist this direction");
01168 }
01169 
01170 #if 0 //TODO
01171 // Return the lane polys to the left up to num_lanes away.
01172 // similar interface to getLaneDir()
01173 void PolyOps::getNumLanesDir(const std::vector<poly>& polys,
01174             std::vector<poly>& to_polys,
01175             const int relative,
01176             const int direction,
01177             const player_pose2d_t &pose,
01178             const unsigned num_lanes) 
01179 {
01180   int poly_index;
01181   player_pose2d_t my_pose = pose;
01182   poly_list_t dir_polys;
01183   to_polys.clear();
01184   for (unsigned i = 0; i < num_lanes; i++) {
01185     getLaneDir(polys, dir_polys, relative, direction, my_pose);
01186     poly_index = getClosestPoly(dir_polys, my_pose);
01187     if (poly_index < 0)
01188       return;
01189     AddLanePolys(dir_polys, to_polys, dir_polys.at(poly_index).start_way);
01190     my_pose.px = dir_polys.at(poly_index).midpoint.x;
01191     my_pose.py = dir_polys.at(poly_index).midpoint.y;
01192   }
01193   if (to_polys.size() == 0) // no lanes gotten
01194     ROS_DEBUG("no lanes in given direction when expected");
01195 }
01196 #endif
01197 
01198 ElementID PolyOps::getReverseLane(const std::vector<poly>& polys,
01199                                   const MapPose &pose)
01200 {
01201   poly_list_t to_polys;
01202 
01203   ElementID return_id;
01204   
01205   int base_index = getClosestPoly(polys, pose);
01206   if (base_index < 0)
01207     return return_id;
01208 
01209   float base_heading=polys.at(base_index).heading;  
01210   MapPose my_pose = pose;
01211 
01212   while (true)
01213     {
01214       getLaneDir(polys,to_polys,0,+1,my_pose);
01215       int poly_index = getClosestPoly(to_polys, my_pose);
01216       if (poly_index < 0)
01217         break;
01218       float lane_heading=to_polys.at(poly_index).heading;
01219       if (fabs(Coordinates::normalize(lane_heading - base_heading)) > HALFPI)
01220         return to_polys.at(poly_index).end_way;
01221       my_pose.map.x = to_polys.at(poly_index).midpoint.x;
01222       my_pose.map.y = to_polys.at(poly_index).midpoint.y;
01223     }
01224   return return_id;
01225 }
01226 
01227 // Print useful information of each polygon to ROS log
01228 void PolyOps::printPolygons(const poly_list_t& polys) 
01229 {
01230   for (uint i = 0; i < polys.size(); ++i) {
01231     poly p = polys.at(i);
01232     ROS_INFO("poly: %d S/T/CW: %d/%d/%d start: %d.%d.%d "
01233              "end: %d.%d.%d mp: %f,%f",
01234              p.poly_id, p.is_stop, p.is_transition, p.contains_way,
01235              p.start_way.seg, p.start_way.lane, p.start_way.pt,
01236              p.end_way.seg, p.end_way.lane, p.end_way.pt,
01237              p.midpoint.x, p.midpoint.y);
01238   }
01239 }
01240 
01241 // Return all the transition polys in the polys passed
01242 poly_list_t PolyOps::getTransitionPolys(const poly_list_t& polys) {
01243   poly_list_t tran_polys;
01244   for (uint i = 0; i < polys.size(); ++i)
01245     if (polys.at(i).is_transition)
01246       tran_polys.push_back(polys.at(i));
01247   return tran_polys;
01248 }
01249   
01250 #if 0 //TODO
01251 // Return the closest polygon relative to pose in the seg/lane of concern
01252 poly PolyOps::getClosestPolyInLane(const std::vector<poly>& polys,
01253                                    const player_pose2d_t& pose,
01254                                    const ElementID id) 
01255 {
01256   std::vector<poly> lane_polys;
01257   AddLanePolys(polys, lane_polys, id);
01258   int poly_index = getClosestPoly(lane_polys, pose);
01259   if (poly_index == -1) {
01260     poly null_poly;
01261     null_poly.poly_id = -1;
01262     return null_poly;
01263   }
01264   else
01265     return lane_polys.at(poly_index);
01266 }
01267 #endif
01268 
01269 // Check if polygon is valid
01270 bool PolyOps::isValidPoly(const poly& p) 
01271 {
01272   if (p.poly_id == -1)
01273     return false;
01274   else
01275     return true;
01276 }
01277 
01278 #if 0 //TODO
01279 // Determine if the pose heading is same dirction as the polygon heading
01280 bool PolyOps::travelingCorrectDir(const poly& p, const player_pose2d_t& pose) 
01281 {
01282   float theta = Coordinates::normalize(pose.pa - p.heading);
01283   if (fabs(theta) < HALFPI)
01284     return true;
01285   else
01286     return false;
01287 }
01288 #endif
01289 
01290 MapXY PolyOps::GetClosestPointToLine(MapXY A, MapXY B, 
01291                                      MapXY P, bool segmentClamp)
01292 //set 'segmentClamp' to true if you want the closest point on the
01293 //segment, not just the line.
01294 {
01295   MapXY AP = P - A;
01296   MapXY AB = B - A;
01297   float ab2 = AB.x*AB.x + AB.y*AB.y;
01298   float ap_ab = AP.x*AB.x + AP.y*AB.y;
01299   float t = ap_ab / ab2;
01300   if (segmentClamp)
01301     {
01302       if (t < 0.0f) 
01303         t = 0.0f;
01304       else if (t > 1.0f) 
01305         t = 1.0f;
01306     }
01307   AB.x*=t;
01308   AB.y*=t;
01309   
01310   MapXY Closest = A + AB;
01311   return Closest;
01312 }
01313 
01314 // get polys from from_id to to_id and store in to_polys
01315 void PolyOps::getPolysBetweenWayPts(const std::vector<poly> &from_polys,
01316                                    std::vector<poly> &to_polys,
01317                                    ElementID from_id, ElementID to_id) 
01318 {
01319 
01320   to_polys.clear();
01321   // make sure the points are valid
01322   if (!from_id.valid() or !to_id.valid()) {
01323     ROS_DEBUG("points not valid");
01324     return;
01325   }
01326   // make sure the points are in the same lane
01327   if (!from_id.same_lane(to_id)) {
01328     ROS_DEBUG("points not in same lane");
01329     return;
01330   }
01331 
01332   int from_index = -1;
01333   // find poly matching from_id
01334   for (uint i = 0; i < from_polys.size(); ++i) {
01335     if (from_polys.at(i).start_way.pt == from_id.pt) {
01336       from_index = i;
01337       break;
01338     }
01339   }
01340 
01341   // make sure we found a poly with the eid
01342   if (from_index == -1)
01343     return;
01344 
01345   // collect polys between points in sequential order
01346   for (uint i = from_index; i < from_polys.size(); ++i) {
01347     if (from_polys.at(i).start_way.pt < to_id.pt)
01348       to_polys.push_back(from_polys.at(i));
01349   }
01350 
01351 }
01352 
01353 // assumptions:
01354 // - on a 2 lane road
01355 // - way0 and way1 are from comm/nav plan
01356 std::vector<MapXY> PolyOps::getRoadPerimeterPoints(
01357                 const std::vector<poly>& polys,
01358                 const ElementID way0, const ElementID way1) 
01359 {
01360 
01361   std::vector<poly> all_polys_in_curr_lane;
01362   std::vector<poly> all_polys_in_left_lane;
01363   std::vector<poly> polys_in_curr_lane_forward;
01364   std::vector<poly> polys_in_left_lane_reverse;
01365   // perimeter points of road counter clockwise from pose
01366   std::vector<MapXY> perim_points;
01367   perim_points.clear();
01368 
01369   // check for invalid inputs and fail gracefully
01370   if (!way0.valid() or !way1.valid() or polys.empty() or (way1 < way0))
01371     return perim_points;
01372 
01373   // get poly that has way0
01374   int p_idx_way0 = get_waypoint_index(polys, way0);
01375   if (p_idx_way0 < 0)
01376     return perim_points;
01377   poly way0_poly = polys.at(p_idx_way0);
01378   // get pose that has way0
01379   MapPose way0_pose(way0_poly.midpoint, way0_poly.heading);
01380 
01381   // get poly that has way1
01382   int p_idx_way1 = get_waypoint_index(polys, way1);
01383   if (p_idx_way1 < 0)
01384     return perim_points;
01385   poly way1_poly = polys.at(p_idx_way1);
01386   // get pose that has way1
01387   MapPose way1_pose(way1_poly.midpoint, way1_poly.heading);
01388 
01389   // get all polys in current lane
01390   getLaneDir(polys, all_polys_in_curr_lane, 0, 0, way0_pose);
01391   // get polys_in_curr_lane_forward from way0 to way1
01392   int way0_pi = getPolyIndex(all_polys_in_curr_lane, way0_poly);
01393   int way1_pi = getPolyIndex(all_polys_in_curr_lane, way1_poly);
01394   CollectPolys(all_polys_in_curr_lane, polys_in_curr_lane_forward,
01395                   way0_pi, way1_pi);
01396 
01397   // get all polys in left lane
01398   getLaneDir(polys, all_polys_in_left_lane, 0, +1, way0_pose);
01399   int ll_way0_pi = getClosestPoly(all_polys_in_left_lane, way0_pose);
01400   if (ll_way0_pi < 0) {
01401     ROS_DEBUG("no poly found 0");
01402     return perim_points;
01403   }
01404   int ll_way1_pi = getClosestPoly(all_polys_in_left_lane, way1_pose);
01405   if (ll_way1_pi < 0) {
01406     ROS_DEBUG("no poly found 1");
01407     return perim_points;
01408   }
01409   poly ll_way0_poly = all_polys_in_left_lane.at(ll_way0_pi);
01410   poly ll_way1_poly = all_polys_in_left_lane.at(ll_way1_pi);
01411   // get polys_in_left_lane_reverse from way1 to way0
01412   ll_way0_pi = getPolyIndex(all_polys_in_left_lane, ll_way0_poly);
01413   ll_way1_pi = getPolyIndex(all_polys_in_left_lane, ll_way1_poly);
01414   CollectPolys(all_polys_in_left_lane, polys_in_left_lane_reverse,
01415                   ll_way1_pi, ll_way0_pi);
01416 
01417   // get the perimeter points on the right side of the road
01418   for (uint i = 0; i < polys_in_curr_lane_forward.size(); ++i) {
01419     perim_points.push_back(polys_in_curr_lane_forward.at(i).p4);
01420   }
01421   if (polys_in_curr_lane_forward.size() > 0)
01422     perim_points.push_back(polys_in_curr_lane_forward.back().p3);
01423 
01424   // get the perimeter points on the left side of the road
01425   for (uint i = 0; i < polys_in_left_lane_reverse.size(); ++i) {
01426     perim_points.push_back(polys_in_left_lane_reverse.at(i).p2);
01427   }
01428   if (polys_in_left_lane_reverse.size() > 0)
01429     perim_points.push_back(polys_in_left_lane_reverse.back().p1);
01430 
01431   return perim_points;
01432 
01433 }
01434 
01435 std::vector<MapXY> PolyOps::getRoadPerimeterPoints(
01436                 const std::vector<poly>& polys,
01437                 const ElementID way0) 
01438 {
01439 
01440   ElementID way1;
01441   std::vector<poly> all_polys_in_curr_lane;
01442   std::vector<poly> all_polys_in_left_lane;
01443   std::vector<poly> polys_in_curr_lane_forward;
01444   std::vector<poly> polys_in_left_lane_reverse;
01445   // perimeter points of road counter clockwise from pose
01446   std::vector<MapXY> perim_points;
01447   perim_points.clear();
01448 
01449   // check for invalid inputs and fail gracefully
01450   if (!way0.valid() or polys.empty())
01451     return perim_points;
01452 
01453   // get poly that has way0
01454   int p_idx_way0 = get_waypoint_index(polys, way0);
01455   if (p_idx_way0 < 0)
01456     return perim_points;
01457   poly way0_poly = polys.at(p_idx_way0);
01458   // get pose that has way0
01459   MapPose way0_pose(way0_poly.midpoint, way0_poly.heading);
01460 
01461   // get all polys in current lane
01462   getLaneDir(polys, all_polys_in_curr_lane, 0, 0, way0_pose);
01463   // get first transition poly ahead in lane to be way1
01464   //fprintf(stderr,"all_polys_in_curr_lane.size() = %d\n",
01465   //        all_polys_in_curr_lane.size());
01466   for (uint i = 0; i < all_polys_in_curr_lane.size(); ++i) {
01467     if (all_polys_in_curr_lane.at(i).is_stop and
01468         all_polys_in_curr_lane.at(i).start_way > way0) {
01469       way1 = all_polys_in_curr_lane.at(i).start_way;
01470       break;
01471     }
01472     if (i == all_polys_in_curr_lane.size()-1)
01473       way1 = all_polys_in_curr_lane.at(i).start_way;
01474   }
01475   //fprintf(stderr,"way1: %d.%d.%d\n",way1.seg,way1.lane,way1.pt);
01476 
01477   if (!way1.valid())
01478     return perim_points;
01479 
01480   // get poly that has way1
01481   int p_idx_way1 = get_waypoint_index(polys, way1);
01482   if (p_idx_way1 < 0)
01483     return perim_points;
01484   poly way1_poly = polys.at(p_idx_way1);
01485   // get pose that has way1
01486   MapPose way1_pose(way1_poly.midpoint, way1_poly.heading);
01487 
01488   // get polys_in_curr_lane_forward from way0 to way1
01489   int way0_pi = getPolyIndex(all_polys_in_curr_lane, way0_poly);
01490   int way1_pi = getPolyIndex(all_polys_in_curr_lane, way1_poly);
01491   CollectPolys(all_polys_in_curr_lane, polys_in_curr_lane_forward,
01492                   way0_pi, way1_pi);
01493 
01494   // get all polys in left lane
01495   getLaneDir(polys, all_polys_in_left_lane, 0, +1, way0_pose);
01496   int ll_way0_pi = getClosestPoly(all_polys_in_left_lane, way0_pose);
01497   if (ll_way0_pi < 0) {
01498     ROS_DEBUG("no poly found 0");
01499     return perim_points;
01500   }
01501   int ll_way1_pi = getClosestPoly(all_polys_in_left_lane, way1_pose);
01502   if (ll_way1_pi < 0) {
01503     ROS_DEBUG("no poly found 1");
01504     return perim_points;
01505   }
01506   poly ll_way0_poly = all_polys_in_left_lane.at(ll_way0_pi);
01507   poly ll_way1_poly = all_polys_in_left_lane.at(ll_way1_pi);
01508   // get polys_in_left_lane_reverse from way1 to way0
01509   ll_way0_pi = getPolyIndex(all_polys_in_left_lane, ll_way0_poly);
01510   ll_way1_pi = getPolyIndex(all_polys_in_left_lane, ll_way1_poly);
01511   CollectPolys(all_polys_in_left_lane, polys_in_left_lane_reverse,
01512                   ll_way1_pi, ll_way0_pi);
01513 
01514   // get the perimeter points on the right side of the road
01515   for (uint i = 0; i < polys_in_curr_lane_forward.size(); ++i) {
01516     perim_points.push_back(polys_in_curr_lane_forward.at(i).p4);
01517   }
01518   if (polys_in_curr_lane_forward.size() > 0)
01519     perim_points.push_back(polys_in_curr_lane_forward.back().p3);
01520 
01521   // get the perimeter points on the left side of the road
01522   for (uint i = 0; i < polys_in_left_lane_reverse.size(); ++i) {
01523     perim_points.push_back(polys_in_left_lane_reverse.at(i).p2);
01524   }
01525   if (polys_in_left_lane_reverse.size() > 0)
01526     perim_points.push_back(polys_in_left_lane_reverse.back().p1);
01527 
01528   return perim_points;
01529 
01530 }


art_map
Author(s): David Li, Patrick Beeson, Bartley Gillen, Tarun Nimmagadda, Mickey Ristroph, Michael Quinlan, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:08:34