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