art_nav: art_nav: estop.cc Source File

00001 /*
00002  *  Navigator E-stop controller finite state machine
00003  *
00004  *  Copyright (C) 2007, 2010, Austin Robot Technology
00005  *
00006  *  License: Modified BSD Software License Agreement
00007  *
00008  *  $Id: estop.cc 1539 2011-05-09 04:09:20Z jack.oquin $
00009  */
00010 
00011 #include "navigator_internal.h"
00012 #include "Controller.h"
00013 #include "estop.h"
00014 
00015 #include "halt.h"
00016 #include "run.h"
00017 
00018 Estop::Estop(Navigator *navptr, int _verbose):
00019   Controller(navptr, _verbose)
00020 {
00021   // initialize transition table, unused entries cause an error action
00022   for (int event = 0; event < (int) NavEstopEvent::N_events; ++event)
00023     for (int state = 0; state < (int) NavEstopState::N_states; ++state)
00024       {
00025         transtion_t *xp = &ttable[event][state];
00026         xp->action = &Estop::ActionError;
00027         xp->next = (NavEstopState::state_t) state;
00028       }
00029 
00030   // initialize transition table:
00031 
00032   Add(NavEstopEvent::Abort,     &Estop::ActionToAbort,
00033       NavEstopState::Pause,     NavEstopState::Done);
00034   Add(NavEstopEvent::Abort,     &Estop::ActionToAbort,
00035       NavEstopState::Run,       NavEstopState::Done);
00036   Add(NavEstopEvent::Abort,     &Estop::ActionToAbort,
00037       NavEstopState::Suspend,   NavEstopState::Done);
00038   Add(NavEstopEvent::Abort,     &Estop::ActionInDone,
00039       NavEstopState::Done,      NavEstopState::Done);
00040 
00041   Add(NavEstopEvent::None,      &Estop::ActionInPause,
00042       NavEstopState::Pause,     NavEstopState::Pause);
00043   Add(NavEstopEvent::None,      &Estop::ActionInRun,
00044       NavEstopState::Run,       NavEstopState::Run);
00045   Add(NavEstopEvent::None,      &Estop::ActionInSuspend,
00046       NavEstopState::Suspend,   NavEstopState::Suspend);
00047   Add(NavEstopEvent::None,      &Estop::ActionInDone,
00048       NavEstopState::Done,      NavEstopState::Done);
00049 
00050   Add(NavEstopEvent::Pause,     &Estop::ActionInPause,
00051       NavEstopState::Pause,     NavEstopState::Pause);
00052   Add(NavEstopEvent::Pause,     &Estop::ActionToPause,
00053       NavEstopState::Run,       NavEstopState::Pause);
00054   Add(NavEstopEvent::Pause,     &Estop::ActionToPause,
00055       NavEstopState::Suspend,   NavEstopState::Pause);
00056   Add(NavEstopEvent::Pause,     &Estop::ActionInDone,
00057       NavEstopState::Done,      NavEstopState::Done);
00058 
00059   Add(NavEstopEvent::Quit,      &Estop::ActionToDone,
00060       NavEstopState::Pause,     NavEstopState::Done);
00061   Add(NavEstopEvent::Quit,      &Estop::ActionToDone,
00062       NavEstopState::Run,       NavEstopState::Done);
00063   Add(NavEstopEvent::Quit,      &Estop::ActionToDone,
00064       NavEstopState::Suspend,   NavEstopState::Done);
00065   Add(NavEstopEvent::Quit,      &Estop::ActionInDone,
00066       NavEstopState::Done,      NavEstopState::Done);
00067 
00068   Add(NavEstopEvent::Run,       &Estop::ActionToRun,
00069       NavEstopState::Pause,     NavEstopState::Run);
00070   Add(NavEstopEvent::Run,       &Estop::ActionInRun,
00071       NavEstopState::Run,       NavEstopState::Run);
00072   Add(NavEstopEvent::Run,       &Estop::ActionToRun,
00073       NavEstopState::Suspend,   NavEstopState::Run);
00074   Add(NavEstopEvent::Run,       &Estop::ActionInDone,
00075       NavEstopState::Done,      NavEstopState::Done);
00076 
00077   // Note that the Suspend event is allowed even in the Done state.
00078   // This allows driving the car with the joystick after the mission
00079   // is complete.  Although it is possible to go from Done to Suspend
00080   // to Run, the commander has already shut down, so the car will not
00081   // begin a new mission.
00082   Add(NavEstopEvent::Suspend,   &Estop::ActionToSuspend,
00083       NavEstopState::Pause,     NavEstopState::Suspend);
00084   Add(NavEstopEvent::Suspend,   &Estop::ActionToSuspend,
00085       NavEstopState::Run,       NavEstopState::Suspend);
00086   Add(NavEstopEvent::Suspend,   &Estop::ActionInSuspend,
00087       NavEstopState::Suspend,   NavEstopState::Suspend);
00088   Add(NavEstopEvent::Suspend,   &Estop::ActionToSuspend,
00089       NavEstopState::Done,      NavEstopState::Suspend);
00090 
00091   // allocate subordinate controllers
00092   run = new Run(navptr, _verbose);
00093   halt = new Halt(navptr, _verbose);
00094   reset_me();
00095 }
00096 
00097 Estop::~Estop()
00098 {
00099   delete halt;
00100   delete run;
00101 };
00102 
00103 // add a transition to the table
00104 void Estop::Add(NavEstopEvent::event_t event, action_t action,
00105                 NavEstopState::state_t from_state,
00106                 NavEstopState::state_t to_state)
00107 {
00108   transtion_t *xp = &ttable[event][from_state];
00109   xp->action = action;
00110   xp->next = to_state;
00111 }
00112 
00113 Controller::result_t Estop::control(pilot_command_t &pcmd)
00114 {
00115   event = current_event();
00116 
00117   // state transition table pointer
00118   transtion_t *xp = &ttable[event.Value()][state.Value()];
00119 
00120   // do state transition
00121   prev = state;
00122   state = xp->next;
00123 
00124   if (state != prev)
00125     {
00126       // update navigator state message
00127       //ROS_DEBUG_STREAM
00128       ROS_INFO_STREAM("Navigator E-stop state changing from " << prev.Name()
00129                        << " to " << state.Name()
00130                        << ", event = " << event.Name());
00131       navdata->estop.state = (art_msgs::EstopState::_state_type) state.Value();
00132     }
00133 
00134   // perform transition action, returning next Pilot command
00135   action_t action = xp->action;
00136   return (this->*action)(pcmd);
00137 }
00138 
00139 NavEstopEvent Estop::current_event(void)
00140 {
00141   NavEstopEvent nevent = pending_event;
00142 
00143   // see if event pending from previous cycle
00144   if (nevent == NavEstopEvent::None)
00145     {
00146       // no pending event, translate order behavior into FSM event
00147       switch (order->behavior.value)
00148         {
00149         case NavBehavior::Abort:
00150           nevent = NavEstopEvent::Abort;
00151           break;
00152 
00153         case NavBehavior::Pause:
00154           nevent = NavEstopEvent::Pause;
00155           break;
00156 
00157         case NavBehavior::Quit:
00158           nevent = NavEstopEvent::Quit;
00159           break;
00160 
00161         case NavBehavior::Run:
00162           nevent = NavEstopEvent::Run;
00163           break;
00164 
00165         case NavBehavior::Suspend:
00166           nevent = NavEstopEvent::Suspend;
00167           break;
00168 
00169         default:
00170           // Other behaviors are handled by lower-level controllers,
00171           // only while in Run state.  They do not affect the E-stop
00172           // controller.
00173           nevent = NavEstopEvent::None;
00174         }
00175     }
00176   else
00177     {
00178       // event pending from previous cycle
00179       pending_event = NavEstopEvent::None; // no longer pending
00180     }
00181   return nevent;
00182 }
00183 
00184 void Estop::reset(void)
00185 {
00186   trace_reset("Estop");
00187   reset_me();
00188   halt->reset();
00189   run->reset();
00190 }
00191 
00192 void Estop::reset_me(void)
00193 {
00194   state = NavEstopState();              // initial state
00195   pending_event = NavEstopEvent::None;
00196 }
00197 
00198 
00200 // state transition action methods
00202 
00203 Controller::result_t Estop::ActionError(pilot_command_t &pcmd)
00204 {
00205   ROS_FATAL("Invalid Navigator E-stop event %s, state %s",
00206             event.Name(), prev.Name());
00207   halt->control(pcmd);
00208   pending_event = NavEstopEvent::Abort;
00209   return NotImplemented;
00210 }
00211 
00212 // steady state actions
00213 
00214 Controller::result_t Estop::ActionInDone(pilot_command_t &pcmd)
00215 {
00216   navdata->flasher = false;
00217   navdata->alarm = false;
00218   return halt->control(pcmd);
00219 }  
00220 
00221 Controller::result_t Estop::ActionInPause(pilot_command_t &pcmd)
00222 {
00223   navdata->flasher = true;
00224   navdata->alarm = false;
00225   return halt->control(pcmd);
00226 }
00227 
00228 Controller::result_t Estop::ActionInRun(pilot_command_t &pcmd)
00229 {
00230   navdata->flasher = true;
00231   navdata->alarm = true;
00232   result_t result = run->control(pcmd);
00233   if (result == NotImplemented || 
00234       result == NotApplicable)
00235     pending_event = NavEstopEvent::Abort;
00236   return result;
00237 }
00238 
00239 Controller::result_t Estop::ActionInSuspend(pilot_command_t &pcmd)
00240 {
00241   navdata->flasher = true;
00242   navdata->alarm = false;
00243   return halt->control(pcmd);
00244 }
00245 
00246 // state entry actions
00247 
00248 Controller::result_t Estop::ActionToAbort(pilot_command_t &pcmd)
00249 {
00250   ART_MSG(1, "Robot disabled!");
00251   return ActionInDone(pcmd);
00252 }
00253 
00254 Controller::result_t Estop::ActionToDone(pilot_command_t &pcmd)
00255 {
00256   ART_MSG(1, "Mission accomplished!");
00257   return ActionInDone(pcmd);
00258 }
00259 
00260 Controller::result_t Estop::ActionToPause(pilot_command_t &pcmd)
00261 {
00262   ART_MSG(1, "Robot pausing!");
00263   return ActionInPause(pcmd);
00264 }
00265 
00266 Controller::result_t Estop::ActionToRun(pilot_command_t &pcmd)
00267 {
00269   ART_MSG(1, "Robot running!");
00270   return ActionInRun(pcmd);
00271 }
00272 
00273 Controller::result_t Estop::ActionToSuspend(pilot_command_t &pcmd)
00274 {
00275   ART_MSG(1, "Autonomous operation suspended!");
00276   // reset last_waypt, so commander will Initialize again for next Run 
00277   navdata->last_waypt = ElementID().toMapID();
00278   return ActionInSuspend(pcmd);
00279 }