sto_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_STO_STATE_MACHINE_H
19 #define PRBT_HARDWARE_SUPPORT_STO_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 
38 #define COLOR_GREEN "\033[32m"
39 #define COLOR_GREEN_BOLD "\033[1;32m"
40 
41 #define STATE_ENTER_OUTPUT ROS_DEBUG_STREAM_NAMED("STOStateMachine", "Event: " << className(boost::core::demangle(typeid(ev).name())) \
42  << " - Entering: " << COLOR_GREEN_BOLD << className(boost::core::demangle(typeid(*this).name())) << COLOR_GREEN);
43 #define STATE_EXIT_OUTPUT ROS_DEBUG_STREAM_NAMED("STOStateMachine", "Event: " << className(boost::core::demangle(typeid(ev).name())) \
44  << " - Leaving: " << className(boost::core::demangle(typeid(*this).name())));
45 #define ACTION_OUTPUT ROS_DEBUG_STREAM_NAMED("STOStateMachine", "Event: " << className(boost::core::demangle(typeid(ev).name())) \
46  << " - Action: " << className(boost::core::demangle(typeid(*this).name())));
47 
55 {
56 public:
57  AsyncStoTask(const TServiceCallFunc &operation, const std::function<void()> &finished_handler)
58  : operation_(operation),
59  finished_handler_(finished_handler)
60  {}
61 
65  void execute()
66  {
67  if (operation_)
68  {
69  operation_();
70  }
71  }
72 
77  {
79  }
80 
81 private:
83  std::function<void()> finished_handler_;
84 };
85 
87 using StoTaskQueue = std::queue<AsyncStoTask>;
88 
89 namespace msm = boost::msm;
90 namespace mpl = boost::mpl;
91 using namespace msm::front;
92 
101 class StoStateMachine_ : public msm::front::state_machine_def<StoStateMachine_> // CRTP
102 {
103 public:
112  StoStateMachine_(const TServiceCallFunc &recover_operation,
113  const TServiceCallFunc &halt_operation,
114  const TServiceCallFunc &hold_operation,
115  const TServiceCallFunc &unhold_operation)
116  : recover_op_(recover_operation),
117  halt_op_(halt_operation),
118  hold_op_(hold_operation),
119  unhold_op_(unhold_operation)
120  {}
121 
123  // States //
125 
126  struct RobotInactive : public msm::front::state<>
127  {
128  template <class Event, class FSM>
129  void on_entry(Event const &ev, FSM &)
130  {
132  }
133  template <class Event, class FSM>
134  void on_exit(Event const &ev, FSM &)
135  {
137  }
138  };
139  struct RobotActive : public msm::front::state<>
140  {
141  template <class Event, class FSM>
142  void on_entry(Event const &ev, FSM &)
143  {
145  }
146  template <class Event, class FSM>
147  void on_exit(Event const &ev, FSM &)
148  {
150  }
151  };
152 
153  struct Enabling : public msm::front::state<>
154  {
155  template <class Event, class FSM>
156  void on_entry(Event const &ev, FSM &)
157  {
159  }
160  template <class Event, class FSM>
161  void on_exit(Event const &ev, FSM &)
162  {
164  }
165  };
166 
167  struct Stopping : public msm::front::state<>
168  {
169  template <class Event, class FSM>
170  void on_entry(Event const &ev, FSM &)
171  {
173  }
174  template <class Event, class FSM>
175  void on_exit(Event const &ev, FSM &)
176  {
178  }
179  };
180 
181  struct StopRequestedDuringEnable : public msm::front::state<>
182  {
183  template <class Event, class FSM>
184  void on_entry(Event const &ev, FSM &)
185  {
187  }
188  template <class Event, class FSM>
189  void on_exit(Event const &ev, FSM &)
190  {
192  }
193  };
194 
195  struct EnableRequestedDuringStop : public msm::front::state<>
196  {
197  template <class Event, class FSM>
198  void on_entry(Event const &ev, FSM &)
199  {
201  }
202  template <class Event, class FSM>
203  void on_exit(Event const &ev, FSM &)
204  {
206  }
207  };
208 
209 
212 
214  // Events //
216 
220  struct sto_updated
221  {
222  sto_updated(const bool sto)
223  : sto_(sto){}
224 
225  bool sto_;
226  };
227 
229  {};
230 
231  struct halt_done
232  {};
233 
234  struct hold_done
235  {};
236 
237  struct unhold_done
238  {};
239 
241  // Guards //
243 
244  struct sto_true
245  {
246  template <class EVT, class FSM, class SourceState, class TargetState>
247  bool operator()(EVT const &evt, FSM &, SourceState &, TargetState &)
248  {
249  return evt.sto_;
250  }
251  };
252 
253  struct sto_false
254  {
255  template <class EVT, class FSM, class SourceState, class TargetState>
256  bool operator()(EVT const &evt, FSM &, SourceState &, TargetState &)
257  {
258  return !evt.sto_;
259  }
260  };
261 
263  // Actions //
265 
272  {
273  template <class EVT, class FSM, class SourceState, class TargetState>
274  void operator()(EVT const &ev, FSM &fsm, SourceState &, TargetState &)
275  {
277 
278  fsm.task_queue_.push(AsyncStoTask(fsm.recover_op_, [&fsm]() { fsm.process_event(recover_done()); }));
279  }
280  };
281 
287  struct halt_start
288  {
289  template <class EVT, class FSM, class SourceState, class TargetState>
290  void operator()(EVT const &ev, FSM &fsm, SourceState &, TargetState &)
291  {
293 
294  fsm.task_queue_.push(AsyncStoTask(fsm.halt_op_, [&fsm]() { fsm.process_event(halt_done()); }));
295  }
296  };
297 
303  struct hold_start
304  {
305  template <class EVT, class FSM, class SourceState, class TargetState>
306  void operator()(EVT const &ev, FSM &fsm, SourceState &, TargetState &)
307  {
309 
310  fsm.task_queue_.push(AsyncStoTask(fsm.hold_op_, [&fsm]() { fsm.process_event(hold_done()); }));
311  }
312  };
313 
320  {
321  template <class EVT, class FSM, class SourceState, class TargetState>
322  void operator()(EVT const &ev, FSM &fsm, SourceState &, TargetState &)
323  {
325 
326  fsm.task_queue_.push(AsyncStoTask(fsm.unhold_op_, [&fsm]() { fsm.process_event(unhold_done()); }));
327  }
328  };
329 
331  // Transitions //
333 
334  struct transition_table : mpl::vector<
335  // Start Event Target Action Guard
336  // +---------------------------+--------------+---------------------------+--------------+----------+
337  Row< RobotInactive , sto_updated , Enabling , recover_start, sto_true >,
338  Row< RobotInactive , sto_updated , none , none , sto_false>,
339  Row< Enabling , sto_updated , none , none , sto_true >,
340  Row< Enabling , sto_updated , StopRequestedDuringEnable , none , sto_false>,
341  Row< Enabling , recover_done , none , unhold_start , none >,
342  Row< Enabling , unhold_done , RobotActive , none , none >,
343  Row< StopRequestedDuringEnable , sto_updated , none , none , none >,
344  Row< StopRequestedDuringEnable , recover_done , Stopping , halt_start , none >,
345  Row< StopRequestedDuringEnable , unhold_done , Stopping , hold_start , none >,
346  Row< RobotActive , sto_updated , none , none , sto_true >,
347  Row< RobotActive , sto_updated , Stopping , hold_start , sto_false>,
348  Row< Stopping , sto_updated , EnableRequestedDuringStop , none , sto_true >,
349  Row< Stopping , hold_done , none , halt_start , none >,
350  Row< Stopping , halt_done , RobotInactive , none , none >,
351  Row< EnableRequestedDuringStop , hold_done , none , halt_start , none >,
352  Row< EnableRequestedDuringStop , halt_done , Enabling , recover_start, none >,
353  Row< EnableRequestedDuringStop , sto_updated , Stopping , none , sto_false>,
354  Row< EnableRequestedDuringStop , sto_updated , none , none , sto_true >
355  // +---------------------------+--------------+---------------------------+--------------+----------+
356  > {};
357 
360 
363 
366 
369 
372 };
373 
375 typedef msm::back::state_machine<StoStateMachine_> StoStateMachine;
376 
377 } // namespace prbt_hardware_support
378 
379 #endif // PRBT_HARDWARE_SUPPORT_STO_STATE_MACHINE_H
Pushes the recover-task on the task queue.
std::function< void()> finished_handler_
msm::back::state_machine< StoStateMachine_ > StoStateMachine
The top-level (back-end) state machine.
TServiceCallFunc unhold_op_
The unhold operation.
Pushes the hold-task on the task queue.
StoStateMachine_(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.
StoTaskQueue task_queue_
The task queue.
Pushes the unhold-task on the task queue.
std::queue< AsyncStoTask > StoTaskQueue
Define the task queue type.
void operator()(EVT const &ev, FSM &fsm, SourceState &, TargetState &)
bool operator()(EVT const &evt, FSM &, SourceState &, TargetState &)
void operator()(EVT const &ev, FSM &fsm, SourceState &, TargetState &)
bool operator()(EVT const &evt, FSM &, SourceState &, TargetState &)
AsyncStoTask(const TServiceCallFunc &operation, const std::function< void()> &finished_handler)
TServiceCallFunc halt_op_
The halt operation.
An AsyncStoTask is represented by a task execution and a completion signalling.
void execute()
Execute the task.
#define ACTION_OUTPUT
#define STATE_EXIT_OUTPUT
TServiceCallFunc recover_op_
The recover operation.
std::function< bool()> TServiceCallFunc
RobotInactive initial_state
Initial state.
void operator()(EVT const &ev, FSM &fsm, SourceState &, TargetState &)
void signalCompletion()
Signal completion of the task execution.
void operator()(EVT const &ev, FSM &fsm, SourceState &, TargetState &)
TServiceCallFunc hold_op_
The hold operation.
Pushes the halt-task on the task queue.
#define STATE_ENTER_OUTPUT


prbt_hardware_support
Author(s):
autogenerated on Fri Feb 28 2020 03:16:57