Graph.cc
Go to the documentation of this file.
00001 //Tarun Nimmagadda
00002 //The University of Texas, Austin
00003 
00004 //Defines methods on the Graph Data Structure
00005 
00006 #include <iostream>
00007 #include <float.h>
00008 
00009 #include <art/UTM.h>
00010 #include <art_map/euclidean_distance.h>
00011 #include <art_map/Graph.h>
00012 
00013 WayPointEdgeList Graph::edges_from(const waypt_index_t index) const {
00014   WayPointEdgeList new_edges;
00015   for(uint i=0; i<edges_size; i++) {
00016     if(edges[i].startnode_index == index)
00017       new_edges.push_back(edges[i]);
00018   }
00019   return new_edges;
00020 };
00021 
00022 WayPointEdgeList Graph::edges_leaving_segment(const segment_id_t seg) const {
00023   WayPointEdgeList new_edges;
00024   for(uint i=0; i<edges_size; i++) {
00025     if(get_node_by_index(edges[i].startnode_index)->id.seg == seg)
00026       new_edges.push_back(edges[i]);
00027   }
00028   return new_edges;
00029 };
00030 
00031 WayPointNode* Graph::get_node_by_index(const waypt_index_t index) const {
00032   for(uint i=0; i<nodes_size; i++) 
00033     if(nodes[i].index == index)
00034       return &nodes[i];
00035   return NULL;
00036 };
00037 
00038 WayPointNode* Graph::get_node_by_id(const ElementID id) const {
00039   for(uint i=0; i<nodes_size; i++) {
00040     if(nodes[i].id == id)
00041       return &nodes[i];
00042   }
00043   return NULL;
00044 };
00045 
00046 WayPointNode* Graph::get_closest_node(const MapXY &p) const {
00047   WayPointNode* closest = NULL;
00048   float distance = 0;
00049   float new_distance = 0;
00050   for(uint i=0; i<nodes_size; i++) {
00051     new_distance = Euclidean::DistanceTo(p, nodes[i].map);
00052     if(closest == NULL || new_distance < distance) {
00053       closest = &nodes[i];
00054       distance = new_distance;
00055     }
00056   }
00057   return closest;
00058 };
00059 
00060 WayPointNode* Graph::get_closest_node_within_radius(const MapXY &p) const {
00061   WayPointNode* closest = NULL;
00062   float distance = 0;
00063   float new_distance = 0;
00064   for(uint i=0; i<nodes_size; i++) {
00065     new_distance = Euclidean::DistanceTo(p, nodes[i].map);
00066     if((closest == NULL || new_distance < distance)
00067        && (new_distance < nodes[i].lane_width)) {
00068       closest = &nodes[i];
00069       distance = new_distance;
00070     }
00071   }
00072   return closest;
00073 };
00074 
00075 
00076 /*
00077 ZoneList get_zones(const Graph& graph) {
00078   //    int number_of_zones = get_number_of_zones(graph);
00079   std::pair<int, int> zone_min_max = get_number_of_zones(graph);
00080   int min_zone = zone_min_max.first;
00081   int max_zone = zone_min_max.second;   
00082   ZoneList zonelist;
00083   WayPointNodeList nodelist;
00084   for (int i = min_zone; i <= max_zone; i++){
00085     for(int j=0; j<nodes_size; j++) {
00086       if(nodes[j].id.seg == i && nodes[j].is_perimeter)
00087         nodelist.push_back(nodes[j]);
00088     }
00089     //      std::sort(nodelist.begin(), nodelist.end());
00090     printf("num_points %d\n", nodelist.size());
00091     zonelist.push_back(nodelist);
00092   }
00093   return zonelist;
00094 };
00095 
00096 
00097 std::pair<int, int> get_number_of_zones(const Graph& graph){
00098   int min_zone = INT_MAX;
00099   int max_zone = INT_MIN;
00100   for(int i=0; i<nodes_size; i++) {
00101     if (nodes[i].is_perimeter){
00102       min_zone = std::min(min_zone, (int)nodes[i].id.seg);
00103       max_zone = std::max(max_zone, (int)nodes[i].id.seg);
00104     }
00105   }
00106   //    int number_of_zones = max_zone - min_zone;
00107   //    if (min_zone != 1) printf("Min Zone ID is not 0\n");
00108   //    if (number_of_zones > 0) return number_of_zones;
00109   //    else return 0;
00110   printf("Zones start:%d -> end:%d\n", min_zone, max_zone);
00111   return std::pair<int, int>(min_zone, max_zone);
00112 };
00113 */
00114 
00115 
00116 void Graph::save(const char* fName){
00117   WayPointNode node;
00118   WayPointEdge edge;
00119   FILE* f = fopen(fName,"wb");
00120   fprintf(f, "GRAPH-STATE\n");
00121   fprintf(f, "Node_Number %d\n", nodes_size);
00122   fprintf(f, "Edge_Number %d\n", edges_size);
00123   for (uint i = 0; i < nodes_size; i++){
00124     node = nodes[i];
00125     fprintf(f, "Node ");
00126     //LatLong
00127     fprintf(f, "%.10lf, %.10lf; ", node.ll.latitude, node.ll.longitude);
00128     //MapXY
00129     fprintf(f, "%f, %f; ", node.map.x, node.map.y);
00130     //ElementID
00131     fprintf(f, "%d, %d, %d; ", node.id.seg, node.id.lane, node.id.pt);
00132     //Waypoint Index
00133     fprintf(f, "%d; ", node.index);
00134     //FLAGS
00135     fprintf(f, "%d, %d, %d, %d, %d, %d; ", node.is_entry, node.is_exit, node.is_goal, node.is_spot, node.is_stop, node.is_perimeter);
00136     //Checkpoint ID
00137     fprintf(f, "%d; ", node.checkpoint_id);
00138     //Lane Width
00139     fprintf(f, "%f; ", node.lane_width);
00140     //END NODE
00141     fprintf(f, "\n");
00142     
00143   }
00144   for (uint i = 0; i < edges_size; i++){
00145     edge = edges[i];
00146     fprintf(f, "Edge ");
00147     //Start, End Nodes
00148     fprintf(f, "%d, %d; ", edge.startnode_index, edge.endnode_index);
00149     //Distance
00150     fprintf(f, "%f; ", edge.distance);
00151     //Speeds
00152     fprintf(f, "%f, %f; ", edge.speed_max, edge.speed_min);
00153     //FLAGS
00154     fprintf(f, "%d; ", edge.is_exit);
00155     //Lane Markings
00156     fprintf(f, "%d, %d; ", edge.left_boundary, edge.right_boundary);
00157     //END EDGE
00158     fprintf(f, "\n");
00159   }
00160   fclose (f);
00161 }
00162 
00163 bool Graph::load(const char* fName){
00164   WayPointNode node;
00165   WayPointEdge edge;
00166   clear();
00167   
00168   std::ifstream graph_file;
00169   graph_file.open(fName);
00170   if (!graph_file){
00171     printf("Error in opening Graph Log file\n");
00172     return false;
00173   }
00174   
00175 
00176   int number_of_nodes = 0, number_of_edges = 0;
00177   int line_number = 0, current_node = 0, current_edge = 0;
00178   bool valid = true;
00179   std::string lineread;
00180   while(getline(graph_file, lineread) ) // Read line by line
00181     {
00182       line_number++;
00183       std::string token;
00184       char token_char [lineread.size()+1];
00185       //Read in one line
00186       sscanf(lineread.c_str(), "%s", token_char);
00187       token.assign(token_char);
00188 
00189       //      printf("Token: |%s|\n", token.c_str());
00190       
00191       if (line_number == 1){
00192         if (!(token.compare("GRAPH-STATE") == 0)) return false;
00193       }
00194       
00195       else if (line_number == 2){
00196         if (!(token.compare("Node_Number") == 0)) {return false;}
00197         else {
00198           number_of_nodes = parse_integer(lineread, std::string("Node_Number"), valid);
00199         }
00200         if (!valid) return false;
00201         else {
00202           nodes_size = number_of_nodes;
00203           nodes = new WayPointNode[nodes_size];
00204         }
00205       }
00206       else if (line_number == 3){
00207         if (!(token.compare("Edge_Number") == 0)) {return false;}
00208         else {
00209           number_of_edges = parse_integer(lineread, std::string("Edge_Number"), valid);
00210         }
00211         if (!valid) return false;
00212         else {
00213           edges_size = number_of_edges;
00214           edges.resize(edges_size);
00215         }
00216       }
00217       else if (token.compare("Node") == 0){
00218         current_node++;
00219         node = parse_node(lineread, valid);
00220         if (!valid) return false;
00221         else nodes[current_node-1] = node;
00222       }
00223       else if (token.compare("Edge") == 0){
00224         current_edge++;
00225         edge = parse_edge(lineread, valid);
00226         if (!valid) return false;
00227         else edges[current_edge-1] = edge;
00228       }
00229       else return false;
00230     }
00231   if (line_number < 3)return false;
00232   else if (current_node != number_of_nodes) return false;
00233   else if (current_edge != number_of_edges) return false;
00234   //ONE MORE CONDITION: CHECK CURRENT_EDGE, CURRENT_NODE
00235   else return true;
00236 }
00237 
00238 void Graph::clear(){
00239   for(uint i = 0; i < nodes_size; i++)
00240     nodes[i].clear();
00241   nodes_size = 0;
00242   for(uint i = 0; i < edges_size; i++)
00243     edges[i].clear();
00244   edges_size = 0;
00245   
00246 }
00247 
00248 void Graph::printNodes(){
00249   printf("\nNodes: \n");
00250   for(uint i = 0; i < nodes_size; i++){
00251     printf("%2d: ", nodes[i].index);
00252     printf("%2d.%2d.%2d ", nodes[i].id.seg, nodes[i].id.lane, nodes[i].id.pt);
00253     // printf("Lat: %4.4f, Long: %4.2f ", 
00254     //  nodes[i].ll.latitude, nodes[i].ll.longitude);
00255     printf(",Width: %2.3f ", nodes[i].lane_width);
00256     printf("CKPT: %s, STOP: %s", nodes[i].is_goal?"true ":"false", 
00257            nodes[i].is_stop?"true ":"false");
00258     printf(", ENTRY: %s, EXIT: %s", nodes[i].is_entry?"true ":"false", 
00259            nodes[i].is_exit?"true ":"false");
00260     printf(", SPOT: %s\n", nodes[i].is_spot?"true ":"false");
00261   }
00262 };
00263 
00264 void Graph::printEdges(){
00265   printf("\nEdges: \n");
00266   for(uint i = 0; i < edges_size; i++){
00267     printf("%3d: ", i);
00268     printf("%3d to %3d ", edges[i].startnode_index, edges[i].endnode_index);
00269     printf("Boundary- Left:%2d, Right:%2d, ", 
00270            edges[i].left_boundary, edges[i].right_boundary);
00271     printf("Speed- Min:%f, Max:%f, ", 
00272            edges[i].speed_min, edges[i].speed_max);
00273     printf(",EXIT: %s\n", edges[i].is_exit?"true ":"false");
00274   }
00275 };
00276 
00277 void Graph::printNodesFile(const char* fName){
00278   FILE* f = fopen(fName,"wb");
00279   fprintf(f,"Nodes: \n");
00280   for(uint i = 0; i < nodes_size; i++){
00281     fprintf(f,"%2d: ", nodes[i].index);
00282     fprintf(f,"%2d.%2d.%2d ", nodes[i].id.seg, 
00283             nodes[i].id.lane, nodes[i].id.pt);
00284     // printf("Lat: %4.4f, Long: %4.2f ", 
00285     //  nodes[i].ll.latitude, nodes[i].ll.longitude);
00286     fprintf(f,",Width: %2.3f ", nodes[i].lane_width);
00287     fprintf(f,"CKPT: %s, STOP: %s", nodes[i].is_goal?"true ":"false", 
00288             nodes[i].is_stop?"true ":"false");
00289     fprintf(f,", ENTRY: %s, EXIT: %s", nodes[i].is_entry?"true ":"false", 
00290             nodes[i].is_exit?"true ":"false");
00291     fprintf(f,", SPOT: %s\n", nodes[i].is_spot?"true ":"false");
00292   }
00293   fclose(f);
00294 };
00295 
00296 void Graph::printEdgesFile(const char* fName){
00297   FILE* f = fopen(fName,"wb");
00298   fprintf(f,"Edges: \n");
00299   for(uint i = 0; i < edges_size; i++){
00300     fprintf(f,"%3d: ", i);
00301     fprintf(f,"%3d to %3d ", edges[i].startnode_index, edges[i].endnode_index);
00302     fprintf(f,"Boundary- Left:%2d, Right:%2d, ", 
00303             edges[i].left_boundary, edges[i].right_boundary);
00304     fprintf(f,",EXIT: %s\n", edges[i].is_exit?"true ":"false");
00305   }
00306   fclose(f);
00307 }; 
00308 
00309 bool Graph::rndf_is_gps() {
00310   double minlat=INFINITY;
00311   double minlong=INFINITY;
00312   double maxlat=-INFINITY;
00313   double maxlong=-INFINITY;
00314 
00315   for(uint i = 0; i < nodes_size; i++){
00316     minlat = std::min(minlat,nodes[i].ll.latitude);
00317     minlong = std::min(minlong,nodes[i].ll.longitude);
00318     maxlat = std::max(maxlat,nodes[i].ll.latitude);
00319     maxlong = std::max(maxlong,nodes[i].ll.longitude);
00320   }
00321 
00322   if (minlat == INFINITY ||
00323       maxlat == INFINITY ||
00324       minlong == INFINITY ||
00325       maxlong == INFINITY)
00326     return false;
00327 
00328   if (maxlat-minlat <= 2 && // 138 miles
00329       maxlong-minlong <= 2) //84 miles
00330     return true;
00331 
00332   return false;
00333 
00334 }
00335 
00336 void Graph::xy_rndf() {
00337   for(uint i = 0; i < nodes_size; i++){
00338     nodes[i].map.x = nodes[i].ll.latitude;
00339     nodes[i].map.y = nodes[i].ll.longitude;
00340   }
00341   
00342   for(uint i = 0; i < edges_size; i++){
00343     WayPointNode* start=get_node_by_index(edges[i].startnode_index);
00344     WayPointNode* end=get_node_by_index(edges[i].endnode_index);
00345     
00346     edges[i].distance=Euclidean::DistanceTo(start->map,end->map);
00347   }
00348 }
00349 
00357 void Graph::find_mapxy(void)
00358 {
00359   if (nodes_size < 1)
00360     {
00361       ROS_INFO("No graph nodes available for conversion to MapXY");
00362       return;
00363     }
00364 
00365   // Compute the MapXY value corresponding to the first way-point in
00366   // the graph.  Use temporaries because MapXY is defined in floats,
00367   // while the UTM function returns doubles.
00368   double tX;
00369   double tY;  
00370   UTM::UTM(nodes[0].ll.latitude, nodes[0].ll.longitude, &tX, &tY);
00371 
00372   // Round UTM origin of map to nearest UTM grid intersection.  All
00373   // odometry is reported relative to that location.
00374   double grid_x = (rint(tX/UTM::grid_size) * UTM::grid_size);
00375   double grid_y = (rint(tY/UTM::grid_size) * UTM::grid_size);
00376   nodes[0].map = MapXY(tX - grid_x, tY - grid_y);
00377   ROS_INFO("UTM grid of first way-point: (%.f, %.f)", grid_x, grid_y);
00378 
00379   // Relocate all other way-points relative to (grid_x, grid_y),
00380   // instead of individually relocating each way-point.  This may
00381   // avoid some discontinuities if the map happens to span a grid
00382   // boundary.
00383   for(uint i = 1; i < nodes_size; i++)
00384     {
00385       UTM::UTM(nodes[i].ll.latitude, nodes[i].ll.longitude, &tX, &tY);
00386       nodes[i].map = MapXY(tX - grid_x, tY - grid_y);
00387     }
00388 
00389   for(uint i = 0; i < edges_size; i++){
00390     WayPointNode* start=get_node_by_index(edges[i].startnode_index);
00391     WayPointNode* end=get_node_by_index(edges[i].endnode_index);
00392     
00393     edges[i].distance=Euclidean::DistanceTo(start->map,end->map);
00394     //    std::cerr << "Edge "<<i<<" "<<start->id.name().str<<" "<<
00395     //      end->id.name().str<<" "<<edges[i].distance<<std::endl;
00396   }
00397 }
00398 
00399 bool Graph::passing_allowed(int index, int index2, bool left) {
00400   if (index < 0 || index >= (int)nodes_size)
00401     return false;
00402   
00403   WayPointNode node1=nodes[index];
00404   WayPointNode node2=nodes[index2];
00405   
00406   ElementID ahead=ElementID(node1.id.seg,node1.id.lane,node1.id.pt+1);
00407   
00408   bool found_ahead=false;
00409 
00410   for (uint i=0; i<nodes_size; i++)
00411     if (nodes[i].id==ahead)
00412       {
00413         found_ahead=true;
00414         break;
00415       }
00416 
00417   if (!found_ahead)
00418     return true;
00419         
00420                                
00421 
00422   for (uint i=0; i<edges.size(); i++)
00423     {
00424       if (edges.at(i).startnode_index==node1.index)
00425         {
00426           ElementID neighbor_id=nodes[edges.at(i).endnode_index].id;
00427           if (neighbor_id.seg==node1.id.seg &&
00428               neighbor_id.lane==node1.id.lane &&
00429               neighbor_id.pt==node1.id.pt+1) {
00430             if (left)
00431               {
00432                 if (edges.at(i).left_boundary==DOUBLE_YELLOW ||
00433                     edges.at(i).left_boundary==SOLID_YELLOW ||
00434                     edges.at(i).left_boundary==SOLID_WHITE)
00435                   return false;
00436               }
00437             else
00438               {
00439                 if (edges.at(i).right_boundary==DOUBLE_YELLOW ||
00440                     edges.at(i).right_boundary==SOLID_YELLOW ||
00441                     edges.at(i).right_boundary==SOLID_WHITE)
00442                   return false;
00443               }
00444           }
00445         }
00446       if (edges.at(i).startnode_index==node2.index)
00447         {
00448           ElementID neighbor_id= nodes[edges.at(i).endnode_index].id;
00449           if (neighbor_id.seg==node2.id.seg &&
00450               neighbor_id.lane==node2.id.lane &&
00451               neighbor_id.pt==node2.id.pt+1) {
00452             if (!left)
00453               {
00454                 if (edges.at(i).left_boundary==DOUBLE_YELLOW ||
00455                     edges.at(i).left_boundary==SOLID_YELLOW ||
00456                     edges.at(i).left_boundary==SOLID_WHITE)
00457                   return false;
00458               }
00459             else
00460               {
00461                 if (edges.at(i).right_boundary==DOUBLE_YELLOW ||
00462                     edges.at(i).right_boundary==SOLID_YELLOW ||
00463                     edges.at(i).right_boundary==SOLID_WHITE)
00464                   return false;
00465               }
00466           }
00467         }
00468     }
00469   
00470   return true;
00471 
00472 }
00473 
00474 bool Graph::lanes_in_same_direction(int index1,int index2, bool& left_lane) {
00475   if (index1<0 || index2<0 ||
00476       index1>=(int)nodes_size ||
00477       index2>=(int)nodes_size)
00478     return false;
00479 
00480   ElementID el1=ElementID(nodes[index1].id);
00481   el1.pt+=1;
00482 
00483   ElementID el2=ElementID(nodes[index2].id);
00484   el2.pt+=1;
00485 
00486   int ind1=-1;
00487   int ind2=-1;
00488 
00489   for (uint i=0; i<nodes_size; i++)
00490     {
00491       if (nodes[i].id==el1)
00492         ind1=i;
00493       if (nodes[i].id==el2)
00494         ind2=i;
00495     }
00496   
00497   float head1;
00498   float head2;
00499 
00500   if (ind1>=0 && ind2>=0)
00501     {
00502       head1=atan2f(nodes[ind1].map.y-nodes[index1].map.y,
00503                    nodes[ind1].map.x-nodes[index1].map.x);
00504       head2=atan2f(nodes[ind2].map.y-nodes[index2].map.y,
00505                    nodes[ind2].map.x-nodes[index2].map.x);
00506 
00507       MapPose p1(nodes[index1].map,head1);
00508       
00509       if (Coordinates::bearing(p1,nodes[index2].map) > 0)
00510         left_lane=true;
00511       else left_lane=false;
00512 
00513     }
00514   else
00515     {
00516       el1.pt-=2;
00517       el2.pt-=2;
00518       ind1=-1;
00519       ind2=-1;
00520 
00521       for (uint i=0; i<nodes_size; i++)
00522         {
00523           if (nodes[i].id==el1)
00524             ind1=i;
00525           if (nodes[i].id==el2)
00526             ind2=i;
00527         }
00528 
00529       if (ind1>=0 && ind2>=0)
00530         {
00531           head1=atan2(nodes[ind1].map.y-nodes[index1].map.y,
00532                       nodes[ind1].map.x-nodes[index1].map.x);
00533           head2=atan2(nodes[ind2].map.y-nodes[index2].map.y,
00534                       nodes[ind2].map.x-nodes[index2].map.x);
00535 
00536           MapPose p1(nodes[index1].map,head1);
00537           
00538           if (Coordinates::bearing(p1,nodes[index2].map) < 0)
00539             left_lane=true;
00540           else left_lane=false;
00541         }
00542       else return false;
00543     }
00544 
00545   if (fabsf(Coordinates::normalize(head1-head2)) < HALFPI)
00546     return true;
00547   else return false;
00548 
00549 }
00550 
00551 void Graph::find_implicit_edges() {
00552   
00553   for (unsigned i=0; i< nodes_size; i++)
00554     {
00555       WayPointNode node1=nodes[i];
00556       if (node1.is_stop || node1.is_perimeter ||
00557           node1.is_spot)
00558         continue;
00559       {// left case
00560         float min_dist=FLT_MAX;
00561         int min_index=-1;
00562         for (unsigned j=0; j< nodes_size; j++)
00563           if (i!=j)
00564             {
00565               WayPointNode node2=nodes[j];
00566               if (node1.id.seg==node2.id.seg &&
00567                   node1.id.lane-1==node2.id.lane)
00568                 {
00569                   float curr_dist=Euclidean::DistanceTo(node1.map,node2.map);
00570                   if (curr_dist < min_dist)
00571                     {
00572                       min_dist=curr_dist;
00573                       min_index=j;
00574                     }
00575                 }
00576             }
00577         if (min_index<0)
00578           continue;
00579         if (nodes[min_index].is_stop || nodes[min_index].is_perimeter ||
00580             nodes[min_index].is_spot)
00581           continue;
00582         bool left_lane;
00583         if (lanes_in_same_direction(i,min_index, left_lane))
00584           {
00585             if (passing_allowed(i,min_index,left_lane))
00586               {
00587                 WayPointEdge new_edge;
00588                 new_edge.startnode_index=i;
00589                 new_edge.endnode_index=min_index;
00590                 //HACK
00591                 new_edge.distance=30;
00592                 new_edge.speed_max=6.0; // TODO: Something nicer
00593                 new_edge.blocked=false;
00594                 new_edge.is_implicit=true;
00595                 edges.push_back(new_edge);
00596                 edges_size++;
00597               }
00598           }
00599       }
00600       
00601       {// right case
00602         float min_dist=FLT_MAX;
00603         int min_index=-1;
00604         for (unsigned j=0; j< nodes_size; j++)
00605           if (i!=j)
00606             {
00607               WayPointNode node2=nodes[j];
00608               if (node1.id.seg==node2.id.seg &&
00609                   node1.id.lane+1==node2.id.lane)
00610                 {
00611                   float curr_dist=Euclidean::DistanceTo(node1.map,node2.map) < min_dist;
00612                   if (curr_dist < min_dist)
00613                     {
00614                       min_dist=curr_dist;
00615                       min_index=j;
00616                     }
00617                 }
00618             }
00619         if (min_index<0)
00620           continue;
00621         bool left_lane;
00622         if (lanes_in_same_direction(i,min_index, left_lane))
00623           {
00624             if (passing_allowed(i,min_index,left_lane))
00625               {
00626                 WayPointEdge new_edge;
00627                 new_edge.startnode_index=i;
00628                 new_edge.endnode_index=min_index;
00629                 //HACK
00630                 new_edge.distance=25;
00631                 new_edge.speed_max=6.0; // TODO: something smarter
00632                 new_edge.blocked=false;
00633                 new_edge.is_implicit=true;
00634                 edges.push_back(new_edge);
00635                 edges_size++;
00636               }
00637           }
00638       }
00639     }
00640 }
00641 
00642       
00643 int parse_integer(std::string line, std::string token, 
00644                   bool& valid){
00645   int integer;
00646   if (!sscanf(line.c_str(), "%*s %d", &integer)){
00647     valid=false;
00648   }
00649   return integer; 
00650 };
00651 
00652 WayPointNode parse_node(std::string line, bool& valid){
00653   WayPointNode node;
00654   double f1, f2;
00655   float f3, f4, f5;
00656   int d1, d2, d3, d4, d5, d6, d7, d8, d9, d10, d11;
00657   if (sscanf(line.c_str(), 
00658              "Node %lf, %lf; %f, %f; %d, %d, %d; %d; %d, %d, %d, %d, %d, %d; %d; %f;", 
00659              &f1, &f2, &f3, &f4, &d1, &d2, &d3, &d4, &d5, &d6, &d7, &d8, &d9, &d10, &d11, &f5) 
00660       == 16){
00661     valid = true;
00662     node.ll.latitude = f1;
00663     node.ll.longitude = f2;
00664     node.map.x = f3;
00665     node.map.y = f4;
00666     node.id.seg = d1;
00667     node.id.lane = d2;
00668     node.id.pt = d3;
00669     node.index = d4;
00670     node.is_entry = d5;
00671     node.is_exit = d6;
00672     node.is_goal = d7;
00673     node.is_spot = d8;
00674     node.is_stop = d9;
00675     node.is_perimeter = d10;
00676     node.checkpoint_id = d11;
00677     node.lane_width = f5;
00678   }
00679   else
00680     valid=false;
00681   //printf("--> %s\n", line.c_str());
00682   return node;
00683 
00684 
00685 };
00686 
00687 WayPointEdge parse_edge(std::string line, bool& valid){
00688 
00689   WayPointEdge edge;
00690   float f1,f2,f3;
00691   int d1, d2, d6, d7, d8;
00692   if (sscanf(line.c_str(), 
00693              "Edge %d, %d; %f; %f, %f; %d; %d, %d; ",
00694              &d1, &d2, &f1, &f2, &f3, &d6, &d7, &d8) 
00695       == 8){
00696     valid = true;
00697     edge.startnode_index = d1;
00698     edge.endnode_index = d2;
00699     edge.distance = f1;
00700     edge.speed_max = f2;
00701     edge.speed_min = f3;
00702     edge.is_exit = d6;
00703     edge.left_boundary = static_cast<Lane_marking>(d7);
00704     edge.right_boundary = static_cast<Lane_marking>(d8);
00705   }
00706   else
00707     valid=false;
00708   //printf("--> %s\n", line.c_str());
00709   return edge;
00710 
00711 };
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


art_map
Author(s): David Li, Patrick Beeson, Bartley Gillen, Tarun Nimmagadda, Mickey Ristroph, Michael Quinlan, Jack O'Quin
autogenerated on Tue Sep 24 2013 10:41:51