00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include "command.h"
00015 #include "FSM.h"
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050 CmdrFSM::CmdrFSM(Commander *cmdr_ptr, int verbosity)
00051 {
00052 verbose = verbosity;
00053 cmdr = cmdr_ptr;
00054
00055
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
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
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
00131 CmdrEvent event = current_event();
00132
00133
00134 transtion_t *xp = &ttable[event.Value()][state.Value()];
00135
00136
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
00145 action_t action = xp->action;
00146 return (this->*action)(event);
00147 }
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159 CmdrEvent CmdrFSM::current_event()
00160 {
00161
00162
00163 if (cmdr->route->size()==0)
00164 {
00165 current_way = ElementID(navstate.last_waypt);
00166
00167
00168
00169 if (current_way == cmdr->goal.id)
00170 cmdr->next_checkpoint();
00171
00172 ROS_DEBUG("Calling EnterLane event");
00173 return CmdrEvent::EnterLane;
00174 }
00175
00176 bool new_goal1 = false;
00177 bool new_goal2 = false;
00178
00179
00180
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
00190
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
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
00215
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
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
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);
00251
00252
00253
00254 if (finished)
00255 {
00256
00257 event = CmdrEvent::Done;
00258 }
00259
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
00273 event = CmdrEvent::Wait;
00274 }
00275
00276
00277 ROS_DEBUG_STREAM("Current event = " << event.Name());
00278
00279 return event;
00280 }
00281
00282
00284
00286
00287
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
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
00331 return cmdr->prepare_order(art_msgs::Behavior::Go);
00332 }
00333
00334
00335
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
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 }