evade.cc
Go to the documentation of this file.
00001 /*
00002  *  Navigator evade obstacle controller
00003  *
00004  *  Copyright (C) 2007, 2010, Austin Robot Technology
00005  *
00006  *  License: Modified BSD Software License Agreement
00007  *
00008  *  $Id: evade.cc 519 2010-09-01 04:39:33Z jack.oquin $
00009  */
00010 
00011 #include "navigator_internal.h"
00012 #include "Controller.h"
00013 #include "course.h"
00014 #include "evade.h"
00015 #include "obstacle.h"
00016 
00017 #include "halt.h"
00018 #include "lane_edge.h"
00019 #include "safety.h"
00020 #include "ntimer.h"
00021 
00022 static const char *state_name[] =
00023   {
00024     "Init",
00025     "Leave",
00026     "Wait",
00027     "Return",
00028   };
00029 
00030 Evade::Evade(Navigator *navptr, int _verbose):
00031   Controller(navptr, _verbose)
00032 {
00033   evade_timer = new NavTimer(nav->cycle);
00034   halt = new Halt(navptr, _verbose);
00035   lane_edge = new LaneEdge(navptr, _verbose);
00036   safety = new Safety(navptr, verbose);
00037   reset_me();
00038 }
00039 
00040 Evade::~Evade()
00041 {
00042   delete evade_timer;
00043   delete halt;
00044   delete lane_edge;
00045   delete safety;
00046 }
00047 
00048 void Evade::configure()
00049 {
00050   evade_delay = cf->ReadFloat(section, "evade_delay", 7.0);
00051   ART_MSG(2, "\tevade delay is %.1f seconds", evade_delay);
00052 
00053   evade_offset_ratio = cf->ReadFloat(section, "evade_offset_ratio", -2.0);
00054   ART_MSG(2, "\tevade offset ratio is %.3f", evade_offset_ratio);
00055 
00056   evasion_speed = cf->ReadFloat(section, "evasion_speed", 3.0);
00057   ART_MSG(2, "\tevasion speed is %.1f m/s", evasion_speed);
00058 
00059   halt->configure(cf, section);
00060   lane_edge->configure(cf, section);
00061   safety->configure(cf, section);
00062 }
00063 
00064 // This controller is called from Road when a car is approaching in
00065 // the current lane.  Its job is to leave the lane to the right, wait
00066 // until there is no longer a car approaching, then return Finished.
00067 //
00068 // returns:
00069 //      safety controller results, normally;
00070 //      Finished, when clear to return to normal travel lane.
00071 Controller::result_t Evade::control(pilot_command_t &pcmd)
00072 {
00073   result_t result = Blocked;
00074 
00075   if (verbose >= 4)
00076     ART_MSG(2, "Evade state is %s", state_name[state]);
00077 
00078   switch (state)
00079     {
00080     case Init:
00081       // Initial state of the Evade controller, reached via reset().
00082       if (verbose)
00083         ART_MSG(1, "Begin emergency obstacle evasion");
00084       course->turn_signal_on(false);    // signal right
00085       set_state(Leave);
00086       // fall through
00087 
00088     case Leave:
00089       if (course->in_lane(estimate->pos))       // still in lane?
00090         {
00091           // leave lane as long as car is still approaching.
00092           if (obstacle->car_approaching())
00093             {
00094               result = leave_lane_right(pcmd);
00095             }
00096           else
00097             {
00098               if (verbose)
00099                 ART_MSG(1, "No longer need to evade approaching car");
00100               halt->control(pcmd);
00101               course->turn_signal_on(true); // signal left
00102               result = Finished;
00103             }
00104           break;                        // done for this cycle
00105         }
00106 
00107       if (verbose)
00108         ART_MSG(1, "Left travel lane while evading");
00109       set_state(Wait);
00110       // TODO: include obstacle->observation(Nearest_forward).time in
00111       // the delay?
00112       course->turn_signals_both_on();
00113       evade_timer->Start(evade_delay);
00114       // fall through
00115 
00116     case Wait:
00117       // out of lane now, wait until timer expires.
00118       result = halt->control(pcmd);
00119       if (evade_timer->Check())         // timer expired?
00120         {
00121           if (verbose)
00122             ART_MSG(1, "Emergency obstacle evasion timeout");
00123           course->turn_signal_on(true); // signal left
00124           set_state(Return);
00125         }
00126       break;
00127 
00128     case Return:
00129       // Car has been stopped long enough for the timer to expire.
00130       evade_timer->Cancel();
00131       halt->control(pcmd);
00132 
00133       // Regardless of timeout, stay put as long as there is still a
00134       // car approaching
00135       if (obstacle->car_approaching())
00136         result = OK;
00137       else
00138         result = Finished;
00139       break;
00140     }
00141 
00142   // mark way-point reached, if any was bypassed
00143   course->lane_waypoint_reached();
00144 
00145   if (result == Blocked)
00146     result = Unsafe;                    // never return Blocked
00147 
00148   trace("evade controller", pcmd, result);
00149   return result;
00150 }
00151 
00152 // leave lane to the right
00153 //
00154 // returns: safety controller result
00155 Controller::result_t Evade::leave_lane_right(pilot_command_t &pcmd)
00156 {
00157   // go slowly while leaving lane
00158   pcmd.velocity = fminf(pcmd.velocity, evasion_speed);
00159 
00160   result_t result = Unsafe;
00161   if (obstacle->observer_clear(ObserverID::Adjacent_right))
00162     {
00163       result = lane_edge->control(pcmd, evade_offset_ratio);
00164       if (result < Unsafe)
00165         {
00166           // evade right did not stop immediately
00167           if (verbose >= 2)
00168             ART_MSG(5, "leaving lane to right");
00169         }
00170     }
00171   else if (verbose >= 2)
00172     ART_MSG(5, "right lane blocked, not heading for that lane");
00173   return result;
00174 }
00175 
00176 // reset all subordinate controllers
00177 void Evade::reset(void)
00178 {
00179   trace_reset("Evade");
00180   reset_me();
00181   halt->reset();
00182   lane_edge->reset();
00183   safety->reset();
00184 }
00185 
00186 // reset this controller
00187 void Evade::reset_me(void)
00188 {
00189   evade_timer->Cancel();
00190   state = Init;
00191 }
00192 
00193 // perform state transition
00194 void Evade::set_state(state_t newstate)
00195 {
00196   if (state != newstate)
00197     {
00198       if (verbose)
00199         ART_MSG(4, "Evade state changing from %s to %s",
00200                 state_name[state], state_name[newstate]);
00201       state = newstate;
00202     }
00203 }


art_nav
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:08:43