MapLanes.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2007, 2010 David Li, Patrick Beeson, Jack O'Quin
00003  *
00004  *  License: Modified BSD Software License Agreement
00005  * 
00006  *  $Id: MapLanes.cc 1161 2011-03-26 02:10:49Z jack.oquin $
00007  */
00008 
00017 #include <iostream>
00018 #include <iomanip>
00019 #include <art/epsilon.h>
00020 #include <vector>
00021 
00022 #include <art_map/gaussian.h>
00023 #include <art_map/MapLanes.h>
00024 #include <art_map/euclidean_distance.h>
00025 
00026 #include <art_map/rotate_translate_transform.h>
00027 
00028 // intial_latlong specifies whether rndf waypoints and initial
00029 // coordinates are specified in lat/long or map_XY. The boolean
00030 // applies to both since the RNDF itself doesn't specify. This can
00031 // seperated if getting lat/long inital coordinates is inconvineint.
00032 
00033 #define way_poly_size 0.5 // half of length of polygon that goes
00034                           // around waypoints
00035 int writecounter=0;
00036 int aCount=0;
00037 int bCount=0;
00038 int cCount=0;
00039     
00040 int MapLanes::MapRNDF(Graph* _graph, float _max_poly_size)
00041 {
00042   graph=_graph;
00043 
00044   max_poly_size=fmaxf(_max_poly_size, MIN_POLY_SIZE);
00045 
00046   allPolys.clear();
00047   //filtPolys.clear();
00048 
00049   //initilize poly id counter
00050   poly_id_counter = 0;
00051 
00052   ROS_INFO_STREAM("Starting with " << graph->nodes_size << " nodes in graph");
00053 
00054   rX=0.0;
00055   rY=0.0;
00056   rOri=0.0;
00057 
00058   cX=0.0;
00059   cX=0.0;
00060   #ifdef DEBUGMAP
00061     debugFile=fopen("mapDebug.txt","wb");
00062   #endif
00063 
00064   MakePolygons();
00065   SetFilteredPolygons();
00066   
00067   ROS_DEBUG("MapLanes constructed successfully");
00068   return 0;                             // success
00069 }
00070 
00071 void MapLanes::MakePolygons()
00072 {
00073   // Add Waypoints to WayPointImage
00074   std::vector<WayPointNode> lane;
00075   std::vector<Point2f> lane_pt;
00076   std::vector<int> lane_map;
00077 
00078   SmoothCurve lc,rc;
00079 
00080   ElementID prev_lane;             // used to determine when the edges
00081                                    // switch lanes
00082 
00083   // Walk along graph edges, pushing nodes (and the associated points
00084   // that the curve code uses) from same lane onto a list, then
00085   // process the list whenever a new lane is encountered (or an transition)
00086   // or before leaving the function if the list isn't empty.
00087 
00088   for(uint j = 0; j < graph->edges_size; j++)
00089     { 
00090       WayPointEdge e=graph->edges[j];
00091       if (e.is_implicit)
00092         continue;
00093       WayPointNode w1=graph->nodes[e.startnode_index];
00094       WayPointNode w2=graph->nodes[e.endnode_index];
00095       
00096       if ((!e.is_exit || (w1.id.same_lane(w2.id) &&
00097                           w1.id.pt+1==w2.id.pt))
00098           && !w1.is_perimeter && !w1.is_spot &&
00099           !w2.is_perimeter && !w2.is_spot)
00100         // if lane push next waypoint onto list
00101         {
00102           if (w1.id.seg != prev_lane.seg ||
00103               w1.id.lane != prev_lane.lane ||
00104               w1.id.pt != prev_lane.pt)
00105             //if new lane push start waypoint onto list
00106             {
00107               // If last lane info is still around, process it
00108               if (lane.size()>1)
00109                 {
00110                   Point2f diff_pt=lane_pt[1]-lane_pt[0];
00111                   Point2f diff_pt2=lane_pt[lane_pt.size()-1]-
00112                     lane_pt[lane_pt.size()-2];
00113                   SmoothCurve c=
00114                     SmoothCurve(lane_pt,
00115                                 atan2f(diff_pt[1],diff_pt[0]), 1,
00116                                 atan2f(diff_pt2[1],diff_pt2[0]), 1);
00117                   
00118                   for (uint i=0;i<lane.size()-1;i++)
00119                     {
00120                       MakeLanePolygon(lane[i],lane[i+1],e,
00121                                       c.knots[lane_map[i]],
00122                                       c.knots[lane_map[i+1]],
00123                                       c, true,
00124                                       0,0,lc,
00125                                       0,0,rc);
00126                     }
00127                 }
00128               
00129               // Set up new lane
00130               lane.clear();
00131               lane_pt.clear();
00132               lane_map.clear();
00133               
00134               lane.push_back(w1);
00135               Point2f pt1(w1.map.x,w1.map.y);
00136               lane_pt.push_back(pt1);
00137               lane_map.push_back(lane_pt.size()-1);
00138             }
00139           
00140           // Fill in rest of lane
00141           lane.push_back(w2);
00142           Point2f pt2(w2.map.x,w2.map.y);
00143           lane_pt.push_back(pt2);
00144           lane_map.push_back(lane_pt.size()-1);
00145           
00146           prev_lane=w2.id;
00147         }
00148       else if (!w1.is_spot &&
00149                !w2.is_spot &&
00150                (!w1.is_perimeter || !w2.is_perimeter) &&
00151                (w1.id.seg != w2.id.seg ||
00152                 w1.id.lane == w2.id.lane))
00153         // Transition; 
00154         {
00155           if (lane.size()>1)
00156             // Process out previous lane if one exists
00157             {
00158               Point2f diff_pt=lane_pt[1]-lane_pt[0];
00159               Point2f diff_pt2=lane_pt[lane_pt.size()-1]-
00160                 lane_pt[lane_pt.size()-2];
00161               
00162               SmoothCurve c=
00163                 SmoothCurve(lane_pt,
00164                             atan2f(diff_pt[1],diff_pt[0]), 1,
00165                             atan2f(diff_pt2[1],diff_pt2[0]), 1);
00166               
00167               for (uint i=0;i<lane.size()-1;i++)
00168                 MakeLanePolygon(lane[i],lane[i+1],e,
00169                                 c.knots[lane_map[i]],
00170                                 c.knots[lane_map[i+1]],
00171                                 c, true,
00172                                 0,0,lc,0,0,rc);
00173             }
00174           
00175           // Make transition polygons
00176           lane.clear();
00177           lane_pt.clear();
00178           lane_map.clear();       
00179 
00180           int base_ind=0;
00181           
00182           // Exits have 2 waypoints, but curves need 3 or more.  Go
00183           // get neighboring waypoints to entrance and transition if
00184           // they exist.
00185           
00186           ElementID pre=w1.id;
00187           pre.pt--;
00188           
00189           WayPointNode* w0=graph->get_node_by_id(pre);
00190           
00191           if (w0 != NULL && w0->id.lane != 0) {
00192             lane.push_back(*w0);
00193             Point2f pt(w0->map.x,w0->map.y);
00194             lane_pt.push_back(pt);
00195             lane_map.push_back(lane_pt.size()-1);
00196             base_ind=1;
00197           }
00198           
00199           // Push 2 (or 3 or 4 if found) waypoints onto list to find
00200           // curve.
00201           lane.push_back(w1);
00202           Point2f pt1(w1.map.x,w1.map.y);
00203           lane_pt.push_back(pt1);
00204           lane_map.push_back(lane_pt.size()-1);   
00205 
00206           lane.push_back(w2);
00207           Point2f pt2(w2.map.x,w2.map.y);
00208           lane_pt.push_back(pt2);
00209           lane_map.push_back(lane_pt.size()-1);   
00210 
00211           ElementID post=w2.id;
00212           post.pt++;
00213           
00214           WayPointNode* w3=graph->get_node_by_id(post);
00215           
00216           if (w3!=NULL && w3->id.lane != 0)
00217             {
00218               lane.push_back(*w3);
00219               Point2f pt(w3->map.x,w3->map.y);
00220               lane_pt.push_back(pt);
00221               lane_map.push_back(lane_pt.size()-1);
00222             }
00223           
00224           
00225           if (lane_pt.size()<2)
00226             {
00227               WayPointNode w4=w2;
00228               w4.map=w2.map+w2.map-w1.map;
00229               lane.push_back(w4);
00230               Point2f pt(w4.map.x,w4.map.y);
00231               lane_pt.push_back(pt);
00232               lane_map.push_back(lane_pt.size()-1);
00233             }
00234             
00235           
00236           Point2f diff_pt=lane_pt[1]-lane_pt[0];
00237           Point2f diff_pt2=lane_pt[lane_pt.size()-1]-
00238             lane_pt[lane_pt.size()-2];
00239           
00240           SmoothCurve c=
00241             SmoothCurve(lane_pt,
00242                         atan2f(diff_pt[1],diff_pt[0]), 1,
00243                         atan2f(diff_pt2[1],diff_pt2[0]), 1);
00244           
00245           MakeTransitionPolygon(lane[base_ind],lane[base_ind+1],e,
00246                                 c.knots[lane_map[base_ind]],c.knots[lane_map[base_ind+1]],c);
00247           
00248           // Clear out transition
00249           lane.clear();
00250           lane_pt.clear();
00251           lane_map.clear();
00252           prev_lane=ElementID();
00253         }
00254     }
00255   
00256   // If last lane info is still around, process it
00257   if (lane.size()>1)
00258     {
00259       // Clear out previous lane
00260       Point2f diff_pt=lane_pt[1]-lane_pt[0];
00261       Point2f diff_pt2=lane_pt[lane_pt.size()-1]-
00262         lane_pt[lane_pt.size()-2];
00263       SmoothCurve c=
00264         SmoothCurve(lane_pt,
00265                     atan2f(diff_pt[1],diff_pt[0]), 1,
00266                     atan2f(diff_pt2[1],diff_pt2[0]), 1);
00267       
00268       for (uint i=0;i<lane.size()-1;i++) {
00269         // Find the edge that links lane[i] and lane[i+1]
00270         WayPointEdge e;
00271         for(uint j = 0; j < graph->edges_size; j++)
00272           { 
00273             e=graph->edges[j];
00274             
00275             if(graph->nodes[e.startnode_index].id.pt==lane[i].id.pt
00276                && graph->nodes[e.startnode_index].id.lane==lane[i].id.lane
00277                && graph->nodes[e.startnode_index].id.seg==lane[i].id.seg
00278                && graph->nodes[e.endnode_index].id.pt==lane[i+1].id.pt
00279                && graph->nodes[e.endnode_index].id.lane==lane[i+1].id.lane
00280                && graph->nodes[e.endnode_index].id.seg==lane[i+1].id.seg)
00281               
00282               break;
00283           }
00284         
00285         MakeLanePolygon(lane[i],lane[i+1],e,
00286                         c.knots[lane_map[i]],
00287                         c.knots[lane_map[i+1]],
00288                         c, true,0,0,lc,0,0,rc);
00289       }
00290     }
00291   
00292   // Set up new lane
00293   lane.clear();
00294   lane_pt.clear();
00295   lane_map.clear();
00296 }
00297 
00298 void MapLanes::SetFilteredPolygons()
00299 {
00300   for (int i=0; i<(int)allPolys.size(); i++)
00301     {
00302       FilteredPolygon p;
00303       p.SetPolygon(allPolys.at(i));
00304       filtPolys.push_back(p);
00305     }
00306 
00307   #ifdef DEBUGMAP
00308   for (int i=0; i<(int)filtPolys.size(); i++) {
00309     WritePolygonToDebugFile(i); 
00310   }
00311   #endif
00312 }
00313 
00314 poly MapLanes::build_waypoint_poly(const WayPointNode& w1,
00315                                    const WayPointEdge& e,
00316                                    const Point2f& _pt,                    
00317                                    float time,
00318                                    SmoothCurve& c)
00319 {
00320   // Given a waypoint location, make a plygon 1 meter deep around the
00321   // waypoint.
00322 
00323   rotate_translate_transform trans;
00324   posetype origin(0,0,0);
00325   
00326   
00327   Point2f back_pt=c.evaluatePoint(time+way_poly_size);
00328   Point2f front_pt=c.evaluatePoint(time-way_poly_size);
00329   Point2f pt=_pt;
00330 
00331   //  static Point2f defaultpt;
00332 
00333   // If time <0 or > maxtime, find it and fix it.
00334   if (back_pt[0]==pt[0] &&
00335       back_pt[1]==pt[1])
00336     back_pt=pt+(pt-front_pt);
00337 
00338   if (front_pt[0]==pt[0] &&
00339       front_pt[1]==pt[1])
00340     front_pt=pt-(back_pt-pt);
00341       
00342 
00343   Point2f diff_back=back_pt-pt;
00344   Point2f diff_front=pt-front_pt;
00345   
00346   float back_angle=atan2f(diff_back[1],diff_back[0]);
00347   float front_angle=atan2f(diff_front[1],diff_front[0]);
00348 
00349   // lane
00350   posetype refway1(back_pt[0],back_pt[1],back_angle);
00351   trans.find_transform(origin,refway1);
00352   posetype p2=trans.apply_transform(posetype(0, w1.lane_width/2,0));
00353   posetype p3=trans.apply_transform(posetype(0,-w1.lane_width/2,0));
00354 
00355   posetype refway2(front_pt[0],front_pt[1],front_angle);
00356   trans.find_transform(origin,refway2);
00357   posetype p1=trans.apply_transform(posetype(0, w1.lane_width/2,0));
00358   posetype p4=trans.apply_transform(posetype(0,-w1.lane_width/2,0));
00359   
00360   poly newPoly;
00361   
00362   newPoly.p1.x=p1.x;
00363   newPoly.p1.y=p1.y;
00364   
00365   newPoly.p2.x=p2.x;
00366   newPoly.p2.y=p2.y;      
00367   
00368   newPoly.p3.x=p3.x;
00369   newPoly.p3.y=p3.y;      
00370   
00371   newPoly.p4.x=p4.x;
00372   newPoly.p4.y=p4.y;      
00373   
00374   // Update some details
00375   newPoly.start_way.seg = w1.id.seg;
00376   newPoly.start_way.lane = w1.id.lane;
00377   newPoly.start_way.pt = w1.id.pt;
00378   newPoly.end_way.seg = w1.id.seg;
00379   newPoly.end_way.lane = w1.id.lane;
00380   newPoly.end_way.pt = w1.id.pt;
00381   newPoly.is_stop = w1.is_stop;
00382   newPoly.is_transition = false;
00383   newPoly.contains_way = true;
00384 #if 0 //TODO
00385   newPoly.left_boundary.lane_marking = e.left_boundary.lane_marking;
00386   newPoly.right_boundary.lane_marking = e.right_boundary.lane_marking;
00387 #endif
00388 
00389   // set initial heading based on map coordinates
00390   newPoly.heading = ops.PolyHeading(newPoly);
00391   MapXY cpt = ops.centerpoint(newPoly);
00392   newPoly.midpoint.x = cpt.x;
00393   newPoly.midpoint.y = cpt.y;
00394   newPoly.length = ops.getLength(newPoly);
00395   return newPoly;
00396 }
00397 
00398 
00399 void MapLanes::MakeLanePolygon(WayPointNode w1, WayPointNode w2,
00400                                WayPointEdge e,
00401                                float time1, float time2,
00402                                SmoothCurve& c,
00403                                bool new_edge,
00404                                float ltime1, float ltime2,
00405                                SmoothCurve& lc,
00406                                float rtime1, float rtime2,
00407                                SmoothCurve& rc)
00408 {
00409 
00410   if (time2 <= time1)
00411     return;
00412 
00413   // Not necessary because of how this is called, but do this just in
00414   // case
00415   if (poly_id_counter==0)
00416     new_edge=true;
00417 
00418   Point2f w1_pt=c.evaluatePoint(time1);
00419   Point2f w2_pt=c.evaluatePoint(time2);
00420 
00421 
00422 
00423   
00424   // If new edge (not called recursively)
00425   if (new_edge) 
00426     {
00427       poly poly_w1;
00428 
00429       // if new lane, make polygon for start node
00430       if (poly_id_counter == 0 ||
00431           ElementID(allPolys[poly_id_counter-1].end_way) != w1.id)
00432         {
00433           poly_w1=build_waypoint_poly(w1, e, w1_pt, time1, c);  
00434           
00435           poly_w1.poly_id = poly_id_counter;
00436           
00437           // Add the poly to the list
00438           poly_id_counter++;
00439           allPolys.push_back(poly_w1);
00440           
00441         }
00442       else  
00443         // If not a new lane, get lane waypoint, which should be last
00444         // one pushed onto list
00445         poly_w1=allPolys[poly_id_counter-1];
00446       
00447       // Make new polygon around second waypoint, but only add it
00448       // after recursive all for intermediate polygons
00449       poly poly_w2=build_waypoint_poly(w2,e, w2_pt, time2, c);
00450       
00451 
00452       time1=time1+way_poly_size;      
00453       time2=time2-way_poly_size;
00454 
00455       w1.map=ops.midpoint(poly_w1.p2,poly_w1.p3);
00456       w2.map=ops.midpoint(poly_w2.p1,poly_w2.p4);
00457 
00458       float edist=
00459         Euclidean::DistanceTo(w1.map,w2.map);
00460 
00461       float cdist = fmax(time2-time1,0.0);
00462 
00463       // Only fill rest of lane if two waypoints are further than
00464       // their 1 meter polygons -- probably always true
00465       if (cdist>Epsilon::float_value && edist>Epsilon::float_value)
00466         {
00467           std::vector<Point2f> left_curve_pts;
00468           left_curve_pts.push_back(Point2f(poly_w1.p2.x,poly_w1.p2.y));
00469           left_curve_pts.push_back(Point2f(poly_w2.p1.x,poly_w2.p1.y));
00470 
00471     bool straight=false;
00472 //     #ifdef NQE
00473 //       if (poly_w1.start_way.seg==14 || poly_w1.start_way.seg==15 || poly_w1.start_way.seg==16 || poly_w1.start_way.seg==17) straight=true;
00474 //     #endif
00475     if (straight) {
00476       lc= 
00477             SmoothCurve(left_curve_pts,
00478                         atan2f(poly_w1.p2.y-poly_w1.p1.y,
00479                                poly_w1.p2.x-poly_w1.p1.x), 1,
00480                         atan2f(poly_w1.p2.y-poly_w1.p1.y,
00481                                poly_w1.p2.x-poly_w1.p1.x), 1);
00482           
00483           ltime1=lc.knots[0];
00484           ltime2=lc.knots[1];
00485 
00486           std::vector<Point2f> right_curve_pts;
00487           right_curve_pts.push_back(Point2f(poly_w1.p3.x,poly_w1.p3.y));
00488           right_curve_pts.push_back(Point2f(poly_w2.p4.x,poly_w2.p4.y));
00489 
00490           rc=
00491             SmoothCurve(right_curve_pts,
00492                         atan2f(poly_w1.p3.y-poly_w1.p4.y,
00493                                poly_w1.p3.x-poly_w1.p4.x), 1,
00494                         atan2f(poly_w1.p3.y-poly_w1.p4.y,
00495                                poly_w1.p3.x-poly_w1.p4.x), 1);
00496           
00497       } else {
00498           lc= 
00499             SmoothCurve(left_curve_pts,
00500                         atan2f(poly_w1.p2.y-poly_w1.p1.y,
00501                                poly_w1.p2.x-poly_w1.p1.x), 1,
00502                         atan2f(poly_w2.p2.y-poly_w2.p1.y,
00503                                poly_w2.p2.x-poly_w2.p1.x), 1);
00504           
00505           ltime1=lc.knots[0];
00506           ltime2=lc.knots[1];
00507 
00508           std::vector<Point2f> right_curve_pts;
00509           right_curve_pts.push_back(Point2f(poly_w1.p3.x,poly_w1.p3.y));
00510           right_curve_pts.push_back(Point2f(poly_w2.p4.x,poly_w2.p4.y));
00511 
00512           rc=
00513             SmoothCurve(right_curve_pts,
00514                         atan2f(poly_w1.p3.y-poly_w1.p4.y,
00515                                poly_w1.p3.x-poly_w1.p4.x), 1,
00516                         atan2f(poly_w2.p3.y-poly_w2.p4.y,
00517                                poly_w2.p3.x-poly_w2.p4.x), 1);
00518           
00519     }
00520           rtime1=rc.knots[0];
00521           rtime2=rc.knots[1];
00522 
00523           //Make rest of lane
00524           WayPointNode midpoint=w2;
00525           midpoint.lane_width=(w1.lane_width + w2.lane_width)/2.0;
00526           
00527           // Split rest of edge in half and call recursively.
00528           midpoint.map=ops.midpoint(w1.map,w2.map);
00529           
00530           float midtime=(time1+time2)/2;
00531 
00532           float lmidtime=(ltime1+ltime2)/2;
00533           float rmidtime=(rtime1+rtime2)/2;
00534 
00535           MakeLanePolygon(w1, midpoint, e, time1, midtime, c, false,
00536                           ltime1, lmidtime, lc, rtime1, rmidtime, rc);
00537           
00538           midpoint.id=w1.id;
00539           
00540           MakeLanePolygon(midpoint, w2, e, midtime, time2, c, false,
00541                           lmidtime, ltime2, lc, rmidtime, rtime2, rc);
00542         }
00543       
00544       // Now add final waypoint polygon
00545       poly_w2.poly_id = poly_id_counter;
00546       
00547       // Force last polygon before waypoint to touch waypoint.
00548       allPolys[poly_id_counter-1].p2=poly_w2.p1;
00549       allPolys[poly_id_counter-1].p3=poly_w2.p4;
00550 
00551       // Add the poly to the list
00552       poly_id_counter++;
00553       allPolys.push_back(poly_w2);
00554 
00555     }
00556   else 
00557     // w1 and w2 aren't original waypoints for the edge -- this is a
00558     // recursive call.
00559     {
00560       
00561       //sqrtf(powf(w1_pt[0]-w2_pt[0],2) +
00562       //                          powf(w1_pt[1]-w2_pt[1],2));
00563       
00564       // After recursive calls, eventaully get two points really
00565       // close.  Closeness is determined by comparing Euclidean
00566       // distance to curve distance.
00567       
00568       float edist=
00569         Euclidean::DistanceTo(w1.map,w2.map);
00570 
00571       float cdist = fmax(time2-time1,0.0);
00572 
00573       if(Epsilon::equal(cdist,0.0) ||
00574          Epsilon::equal(edist,0.0) ||
00575          cdist <= max_poly_size*edist/cdist)
00576         {
00577                   
00578           poly newPoly;
00579           
00580           // Create the edges of the poly
00581           newPoly.p1=allPolys[poly_id_counter-1].p2;
00582           newPoly.p4=allPolys[poly_id_counter-1].p3;
00583 
00584           Point2f point2=lc.evaluatePoint(ltime2);
00585           Point2f point3=rc.evaluatePoint(rtime2);
00586 
00587           newPoly.p2.x=point2[0];
00588           newPoly.p2.y=point2[1];
00589           
00590           newPoly.p3.x=point3[0];
00591           newPoly.p3.y=point3[1];
00592           
00593 
00594           // Update some details
00595           newPoly.start_way.seg = w1.id.seg;
00596           newPoly.start_way.lane = w1.id.lane;
00597           newPoly.start_way.pt = w1.id.pt;
00598           newPoly.end_way.seg = w2.id.seg;
00599           newPoly.end_way.lane = w2.id.lane;
00600           newPoly.end_way.pt = w2.id.pt;
00601           newPoly.is_stop = false;
00602           newPoly.is_transition = false;
00603           newPoly.contains_way = false;
00604           newPoly.poly_id = poly_id_counter;
00605 #if 0 //TODO
00606           newPoly.left_boundary=e.left_boundary;
00607           newPoly.right_boundary=e.right_boundary;
00608 #endif
00609           
00610           // set initial heading based on map coordinates
00611           newPoly.heading = ops.PolyHeading(newPoly);
00612           MapXY cpt = ops.centerpoint(newPoly);
00613           newPoly.midpoint.x = cpt.x;
00614           newPoly.midpoint.y = cpt.y;
00615           newPoly.length = ops.getLength(newPoly);
00616 
00617           // Add the poly to the list
00618           poly_id_counter++;
00619           allPolys.push_back(newPoly);
00620           
00621         }
00622       else
00623         {
00624           // Make sure points are on curve, they were paased in as
00625           // Euclidean midpoints i nthe recursive call.
00626           w1.map.x=w1_pt[0];
00627           w1.map.y=w1_pt[1];
00628           w2.map.x=w2_pt[0];
00629           w2.map.y=w2_pt[1];
00630           
00631           WayPointNode midpoint=w2;
00632           midpoint.lane_width=(w1.lane_width + w2.lane_width)/2.0;
00633           
00634           // Make midpoint for recursive call.
00635           midpoint.map.x=(w1.map.x+w2.map.x)/2;
00636           midpoint.map.y=(w1.map.y+w2.map.y)/2;
00637 
00638           float midtime=(time2+time1)/2;
00639           float lmidtime=(ltime1+ltime2)/2;
00640           float rmidtime=(rtime1+rtime2)/2;
00641 
00642           MakeLanePolygon(w1, midpoint, e, time1, midtime, c, false,
00643                           ltime1,lmidtime,lc,rtime1,rmidtime,rc);
00644           midpoint.id=w1.id;
00645           MakeLanePolygon(midpoint, w2, e, midtime, time2, c, false,
00646                           lmidtime,ltime2,lc,rmidtime,rtime2,rc);
00647         }
00648     }
00649 }
00650 
00651 
00652 void MapLanes::MakeTransitionPolygon(WayPointNode w1, WayPointNode w2,
00653                                      WayPointEdge e,
00654                                      float time1, float time2,
00655                                      SmoothCurve& c)
00656 {
00657   // Assumes Transition edges come after lane edges in the edge list
00658   poly poly_w1, poly_w2;
00659   
00660   int index_w1=-1;
00661   int index_w2=-1;
00662 
00663   //Transitions assume that 1 meter polygons around waypoints exist.  Look
00664   //them up so the new polygons can be attached at the ends.
00665 
00666   for (uint i=0; i<allPolys.size() && (index_w1<0 || index_w2<0); i++) 
00667     if (allPolys[i].contains_way) {
00668       if (ElementID(allPolys[i].start_way) == w1.id) 
00669         {
00670           poly_w1=allPolys[i];
00671           index_w1=i;
00672         }
00673       else if (ElementID(allPolys[i].start_way) == w2.id)
00674         {
00675           poly_w2=allPolys[i];
00676           index_w2=i;
00677         }
00678     }
00679 
00680   if (index_w1<0)
00681     {
00682       Point2f w1_pt=c.evaluatePoint(time1);
00683       poly_w1=build_waypoint_poly(w1, e, w1_pt, time1, c);  
00684     }
00685   
00686   time1+=way_poly_size;
00687   
00688   if (index_w2<0) 
00689     {
00690       Point2f w2_pt=c.evaluatePoint(time2);
00691       poly_w2=build_waypoint_poly(w2, e, w2_pt, time2, c);  
00692       
00693     }
00694   
00695   time2-=way_poly_size;  
00696   
00697   float cdist = fmax(0.0,time2-time1);
00698 
00699   w1.map=ops.midpoint(poly_w1.p2,poly_w1.p3);
00700   w2.map=ops.midpoint(poly_w2.p1,poly_w2.p4);
00701   
00702   float edist=
00703     Euclidean::DistanceTo(w1.map,w2.map);
00704   
00705   
00706   // Probably always true, but is two waypoints are REALLY close
00707   // attach them and quit (in ELSE).
00708   if (cdist>Epsilon::float_value && edist>Epsilon::float_value)
00709     {
00710       // Do recursive call on transition the same way we do on and edge in a
00711       // lane.
00712 
00713           std::vector<Point2f> left_curve_pts;
00714           left_curve_pts.push_back(Point2f(poly_w1.p2.x,poly_w1.p2.y));
00715           left_curve_pts.push_back(Point2f(poly_w2.p1.x,poly_w2.p1.y));
00716 
00717           SmoothCurve lc=
00718             SmoothCurve(left_curve_pts,
00719                         atan2f(poly_w1.p2.y-poly_w1.p1.y,
00720                                poly_w1.p2.x-poly_w1.p1.x), 1,
00721                         atan2f(poly_w2.p2.y-poly_w2.p1.y,
00722                                poly_w2.p2.x-poly_w2.p1.x), 1);
00723           
00724           float ltime1=lc.knots[0];
00725           float ltime2=lc.knots[1];
00726 
00727           std::vector<Point2f> right_curve_pts;
00728           right_curve_pts.push_back(Point2f(poly_w1.p3.x,poly_w1.p3.y));
00729           right_curve_pts.push_back(Point2f(poly_w2.p4.x,poly_w2.p4.y));
00730 
00731           SmoothCurve rc=
00732             SmoothCurve(right_curve_pts,
00733                         atan2f(poly_w1.p3.y-poly_w1.p4.y,
00734                                poly_w1.p3.x-poly_w1.p4.x), 1,
00735                         atan2f(poly_w2.p3.y-poly_w2.p4.y,
00736                                poly_w2.p3.x-poly_w2.p4.x), 1);
00737           
00738           
00739           float rtime1=rc.knots[0];
00740           float rtime2=rc.knots[1];
00741 
00742           //Make rest of lane
00743           WayPointNode midpoint=w2;
00744           midpoint.lane_width=(w1.lane_width + w2.lane_width)/2.0;
00745           
00746           // Split rest of edge in half and call recursively.
00747           midpoint.map=ops.midpoint(w1.map,w2.map);
00748           
00749           float midtime=(time1+time2)/2;
00750           float lmidtime=(ltime1+ltime2)/2;
00751           float rmidtime=(rtime1+rtime2)/2;
00752 
00753           int next_polyid=poly_id_counter;
00754 
00755           e.left_boundary=UNDEFINED;
00756           e.right_boundary=UNDEFINED;
00757 
00758           transition=true;
00759           trans_index=index_w1;
00760 
00761           MakeLanePolygon(w1, midpoint, e, time1, midtime, c, false,
00762                           ltime1,lmidtime,lc,rtime1,rmidtime,rc);
00763           
00764           // Ensure start of transition attaches to polygon around starting
00765           // waypoint.
00766           
00767           // Remove for end of transition polygons      
00768           allPolys[next_polyid].p1=poly_w1.p2;
00769           allPolys[next_polyid].p4=poly_w1.p3;
00770           
00771           midpoint.id=w1.id;
00772 
00773           MakeLanePolygon(midpoint, w2, e,  midtime, time2, c, false,
00774                           lmidtime,ltime2,lc,rmidtime,rtime2,rc);
00775           // Ensure end of transition attaches to polygon around ending
00776           // waypoint.
00777           
00778           // Remove for end of transition polygons      
00779           allPolys[poly_id_counter-1].p2=poly_w2.p1;
00780           allPolys[poly_id_counter-1].p3=poly_w2.p4;
00781           
00782           for (uint i=next_polyid; i<allPolys.size(); i++)
00783             allPolys[i].is_transition=true;
00784     }
00785   else
00786     if (index_w1 >=0 && index_w2 >=0)
00787       {
00788         allPolys[index_w1].p2=allPolys[index_w2].p1;
00789         allPolys[index_w1].p3=allPolys[index_w2].p4;
00790       }
00791 }
00792 
00797 int MapLanes::getAllLanes(art_msgs::ArtLanes *lanes)
00798 {
00799   lanes->polygons.clear();
00800 
00801   for(unsigned int i = 0; i < filtPolys.size(); i++)
00802     {
00803       art_msgs::ArtQuadrilateral temp = filtPolys.at(i).GetQuad();
00804       lanes->polygons.push_back(temp);
00805     }
00806 
00807   return lanes->polygons.size();
00808 }
00809 
00810 
00811 int MapLanes::getLanes(art_msgs::ArtLanes *lanes, MapXY here)
00812 {
00813   if (range < 0)
00814     return getAllLanes(lanes);
00815 
00816   lanes->polygons.clear();
00817 
00818   for(unsigned int i = 0; i < filtPolys.size(); i++)
00819     {
00820       art_msgs::ArtQuadrilateral temp = filtPolys.at(i).GetQuad();
00821       float dist = Euclidean::DistanceTo(MapXY(temp.midpoint), here);
00822       
00823       if(dist <= range)
00824         {
00825           lanes->polygons.push_back(temp);
00826           allPolys[i] = poly(temp);
00827         }
00828     }
00829 
00830   ROS_DEBUG_STREAM("found " << lanes->polygons.size()
00831                    << " polygons within " << range
00832                    << " meters of (" << here.x << "," << here.y << ")");
00833 
00834   return 0;
00835 }
00836 
00837 int MapLanes::getVisionLanes(art_msgs::ArtLanes *lanes, float x, float y,
00838                              float heading)
00839 {
00840   if (range < 0)
00841     return getAllLanes(lanes);
00842 
00843   lanes->polygons.clear();
00844 
00845   int index = ops.getContainingPoly(allPolys,x,y);
00846   if (index < 0)
00847     {
00848       return 0;
00849     }
00850 
00851   poly current = allPolys.at(index);
00852   for(unsigned int i = 0; i < filtPolys.size(); i++)
00853     {
00854       art_msgs::ArtQuadrilateral temp = filtPolys.at(i).GetQuad();
00855 
00856       if (temp.start_way.lane != current.start_way.lane
00857           || temp.start_way.seg != current.start_way.seg
00858           || temp.is_transition
00859           || temp.contains_way)
00860         continue;
00861 
00862     float angle=AngleFromXY(x,y,heading,temp.midpoint.x,temp.midpoint.y);
00863     float dist=DistFromXY(x,y,temp.midpoint.x,temp.midpoint.y);
00864   
00865     if((dist <= range) && (dist>10.0) && (fabs(angle)<DEG_T_RAD*25))
00866       {
00867         lanes->polygons.push_back(temp);
00868       }  
00869     }
00870 
00871   //testDraw();
00872   return 0;
00873 }
00874 
00875 void MapLanes::UpdatePoly(polyUpdate upPoly, float rrX, float rrY, float rrOri)
00876 {
00877   if (upPoly.poly_id <=0 || upPoly.poly_id>=(int)filtPolys.size()) {
00878     return;
00879   }
00880   if (upPoly.distance<3.0) return;
00881   FilteredPolygon* filt=&(filtPolys.at(upPoly.poly_id));
00882   poly curr=filt->GetPolygon();
00883   
00884   // Don't break waypoints !
00885   if (upPoly.poly_id <=0 || upPoly.poly_id>=(int)filtPolys.size()) {
00886     return;
00887   }
00888   //printf("Good %i \n",upPoly.poly_id);
00889 
00890   //printf("1 %i %lf %lf\n",upPoly.poly_id,upPoly.distance,upPoly.bearing);
00891   poly prev=(filtPolys.at(upPoly.poly_id-1)).GetPolygon();
00892   poly next=(filtPolys.at(upPoly.poly_id+1)).GetPolygon();
00893   // Don't update the bottom points if they touch a waypoint
00894   if (prev.contains_way && (upPoly.point_id==0 || upPoly.point_id==3)) return;
00895   // Don't update the top points if they touch a waypoint
00896   if (next.contains_way && (upPoly.point_id==1 || upPoly.point_id==2)) return;
00897 
00898   //static gaussian g1(0.0,3.0);
00899   //upPoly.distance=upPoly.distance+g1.get_sample_1D();
00900   filt->UpdatePoint(upPoly.point_id,upPoly.distance,upPoly.bearing,upPoly.confidence,rrX,rrY,Normalise_PI(rrOri+PI));
00901   
00902   #ifdef DEBUGMAP
00903    WritePolygonToDebugFile(upPoly.poly_id);
00904   #endif
00905 
00906   int point=0;
00907   if ((upPoly.point_id==0 || upPoly.point_id==3) && curr.poly_id==prev.poly_id+1 && curr.start_way.lane==prev.start_way.lane && curr.start_way.seg==prev.start_way.seg) {
00908      FilteredPolygon* temp=&(filtPolys.at(prev.poly_id));
00909      if (upPoly.point_id==0) point=1; 
00910      if (upPoly.point_id==3) point=2;
00911      temp->UpdatePoint(point,upPoly.distance,upPoly.bearing,upPoly.confidence,rrX,rrY,Normalise_PI(rrOri+PI));
00912      
00913      #ifdef DEBUGMAP
00914        WritePolygonToDebugFile(prev.poly_id);
00915      #endif
00916   }
00917   if ((upPoly.point_id==1 || upPoly.point_id==2) && curr.poly_id==next.poly_id-1 && curr.start_way.lane==next.start_way.lane && curr.start_way.seg==next.start_way.seg) {
00918      FilteredPolygon* temp=&(filtPolys.at(next.poly_id));
00919      if (upPoly.point_id==1) point=0; 
00920      if (upPoly.point_id==2) point=3;
00921      temp->UpdatePoint(point,upPoly.distance,upPoly.bearing,upPoly.confidence,rrX,rrY,Normalise_PI(rrOri+PI));
00922   
00923      #ifdef DEBUGMAP
00924        WritePolygonToDebugFile(next.poly_id);
00925      #endif
00926    }
00927 }
00928 
00929 void MapLanes::UpdateWithCurrent(int i){
00930   static gaussian g1(0.0,1.0);
00931   FilteredPolygon* filt=&(filtPolys.at(i));
00932   poly temp2 = filtPolys.at(i).GetPolygon();
00933   if (temp2.is_transition || temp2.contains_way) return;
00934 
00935   float angle=AngleFromXY(rX,rY,rOri,temp2.p1.x,temp2.p1.y);
00936   float distU=DistFromXY(rX,rY,temp2.p1.x,temp2.p1.y);
00937   if (distU>5 && distU<80 && fabs(angle) < 0.2) filt->UpdatePoint(0,distU+g1.get_sample_1D(),angle,1.0,rX,rY,rOri);
00938       
00939   angle=AngleFromXY(rX,rY,rOri,temp2.p2.x,temp2.p2.y);
00940   distU=DistFromXY(rX,rY,temp2.p2.x,temp2.p2.y);
00941   if (distU>5 && distU<80 && fabs(angle) < 0.2) filt->UpdatePoint(1,distU+g1.get_sample_1D(),angle,1.0,rX,rY,rOri);
00942 
00943   angle=AngleFromXY(rX,rY,rOri,temp2.p3.x,temp2.p3.y);
00944   distU=DistFromXY(rX,rY,temp2.p3.x,temp2.p3.y);
00945   if (distU>5 && distU<80 && fabs(angle) < 0.2) filt->UpdatePoint(2,distU+g1.get_sample_1D(),angle,1.0,rX,rY,rOri);     
00946 
00947   angle=AngleFromXY(rX,rY,rOri,temp2.p4.x,temp2.p4.y);
00948   distU=DistFromXY(rX,rY,temp2.p4.x,temp2.p4.y);
00949   if (distU>5 && distU<80 && fabs(angle) < 0.2) filt->UpdatePoint(3,distU+g1.get_sample_1D(),angle,1.0,rX,rY,rOri);
00950 }
00951 
00952 
00953 void MapLanes::testDraw(bool with_trans)
00954 {
00955   ZonePerimeterList empty_zones;
00956   MapLanes::testDraw(with_trans, empty_zones);
00957 }
00958 
00959 //test function which outputs all polygons to a pgm image.
00960 void MapLanes::testDraw(bool with_trans, const ZonePerimeterList &zones)
00961 {
00962   float max_x = -FLT_MAX;
00963   float min_x = FLT_MAX;
00964   float max_y = -FLT_MAX;
00965   float min_y = FLT_MAX;
00966   
00967   FILE* gpsFile = fopen("gps.kml","wb");
00968 
00969   fprintf(gpsFile,"<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n <kml xmlns=\"http://earth.google.com/kml/2.2\">\n <Document>\n<name>Maplanes Polygon centres</name>\n<description>Path of the polygons</description>\n<Style id=\"yellowLineGreenPoly\">\n<LineStyle>\n<color>7f00ffff</color>\n<width>4</width>\n</LineStyle>\n<PolyStyle>\n<color>7f00ff00</color>\n</PolyStyle>\n</Style>\n<Placemark>\n<name>Absolute Extruded</name>\n<description>Transparent green wall with yellow outlines</description>\n<styleUrl>#yellowLineGreenPoly</styleUrl>\n<LineString>\n<extrude>1</extrude>\n<tessellate>1</tessellate>\n<altitudeMode>clampToGround</altitudeMode>\n<coordinates>\n");
00970 
00971   float yOff = 7.5;//5.17;
00972   float xOff = 2.9; //2.89;
00973 
00974   for(int i = 0; i < (int)filtPolys.size(); i++)
00975     {
00976       poly node=(filtPolys.at(i).GetPolygon());
00977       double lon,lat;
00978       UTM::UTMtoLL(cY+node.midpoint.y+yOff,cX+node.midpoint.x+xOff,
00979                    "11S",lon,lat);
00980       fprintf(gpsFile,"%lf,%lf,0\n",lat,lon);
00981 
00982       max_x=fmax(fmax(fmax(fmax(node.p1.x,node.p2.x),
00983                            node.p3.x), node.p4.x),max_x);
00984       max_y=fmax(fmax(fmax(fmax(node.p1.y,node.p2.y),
00985                            node.p3.y),node.p4.y),max_y);
00986       min_x=fmin(fmin(fmin(fmin(node.p1.x,node.p2.x),
00987                            node.p3.x),node.p4.x),min_x);
00988       min_y=fmin(fmin(fmin(fmin(node.p1.y,node.p2.y),
00989                            node.p3.y),node.p4.y),min_y);
00990     }
00991   fprintf(gpsFile,
00992           "</coordinates>\n</LineString>\n</Placemark>\n</Document>\n</kml>");
00993   fclose(gpsFile);
00994 
00995   // find bounding box for coordinates of all way-points in graph
00996   for(uint i = 0; i < graph->nodes_size; i++)
00997     {
00998       max_x=fmax(graph->nodes[i].map.x,max_x);
00999       max_y=fmax(graph->nodes[i].map.y,max_y);
01000       min_x=fmin(graph->nodes[i].map.x,min_x);
01001       min_y=fmin(graph->nodes[i].map.y,min_y);
01002     }
01003 
01004   int xsize=(int)ceil(max_x - min_x);
01005   int ysize=(int)ceil(max_y - min_y);
01006   
01007   if (xsize < 240)
01008     {
01009       min_x -= (240-xsize)/2;
01010       max_x += (240-xsize)/2;
01011       xsize=240;
01012     }
01013   
01014   if (ysize < 168)
01015     {
01016       min_y -= (168-ysize)/2;
01017       max_y += (168-ysize)/2;
01018       ysize=168;
01019     }
01020 
01021   float ratio=3;
01022   float image_size=xsize*ysize*DEFAULT_RATIO*DEFAULT_RATIO;
01023   if (image_size > (2048.0*2048))
01024     ratio=sqrtf((2047*2047.0)/(xsize*ysize));
01025 
01026   std::cerr << "World size: "<<xsize<<","<<ysize<<std::endl;
01027   std::cerr << "Image size: "<<xsize*ratio<<","<<ysize*ratio<<std::endl;
01028   
01029 
01030   //initialize VisualLanes
01031   DrawLanes* edgeImage = new DrawLanes(xsize,ysize,ratio);
01032   DrawLanes* polyImage = new DrawLanes(xsize,ysize,ratio);
01033   
01034   // Add Waypoints to WayPointImage
01035   for(uint i = 0; i < graph->edges_size; i++)
01036     {
01037       WayPointNode w1=graph->nodes[graph->edges[i].startnode_index];
01038       WayPointNode w2=graph->nodes[graph->edges[i].endnode_index];
01039       edgeImage->addTrace(w1.map.x-min_x, max_y-w1.map.y,
01040                           w2.map.x-min_x, max_y-w2.map.y);
01041 
01042     }
01043 
01044   for(uint i = 0; i < graph->nodes_size; i++)      
01045     {
01046       WayPointNode w1=graph->nodes[i];
01047       polyImage->addWay(w1.map.x-min_x, max_y-w1.map.y);
01048     }
01049 
01050 #if 0 //TODO
01051   for(unsigned i = 0; i < zones.size(); i++)
01052     {
01053       polyImage->addZone(zones[i], min_x, max_y);
01054     }
01055 #endif
01056 
01057   //draw polygons
01058   for(int i = 0; i < (int)filtPolys.size(); i++)
01059     {
01060       poly temp = filtPolys.at(i).GetPolygon();
01061       polyImage->addPoly(temp.p1.x-min_x, temp.p2.x-min_x, 
01062                          temp.p3.x-min_x,temp.p4.x-min_x, 
01063                          max_y-temp.p1.y, max_y-temp.p2.y, 
01064                          max_y-temp.p3.y, max_y-temp.p4.y, 
01065                          temp.is_stop, 
01066                          temp.is_transition && !with_trans);
01067     }
01068   bool drawRobot=false;
01069   if (drawRobot) polyImage->addRobot(rX-min_x,max_y-rY);
01070   //output image
01071 
01072   ROS_INFO("Writing way-point image");
01073   edgeImage->savePGM("wayImage.ppm");
01074 
01075   char* temp=new char[255];
01076   sprintf(temp,"polyImage%i.ppm",writecounter);
01077   writecounter++;
01078 
01079   ROS_INFO("Writing polygons image");
01080   polyImage->savePGM(temp);
01081   delete temp;
01082 
01083 }
01084 
01085 #ifdef DEBUGMAP
01086 void MapLanes::WritePolygonToDebugFile(int i) {
01087   poly node=(filtPolys.at(i).GetPolygon());
01088   fprintf(debugFile,"%i %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %i %i\n",
01089           i,node.p1.x,node.p1.y,node.p2.x,node.p2.y,
01090           node.p3.x,node.p3.y,node.p4.x,node.p4.y,
01091           node.midpoint.x,node.midpoint.y, node.length,
01092           node.left_boundary,node.right_boundary);    
01093 }
01094 #endif
01095 
01096 bool MapLanes::WriteToFile(char* fName) {
01097   FILE* f = fopen(fName,"wb");
01098   if (f==NULL) {
01099     ROS_WARN("MapLanes::WriteToFile Failed - Can't open file");
01100     return false;
01101   }
01102   int sizeAll = allPolys.size();
01103   int sizeFilt = filtPolys.size();
01104 
01105   int ret=fprintf(f,"%i %i\n",sizeAll,sizeFilt);
01106   if (ret<1) {
01107     ROS_WARN("MapLanes::SaveToFile Failed - Failed size write");
01108     return false;
01109   }
01110   
01111   for(int i = 0; i < sizeAll; i++)
01112   {
01113     ret=fwrite(&(allPolys.at(i)),sizeof(poly),1,f);
01114     if (ret<1) {
01115       ROS_WARN("MapLanes::WriteToFile Failed - Failed poly write");
01116       return false;
01117     }
01118   }  
01119   for(int i = 0; i < sizeFilt; i++)
01120   {
01121     ret=fwrite(&(filtPolys.at(i)),sizeof(FilteredPolygon),1,f);
01122     if (ret<1) {
01123       ROS_WARN("MapLanes::WriteToFile Failed - Failed FilteredPoylgon write");
01124       return false;
01125     }
01126   }
01127   fclose(f);
01128   return true;
01129 }
01130 
01131 bool MapLanes::LoadFromFile(char* fName) {
01132   FILE* f = fopen(fName,"rb");
01133 
01134   if (f==NULL) {
01135     ROS_WARN("MapLanes::LoadFromFile Failed - Can't open file");
01136     return false;
01137   }
01138 
01139   int sizeAll = 0;
01140   int sizeFilt = 0;
01141 
01142   int ret = fscanf(f,"%i %i\n",&sizeAll,&sizeFilt);
01143 
01144   if (ret < 1) {
01145     ROS_WARN("MapLanes::LoadFromFile Failed - Failed size read");
01146     allPolys.clear();
01147     filtPolys.clear();
01148     return false;
01149   }
01150   if (sizeAll < 0 || sizeFilt < 0) {
01151     ROS_WARN("MapLanes::LoadFromFile Failed - Sizes < 0");
01152     allPolys.clear();
01153     filtPolys.clear();
01154     return false;
01155   }
01156  
01157   // Check filesize
01158   fpos_t position;
01159   fgetpos (f, &position);
01160   long now=ftell(f);
01161   fseek (f, 0, SEEK_END);
01162   long size=ftell(f)-now;
01163   fsetpos(f,&position);
01164   int expected=(sizeAll*sizeof(poly)) + (sizeFilt*sizeof(FilteredPolygon));
01165   if (size!=expected) {
01166     ROS_WARN("MapLanes::LoadFromFile Failed - Incorred File Size");
01167     allPolys.clear();
01168     filtPolys.clear();
01169     return false;
01170   }
01171 
01172   allPolys.clear();
01173   filtPolys.clear();
01174 
01175   poly p;
01176   for(int i = 0; i < sizeAll; i++)
01177   {
01178     ret=fread(&p,sizeof(poly),1,f);
01179     if (ret<1) {
01180       ROS_WARN("MapLanes::LoadFromFile Failed - Failed poly read");
01181       allPolys.clear();
01182       filtPolys.clear();
01183       return false;
01184     }
01185     allPolys.push_back(p);
01186   }  
01187   FilteredPolygon fp;
01188   for(int i = 0; i < sizeFilt; i++)
01189   {
01190     ret=fread(&fp,sizeof(FilteredPolygon),1,f);
01191     if (ret<1) {
01192       ROS_WARN("MapLanes::LoadFromFile Failed - Failed FilteredPolygon read");
01193       allPolys.clear();
01194       filtPolys.clear();
01195       return false;
01196     }
01197     filtPolys.push_back(fp);
01198   }
01199   fclose(f);
01200   return true;
01201 }


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