art_nav: art_nav: parking.cc Source File

00001 /*
00002  *  Navigator parking controller
00003  *
00004  *  Copyright (C) 2007, 2010, Austin Robot Technology
00005  *
00006  *  License: Modified BSD Software License Agreement
00007  *
00008  *  Author: Patrick Beeson
00009  *
00010  *  $Id: parking.cc 626 2010-09-25 04:07:12Z jack.oquin $
00011  */
00012 
00013 #include "navigator_internal.h"
00014 #include "obstacle.h"
00015 #include "Controller.h"
00016 #include "course.h"
00017 #include "parking.h"
00018 #include <art_map/rotate_translate_transform.h>
00019 #include <art/steering.h>
00020 
00021 #include "safety.h"
00022 #include "stop.h"
00023 #include "halt.h"
00024 
00025 int find_spot(const std::vector<WayPointNode> &new_waypts)
00026 {
00027   for (uint i=0; i < new_waypts.size()-1;i++)
00028     if (new_waypts[i].is_spot &&
00029         new_waypts[i+1].is_spot &&
00030         new_waypts[i].id.lane==
00031         new_waypts[i+1].id.lane)
00032       {
00033         return (int)i;
00034       }
00035   return -1;
00036 }
00037 
00038 poly adjust_spot_and_return_poly(WayPointNode &p1,
00039                                  WayPointNode &p2,
00040                                  float dx,
00041                                  float dy) {
00042   poly spot;
00043 
00044   {
00045     posetype way_pose(p1.map.x, p1.map.y,
00046                       atan2f(p2.map.y-p1.map.y,
00047                              p2.map.x-p1.map.x));
00048     
00049     rotate_translate_transform trans;
00050     trans.find_transform(posetype(),way_pose);
00051 
00052     float dist=Euclidean::DistanceTo(p1.map,
00053                                      p2.map);  
00054     
00055     p1.map = MapXY(trans.apply_transform(posetype(dx, dy, 0)));
00056     p2.map = MapXY(trans.apply_transform(posetype(dx + dist, dy, 0)));
00057   }
00058 
00059   posetype way_pose(p1.map.x, p1.map.y,
00060                     atan2f(p2.map.y-p1.map.y,
00061                            p2.map.x-p1.map.x));
00062   
00063   rotate_translate_transform trans;
00064   trans.find_transform(posetype(),way_pose);
00065   
00066   float dist=Euclidean::DistanceTo(p1.map,
00067                                    p2.map);  
00068   
00069   // TODO: Make the + 1 a cfg option
00070   spot.p1 = MapXY(trans.apply_transform(posetype(0, p1.lane_width/2, 0)));
00071   spot.p2 = MapXY(trans.apply_transform(posetype(dist + 1, p2.lane_width/2, 0)));
00072   spot.p3 = MapXY(trans.apply_transform(posetype(dist + 1, -p2.lane_width/2, 0)));
00073   spot.p4 = MapXY(trans.apply_transform(posetype(0, -p1.lane_width/2, 0)));
00074 
00075   /*  
00076   fprintf(stderr, "p1=(%.3f, %.3f)\n", spot.p1.x, spot.p1.y);
00077   fprintf(stderr, "p2=(%.3f, %.3f)\n", spot.p2.x, spot.p2.y);
00078   fprintf(stderr, "p3=(%.3f, %.3f)\n", spot.p3.x, spot.p3.y);
00079   fprintf(stderr, "p4=(%.3f, %.3f)\n", spot.p4.x, spot.p4.y);
00080   */
00081   return spot;
00082 }
00083 
00084 int PARK_control::adjust_spot(std::vector<WayPointNode> &new_waypts,
00085                             ObstacleList obstacles,
00086                             float max_x_offset,
00087                             float max_y_offset,
00088                             float step_size) {
00089   PolyOps pops;
00090   int i = find_spot(new_waypts);
00091   if(i >= 0 && i < (int)new_waypts.size() - 1) {
00092     for(float dx = 0; dx <= max_x_offset; dx += step_size) {
00093       for(int x_sign = -1; x_sign <= 1; x_sign += 2) {
00094         for(float dy = 0; dy <= max_y_offset; dy += step_size) {
00095           for(int y_sign = -1; y_sign <= 1; y_sign += 2) {
00096             WayPointNode p1_new = new_waypts.at(i);
00097             WayPointNode p2_new = new_waypts.at(i+1);
00098             
00099             poly spot = adjust_spot_and_return_poly(p1_new,
00100                                                     p2_new,
00101                                                     x_sign * dx,
00102                                                     y_sign * dy);
00103             
00104             int obstacles_in_spot = 0;
00105             
00106             for(unsigned j = 0; j < obstacles.size(); j++) {
00107               if(pops.pointInPoly_ratio(obstacles[j], spot,1.1))
00108                 obstacles_in_spot++;
00109             }
00110             
00111             if(obstacles_in_spot <= min_obst) {
00112               float distt=sqrtf(dx*dx+dy*dy);
00113               if (distt <= min_adj_dist)
00114                 {
00115                   min_adj_dist=distt;
00116                   min_obst=obstacles_in_spot;
00117                   new_waypts.at(i) = p1_new;
00118                   new_waypts.at(i+1) = p2_new;
00119                   //          std::cerr<<"ADJUSTING SPOT BY X: "<<x_sign * dx
00120                   //                   <<" AND Y: "<<y_sign * dy<<"\n";
00121                   return 0;
00122                 }
00123             }
00124           }
00125         }
00126       }
00127     }
00128   }
00129   return -1;
00130 }
00131 
00132 PARK_control::PARK_control(Navigator *navptr, int _verbose):
00133   Controller(navptr, _verbose)
00134 {
00135   safety = new Safety(navptr, _verbose, 2);
00136   stop = new Stop(navptr, _verbose);
00137   halt = new Halt(navptr, _verbose);
00138   reset_me();
00139 
00140   state=hit_waypoint;
00141 }
00142 
00143 PARK_control::~PARK_control()
00144 {
00145   delete safety;
00146   delete stop;
00147   delete halt;
00148 }
00149 
00150 // configuration method
00151 void PARK_control::configure(ConfigFile* cf, int section)
00152 {
00153   park_max_speed = cf->ReadFloat(section, "park_max_speed", 0.5);
00154   
00155   // maximum speed when inside a park_control
00156   speed_limit = cf->ReadFloat(section, "park_control_speed_limit", 1.0);
00157   ART_MSG(2, "\tspeed limit for park_control is %.1f m/s", speed_limit);
00158   
00159   min_distance = cf->ReadFloat(section, "park_min_distance", 0.5);
00160   ART_MSG(2, "\tmin_distance for park_control is %.1f m/s", min_distance);
00161   
00162   min_theta = cf->ReadFloat(section, "park_min_theta", 0.25);
00163   ART_MSG(2, "\tmin_theta for park_control is %.1f m/s", min_theta);
00164   
00165   park_turn_ratio = cf->ReadFloat(section, "park_turn_ratio", 1.0);
00166   ART_MSG(2, "\tturn_ratio for park_control is %.3f", park_turn_ratio);
00167   
00168   find_a_better_spot = cf->ReadBool(section, "find_a_better_spot", false);
00169   ART_MSG(2, "\tfind_a_better_spot is %d", find_a_better_spot);
00170 
00171   find_spot_max_x_offset = cf->ReadFloat(section,
00172                                          "find_spot_max_x_offset", 1.0);
00173   ART_MSG(2, "\tfind_spot_max_x_offset is %.3f m", find_spot_max_x_offset);
00174 
00175   find_spot_max_y_offset = cf->ReadFloat(section,
00176                                          "find_spot_max_y_offset", 1.0);
00177   ART_MSG(2, "\tfind_spot_max_y_offset is %.3f m", find_spot_max_y_offset);
00178 
00179   find_spot_step_size = cf->ReadFloat(section, "find_spot_step_size", 0.1);
00180   ART_MSG(2, "\tfind_spot_step_size is %.3f m", find_spot_step_size);
00181 
00182   safety->configure(cf, section);
00183 }
00184 
00185 inline float real_random(float multi=1.0){
00186   return float(random())/RAND_MAX*multi;
00187 }
00188 
00189 
00190 // go to next park_control way-point
00191 //
00192 // returns: OK.
00193 //
00194 Controller::result_t PARK_control::control(pilot_command_t &pcmd,
00195                                          mapxy_list_t obs_list,
00196                                          bool voronoi_stuck) {
00197 
00198   result_t result = OK;  
00199   if (voronoi_stuck)
00200     {
00201       pcmd.velocity=speed_limit;
00202       navdata->reverse=true;
00203       pcmd.yawRate=0.0;
00204       result_t safe=safety->control(pcmd);
00205       int count=1;
00206       bool neg=false;
00207       float rand=real_random(Steering::maximum_yaw/2);
00208       while (safe>=Unsafe && fabsf(pcmd.yawRate) <= Steering::maximum_yaw)
00209         {
00210           if (!neg)
00211             pcmd.yawRate=rand*count;
00212           else
00213             {
00214               pcmd.yawRate=-rand*count;
00215               count++;
00216             }
00217           neg=!neg;
00218           pcmd.velocity=speed_limit;
00219           safe=safety->control(pcmd);
00220         }
00221       if (safe >= Unsafe)
00222         {
00223           pcmd.velocity=speed_limit;
00224           navdata->reverse=false;
00225           pcmd.yawRate=0;
00226           result_t safe=safety->control(pcmd);
00227           int count =1;
00228           bool neg=false;
00229           while (safe>=Unsafe && fabsf(pcmd.yawRate) <= Steering::maximum_yaw)
00230             {
00231               if (!neg)
00232                 pcmd.yawRate=rand*count;
00233               else
00234                 {
00235                   pcmd.yawRate=-rand*count;
00236                   count++;
00237                 }
00238               neg=!neg;
00239               pcmd.velocity=speed_limit;
00240               safe=safety->control(pcmd);
00241             }     
00242         }
00243       // must always run this on the final command
00244       result = safety->control(pcmd);      
00245       
00246 
00247 
00248       last_park_dist=pcmd.velocity;
00249       if (navdata->reverse)
00250         last_park_dist*=-1;
00251       last_park_turn=pcmd.yawRate;
00252       
00253       if (result >=Unsafe)
00254         {
00255           pcmd.velocity=speed_limit;
00256           navdata->reverse=true;
00257         }
00258       
00259       trace("park_control controller", pcmd, result);
00260       return result;
00261     }
00262 
00263   std::vector<WayPointNode> new_waypts;
00264   for(int i = 0; i < N_ORDER_WAYPTS; i++)
00265     new_waypts.push_back(order->waypt[i]);
00266 
00267   if(find_a_better_spot) {
00268     adjust_spot(new_waypts,
00269                 obstacle->lasers->all_obstacle_list,
00270                 find_spot_max_x_offset,
00271                 find_spot_max_y_offset,
00272                 find_spot_step_size);
00273   }
00274 
00275   if (!new_waypts[1].is_spot)
00276     state=hit_waypoint;
00277   else
00278     if ((new_waypts[1].is_spot && new_waypts[2].is_spot &&
00279          new_waypts[1].id.pt==1 && new_waypts[2].id.pt==2))
00280       state=approach_spot;
00281     else
00282       if (new_waypts[1].is_spot && new_waypts[2].is_spot &&
00283           new_waypts[1].id.pt==2 && new_waypts[2].id.pt==1)
00284         state=pull_in;
00285       else state=pull_out;
00286 
00287   if (course->spot_ahead())
00288     spot_points=course->calculate_spot_points(new_waypts);
00289   else if (!course->curr_spot())
00290     spot_points.clear();
00291 
00292   for (uint i=0; i<spot_points.size(); i++)
00293     obs_list.push_back(spot_points[i]);
00294 
00295 
00296 
00297         // Try going striaght back, then various ways of backwards and
00298         // turning.  Then forwards.
00299         
00300   
00301   switch (state)
00302     {
00303     case hit_waypoint:
00304     case approach_spot:
00305       {
00306         posetype way_pose(new_waypts[1].map.x,new_waypts[1].map.y,
00307                           atan2f(new_waypts[2].map.y-new_waypts[1].map.y,
00308                                  new_waypts[2].map.x-new_waypts[1].map.x));
00309         
00310         rotate_translate_transform trans;
00311         
00312         trans.find_transform(posetype(ArtVehicle::front_bumper_px,0,0),
00313                              way_pose);
00314         posetype npose=trans.apply_transform(posetype(0,0,0));
00315         
00316         new_end_pose.px=npose.x;
00317         new_end_pose.py=npose.y;
00318         new_end_pose.pa=npose.theta;
00319       }
00320       break;
00321     case pull_in:
00322       {
00323         posetype way_pose(new_waypts[1].map.x,new_waypts[1].map.y,
00324                           atan2f(new_waypts[1].map.y-new_waypts[2].map.y,
00325                                  new_waypts[1].map.x-new_waypts[2].map.x));
00326         
00327         rotate_translate_transform trans;
00328         
00329         trans.find_transform(posetype(ArtVehicle::front_bumper_px,0,0),
00330                              way_pose);
00331         posetype npose=trans.apply_transform(posetype());
00332         
00333         new_end_pose.px=npose.x;
00334         new_end_pose.py=npose.y;
00335         new_end_pose.pa=npose.theta;
00336       }
00337       break;
00338     case pull_out:
00339       {
00340         posetype way_pose(new_waypts[1].map.x,new_waypts[1].map.y,
00341                           atan2f(new_waypts[0].map.y-new_waypts[1].map.y,
00342                                  new_waypts[0].map.x-new_waypts[1].map.x));
00343         rotate_translate_transform trans;
00344         
00345         trans.find_transform(posetype(ArtVehicle::front_bumper_px*2.0,0,0),
00346                              way_pose);
00347         posetype npose=trans.apply_transform(posetype());
00348         
00349         new_end_pose.px=npose.x;
00350         new_end_pose.py=npose.y;
00351         new_end_pose.pa=npose.theta;
00352       }
00353       break;
00354     }
00355   
00356 
00357   
00358   pcmd.velocity=speed_limit;
00359   navdata->reverse=false;
00360   
00361   if (Epsilon::equal(estimate->vel.px,0.0))
00362     halting=false;
00363   else if (Epsilon::gte(fabsf(estimate->vel.px),park_max_speed))
00364     halting=true;
00365   //halting=false;
00366   
00367   //Find Parking controller and run first step.
00368   result = initialize(pcmd, obs_list);
00369   
00370   if (result==Finished)
00371     {
00372       ART_MSG(3,"PARK Finished");
00373       halting=true;
00374       halt->control(pcmd);
00375       return result;
00376     }
00377   else 
00378     if (halting)
00379       {
00380         //      ART_MSG(1,"PARK Halting");
00381         pcmd.velocity=0.0;
00382         pcmd.yawRate=lastYaw;
00383         return OK;
00384       }
00385     else
00386       {
00387         if (result==OK)
00388           {
00389             
00390             if (verbose >= 2)
00391               {
00392                 ART_MSG(5, "Go to pose %.3f,%.3f,%.3f",
00393                         new_end_pose.px, new_end_pose.py, new_end_pose.pa);
00394               } 
00395           }
00396         else
00397           {
00398             // Try going striaght back, then various ways of backwards and
00399             // turning.  Then forwards.
00400             
00401             ART_MSG(3,"Not App.");
00402             pcmd.velocity=speed_limit;
00403             navdata->reverse=true;
00404             pcmd.yawRate=0;
00405             result_t safe=safety->control(pcmd);
00406             int count=1;
00407             bool neg=false;
00408             float rand=real_random(Steering::maximum_yaw/2);
00409             while (safe>=Unsafe && fabsf(pcmd.yawRate) <= Steering::maximum_yaw)
00410               {
00411                 if (!neg)
00412                   pcmd.yawRate=rand*count;
00413                 else
00414                   {
00415                     pcmd.yawRate=-rand*count;
00416                     count++;
00417                   }
00418                 neg=!neg;
00419                 pcmd.velocity=speed_limit;
00420                 safe=safety->control(pcmd);
00421               }
00422             if (safe >= Unsafe)
00423               {
00424                 pcmd.velocity=speed_limit;
00425                 navdata->reverse=false;
00426                 pcmd.yawRate=0;
00427                 result_t safe=safety->control(pcmd);
00428                 int count =1;
00429                 bool neg=false;
00430                 while (safe>=Unsafe && fabsf(pcmd.yawRate) <= Steering::maximum_yaw)
00431                   {
00432                     if (!neg)
00433                       pcmd.yawRate=rand*count;
00434                     else
00435                       {
00436                         pcmd.yawRate=-rand*count;
00437                         count++;
00438                       }
00439                     neg=!neg;
00440                     pcmd.velocity=speed_limit;
00441                     safe=safety->control(pcmd);
00442                   }     
00443               }
00444           }
00445         
00446       }
00447   
00448   
00449   // must always run this on the final command
00450   result = safety->control(pcmd);      
00451 
00452   if (result >=Unsafe)
00453     {
00454       pcmd.velocity=speed_limit;
00455       navdata->reverse=true;
00456     }
00457   
00458   pcmd.velocity=fminf(speed_limit,pcmd.velocity);
00459   
00460   last_park_dist=pcmd.velocity;
00461   if (navdata->reverse)
00462     last_park_dist*=-1;
00463   last_park_turn=pcmd.yawRate;
00464   
00465   lastYaw=pcmd.yawRate;
00466   trace("park_control controller", pcmd, result);
00467   return result;
00468 }
00469 
00470 // reset all subordinate controllers
00471 void PARK_control::reset(void)
00472 {
00473   trace_reset("PARK_control");
00474   reset_me();
00475   halt->reset();
00476   safety->reset();
00477   stop->reset();
00478 }
00479 
00480 
00481 // reset controller
00482 void PARK_control::reset_me(void)
00483 {
00484   navdata->reverse = false;
00485   halting=true;
00486   park_distance=0.0;
00487   park_turn=0.0;
00488   last_park_turn=0.0;
00489   last_park_dist=0.0;
00490   min_adj_dist=Infinite::distance;
00491   min_obst=10000;
00492 }
00493 
00494 
00495 // initialize U-turn operation
00496 Controller::result_t PARK_control::initialize(pilot_command_t& pcmd,
00497                                             const mapxy_list_t& obs_list)
00498 {
00499   int pcase=1;
00500 
00501   // Bearing to goal
00502   park_turn =Coordinates::bearing(estimate->pos,MapXY(new_end_pose));
00503   // Distance to goal
00504   park_distance = Euclidean::DistanceTo(new_end_pose,estimate->pos);
00505   
00506   // heading offset
00507   float dheading=Coordinates::normalize(new_end_pose.pa-estimate->pos.pa);
00508   
00509 
00510 
00511   if (hit_way_point(park_distance, park_turn, dheading) &&
00512       Epsilon::equal(estimate->vel.px,0.0))
00513     return Finished;  
00514   
00515   if (halting)
00516     return OK;
00517 
00518   float b=fabsf(park_turn);
00519   float h=fabsf(dheading);
00520 
00521 
00522 
00523   if (b < DTOR(45))
00524     if (h < DTOR(45))
00525       pcase=1;
00526     else if (h <DTOR(135) && dheading > 0)
00527       pcase=5;
00528     else if (h <DTOR(135) && dheading < 0)
00529       pcase=7;
00530     else pcase=6;
00531   else if (b < DTOR(135) && park_turn >0)
00532     if (h < DTOR(45))
00533       pcase=8;
00534     else if (h <DTOR(135) && dheading > 0)
00535       pcase=2;
00536     else if (h <DTOR(135) && dheading < 0)
00537       pcase=10;
00538     else pcase=9;
00539   else if (b < DTOR(135) && park_turn < 0)
00540     if (h < DTOR(45))
00541       pcase=14;
00542     else if (h <DTOR(135) && dheading > 0)
00543       pcase=15;
00544     else if (h <DTOR(135) && dheading < 0)
00545       pcase=4;
00546     else pcase=16;
00547   else
00548     if (h < DTOR(45))
00549       pcase=11;
00550     else if (h <DTOR(135) && dheading > 0)
00551       pcase=12;
00552     else if (h <DTOR(135) && dheading < 0)
00553       pcase=13;
00554     else pcase=3;
00555 
00556   std::cerr << "Bearing: "<<RTOD(park_turn)<<"\t dhead: "<<RTOD(dheading)<<"\t distance "<<park_distance<<std::endl;
00557   std::cerr << "Case: "<<pcase<<"\n";
00558 
00559   switch(pcase)
00560     {
00561     case 1:
00562       if (fabsf(sinf(park_turn)*park_distance) <
00563           fabsf(sinf(dheading)*(park_distance+ 3.5)))
00564         park_turn=dheading;
00565       break;
00566     case 2:
00567     case 4:
00568       if (fabsf(dheading) > fabsf(park_turn))
00569         park_turn = dheading;
00570       break;
00571     case 3:
00572       {
00573         float new_turn=Coordinates::normalize(park_turn+M_PI);
00574         float new_head=Coordinates::normalize(dheading+M_PI);
00575         if (fabsf(sinf(new_turn)*park_distance) <
00576             fabsf(sinf(new_head)*(park_distance+ 3.5)))
00577           park_turn=dheading;
00578         park_distance*=-1;
00579         navdata->reverse=true;
00580       }
00581       break;
00582     case 5:
00583     case 6:
00584     case 7:
00585     case 9:
00586     case 16:
00587       break;
00588     case 8:
00589     case 10:
00590     case 14:
00591     case 15:
00592       park_turn*=1;
00593       park_distance*=-1;
00594       navdata->reverse=true;
00595       break;
00596     case 11:
00597       park_distance*=-1;
00598       navdata->reverse=true;
00599       park_turn=0;
00600       break;
00601     case 12:
00602     case 13:
00603       park_turn=dheading;
00604       park_distance*=-1;
00605       navdata->reverse=true;
00606       break;
00607     }
00608 
00609   if (fabsf(park_distance) < fmaxf(Steering::steer_speed_min,pcmd.velocity) && fabsf(park_turn) < Steering::maximum_yaw)
00610     pcmd.yawRate=park_turn;
00611   else
00612     pcmd.yawRate= fmaxf(Steering::steer_speed_min, pcmd.velocity)/
00613       fabs(park_distance)* park_turn * park_turn_ratio;
00614   pcmd.yawRate=fmaxf(-Steering::maximum_yaw/2,
00615                      fminf(Steering::maximum_yaw/2,pcmd.yawRate));
00616   
00617   if (safety->control(pcmd) < Unsafe)
00618     {
00619       return OK;
00620     }
00621   pcmd.velocity=speed_limit;
00622   navdata->reverse=true;
00623   pcmd.yawRate=dheading;
00624 
00625   if (safety->control(pcmd) < Unsafe)
00626     {
00627       return OK;
00628     }
00629 
00630   navdata->reverse=!navdata->reverse;
00631   pcmd.yawRate*=1;
00632   pcmd.velocity=speed_limit;
00633   
00634   if (safety->control(pcmd) < Unsafe)
00635     {
00636       return OK;
00637     }  
00638 
00639 
00640   return NotApplicable;
00641 
00642 }
00643 
00644 bool PARK_control::hit_way_point(float goal_distance, float goal_turn,
00645                                float dheading) {
00646   bool done=false;
00647   
00648   float car_bearing=Coordinates::bearing(new_end_pose,MapXY(estimate->pos));
00649   
00650   switch(state)
00651     {
00652     case hit_waypoint:
00653     case approach_spot:
00654       {
00655         if (fabsf(dheading) < DTOR(30))
00656           if (goal_distance < 1.0 ||
00657               (fabsf(car_bearing) < HALFPI))
00658             done=true;
00659       }
00660       break;
00661     case pull_in:
00662       {
00663         //      if (fabsf(dheading) < DTOR(20))
00664         if (goal_distance < 0.75 ||
00665             fabsf(car_bearing) < HALFPI)
00666           done=true;
00667       }
00668       break;
00669     case pull_out:
00670       {
00671         if (fabsf(dheading) > DTOR(45) ||
00672             fabsf(car_bearing) > HALFPI ||
00673             goal_distance < 1.0)
00674           //        fabsf(goal_turn) < HALFPI)
00675           done=true;
00676       }
00677       break;
00678     }      
00679 
00680   course->no_waypoint_reached();
00681 
00682   if (done)
00683     {
00684       navdata->last_waypt=order->waypt[1].id;
00685     }
00686 
00687   return done;
00688   
00689 }
00690 
00691 bool PARK_control::small_segment(float distance, float turn) {
00692   return (fabsf(distance) < min_distance &&
00693           fabsf(turn) < min_theta);
00694 }
00695