run_permitted_state_machine.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2019 Pilz GmbH & Co. KG
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8 
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13 
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
18 #ifndef PRBT_HARDWARE_SUPPORT_RUN_PERMITTED_STATE_MACHINE_H
19 #define PRBT_HARDWARE_SUPPORT_RUN_PERMITTED_STATE_MACHINE_H
20 
21 #include <queue>
22 #include <string>
23 
24 #include <ros/ros.h>
25 
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>
30 #include <ros/console.h>
31 
34 
35 namespace prbt_hardware_support
36 {
37 #define COLOR_GREEN "\033[32m"
38 #define COLOR_GREEN_BOLD "\033[1;32m"
39 
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())));
53 
61 {
62 public:
63  AsyncRunPermittedTask(const TServiceCallFunc& operation, const std::function<void()>& finished_handler)
64  : operation_(operation), finished_handler_(finished_handler)
65  {
66  }
67 
71  void execute()
72  {
73  if (operation_)
74  {
75  operation_();
76  }
77  }
78 
83  {
85  }
86 
87 private:
89  std::function<void()> finished_handler_;
90 };
91 
93 using RunPermittedTaskQueue = std::queue<AsyncRunPermittedTask>;
94 
95 namespace msm = boost::msm;
96 namespace mpl = boost::mpl;
97 using namespace msm::front;
98 
107 class RunPermittedStateMachine_ : public msm::front::state_machine_def<RunPermittedStateMachine_> // CRTP
108 {
109 public:
118  RunPermittedStateMachine_(const TServiceCallFunc& recover_operation, const TServiceCallFunc& halt_operation,
119  const TServiceCallFunc& hold_operation, const TServiceCallFunc& unhold_operation)
120  : recover_op_(recover_operation), halt_op_(halt_operation), hold_op_(hold_operation), unhold_op_(unhold_operation)
121  {
122  }
123 
125  // States //
127 
128  struct RobotInactive : public msm::front::state<>
129  {
130  template <class Event, class FSM>
131  void on_entry(Event const& ev, FSM&)
132  {
134  }
135  template <class Event, class FSM>
136  void on_exit(Event const& ev, FSM&)
137  {
139  }
140  };
141  struct RobotActive : public msm::front::state<>
142  {
143  template <class Event, class FSM>
144  void on_entry(Event const& ev, FSM&)
145  {
147  }
148  template <class Event, class FSM>
149  void on_exit(Event const& ev, FSM&)
150  {
152  }
153  };
154 
155  struct Enabling : public msm::front::state<>
156  {
157  template <class Event, class FSM>
158  void on_entry(Event const& ev, FSM&)
159  {
161  }
162  template <class Event, class FSM>
163  void on_exit(Event const& ev, FSM&)
164  {
166  }
167  };
168 
169  struct Stopping : public msm::front::state<>
170  {
171  template <class Event, class FSM>
172  void on_entry(Event const& ev, FSM&)
173  {
175  }
176  template <class Event, class FSM>
177  void on_exit(Event const& ev, FSM&)
178  {
180  }
181  };
182 
183  struct StopRequestedDuringEnable : public msm::front::state<>
184  {
185  template <class Event, class FSM>
186  void on_entry(Event const& ev, FSM&)
187  {
189  }
190  template <class Event, class FSM>
191  void on_exit(Event const& ev, FSM&)
192  {
194  }
195  };
196 
197  struct EnableRequestedDuringStop : public msm::front::state<>
198  {
199  template <class Event, class FSM>
200  void on_entry(Event const& ev, FSM&)
201  {
203  }
204  template <class Event, class FSM>
205  void on_exit(Event const& ev, FSM&)
206  {
208  }
209  };
210 
213 
215  // Events //
217 
222  {
223  run_permitted_updated(const bool run_permitted) : run_permitted_(run_permitted)
224  {
225  }
226 
228  };
229 
231  {
232  };
233 
234  struct halt_done
235  {
236  };
237 
238  struct hold_done
239  {
240  };
241 
242  struct unhold_done
243  {
244  };
245 
247  // Guards //
249 
251  {
252  template <class EVT, class FSM, class SourceState, class TargetState>
253  bool operator()(EVT const& evt, FSM&, SourceState&, TargetState&)
254  {
255  return evt.run_permitted_;
256  }
257  };
258 
260  {
261  template <class EVT, class FSM, class SourceState, class TargetState>
262  bool operator()(EVT const& evt, FSM&, SourceState&, TargetState&)
263  {
264  return !evt.run_permitted_;
265  }
266  };
267 
269  // Actions //
271 
278  {
279  template <class EVT, class FSM, class SourceState, class TargetState>
280  void operator()(EVT const& ev, FSM& fsm, SourceState&, TargetState&)
281  {
283 
284  fsm.task_queue_.push(AsyncRunPermittedTask(fsm.recover_op_, [&fsm]() { fsm.process_event(recover_done()); }));
285  }
286  };
287 
293  struct halt_start
294  {
295  template <class EVT, class FSM, class SourceState, class TargetState>
296  void operator()(EVT const& ev, FSM& fsm, SourceState&, TargetState&)
297  {
299 
300  fsm.task_queue_.push(AsyncRunPermittedTask(fsm.halt_op_, [&fsm]() { fsm.process_event(halt_done()); }));
301  }
302  };
303 
309  struct hold_start
310  {
311  template <class EVT, class FSM, class SourceState, class TargetState>
312  void operator()(EVT const& ev, FSM& fsm, SourceState&, TargetState&)
313  {
315 
316  fsm.task_queue_.push(AsyncRunPermittedTask(fsm.hold_op_, [&fsm]() { fsm.process_event(hold_done()); }));
317  }
318  };
319 
326  {
327  template <class EVT, class FSM, class SourceState, class TargetState>
328  void operator()(EVT const& ev, FSM& fsm, SourceState&, TargetState&)
329  {
331 
332  fsm.task_queue_.push(AsyncRunPermittedTask(fsm.unhold_op_, [&fsm]() { fsm.process_event(unhold_done()); }));
333  }
334  };
335 
337  // Transitions //
339 
341  : mpl::vector<
342  // Start Event Target Action Guard
343  // +---------------------------+--------------+---------------------------+--------------+----------+
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>
360  // +---------------------------+--------------+---------------------------+--------------+----------+
361  >
362  {
363  };
364 
367 
370 
373 
376 
379 };
380 
382 typedef msm::back::state_machine<RunPermittedStateMachine_> RunPermittedStateMachine;
383 
384 } // namespace prbt_hardware_support
385 
386 #endif // PRBT_HARDWARE_SUPPORT_RUN_PERMITTED_STATE_MACHINE_H
void operator()(EVT const &ev, FSM &fsm, SourceState &, TargetState &)
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.
bool operator()(EVT const &evt, FSM &, SourceState &, TargetState &)
TServiceCallFunc recover_op_
The recover operation.
bool operator()(EVT const &evt, FSM &, SourceState &, TargetState &)
void operator()(EVT const &ev, FSM &fsm, SourceState &, TargetState &)
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 &)
#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.
#define ACTION_OUTPUT
An AsyncRunPermittedTask is represented by a task execution and a completion signalling.
void signalCompletion()
Signal completion of the task execution.


prbt_hardware_support
Author(s):
autogenerated on Wed Oct 28 2020 03:12:06