safety.cc
Go to the documentation of this file.
00001 #include "safety.h"
00002 #include "Controller.h"
00003 #include "obstacle.h"
00004 #include <art/steering.h>
00005 #include <art/Safety.h>
00006 
00007 Safety::Safety(Navigator *navptr, int _verbose, int mode):
00008   Controller(navptr, _verbose)
00009   {
00010     _mode=mode; // 0 for normal, 1 for zone, 2 for parking
00011     safety = new Safety_Distance(_verbose);
00012   }
00013 
00014 Safety::~Safety()
00015   {
00016     delete safety;
00017   }
00018 
00019 void Safety::configure(ConfigFile* cf, int section)
00020 {
00021 
00022   if (_mode==0)
00023     {
00024       far_slow_ratio = cf->ReadFloat(section, "far_slow_ratio", 0.75);
00025       near_slow_ratio = cf->ReadFloat(section, "near_slow_ratio", 0.5);
00026       
00027       far_safety_time = cf->ReadFloat(section, "far_safety_time", 3);
00028       near_safety_time = cf->ReadFloat(section, "near_safety_time", 2);
00029       collision_safety_time = cf->ReadFloat(section, "collision_safety_time", 1);
00030     }
00031   else if (_mode==1)
00032     {
00033       far_slow_ratio = cf->ReadFloat(section, "zone_far_slow_ratio", 0.75);
00034       near_slow_ratio = cf->ReadFloat(section, "zone_near_slow_ratio", 0.5);
00035       
00036       far_safety_time = cf->ReadFloat(section, "zone_far_safety_time", 3);
00037       near_safety_time = cf->ReadFloat(section, "zone_near_safety_time", 2);
00038       collision_safety_time = cf->ReadFloat(section, "zone_collision_safety_time", 1.0);
00039     }
00040   else{
00041     far_slow_ratio = cf->ReadFloat(section, "parking_far_slow_ratio", 0.75);
00042     near_slow_ratio = cf->ReadFloat(section, "parking_near_slow_ratio", 0.5);
00043     
00044     far_safety_time = cf->ReadFloat(section, "parking_far_safety_time", 1.5);
00045     near_safety_time = cf->ReadFloat(section, "parking_near_safety_time", 1);
00046     collision_safety_time = cf->ReadFloat(section, "parking_collision_safety_time", 1.0);
00047   }
00048   
00049   safety_speed = cf->ReadFloat(section, "safety_speed", 2.0);
00050   
00051   ART_MSG(2, "Safety is in mode %d",_mode);
00052   ART_MSG(2, "\tMinimum nonzero safety speed is %.3f m/s", safety_speed);
00053   
00054   ART_MSG(2, "\tMultiply speed by %.2f if collision in %f seconds", 
00055           far_slow_ratio, far_safety_time);
00056   ART_MSG(2, "\tMultiply speed by %.2f if collision in %f seconds", 
00057           near_slow_ratio, near_safety_time);
00058   ART_MSG(2, "\tBrake immediately if collision in %f seconds", 
00059           near_safety_time);
00060 }
00061 
00062 // safety controller -- always runs last in every road state
00063 //
00064 // This controller is stateless, with no side-effects.  It may be
00065 // called more than once in a single cycle to explore avoidance
00066 // alternatives.  It may reduce the requested speed, but requests no
00067 // heading changes.
00068 //
00069 // returns:
00070 //      OK, if no immediate danger;
00071 //      Caution, if obstacle far away and reducing speed;
00072 //      Beware, if obstacle near and reducing speed;
00073 //      Unsafe, if collision imminent and stopping;
00074 //      Blocked, if stopped and unable to proceed.
00075 //
00076 Controller::result_t Safety::control(pilot_command_t &retpcmd)
00077 {
00078   pilot_command_t pcmd=retpcmd;
00079   if (navdata->reverse)
00080     pcmd.velocity*=-1;
00081 
00082   // Look to see if current velocity is going to hit anything.  If so
00083   // slam on brakes.
00084   player_pose2d_t tmp=estimate->pos;
00085   float min_val=Infinite::distance;
00086     
00087 
00088 
00089   float ret_val=
00090     safety->minimum_obstacle_to_arc(tmp,
00091                                     estimate->vel.px*collision_safety_time,
00092                                     estimate->vel.pa*collision_safety_time,
00093                                     obstacle->lasers->all_obstacle_list,
00094                                     tmp);
00095   if (Epsilon::equal(ret_val,0.0))
00096     {
00097       return halt_immediately(retpcmd);
00098     }    
00099 
00100   // While our current trajectory will hit something nearby, keep
00101   // reducing the speed.
00102   bool collision_obst=false;
00103   bool collision;
00104   result_t result = OK;
00105 
00106   do {
00107     tmp=estimate->pos;
00108     min_val=Infinite::distance;
00109     
00110     collision=false;
00111     
00112     float ret_val=
00113       safety->minimum_obstacle_to_arc(tmp,
00114                                       pcmd.velocity*near_safety_time,
00115                                       pcmd.yawRate*near_safety_time,
00116                                       obstacle->lasers->all_obstacle_list,
00117                                       tmp);
00118     if (Epsilon::equal(ret_val,0.0))
00119       {
00120         // collision imminent
00121         collision_obst=true;
00122         collision=true;
00123         result = Beware;
00124         if (pcmd.velocity > safety_speed/near_slow_ratio)
00125           {
00126             if (verbose >= 3)
00127               ART_MSG(2, "Collision potential, multiply by %.3f: "
00128                       "from %.3f, %.3f",
00129                       near_slow_ratio, pcmd.velocity, pcmd.yawRate);
00130             pcmd.velocity*=near_slow_ratio;
00131             //pcmd.yawRate*=near_slow_ratio;
00132           }
00133         else if (pcmd.velocity > safety_speed)
00134           {
00135             float ratio=safety_speed/pcmd.velocity;
00136             if (verbose >= 3)
00137               ART_MSG(2, "Collision potential, multiply by %.3f: "
00138                       "from %.3f, %.3f",
00139                       ratio, pcmd.velocity, pcmd.yawRate);
00140             pcmd.velocity*=ratio;
00141             //pcmd.yawRate*=ratio;
00142           }
00143         else
00144           {
00145             return halt_immediately(retpcmd);
00146           }
00147       }
00148   } while (collision);
00149   
00150   if (collision_obst)
00151     {
00152       retpcmd=pcmd;
00153       retpcmd.velocity=fabsf(retpcmd.velocity);
00154       trace("safety controller", retpcmd, result);
00155       return result;
00156     }
00157 
00158   // If something out ahead, slow down a bit.  
00159 
00160   // tmp should already be at point near_safety_time seconds ahead of
00161   // current position
00162 
00163   ret_val=
00164     safety->minimum_obstacle_to_arc(tmp,
00165                                     pcmd.velocity*(far_safety_time-near_safety_time),
00166                                     pcmd.yawRate*(far_safety_time-near_safety_time),
00167                                     obstacle->lasers->all_obstacle_list,
00168                                     tmp);
00169   if (Epsilon::equal(ret_val,0.0))
00170     {
00171       // no immediate collision, but danger ahead
00172       result = Caution;
00173       if (pcmd.velocity > safety_speed/far_slow_ratio)
00174         {
00175           if (verbose >= 3)
00176             ART_MSG(2, "Danger ahead, slowing by %.3f times: from %.3f, %.3f ", 
00177                     far_slow_ratio, pcmd.velocity, pcmd.yawRate);
00178           pcmd.velocity=pcmd.velocity*far_slow_ratio;
00179           //pcmd.yawRate=pcmd.yawRate*far_slow_ratio;
00180         }
00181       else if (pcmd.velocity > safety_speed)
00182         {
00183           float ratio=safety_speed/pcmd.velocity;
00184           if (verbose >= 3)
00185             ART_MSG(2, "Danger ahead, slowing by %.3f times: from %.3f, %.3f ", 
00186                     ratio, pcmd.velocity, pcmd.yawRate);
00187           pcmd.velocity=pcmd.velocity*ratio;
00188           //pcmd.yawRate=pcmd.yawRate*ratio;
00189         }
00190     }    
00191   retpcmd=pcmd;
00192   retpcmd.velocity=fabsf(retpcmd.velocity);
00193   trace("safety controller", pcmd, result);
00194   return result;
00195 }
00196 
00197 // return result and Pilot command for an immediate halt
00198 //
00199 // returns:
00200 //      Unsafe, if car still moving
00201 //      Blocked, if car already stopped
00202 Controller::result_t Safety::halt_immediately(pilot_command_t &pcmd)
00203 {
00204   if (verbose >= 3)
00205     ART_MSG(2, "Collision imminent, halting: from %.3f, %.3f",
00206             pcmd.velocity, pcmd.yawRate);
00207   pcmd.velocity=0.0;
00208   //pcmd.yawRate=0.0;
00209 
00210   // return Blocked if already stopped and still requesting halt
00211   result_t result = Unsafe;
00212   if (navdata->stopped)
00213     result = Blocked;
00214   trace("safety controller", pcmd, result);
00215   return result;
00216 }
 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