18 #ifndef PRBT_HARDWARE_SUPPORT_RUN_PERMITTED_STATE_MACHINE_H 19 #define PRBT_HARDWARE_SUPPORT_RUN_PERMITTED_STATE_MACHINE_H 26 #include <boost/msm/back/state_machine.hpp> 27 #include <boost/msm/front/functor_row.hpp> 28 #include <boost/msm/front/state_machine_def.hpp> 29 #include <boost/core/demangle.hpp> 37 #define COLOR_GREEN "\033[32m" 38 #define COLOR_GREEN_BOLD "\033[1;32m" 40 #define STATE_ENTER_OUTPUT \ 41 ROS_DEBUG_STREAM_NAMED("RunPermittedStateMachine", \ 42 "Event: " << className(boost::core::demangle(typeid(ev).name())) \ 43 << " - Entering: " << COLOR_GREEN_BOLD \ 44 << className(boost::core::demangle(typeid(*this).name())) << COLOR_GREEN); 45 #define STATE_EXIT_OUTPUT \ 46 ROS_DEBUG_STREAM_NAMED("RunPermittedStateMachine", \ 47 "Event: " << className(boost::core::demangle(typeid(ev).name())) \ 48 << " - Leaving: " << className(boost::core::demangle(typeid(*this).name()))); 49 #define ACTION_OUTPUT \ 50 ROS_DEBUG_STREAM_NAMED("RunPermittedStateMachine", \ 51 "Event: " << className(boost::core::demangle(typeid(ev).name())) \ 52 << " - Action: " << className(boost::core::demangle(typeid(*this).name()))); 95 namespace msm = boost::msm;
96 namespace mpl = boost::mpl;
120 : recover_op_(recover_operation), halt_op_(halt_operation), hold_op_(hold_operation), unhold_op_(unhold_operation)
130 template <
class Event,
class FSM>
135 template <
class Event,
class FSM>
143 template <
class Event,
class FSM>
148 template <
class Event,
class FSM>
157 template <
class Event,
class FSM>
162 template <
class Event,
class FSM>
171 template <
class Event,
class FSM>
176 template <
class Event,
class FSM>
185 template <
class Event,
class FSM>
190 template <
class Event,
class FSM>
199 template <
class Event,
class FSM>
204 template <
class Event,
class FSM>
252 template <
class EVT,
class FSM,
class SourceState,
class TargetState>
253 bool operator()(EVT
const& evt, FSM&, SourceState&, TargetState&)
255 return evt.run_permitted_;
261 template <
class EVT,
class FSM,
class SourceState,
class TargetState>
262 bool operator()(EVT
const& evt, FSM&, SourceState&, TargetState&)
264 return !evt.run_permitted_;
279 template <
class EVT,
class FSM,
class SourceState,
class TargetState>
280 void operator()(EVT
const& ev, FSM& fsm, SourceState&, TargetState&)
284 fsm.task_queue_.push(
AsyncRunPermittedTask(fsm.recover_op_, [&fsm]() { fsm.process_event(recover_done()); }));
295 template <
class EVT,
class FSM,
class SourceState,
class TargetState>
296 void operator()(EVT
const& ev, FSM& fsm, SourceState&, TargetState&)
300 fsm.task_queue_.push(
AsyncRunPermittedTask(fsm.halt_op_, [&fsm]() { fsm.process_event(halt_done()); }));
311 template <
class EVT,
class FSM,
class SourceState,
class TargetState>
312 void operator()(EVT
const& ev, FSM& fsm, SourceState&, TargetState&)
316 fsm.task_queue_.push(
AsyncRunPermittedTask(fsm.hold_op_, [&fsm]() { fsm.process_event(hold_done()); }));
327 template <
class EVT,
class FSM,
class SourceState,
class TargetState>
328 void operator()(EVT
const& ev, FSM& fsm, SourceState&, TargetState&)
332 fsm.task_queue_.push(
AsyncRunPermittedTask(fsm.unhold_op_, [&fsm]() { fsm.process_event(unhold_done()); }));
344 Row<RobotInactive, run_permitted_updated, Enabling, recover_start, run_permitted_true>,
345 Row<RobotInactive, run_permitted_updated, none, none, run_permitted_false>,
346 Row<Enabling, run_permitted_updated, none, none, run_permitted_true>,
347 Row<Enabling, run_permitted_updated, StopRequestedDuringEnable, none, run_permitted_false>,
348 Row<Enabling, recover_done, none, unhold_start, none>, Row<Enabling, unhold_done, RobotActive, none, none>,
349 Row<StopRequestedDuringEnable, run_permitted_updated, none, none, none>,
350 Row<StopRequestedDuringEnable, recover_done, Stopping, halt_start, none>,
351 Row<StopRequestedDuringEnable, unhold_done, Stopping, hold_start, none>,
352 Row<RobotActive, run_permitted_updated, none, none, run_permitted_true>,
353 Row<RobotActive, run_permitted_updated, Stopping, hold_start, run_permitted_false>,
354 Row<Stopping, run_permitted_updated, EnableRequestedDuringStop, none, run_permitted_true>,
355 Row<Stopping, hold_done, none, halt_start, none>, Row<Stopping, halt_done, RobotInactive, none, none>,
356 Row<EnableRequestedDuringStop, hold_done, none, halt_start, none>,
357 Row<EnableRequestedDuringStop, halt_done, Enabling, recover_start, none>,
358 Row<EnableRequestedDuringStop, run_permitted_updated, Stopping, none, run_permitted_false>,
359 Row<EnableRequestedDuringStop, run_permitted_updated, none, none, run_permitted_true>
386 #endif // PRBT_HARDWARE_SUPPORT_RUN_PERMITTED_STATE_MACHINE_H void on_exit(Event const &ev, FSM &)
RobotInactive initial_state
Initial state.
void on_entry(Event const &ev, FSM &)
void on_exit(Event const &ev, FSM &)
void operator()(EVT const &ev, FSM &fsm, SourceState &, TargetState &)
RunPermittedTaskQueue task_queue_
The task queue.
RunPermittedStateMachine_(const TServiceCallFunc &recover_operation, const TServiceCallFunc &halt_operation, const TServiceCallFunc &hold_operation, const TServiceCallFunc &unhold_operation)
Construct the front-end state machine. Store the required task execution functions.
TServiceCallFunc halt_op_
The halt operation.
run_permitted_updated(const bool run_permitted)
void on_exit(Event const &ev, FSM &)
Pushes the unhold-task on the task queue.
bool operator()(EVT const &evt, FSM &, SourceState &, TargetState &)
void on_entry(Event const &ev, FSM &)
TServiceCallFunc operation_
Pushes the recover-task on the task queue.
TServiceCallFunc recover_op_
The recover operation.
std::function< void()> finished_handler_
bool operator()(EVT const &evt, FSM &, SourceState &, TargetState &)
TServiceCallFunc unhold_op_
The unhold operation.
Pushes the hold-task on the task queue.
void on_exit(Event const &ev, FSM &)
TServiceCallFunc hold_op_
The hold operation.
void on_entry(Event const &ev, FSM &)
void operator()(EVT const &ev, FSM &fsm, SourceState &, TargetState &)
void on_entry(Event const &ev, FSM &)
std::queue< AsyncRunPermittedTask > RunPermittedTaskQueue
Define the task queue type.
AsyncRunPermittedTask(const TServiceCallFunc &operation, const std::function< void()> &finished_handler)
void operator()(EVT const &ev, FSM &fsm, SourceState &, TargetState &)
void on_entry(Event const &ev, FSM &)
void on_exit(Event const &ev, FSM &)
void execute()
Execute the task.
#define STATE_EXIT_OUTPUT
#define STATE_ENTER_OUTPUT
void operator()(EVT const &ev, FSM &fsm, SourceState &, TargetState &)
std::function< bool()> TServiceCallFunc
msm::back::state_machine< RunPermittedStateMachine_ > RunPermittedStateMachine
The top-level (back-end) state machine.
An AsyncRunPermittedTask is represented by a task execution and a completion signalling.
void on_entry(Event const &ev, FSM &)
Pushes the halt-task on the task queue.
void on_exit(Event const &ev, FSM &)
void signalCompletion()
Signal completion of the task execution.
Holds the updated run_permitted value.