FSM.cc
Go to the documentation of this file.
00001 /* -*- mode: C++ -*-
00002  *
00003  *  Commander finite state machine implementation
00004  *
00005  *  Copyright (C) 2007, 2010, Austin Robot Technology
00006  *
00007  *  License: Modified BSD Software License Agreement
00008  *
00009  *  $Id: FSM.cc 615 2010-09-24 16:07:50Z jack.oquin $
00010  *
00011  *  Author: Jack O'Quin and Patrick Beeson
00012  */
00013 
00014 #include "command.h"
00015 #include "FSM.h"
00016 
00017 // Commander state transition design notes:
00018 //
00019 // States are nodes in a directed graph representation of the
00020 // Commander finite state machine.  The arrows in this graph represent
00021 // a transition from one state to a (possibly) different one.  Each
00022 // arrow is labelled with an event which triggers that transition, and
00023 // has an associated action method.
00024 //
00025 // Make a matrix of all state transitions indexed by state and event
00026 // containing the action method and a (possibly) new state for each
00027 // arrow in the FSM graph.  This table-driven design is complex, but
00028 // it allows adding new states and events with minimal effect on the
00029 // existing implementation.
00030 //
00031 // Commander() logic:
00032 //
00033 //   return control();
00034 //
00035 // This will call current_event() to prioritize all events, returning
00036 // the most urgent.  It will check any timers that are running; timer
00037 // expirations are one set of possible events.  The event priorities
00038 // are independent of state.
00039 //
00040 // The control() method updates the current state, then calls the
00041 // transition-dependent action method from the state transition table.
00042 // Every action method returns a Commander order for this cycle.
00043 //
00044 // Commander could do multiple state transitions in a single cycle.
00045 // Since control() performs the state change before calling the action
00046 // method, in some cases that method might trigger another state
00047 // transition, if necessary.  Currently they do not, which is simpler.
00048 
00049 
00050 CmdrFSM::CmdrFSM(Commander *cmdr_ptr, int verbosity)
00051 {
00052   verbose = verbosity;
00053   cmdr = cmdr_ptr;
00054 
00055   // initialize transition table
00056   for (int event = 0; event < (int) CmdrEvent::N_events; ++event)
00057     for (int state = 0; state < (int) CmdrState::N_states; ++state)
00058       {
00059         transtion_t *xp = &ttable[event][state];
00060         xp->action = &CmdrFSM::ActionError;
00061         xp->next = (CmdrState::state_t) state;
00062       }
00063 
00064   // initialize transition table:
00065 
00066   Add(CmdrEvent::Blocked,       &CmdrFSM::ActionInDone,
00067       CmdrState::Done,          CmdrState::Done);
00068   Add(CmdrEvent::Blocked,       &CmdrFSM::ActionInInit,
00069       CmdrState::Init,          CmdrState::Init);
00070   Add(CmdrEvent::Blocked,       &CmdrFSM::BlockedInRoad,
00071       CmdrState::Road,          CmdrState::Road);
00072 
00073   Add(CmdrEvent::Done,          &CmdrFSM::ActionInDone,
00074       CmdrState::Done,          CmdrState::Done);
00075   Add(CmdrEvent::Done,          &CmdrFSM::ActionToDone,
00076       CmdrState::Init,          CmdrState::Done);
00077   Add(CmdrEvent::Done,          &CmdrFSM::ActionToDone,
00078       CmdrState::Road,          CmdrState::Done);
00079 
00080   Add(CmdrEvent::EnterLane,     &CmdrFSM::ActionInDone,
00081       CmdrState::Done,          CmdrState::Done);
00082   Add(CmdrEvent::EnterLane,     &CmdrFSM::InitToRoad,
00083       CmdrState::Init,          CmdrState::Road);
00084   Add(CmdrEvent::EnterLane,     &CmdrFSM::ActionInRoad,
00085       CmdrState::Road,          CmdrState::Road);
00086 
00087   Add(CmdrEvent::Fail,          &CmdrFSM::ActionInDone,
00088       CmdrState::Done,          CmdrState::Done);
00089   Add(CmdrEvent::Fail,          &CmdrFSM::ActionFail,
00090       CmdrState::Init,          CmdrState::Done);
00091   Add(CmdrEvent::Fail,          &CmdrFSM::ActionFail,
00092       CmdrState::Road,          CmdrState::Done);
00093 
00094 
00095   Add(CmdrEvent::None,          &CmdrFSM::ActionInDone,
00096       CmdrState::Done,          CmdrState::Done);
00097   Add(CmdrEvent::None,          &CmdrFSM::ActionInInit,
00098       CmdrState::Init,          CmdrState::Init);
00099   Add(CmdrEvent::None,          &CmdrFSM::ActionInRoad,
00100       CmdrState::Road,          CmdrState::Road);
00101 
00102   Add(CmdrEvent::Wait,          &CmdrFSM::ActionInDone,
00103       CmdrState::Done,          CmdrState::Done);
00104   Add(CmdrEvent::Wait,          &CmdrFSM::ActionInInit,
00105       CmdrState::Init,          CmdrState::Init);
00106   Add(CmdrEvent::Wait,          &CmdrFSM::ActionWait,
00107       CmdrState::Road,          CmdrState::Road);
00108 
00109   Add(CmdrEvent::Replan,        &CmdrFSM::ActionInDone,
00110       CmdrState::Done,          CmdrState::Done);
00111   Add(CmdrEvent::Replan,        &CmdrFSM::ActionInInit,
00112       CmdrState::Init,          CmdrState::Init);
00113   Add(CmdrEvent::Replan,        &CmdrFSM::ReplanInRoad,
00114       CmdrState::Road,          CmdrState::Road);
00115 }
00116 
00117 // add a transition to the table
00118 void CmdrFSM::Add(CmdrEvent::event_t event, action_t action,
00119                   CmdrState::state_t from_state, CmdrState::state_t to_state)
00120 {
00121   transtion_t *xp = &ttable[event][from_state];
00122   xp->action = action;
00123   xp->next = to_state;
00124 }
00125 
00126 art_msgs::Order CmdrFSM::control(const art_msgs::NavigatorState *_navstate)
00127 {
00128   navstate = *_navstate;
00129 
00130   // get highest priority current event
00131   CmdrEvent event = current_event();
00132 
00133   // state transition structure pointer
00134   transtion_t *xp = &ttable[event.Value()][state.Value()];
00135 
00136   // do state transition
00137   prev = state;
00138   state = xp->next;
00139   if ((state != prev.Value()))
00140     ROS_INFO_STREAM("Commander state changing from " << prev.Name()
00141                      << " to " << state.Name()
00142                      << ", event = " << event.Name());
00143 
00144   // perform transition action, returning next order
00145   action_t action = xp->action;
00146   return (this->*action)(event);
00147 }
00148 
00149 // return most urgent current event
00150 //
00151 // entry:
00152 //      Navigator in Run state.
00153 //      Route initialized.
00154 //      navstate points to current Navigator state message
00155 //
00156 // events with lower numeric values have priority
00157 // less urgent events must remain pending
00158 //
00159 CmdrEvent CmdrFSM::current_event()
00160 {
00161   
00162   // Route is only 0 before we have ever made a plan.
00163   if (cmdr->route->size()==0)
00164     {
00165       current_way = ElementID(navstate.last_waypt);
00166 
00167       // It's entirely possible that the starting point is our first
00168       // goal, if so, check it off now.
00169       if (current_way == cmdr->goal.id)
00170         cmdr->next_checkpoint();
00171       // needed to get from Init to Road state
00172       ROS_DEBUG("Calling EnterLane event");
00173       return CmdrEvent::EnterLane;
00174     }
00175   
00176   bool new_goal1 = false;
00177   bool new_goal2 = false;
00178 
00179   // Walk through plan ticking off states until we see last_waypt.
00180   // Mark if we've passed any goals along the way.
00181   if (ElementID(navstate.last_waypt) != current_way)
00182     {
00183       ElementID old_way=current_way;
00184 
00185       WayPointEdge first_edge;      
00186 
00187       do {
00188 
00189         // Get next edge from plan.  Usually last_waypt is the first node
00190         // in the edge.
00191         cmdr->route->pop_front();
00192         first_edge=cmdr->route->at(0);
00193         WayPointNode* current_node=
00194           cmdr->graph->get_node_by_index(first_edge.startnode_index);
00195         if (current_node == NULL)
00196           {
00197             ROS_ERROR_STREAM("node " << first_edge.startnode_index
00198                              << " is not in the RNDF graph");
00199             return CmdrEvent::Fail;
00200           }
00201         
00202         current_way=current_node->id;
00203 
00204         // Check to see if we passed a goal recently
00205         if (current_way == cmdr->goal.id)
00206           new_goal1=true;
00207 
00208         if (current_way == cmdr->goal2.id)
00209           new_goal2=true;
00210         
00211       } while (ElementID(navstate.last_waypt) != current_way &&
00212                cmdr->route->size() > 1);
00213       
00214       // If never found matching waypoint in plan, then its the last
00215       // thing in the plan.
00216       if (ElementID(navstate.last_waypt) != current_way)
00217         {
00218           WayPointNode* current_node=
00219             cmdr->graph->get_node_by_index(first_edge.endnode_index);
00220           if (current_node == NULL)
00221             {
00222               ROS_ERROR_STREAM("node " << first_edge.endnode_index
00223                                << " is not in the RNDF graph");
00224               return CmdrEvent::Fail;
00225             }
00226           
00227           current_way=current_node->id;
00228 
00229           // Check to see if we passed a goal recently
00230           if (current_way == cmdr->goal.id)
00231             new_goal1=true;
00232           
00233           if (current_way == cmdr->goal2.id)
00234             new_goal2=true;
00235         }       
00236 
00237       ROS_INFO_STREAM("current waypoint changed from "
00238                       << old_way.name().str
00239                       << " to " << current_way.name().str);
00240     }
00241   
00242   bool finished=false;
00243   
00244   // Find new goals if we've passed one recently.
00245   if (new_goal1)
00246     finished = !cmdr->next_checkpoint();
00247   if (new_goal1 && new_goal2)
00248     finished = !cmdr->next_checkpoint();
00249 
00250   CmdrEvent event(CmdrEvent::None);     // default event
00251   
00252   // Process events in order of urgency.  Use only the first, leaving
00253   // the rest pending.  Review these priorities carefully.
00254   if (finished)
00255     {
00256       // no more checkpoints
00257       event = CmdrEvent::Done;
00258     }
00259   // Check for Blocked before normal replanning at goal
00260   else if (ElementID(navstate.replan_waypt) != old_replan)
00261     {
00262       old_replan=navstate.replan_waypt;
00263       if (ElementID(navstate.replan_waypt) != ElementID())
00264         {
00265           if (navstate.road_blocked)
00266             event = CmdrEvent::Blocked;
00267           else event = CmdrEvent::Replan;
00268         }
00269     }
00270   else if (new_goal1 && !cmdr->replan_route())
00271     {
00272       // needed to re-plan, but could not
00273       event = CmdrEvent::Wait;
00274     }
00275   
00276   // log event selected and input states
00277   ROS_DEBUG_STREAM("Current event = " << event.Name());
00278   
00279   return event;
00280 }
00281 
00282 
00284 // state transition action methods
00286 
00287 // error actions
00288 
00289 art_msgs::Order CmdrFSM::ActionError(CmdrEvent event)
00290 {
00291   ROS_ERROR_STREAM("Invalid Commander event " << event.Name()
00292                    << " in state " << prev.Name());
00293   return ActionFail(event);
00294 }
00295 
00296 art_msgs::Order CmdrFSM::ActionFail(CmdrEvent event)
00297 {
00298   ROS_ERROR("Mission failure!");
00299   art_msgs::Order abort_order;
00300   abort_order.behavior.value = art_msgs::Behavior::Abort;
00301   return abort_order;
00302 
00303 }
00304 
00305 art_msgs::Order CmdrFSM::ActionWait(CmdrEvent event)
00306 {
00307   ROS_INFO_THROTTLE(10, "No replan.  Just wait it out.");
00308   return cmdr->prepare_order(art_msgs::Behavior::Go);
00309 }
00310 
00311 
00312 // steady state actions
00313 
00314 art_msgs::Order CmdrFSM::ActionInDone(CmdrEvent event)
00315 {
00316   art_msgs::Order done_order;
00317   done_order.behavior.value = art_msgs::Behavior::Quit;
00318   return done_order;
00319 }  
00320 
00321 art_msgs::Order CmdrFSM::ActionInInit(CmdrEvent event)
00322 {
00323   art_msgs::Order init_order;
00324   init_order.behavior.value = art_msgs::Behavior::Initialize;
00325   return init_order;
00326 }  
00327 
00328 art_msgs::Order CmdrFSM::ActionInRoad(CmdrEvent event)
00329 {
00330   // prepare order for navigator driver
00331   return cmdr->prepare_order(art_msgs::Behavior::Go);
00332 }  
00333 
00334 
00335 // state entry actions
00336 
00337 art_msgs::Order CmdrFSM::ActionToDone(CmdrEvent event)
00338 {
00339   ROS_INFO("Mission completed!");
00340   return ActionInDone(event);
00341 }
00342 
00343 art_msgs::Order CmdrFSM::ActionToRoad(CmdrEvent event)
00344 {
00345   ROS_INFO("On the road.");
00346   return ActionInRoad(event);
00347 }
00348 
00349 // re-planning transitions
00350 
00351 art_msgs::Order CmdrFSM::BlockedInRoad(CmdrEvent event)
00352 {
00353   ROS_INFO("Road blocked, making a new plan.");
00354 
00355   cmdr->blockages->add_block(navstate.replan_waypt);
00356 
00357   if (cmdr->replan_route() == false)
00358     return ActionWait(event);
00359   return ActionInRoad(event);
00360 }
00361 
00362 
00363 art_msgs::Order CmdrFSM::ReplanInRoad(CmdrEvent event)
00364 {
00365   ROS_INFO("Making new plan.");
00366   
00367   navstate.last_waypt=navstate.replan_waypt;
00368 
00369   if (cmdr->replan_route() == false)
00370     return ActionWait(event);
00371   return ActionInRoad(event);
00372 }
00373 
00374 
00375 art_msgs::Order CmdrFSM::InitToRoad(CmdrEvent event)
00376 {
00377   ROS_INFO("On the road, making initial plan.");
00378   
00379   if (cmdr->replan_route() == false)
00380     return ActionFail(event);
00381 
00382   return ActionInRoad(event);
00383 }


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