voronoi_zone.cc
Go to the documentation of this file.
00001 /*
00002  *  Navigator zone controller
00003  *
00004  *  Copyright (C) 2007, 2010, Mickey Ristroph
00005  *
00006  *  License: Modified BSD Software License Agreement
00007  *
00008  *  $Id: voronoi_zone.cc 626 2010-09-25 04:07:12Z jack.oquin $
00009  */
00010 
00011 #include <art/Graph.h>
00012 #include <art/ZoneOps.h>
00013 #include <art/euclidean_distance.h>
00014 
00015 #include "navigator_internal.h"
00016 #include "Controller.h"
00017 #include "course.h"
00018 #include "obstacle.h"
00019 #include "voronoi_zone.h"
00020 #include <art/steering.h>
00021 #include "safety.h"
00022 #include "halt.h"
00023 
00024 VoronoiZone::VoronoiZone(Navigator *navptr, int _verbose):
00025   Controller(navptr, _verbose), in_fake_zone(false)
00026 {
00027   zmanager = NULL;
00028   safety = new Safety(navptr, _verbose, 1);
00029   halt = new Halt(navptr, _verbose);
00030   park = new PARK_control(navptr, _verbose);
00031   follow_lane = new FollowLane(navptr, _verbose);
00032 }
00033 
00034 VoronoiZone::~VoronoiZone()
00035 {
00036   delete safety;
00037   delete park;
00038   delete halt;
00039   delete follow_lane;
00040   if(zmanager != NULL)
00041     delete zmanager;
00042 }
00043 
00044 // configuration method
00045 void VoronoiZone::configure(ConfigFile* cf, int section)
00046 {
00047   // maximum speed when inside a zone
00048   zone_speed_limit = cf->ReadFloat(section, "zone_speed_limit", 1.0);
00049   ART_MSG(2, "\tspeed limit in a zone is %.1f m/s", zone_speed_limit);
00050 
00051   zone_safety_radius = cf->ReadFloat(section, "zone_safety_radius", 2.0);
00052   ART_MSG(2, "\tzone_safety_radius is %.1f m", zone_safety_radius);
00053 
00054   zone_perimeter_sample = cf->ReadFloat(section, "zone_perimeter_sample", 1.0);
00055   ART_MSG(2, "\tzone_perimeter_sample is %.1f m", zone_perimeter_sample);
00056 
00057   fake_zone_border = cf->ReadFloat(section, "fake_zone_border", 10.0);
00058   ART_MSG(2, "\tfake_zone_border is %.1f m", fake_zone_border);
00059 
00060   fake_zone_included_obstacle_radius =
00061     cf->ReadFloat(section, "fake_zone_included_obstacle_radius", 30.0);
00062   ART_MSG(2, "\tfake_zone_included_obstacle_radius is %.1f m",
00063           fake_zone_included_obstacle_radius);
00064 
00065   zone_evg_thin_scale = cf->ReadFloat(section, "zone_evg_thin_scale", 1.0);
00066   ART_MSG(2, "\tzone_evg_thin_scale is %.1f m", zone_evg_thin_scale);
00067 
00068   zone_grid_max_cells = cf->ReadInt(section, "zone_grid_max_cells", 1000);
00069   ART_MSG(2, "\tzone_grid_max_cells is %d", zone_grid_max_cells);
00070 
00071   zone_use_voronoi = cf->ReadBool(section, "zone_use_voronoi", true);
00072   ART_MSG(2, "\tzone_use_voronoi is %d", zone_use_voronoi);
00073 
00074   zone_avoid_obstacles = cf->ReadBool(section, "zone_avoid_obstacles", true);
00075   ART_MSG(2, "\tzone_avoid_obstacles is %d", zone_avoid_obstacles);
00076 
00077   zone_write_graph_to_tmp = cf->ReadBool(section, "zone_write_graph_to_tmp", true);
00078   ART_MSG(2, "\tzone_write_graph_to_tmp is %d", zone_write_graph_to_tmp);
00079 
00080   zone_write_poly_to_tmp = cf->ReadBool(section, "zone_write_poly_to_tmp", false);
00081   ART_MSG(2, "\tzone_write_poly_to_tmp is %d", zone_write_poly_to_tmp);
00082 
00083   zone_write_obstacles_to_tmp = cf->ReadBool(section, "zone_write_obstacles_to_tmp", false);
00084   ART_MSG(2, "\tzone_write_obstacles_to_tmp is %d", zone_write_obstacles_to_tmp);
00085 
00086   zone_aim_point = cf->ReadFloat(section, "zone_aim_point", 8.0);
00087   ART_MSG(2, "\tzone_aim_point is %.3f", zone_aim_point);
00088   
00089   safety->configure(cf, section);
00090   halt->configure(cf, section);
00091   park->configure(cf, section);
00092   follow_lane->configure(cf, section);
00093 }
00094 
00095 // go to next zone way-point
00096 //
00097 // returns: OK.
00098 //
00099 Controller::result_t VoronoiZone::control(pilot_command_t &pcmd)
00100 {
00101   result_t result = OK;
00102 
00103   ART_MSG(8, "ENTERING zone::control()");
00104   if (verbose >= 2)
00105     {
00106       ART_MSG(5, "Go to zone waypoint %s",
00107               order->waypt[1].id.name().str);
00108     }
00109   
00110   // limit speed inside zones
00111   pcmd.velocity = fminf(pcmd.velocity, zone_speed_limit);
00112   
00113   navdata->reverse=false;
00114   
00115   // return desired heading for this cycle
00116   result=set_heading(pcmd);
00117 
00118   // TODO: implement obstacle evasion if Unsafe or Blocked
00119   
00120   pcmd.velocity=fminf(zone_speed_limit,pcmd.velocity);
00121 
00122   
00123   if (!Epsilon::equal(pcmd.yawRate,0.0) &&
00124       !Epsilon::equal(lastYaw,0.0))
00125     if (fabsf(pcmd.yawRate-lastYaw) > Steering::maximum_yaw/2
00126         &&
00127         ((pcmd.yawRate < 0) != (lastYaw < 0)))
00128       pcmd.velocity=0;
00129   
00130   lastYaw=pcmd.yawRate;
00131   
00132   
00133   trace("voronoi_zone controller", pcmd, result);
00134   ART_MSG(8, "EXITING zone::control()");
00135   return result;
00136 }
00137 
00138 // reset all subordinate controllers
00139 void VoronoiZone::reset(void)
00140 {
00141   trace_reset("VoronoiZone");
00142   in_fake_zone = false;
00143   lastYaw=0.0;
00144   safety->reset();
00145   halt->reset();
00146   park->reset();
00147   follow_lane->reset();
00148   if(zmanager != NULL)
00149     delete zmanager;
00150   zmanager = NULL;
00151 
00152   spot_points.clear();
00153 }
00154 
00155 // set heading to next way-point
00156 Controller::result_t VoronoiZone::set_heading(pilot_command_t &pcmd)
00157 {
00158   static bool parking=false;
00159 
00160   if (order->waypt[1].id.lane!=0 &&
00161       !order->waypt[1].is_spot)
00162     if (course->lane_waypoint_reached())
00163       return Finished;
00164   
00165   posetype way_pose;
00166   
00167   if (!order->waypt[1].is_spot ||
00168       (order->waypt[1].is_spot && order->waypt[2].is_spot &&
00169        order->waypt[1].id.pt==1 && order->waypt[2].id.pt==2))
00170     // normal situation -- set pose at spot
00171     {
00172       way_pose=posetype(order->waypt[1].map.x,order->waypt[1].map.y,
00173                         atan2f(order->waypt[2].map.y-order->waypt[1].map.y,
00174                                order->waypt[2].map.x-order->waypt[1].map.x));
00175 // #ifdef NQE
00176 //       if (order->waypt[1].id.seg==43 && 
00177 //        order->waypt[2].id.seg==4) {
00178 //      way_pose=posetype(order->waypt[1].map.x,order->waypt[1].map.y,
00179 //                        atan2f(order->waypt[2].map.y-order->waypt[1].map.y,
00180 //                               order->waypt[2].map.x-order->waypt[1].map.x));
00181 //      way_pose.y-=8;
00182 //      //      way_pose.theta-=DTOR(45);
00183 //       }
00184 // #endif
00185     }
00186   
00187   else
00188     // pulling into spot, put point at front of spot
00189     if (order->waypt[1].is_spot && order->waypt[2].is_spot &&
00190         order->waypt[1].id.pt==2 && order->waypt[2].id.pt==1)
00191       way_pose = posetype(order->waypt[1].map.x,order->waypt[1].map.y,
00192                           atan2f(order->waypt[1].map.y-order->waypt[2].map.y,
00193                                  order->waypt[1].map.x-order->waypt[2].map.x));
00194     else 
00195       // pulling out of spot, put point at front of spot
00196       way_pose=posetype(order->waypt[0].map.x,order->waypt[0].map.y,
00197                         atan2f(order->waypt[0].map.y-order->waypt[1].map.y,
00198                                order->waypt[0].map.x-order->waypt[1].map.x));
00199   
00200   MapXY goal;
00201 
00202   trans.find_transform(posetype(0,0,0),way_pose);
00203   
00204 
00205   if (order->waypt[1].is_spot)
00206     goal=MapXY(trans.apply_transform(posetype(-zone_aim_point,0,0)));
00207   else
00208     if (order->waypt[1].id.lane==0)
00209       goal=MapXY(trans.apply_transform(posetype(-zone_aim_point,0,0)));
00210     else
00211       goal=MapXY(trans.apply_transform(posetype(zone_aim_point,0,0)));
00212 
00213   MapXY aim = goal;
00214   
00215   if (Euclidean::DistanceTo(goal,estimate->pos) < zone_aim_point ||
00216       (parking  && Euclidean::DistanceTo(goal,estimate->pos) < zone_aim_point+5))
00217     {
00218       if (order->waypt[1].is_spot)
00219         {
00220           parking=true;
00221           ART_MSG(3, "Near goal, entering Parking (%.3f %.3f, %.3f %.3f)",
00222                   estimate->pos.px, estimate->pos.py,
00223                   goal.x, goal.y);
00224           result_t park_result=park->control(pcmd, obstacle->lasers->all_obstacle_list, false);
00225           if (park_result==Finished)
00226             {
00227               park->reset();
00228               parking=false;
00229             }
00230           return park_result;
00231         }
00232       else {
00233         ART_MSG(3, "Near goal, entering Follow lane (%.3f %.3f, %.3f %.3f)",
00234                 estimate->pos.px, estimate->pos.py,
00235                 goal.x, goal.y);
00236         pilot_command_t pcopy=pcmd;
00237         result_t res=follow_lane->control(pcopy);
00238         if (res < Unsafe)
00239           {
00240             pcmd=pcopy;
00241             return res;
00242           }
00243       }
00244     }
00245 
00246   course->no_waypoint_reached();
00247 
00248   if (course->spot_ahead())
00249     spot_points=course->calculate_spot_points();
00250   else if (!course->curr_spot())
00251     spot_points.clear();
00252 
00253   mapxy_list_t obs_points=obstacle->lasers->all_obstacle_list;
00254 
00255   for (uint i=0;i<spot_points.size();i++)
00256     obs_points.push_back(spot_points.at(i));
00257   
00258   if(zone_use_voronoi) {
00259     ART_MSG(8, "ENTERING zone::set_heading()");
00260     
00261     MapXY start(estimate->pos);
00262     MapXY end(aim);
00263    
00264     WayPointNodeList nodes;
00265     bool use_zone_manager = false;
00266     if(use_zone_manager) {
00267       bool is_a_zone = ZoneOps::is_a_zone_id(course->zones,
00268                                              order->waypt[1].id.seg);
00269       if(zmanager == NULL ||
00270          (is_a_zone && (order->waypt[1].id.seg != zmanager->starting_id.seg)) ||
00271          (!is_a_zone && (order->waypt[0].id != zmanager->starting_id))) {
00272         if(zmanager != NULL) {
00273           ART_MSG(1, "Deallocating old zone (seg=%d)",
00274                   zmanager->starting_id.seg);
00275           delete zmanager;      
00276         }
00277         
00278         ZonePerimeter zone =
00279           ZoneOps::get_zone_by_id(course->zones, order->waypt[1].id.seg);
00280         
00281         if(zone.perimeter_points.size() < 3) {
00282           std::vector<MapXY> points_to_include;
00283           points_to_include.push_back(start);
00284           points_to_include.push_back(end);
00285           
00286           for(unsigned i = 0;
00287               i < obstacle->lasers->all_obstacle_list.size(); i++) {
00288             if(Euclidean::DistanceTo
00289                (start, obstacle->lasers->all_obstacle_list.at(i)) <
00290                fake_zone_included_obstacle_radius) {
00291               points_to_include.push_back
00292                 (obstacle->lasers->all_obstacle_list.at(i));
00293             }
00294           }
00295           
00296           zone = ZoneOps::build_fake_zone(points_to_include, fake_zone_border);
00297         
00298           if(!in_fake_zone) {
00299             ART_MSG(1, "Making a bogus zone around us and the goal!");
00300             in_fake_zone = true;
00301           }
00302         } else {
00303           if(in_fake_zone) {
00304             ART_MSG(1, "In a zone with at least three perimeter points.");
00305             in_fake_zone = false;
00306           }
00307         }
00308         
00309         MapXY ur = zone.perimeter_points[0].map;
00310         MapXY ll = ur;
00311         ZoneOps::expand_bounding_box_of_waypoints(zone.perimeter_points, ll, ur);
00312         float width = fabs(ur.x-ll.x);
00313         float height = fabs(ur.y-ll.y);
00314         
00315         ART_MSG(1, "Allocating a new zone (seg=%d)", order->waypt[0].id.seg);
00316         ART_MSG(1, "Zone is %.1f by %.1f meters.", width, height);
00317         
00318         zmanager = new ZoneManager(zone,
00319                                    zone_safety_radius,
00320                                    zone_evg_thin_scale,
00321                                    zone_grid_max_cells,
00322                                    zone_write_graph_to_tmp,
00323                                    order->waypt[0].id,
00324                                    ll,
00325                                    ur);
00326       }
00327       
00328       ART_MSG(8, "Getting a path (%.3f, %.3f) -> (%.3f, %.3f)",
00329               start.x, start.y, end.x, end.y);
00330       
00331       
00332       ART_MSG(8, "ENTERING path_through_zone()");
00333       nodes = zmanager->path_through_zone(obs_points,
00334                                           start, end);
00335       
00336     }
00337     else {
00338       ZonePerimeter zone =
00339         ZoneOps::get_zone_by_id(course->zones, order->waypt[1].id.seg);
00340       
00341       bool use_lane_based_zones = false;
00342       if(use_lane_based_zones) {
00343         if(order->waypt[1].id.seg == 10 && order->waypt[0].id.seg == 10) {
00344           std::vector<MapXY> p =
00345             pops.getRoadPerimeterPoints(course->polygons,
00346                                         ElementID(10,1,6),
00347                                         ElementID(10,1,16));
00348           ART_MSG(1, "Building a zone based on the lanes with %d points.",
00349                   p.size());
00350           zone.perimeter_points.clear();
00351           for(unsigned i = 0; i < p.size(); i++) {
00352             WayPointNode node;
00353             node.map = p.at(i);
00354             zone.perimeter_points.push_back(node);
00355           }
00356         }
00357       }
00358       
00359       if(zone.perimeter_points.size() < 3) {
00360         std::vector<MapXY> points_to_include;
00361         points_to_include.push_back(start);
00362         points_to_include.push_back(end);
00363         
00364         for(unsigned i = 0; i < obstacle->lasers->all_obstacle_list.size(); i++) {
00365           if(Euclidean::DistanceTo(start,
00366                                    obstacle->lasers->all_obstacle_list.at(i)) <
00367              fake_zone_included_obstacle_radius) {
00368             points_to_include.push_back(obstacle->lasers->all_obstacle_list.at(i));
00369           }
00370         }
00371         zone = ZoneOps::build_fake_zone(points_to_include, fake_zone_border);
00372         MapXY lr, ul;
00373         float width = 0;
00374         float height = 0;
00375         
00376         if(zone.perimeter_points.size() > 3) {
00377           lr = zone.perimeter_points[0].map;
00378           ul = zone.perimeter_points[2].map;
00379           width = fabs(lr.x-ul.x);
00380           height = fabs(lr.y-ul.y);
00381         }
00382         
00383         if(!in_fake_zone) {
00384           ART_MSG(1, "Making a bogus zone around us and the goal!");
00385           in_fake_zone = true;
00386           
00387           ART_MSG(1, "Fake zone is %.1f by %.1f meters.", width, height);
00388         }
00389       } else {
00390         if(in_fake_zone) {
00391           ART_MSG(1, "In a zone with at least three perimeter points.");
00392           in_fake_zone = false;
00393         }
00394       }
00395       
00396       
00397       ART_MSG(8, "Getting a path (%.3f, %.3f) -> (%.3f, %.3f)",
00398               start.x, start.y, end.x, end.y);
00399       
00400       ART_MSG(8, "ENTERING path_through_zone()");
00401       nodes =
00402         ZoneOps::path_through_zone(zone,
00403                                    zone_perimeter_sample,
00404                                    zone_safety_radius,
00405                                    obs_points,
00406                                    start,
00407                                    end,
00408                                    zone_write_graph_to_tmp,
00409                                    zone_write_poly_to_tmp,
00410                                    zone_write_obstacles_to_tmp,
00411                                    zone_evg_thin_scale,
00412                                    zone_grid_max_cells);
00413     }
00414     ART_MSG(8, "EXITING path_through_zone().");
00415     //    ART_MSG(1, "Current path size=%d", nodes.size());
00416     
00417     if(nodes.size() > 0) {
00418       WayPointNodeList::reverse_iterator i = nodes.rbegin();
00419       while(i < nodes.rend() &&
00420             Euclidean::DistanceTo(start, i->map) > i->lane_width) {
00421         ART_MSG(8, "Can't yet go to point: (%.3f, %.3f) with radius %.3f."
00422                 " Trying one closer...",
00423                 i->map.x, i->map.y, i->lane_width);
00424         i++;
00425       }
00426       
00427       if(i < nodes.rend()) {
00428         aim = i->map;
00429         
00430         if(Euclidean::DistanceTo(end, i->map) < i->lane_width) {
00431           ART_MSG(3, "Yippee! Goal is with radius, go for it!",
00432                   i->map.x, i->map.y, i->lane_width);
00433           aim  = goal;
00434         } else {
00435           ART_MSG(3, "Aiming for intermediate point (%.3f, %.3f)",
00436                   aim.x, aim.y);
00437         }
00438       } else {
00439         // XXX: TODO: MGR: Not sure what reaching this point means....
00440         ART_MSG(3, "We are within no point's safety radius! "
00441                 "Shoot for goal...");
00442       }
00443     } else {
00444       ART_MSG(3, "No path through Voronoi graph! What to do? "
00445               "Shoot for goal...");
00446     }    
00447   }
00448   
00449   // egocentric polar aim point
00450   Polar aim_polar =
00451     Coordinates::MapXY_to_Polar(aim, estimate->pos);
00452   
00453   if (verbose >= 3)
00454     {
00455       ART_MSG(8, "target, current positions: (%.3f, %.3f), (%.3f, %.3f)",
00456               aim.x, aim.y,
00457               estimate->pos.px, estimate->pos.py);
00458       ART_MSG(8, "relative heading: %.3f radians, distance: %.3f meters",
00459               aim_polar.heading, aim_polar.range);
00460     }
00461   
00462   pcmd.yawRate = aim_polar.heading/fmaxf(1.0,estimate->vel.px/2);
00463   
00464   trace("voronoi_zone set_heading", pcmd);
00465   
00466   if (estimate->vel.px > fmaxf(3*zone_speed_limit,3)) {
00467     pcmd.velocity=0.0;
00468     return OK;
00469   }
00470   
00471   if (estimate->vel.px > 1.5*zone_speed_limit)
00472     pcmd.velocity=zone_speed_limit/2;
00473   
00474   result_t safe=safety->control(pcmd);
00475   if (safe>=Unsafe) {
00476     pcmd.velocity=zone_speed_limit;
00477     pcmd.yawRate/=2;
00478     while (fabsf(pcmd.yawRate) >= 0.1 && safety->control(pcmd) >=Unsafe)
00479       {
00480         pcmd.velocity=zone_speed_limit;
00481         pcmd.yawRate/=2;
00482       }
00483     ART_MSG(8, "Stuck.  Calling Parking");
00484     return park->control(pcmd,obstacle->lasers->all_obstacle_list);
00485   }
00486   
00487   ART_MSG(8, "EXITING zone::set_heading()");
00488  
00489   return safe;
00490 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


art_nav
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Tue Sep 24 2013 10:42:11