rndf_lanes.cc
Go to the documentation of this file.
00001 /*
00002  *  Utility to print RNDF lane information.
00003  *
00004  *  Copyright (C) 2010, Austin Robot Technology
00005  *
00006  *  License: Modified BSD Software License Agreement
00007  *
00008  *  $Id: rndf_lanes.cc 549 2010-09-06 16:06:04Z jack.oquin $
00009  */
00010 
00011 #include <stdio.h>
00012 #include <stdlib.h>
00013 #include <unistd.h>                     // for sleep
00014 #include <getopt.h>
00015 #include <signal.h>
00016 #include <string.h>
00017 
00018 #include <iostream>
00019 #include <fstream>
00020 #include <iomanip>
00021 
00022 #include <ros/ros.h>
00023 
00024 #include <art_map/euclidean_distance.h>
00025 #include <art_map/MapLanes.h>
00026 #include <art_map/zones.h>
00027 
00028 //#include <art/lanes.h>
00029 //#include <art/ZoneOps.h>
00030 
00031 // default parameters
00032 char *pname;                            // program name
00033 bool print_polys = false;
00034 bool output_polys = false;
00035 bool make_image = false;
00036 bool with_trans = false;
00037 int verbose = 0;
00038 char *rndf_name;
00039 float poly_size=-1;
00040 
00041 gps_info pos = {0, 0};
00042 RNDF *rndf = NULL;
00043 Graph* graph = NULL;
00044 
00045 float centerx,centery;
00046 #define CMD "rndf_lanes: "              // message prefix
00047   
00048 bool build_RNDF()
00049 {
00050 
00051   rndf = new RNDF(rndf_name);
00052   
00053   if (!rndf->is_valid) {
00054     std::cerr << "RNDF not valid\n";
00055     return false;;
00056   }
00057 
00058   graph = new Graph();
00059 
00060   rndf->populate_graph(*graph);
00061 
00062   // pos.gps_latitude=graph.nodes[0].ll.latitude;
00063   // pos.gps_longitude=graph.nodes[0].ll.longitude;
00064 
00065   if (graph->rndf_is_gps())
00066     {
00067       
00068       std::cout << "RNDF uses GPS waypoints\n";
00069 
00070       if (pos.gps_latitude==0 &&
00071           pos.gps_longitude==0)
00072         {
00073           pos.gps_latitude=graph->nodes[0].ll.latitude;
00074           pos.gps_longitude=graph->nodes[0].ll.longitude;
00075           graph->find_mapxy();
00076 
00077           float min_x=FLT_MAX;
00078           float min_y=FLT_MAX;
00079           float max_x=-FLT_MAX;
00080           float max_y=-FLT_MAX;
00081           double initial_UTM_x;
00082           double initial_UTM_y;
00083           char zone[255];
00084           
00085           LLtoUTM(graph->nodes[0].ll.latitude, 
00086                   graph->nodes[0].ll.longitude, 
00087                   initial_UTM_y,              
00088                   initial_UTM_x,
00089                   zone);
00090           
00091           ZonePerimeterList zones = ZoneOps::build_zone_list_from_rndf(*rndf, *graph);
00092           
00093           for(unsigned j = 0; j < zones.size(); j++) {
00094             ZonePerimeter zzone = zones[j];
00095             for(unsigned i = 0; i < zzone.perimeter_points.size(); i++) {
00096               if (zzone.perimeter_points[i].map.x < min_x)
00097                 min_x=zzone.perimeter_points[i].map.x;
00098               if (zzone.perimeter_points[i].map.y < min_y)
00099                 min_y=zzone.perimeter_points[i].map.y;
00100               if (zzone.perimeter_points[i].map.x > max_x)
00101                 max_x=zzone.perimeter_points[i].map.x;
00102               if (zzone.perimeter_points[i].map.y > max_y)
00103                 max_y=zzone.perimeter_points[i].map.y;
00104             }
00105           }
00106           
00107           for (uint i=0;i<graph->nodes_size;i++)
00108             {
00109               if (graph->nodes[i].map.x < min_x)
00110                 min_x=graph->nodes[i].map.x;
00111               if (graph->nodes[i].map.y < min_y)
00112                 min_y=graph->nodes[i].map.y;
00113               if (graph->nodes[i].map.x > max_x)
00114                 max_x=graph->nodes[i].map.x;
00115               if (graph->nodes[i].map.y > max_y)
00116                 max_y=graph->nodes[i].map.y;
00117             }
00118       
00119           centerx=(max_x+min_x)/2 - graph->nodes[0].map.x + initial_UTM_x;
00120           centery=(max_y+min_y)/2 - graph->nodes[0].map.y + initial_UTM_y;
00121           
00122           double centerlat, centerlong;
00123           
00124           UTMtoLL(centery,centerx,zone,
00125                   centerlat,centerlong);
00126           
00127           std::cout << "Center for RNDF is at lat/long: "<<
00128             std::setprecision(8)<<
00129             centerlat<<", "<<centerlong<<" "<<zone<<std::endl<<std::endl;
00130           
00131           pos.gps_latitude=centerlat;
00132           pos.gps_longitude=centerlong;
00133           
00134         }
00135       graph->find_mapxy();
00136       
00137     }
00138   else
00139     {
00140       std::cout << "RNDF does not use GPS waypoints\n";
00141       graph->xy_rndf();
00142     }
00143   
00144   
00145   return true;
00146 }
00147 
00148 void parse_args(int argc, char *argv[])
00149 {
00150   bool print_usage = false;
00151   const char *options = "hiops:tx:y:v";
00152   int opt = 0;
00153   int option_index = 0;
00154   struct option long_options[] = 
00155     { 
00156       { "help", 0, 0, 'h' },
00157       { "image", 0, 0, 'i' },
00158       { "latitude", 1, 0, 'x' },
00159       { "longitude", 1, 0, 'y' },
00160       { "size", 1, 0, 's' },
00161       { "print", 0, 0, 'p' },
00162       { "output-points", 0, 0, 'o' },
00163       { "trans", 0, 0, 't' },
00164       { "verbose", 0, 0, 'v' },
00165       { 0, 0, 0, 0 }
00166     };
00167 
00168   /* basename $0 */
00169   pname = strrchr(argv[0], '/');
00170   if (pname == 0)
00171     pname = argv[0];
00172   else
00173     pname++;
00174 
00175   opterr = 0;
00176   while ((opt = getopt_long(argc, argv, options,
00177                             long_options, &option_index)) != EOF)
00178     {
00179       switch (opt)
00180         {
00181         case 'i':
00182           make_image = true;
00183           break;
00184 
00185         case 'p':
00186           print_polys = true;
00187           break;
00188 
00189         case 'o':
00190           output_polys = true;
00191           break;
00192 
00193         case 'v':
00194           ++verbose;
00195           break;
00196 
00197         case 'x':
00198           pos.gps_latitude = atof(optarg);
00199           break;
00200 
00201         case 'y':
00202           pos.gps_longitude = atof(optarg);
00203           break;
00204          
00205         case 's':
00206           poly_size = atof(optarg);
00207           break;
00208 
00209         case 't':
00210           with_trans = true;
00211           break;
00212 
00213         default:
00214           fprintf(stderr, "unknown option character %c\n",
00215                   optopt);
00216           /*fallthru*/
00217         case 'h':
00218           print_usage = true;
00219         }
00220     }
00221 
00222   if (print_usage || optind >= argc)
00223     {
00224       fprintf(stderr,
00225               "usage: %s [options] RNDF_name\n\n"
00226               "    Display RNDF lane information.  Possible options:\n"
00227               "\t-h, --help\tprint this message\n"
00228               "\t-i, --image\tmake .pgm image of polygons\n"
00229               "\t-y, --latitude\tinitial pose latitude\n"
00230               "\t-x, --longitude\tinitial pose longitude\n"
00231               "\t-s, --size\tmax polygon size\n"
00232               "\t-p, --print\tprint polygons to stdout\n"
00233               "\t-o, --output-points\tprint polygon points to polys.points\n"
00234               "\t-v, --verbose\tprint verbose messages\n",
00235               pname);
00236       exit(9);
00237     }
00238 
00239   rndf_name = argv[optind];
00240 }
00241 
00242 void PrintPolygons(const lanes_state_msg_t &ldata)
00243 {
00244   uint32_t count = ldata.poly_count;
00245 
00246   if (count > MAX_LANE_ELEMENTS)
00247     {
00248       std::cerr << "too many polygons received ("
00249                 << count << ")" << std::endl;
00250       return;
00251     }
00252 
00253   std::cout << "#: [start,end] <contains> {heading} Sv Tv (p1),(p2),(p3),(p4)"
00254             << std::endl;
00255 
00256   for (unsigned i = 0; i < count; ++i)
00257     {
00258       std::cout << ldata.poly[i].poly_id << ": ";
00259 
00260       std::cout << "[" << ldata.poly[i].start_way.name().str
00261                 << "," << ldata.poly[i].end_way.name().str << "] <";
00262 
00263       if (ldata.poly[i].contains_way)
00264         {
00265           std::cout << ldata.poly[i].start_way.name().str;
00266         }
00267       std::cout << std::fixed << std::setprecision(3)
00268                 << "> {" << ldata.poly[i].heading << "}"
00269                 << " S" << ldata.poly[i].is_stop
00270                 << " T" << ldata.poly[i].is_transition
00271                 << " ("
00272                 << ldata.poly[i].p1.x << "," << ldata.poly[i].p1.y
00273                 << "),("
00274                 << ldata.poly[i].p2.x << "," << ldata.poly[i].p2.y
00275                 << "),("
00276                 << ldata.poly[i].p3.x << "," << ldata.poly[i].p3.y
00277                 << "),("
00278                 << ldata.poly[i].p4.x << "," << ldata.poly[i].p4.y
00279                 << ")" << std::endl;
00280     }
00281 }
00282 
00283 
00284 void OutputPolygons(const lanes_state_msg_t &ldata)
00285 {
00286   uint32_t count = ldata.poly_count;
00287 
00288   if (count > MAX_LANE_ELEMENTS)
00289     {
00290       std::cerr << "too many polygons received ("
00291                 << count << ")" << std::endl;
00292       return;
00293     }
00294 
00295   std::ofstream a_file ( "polys.points" );
00296   for (unsigned i = 0; i < count; ++i)
00297     {
00298       
00299       a_file << ldata.poly[i].p1.x << " " << ldata.poly[i].p1.y << " "
00300              << ldata.poly[i].p2.x << " " << ldata.poly[i].p2.y << " "
00301              << ldata.poly[i].p3.x << " " << ldata.poly[i].p3.y << " "
00302              << ldata.poly[i].p4.x << " " << ldata.poly[i].p4.y
00303              << std::endl;
00304     }
00305   a_file.close();
00306 }
00307 
00308 
00309 int main(int argc, char *argv[]) 
00310 {
00311   int rc;
00312 
00313   parse_args(argc, argv);
00314 
00315   if (!build_RNDF())
00316     {
00317       std::cerr << "RNDF not valid\n";
00318       return 1;
00319     }
00320 
00321   MapLanes *mapl = new MapLanes(verbose);
00322 
00323   rc = mapl->MapRNDF(graph,poly_size);
00324   if (rc != 0)
00325     {
00326       std::cout << "cannot process RNDF! (error code " << rc <<")\n";
00327       return 1;
00328     }
00329 
00330   // Fill in lanedata with all polygons within range of our
00331   // current position.
00332   lanes_state_msg_t lanedata;
00333   rc = mapl->getAllLanes(&lanedata);
00334   if (rc < 0)
00335     {
00336       std::cout << "error getting all polygons!  "<<-rc << " too many polygons to fit in Player message)\n";
00337     }
00338   
00339   if (print_polys)
00340     PrintPolygons(lanedata);
00341 
00342   if (output_polys)
00343     OutputPolygons(lanedata);
00344   if (make_image) {
00345     ZonePerimeterList zones = ZoneOps::build_zone_list_from_rndf(*rndf, *graph);
00346     mapl->SetGPS(centerx,centery);
00347           mapl->testDraw(with_trans, zones);
00348   }
00349   return rc;
00350 }
 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