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;
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,
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))
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))
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
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
00963   adj_lane[0] = polys.at(cur_poly_index).start_way;
00964   adj_lane[1] = polys.at(cur_poly_index).start_way;
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
00973
00974     if (adj_lane[i].lane > 0) {  // lane ID in valid range?
00975
00976       // get adjacent lane polys
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
00983                                          my_cur_poly.midpoint);
00984       if (adj_poly_index[i] == -1)
00985         continue;
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?
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
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
01042   adj_lane[0] = polys.at(cur_poly_index).start_way;
01043   adj_lane[1] = polys.at(cur_poly_index).start_way;
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
01052
01053     if (adj_lane[i].lane > 0) {  // lane ID in valid range?
01054
01055       // get adjacent lane polys
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
01062                       my_cur_poly.midpoint);
01063       if (adj_poly_index[i] == -1)
01064         continue;
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?
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   }
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
01124   adj_lane[0] = polys.at(cur_poly_index).start_way;
01125   adj_lane[1] = polys.at(cur_poly_index).start_way;
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
01134
01135     if (adj_lane[i].lane > 0) {  // lane ID in valid range?
01136
01137       // get adjacent lane polys
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
01144                                          my_cur_poly.midpoint);
01145       if (adj_poly_index[i] == -1)
01146         continue;
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
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;
01219       if (fabs(Coordinates::normalize(lane_heading - base_heading)) > HALFPI)
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
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
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