00001
00002
00003
00004
00005
00006
00007
00008
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
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
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
00078
00079
00080
00081
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
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
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
00118 transtion_t *xp = &ttable[event.Value()][state.Value()];
00119
00120
00121 prev = state;
00122 state = xp->next;
00123
00124 if (state != prev)
00125 {
00126
00127
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
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
00144 if (nevent == NavEstopEvent::None)
00145 {
00146
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
00171
00172
00173 nevent = NavEstopEvent::None;
00174 }
00175 }
00176 else
00177 {
00178
00179 pending_event = NavEstopEvent::None;
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();
00195 pending_event = NavEstopEvent::None;
00196 }
00197
00198
00200
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
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
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
00277 navdata->last_waypt = ElementID().toMapID();
00278 return ActionInSuspend(pcmd);
00279 }