Motorcortex Core  version: 2.7.6
sm_errorhandle.h
1 /*
2  * Developer : Alexey Zakharov (alexey.zakharov@vectioneer.com)
3  * All rights reserved. Copyright (c) 2018 VECTIONEER.
4  */
5 
6 #ifndef SM_ERRORHANDLE_H
7 #define SM_ERRORHANDLE_H
8 
9 #include "sg_monitor.h"
10 #include "sm_errorhandlebase.h"
11 #include "sm_errorpub.h"
12 #include "sm_state.h"
13 #include "sm_statemachine.h"
14 
15 namespace mcx::state_machine {
16 
17 template <typename SUPER_STATE>
18 class ErrorHandle : public ErrorHandleBase {
19  using StateMachineErrorHandle = StateMachine<SUPER_STATE>;
20 
21  template <typename... STATES>
22  void action_variadic(const ErrorGroup& error_group, signal_monitor::MonitorAction action,
23  id_caller<STATES>... states) {
24  auto id_list = {(STATES::id(id_caller<STATES>()))...};
25  for (auto& id : id_list) {
26  state_machine_->getStatePtr(id)->addErrors(error_group, action);
27  }
28  }
29 
30  // dummy for empty parameter pack
31  void action_variadic(const ErrorGroup& error_group, signal_monitor::MonitorAction action) {}
32 
33 public:
34  ErrorHandle(StateMachine<SUPER_STATE>* state_machine) : state_machine_(state_machine) {}
35 
36  ErrorHandle() = default;
37 
38  ErrorHandle(ErrorHandle& orig) = delete;
39 
40  ErrorHandle& operator=(ErrorHandle& orig) = delete;
41 
42  virtual ~ErrorHandle(void) = default;
43 
44  void setStateMachine(StateMachine<SUPER_STATE>* state_machine) { state_machine_ = state_machine; }
45 
46  void addParameters(const std::string& param_name, parameter_server::Parameter* parent) {
47  error_publisher_.addParameters(param_name, parent);
48  }
49 
50  void setName(const std::string& name) { error_publisher_.setName(name); }
51 
52  const std::string& getName() const { return error_publisher_.getName(); }
53 
54  void create(const std::string& name, const char* error_name_prefix, parameter_server::Parameter* parent) {
55  setName(name);
56  addParameters(error_name_prefix, parent);
57  }
58 
59  const std::map<Error, MotorcortexError>& getActiveErrors() const { return error_publisher_.getActiveErrors(); }
60 
61  MotorcortexError getActiveError() const { return error_publisher_.getActiveError(); }
62 
63  bool empty() override { return error_publisher_.empty(); }
64 
65  bool hasError(const Error& error) override { return error_publisher_.hasError(error); }
66 
67  bool hasErrorLevel(MotorcortexErrorLevel error_level) override { return error_publisher_.hasErrorLevel(error_level); }
68 
69  bool acknowledgeAll() { return error_publisher_.clear(false); }
70 
71  bool acknowledgeWarnings() { return error_publisher_.clear(true); }
72 
73  bool acknowledge(bool warnings_only = false) override { return error_publisher_.clear(warnings_only); }
74 
75  bool warning(const Error& error) override {
76  return error_publisher_.addError(error, MotorcortexErrorLevel::motorcortex_ErrorLevel_WARNING);
77  }
78 
79  bool forcedDisengaged(const Error& error) override {
80  return error_publisher_.addError(error, MotorcortexErrorLevel::motorcortex_ErrorLevel_FORCED_DISENGAGE);
81  }
82 
83  bool shutdown(const Error& error) override {
84  return error_publisher_.addError(error, MotorcortexErrorLevel::motorcortex_ErrorLevel_SHUTDOWN);
85  }
86 
87  bool emergencyStop(const Error& error) override {
88  return error_publisher_.addError(error, MotorcortexErrorLevel::motorcortex_ErrorLevel_EMERGENCY_STOP);
89  }
90 
91  void iterate(double dt_sec) {
92  error_publisher_.publish();
93  signal_monitor_.iterate(dt_sec);
94  }
95 
96  const ErrorPublisher& getErrorPublisher() const { return error_publisher_; }
97 
98  signal_monitor::SignalMonitorBase* getSignalMonitor() override { return &signal_monitor_; }
99 
100  // adding errors
101 
102  void enableErrors(const ErrorGroup& error_group) {
103  using namespace signal_monitor;
104  uint32_t counter = 0;
105  while (auto state_ptr = state_machine_->getStatePtr(counter++)) {
106  state_ptr->addErrors(error_group, MonitorAction::ENABLE);
107  }
108  }
109 
110  template <class STATE, typename... STATES>
111  void enableErrors(const ErrorGroup& error_group) {
112  using namespace signal_monitor;
113  int id = STATE::id(id_caller<STATE>());
114  state_machine_->getStatePtr(id)->addErrors(error_group, MonitorAction::ENABLE);
115  action_variadic(error_group, MonitorAction::ENABLE, id_caller<STATES>()...);
116  }
117 
118  void disableErrors(const ErrorGroup& error_group) {
119  using namespace signal_monitor;
120  uint32_t counter = 0;
121  while (auto state_ptr = state_machine_->getStatePtr(counter++)) {
122  state_ptr->addErrors(error_group, MonitorAction::DISABLE);
123  }
124  }
125 
126  template <class STATE, typename... STATES>
127  void disableErrors(const ErrorGroup& error_group) {
128  using namespace signal_monitor;
129  int id = STATE::id(id_caller<STATE>());
130  state_machine_->getStatePtr(id)->addErrors(error_group, MonitorAction::DISABLE);
131  action_variadic(error_group, MonitorAction::DISABLE, id_caller<STATES>()...);
132  }
133 
134  void resetErrors(const ErrorGroup& error_group) {
135  using namespace signal_monitor;
136  uint32_t counter = 0;
137  while (auto state_ptr = state_machine_->getStatePtr(counter++)) {
138  state_ptr->addErrors(error_group, MonitorAction::RESET);
139  }
140  }
141 
142  template <class STATE, typename... STATES>
143  void resetErrors(const ErrorGroup& error_group) {
144  using namespace signal_monitor;
145  int id = STATE::id(id_caller<STATE>());
146  state_machine_->getStatePtr(id)->addErrors(error_group, MonitorAction::RESET);
147  action_variadic(error_group, MonitorAction::RESET, id_caller<STATES>()...);
148  }
149 
150  void addEmergencyStop(const Error& error_id, bool* input_ptr, bool objective_val,
151  TriggerType trigger_type = TriggerType::TRIGGER_ALWAYS, double start_time_s = 0,
152  double stop_time_s = std::numeric_limits<double>::max()) {
153 
154  signal_monitor_.add<StateMachineErrorHandle>(error_id, *input_ptr, objective_val,
155  {&StateMachineErrorHandle::emergencyStop, state_machine_},
156  trigger_type, start_time_s, stop_time_s);
157 
158  addError(error_id);
159  }
160 
161  void addEmergencyStop(const Error& error_id, const bool&& input_val, bool objective_val,
162  TriggerType trigger_type = TriggerType::TRIGGER_ALWAYS, double start_time_s = 0,
163  double stop_time_s = std::numeric_limits<double>::max()) {
164 
165  signal_monitor_.add<StateMachineErrorHandle>(error_id, std::forward<const bool&&>(input_val), objective_val,
166  {&StateMachineErrorHandle::emergencyStop, state_machine_},
167  trigger_type, start_time_s, stop_time_s);
168 
169  addError(error_id);
170  }
171 
172  void addShutdown(const Error& error_id, bool* input_ptr, bool objective_val,
173  TriggerType trigger_type = TriggerType::TRIGGER_ALWAYS, double start_time_s = 0,
174  double stop_time_s = std::numeric_limits<double>::max()) {
175 
176  signal_monitor_.add<StateMachineErrorHandle>(error_id, *input_ptr, objective_val,
177  {&StateMachineErrorHandle::shutdown, state_machine_}, trigger_type,
178  start_time_s, stop_time_s);
179 
180  addError(error_id);
181  }
182 
183  void addShutdown(const Error& error_id, const bool&& input_val, bool objective_val,
184  TriggerType trigger_type = TriggerType::TRIGGER_ALWAYS, double start_time_s = 0,
185  double stop_time_s = std::numeric_limits<double>::max()) {
186 
187  signal_monitor_.add<StateMachineErrorHandle>(error_id, std::forward<const bool&&>(input_val), objective_val,
188  {&StateMachineErrorHandle::shutdown, state_machine_}, trigger_type,
189  start_time_s, stop_time_s);
190 
191  addError(error_id);
192  }
193 
194  void addForceDisengage(const Error& error_id, bool* input_ptr, bool objective_val,
195  TriggerType trigger_type = TriggerType::TRIGGER_ALWAYS, double start_time_s = 0,
196  double stop_time_s = std::numeric_limits<double>::max()) {
197 
198  signal_monitor_.add<StateMachineErrorHandle>(error_id, *input_ptr, objective_val,
199  {&StateMachineErrorHandle::forcedDisengaged, state_machine_},
200  trigger_type, start_time_s, stop_time_s);
201 
202  addError(error_id);
203  }
204 
205  void addForceDisengage(const Error& error_id, const bool&& input_val, bool objective_val,
206  TriggerType trigger_type = TriggerType::TRIGGER_ALWAYS, double start_time_s = 0,
207  double stop_time_s = std::numeric_limits<double>::max()) {
208 
209  signal_monitor_.add<StateMachineErrorHandle>(error_id, std::forward<const bool&&>(input_val), objective_val,
210  {&StateMachineErrorHandle::forcedDisengaged, state_machine_},
211  trigger_type, start_time_s, stop_time_s);
212 
213  addError(error_id);
214  }
215 
216  void addWarning(const Error& error_id, bool* input_ptr, bool objective_val,
217  TriggerType trigger_type = TriggerType::TRIGGER_ALWAYS, double start_time_s = 0,
218  double stop_time_s = std::numeric_limits<double>::max()) {
219 
220  signal_monitor_.add<StateMachineErrorHandle>(error_id, *input_ptr, objective_val,
221  {&StateMachineErrorHandle::warning, state_machine_}, trigger_type,
222  start_time_s, stop_time_s);
223 
224  addError(error_id);
225  }
226 
227  void addWarning(const Error& error_id, const bool&& input_val, bool objective_val,
228  TriggerType trigger_type = TriggerType::TRIGGER_ALWAYS, double start_time_s = 0,
229  double stop_time_s = std::numeric_limits<double>::max()) {
230 
231  signal_monitor_.add<StateMachineErrorHandle>(error_id, std::forward<const bool&&>(input_val), objective_val,
232  {&StateMachineErrorHandle::warning, state_machine_}, trigger_type,
233  start_time_s, stop_time_s);
234 
235  addError(error_id);
236  }
237 
238  void addSignal(const Error& error_id, bool* input_val, bool objective_val,
239  const std::function<bool(const Error& signal_id)>& callback,
240  TriggerType trigger_type = TriggerType::TRIGGER_ALWAYS, double start_time_s = 0,
241  double stop_time_s = std::numeric_limits<double>::max()) {
242 
243  signal_monitor_.add(error_id, *input_val, objective_val, callback, trigger_type, start_time_s, stop_time_s);
244 
245  addError(error_id);
246  }
247 
248 private:
249  void addError(const Error& error);
250 
251  StateMachine<SUPER_STATE>* state_machine_;
252 
253  ErrorPublisher error_publisher_;
254 
255  signal_monitor::SignalMonitor<bool> signal_monitor_;
256 };
257 
258 template <class T>
259 void ErrorHandle<T>::addError(const Error& error) {
260  using namespace signal_monitor;
261  // if subsystem is missing adding 0 subsystem
262  Error errorToAdd(error.getCode(), error.hasSubsystem() ? error.getSubsystem() : 0, error.getInfoPtr());
263  uint32_t counter = 0;
264  while (auto state_ptr = state_machine_->getStatePtr(counter++)) {
265  state_ptr->addErrors(errorToAdd, MonitorAction::ADD);
266  state_ptr->addErrors(errorToAdd, MonitorAction::ENABLE);
267  }
268 }
269 
270 } // namespace mcx::state_machine
271 
272 #endif /* SM_ERROR_HANDLE_H */
mcx::signal_monitor::SignalMonitor< bool >
mcx::signal_monitor::SignalMonitorBase
Definition: sg_monitorbase.h:31
mcx::parameter_server::Parameter
Definition: ps_parameter.h:45
mcx::state_machine::ErrorPublisher
Definition: sm_errorpub.h:22
mcx::state_machine::StateMachine::warning
bool warning(const Error &error)
Executes warning routine, add error code to the error list.
Definition: sm_statemachine.h:722
mcx::state_machine::StateMachine
States machine manager and event interpreter.
Definition: sm_state.h:31
mcx::state_machine::StateMachine::shutdown
bool shutdown(const Error &error)
Executes shutdown routine, add error code to the error list.
Definition: sm_statemachine.h:754
mcx::state_machine::StateMachine::emergencyStop
bool emergencyStop(const Error &error)
Executes e-stop routine, add error code to the error list.
Definition: sm_statemachine.h:770
mcx::signal_monitor::SignalId
Definition: sg_signalid.h:25
_motorcortex_Error
Definition: motorcortex.pb.h:130
mcx::state_machine::id_caller
Definition: sm_state.h:28
mcx::signal_monitor::SignalIdGroup
Definition: sg_signalidgroup.h:20
mcx::state_machine::ErrorHandleBase
Definition: sm_errorhandlebase.h:19
mcx::state_machine::StateMachine::forcedDisengaged
bool forcedDisengaged(const Error &error)
Executes forced disengaged routine, add error code to the error list.
Definition: sm_statemachine.h:738
mcx::state_machine::ErrorHandle
Definition: sm_errorhandle.h:18