abstract_state_machine.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (Apache License)
3  *
4  * Copyright (c) 2018 Plus One Robotics
5  *
6  * Licensed under the Apache License, Version 2.0 (the "License");
7  * you may not use this file except in compliance with the License.
8  * You may obtain a copy of the License at
9  *
10  * http://www.apache.org/licenses/LICENSE-2.0
11  *
12  * Unless required by applicable law or agreed to in writing, software
13  * distributed under the License is distributed on an "AS IS" BASIS,
14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15  * See the License for the specific language governing permissions and
16  * limitations under the License.
17  */
18 
20 #include "packml_sm/common.h"
21 #include "packml_sm/dlog.h"
22 
23 #include <thread>
24 
25 namespace packml_sm
26 {
27 AbstractStateMachine::AbstractStateMachine() : start_time_(std::chrono::steady_clock::now())
28 {
29 }
30 
32 {
33  switch (StatesEnum(getCurrentState()))
34  {
35  case StatesEnum::IDLE:
36  _start();
37  return true;
38  default:
39  DLog::LogWarning("Ignoring START command in current state: %d", getCurrentState());
40  return false;
41  }
42 }
43 
45 {
46  switch (StatesEnum(getCurrentState()))
47  {
49  _clear();
50  return true;
51  default:
52  DLog::LogWarning("Ignoring CLEAR command in current state: %d", getCurrentState());
53  return false;
54  }
55 }
56 
58 {
59  switch (StatesEnum(getCurrentState()))
60  {
63  _reset();
64  return true;
65  default:
66  DLog::LogWarning("Ignoring RESET command in current state: %d", getCurrentState());
67  return false;
68  }
69 }
70 
72 {
73  switch (StatesEnum(getCurrentState()))
74  {
76  _hold();
77  return true;
78  default:
79  DLog::LogWarning("Ignoring HOLD command in current state: %d", getCurrentState());
80  return false;
81  }
82 }
83 
85 {
86  switch (StatesEnum(getCurrentState()))
87  {
88  case StatesEnum::HELD:
89  _unhold();
90  return true;
91  default:
92  DLog::LogWarning("Ignoring HELD command in current state: %d", getCurrentState());
93  return false;
94  }
95 }
96 
98 {
99  switch (StatesEnum(getCurrentState()))
100  {
101  case StatesEnum::EXECUTE:
102  _suspend();
103  return true;
104  default:
105  DLog::LogWarning("Ignoring SUSPEND command in current state: %d", getCurrentState());
106  return false;
107  }
108 }
109 
111 {
112  switch (StatesEnum(getCurrentState()))
113  {
115  _unsuspend();
116  return true;
117  default:
118  DLog::LogWarning("Ignoring UNSUSPEND command in current state: %d", getCurrentState());
119  return false;
120  }
121 }
122 
124 {
125  switch (StatesEnum(getCurrentState()))
126  {
129  case StatesEnum::IDLE:
131  case StatesEnum::EXECUTE:
132  case StatesEnum::HOLDING:
133  case StatesEnum::HELD:
139  _stop();
140  return true;
141  default:
142  DLog::LogWarning("Ignoring STOP command in current state: %d", getCurrentState());
143  return false;
144  }
145 }
146 
148 {
149  switch (StatesEnum(getCurrentState()))
150  {
152  case StatesEnum::STOPPED:
154  case StatesEnum::IDLE:
156  case StatesEnum::EXECUTE:
157  case StatesEnum::HOLDING:
158  case StatesEnum::HELD:
166  _abort();
167  return true;
168  default:
169  DLog::LogWarning("Ignoring ABORT command in current state: %d", getCurrentState());
170  return false;
171  }
172 }
173 
175 {
176  std::lock_guard<std::recursive_mutex> lock(stat_mutex_);
177  auto scheduled_time = calculateTotalTime();
178  float throughput = 0.0f;
179  if (scheduled_time > std::numeric_limits<double>::epsilon())
180  {
181  throughput = success_count_ / scheduled_time;
182  }
183 
184  auto held_time = getHeldTime();
185  auto stopped_time = getStoppedTime();
186  auto suspend_time = getSuspendedTime();
187  auto aborted_time = getAbortedTime();
188 
189  auto operating_time = scheduled_time;
190  operating_time -= stopped_time;
191  operating_time -= suspend_time;
192  operating_time -= aborted_time;
193 
194  float availability = 0.0f;
195  if (scheduled_time > std::numeric_limits<double>::epsilon())
196  {
197  availability = operating_time / scheduled_time;
198  }
199 
200  float performance = 0.0f;
201  if (operating_time > std::numeric_limits<double>::epsilon())
202  {
203  performance = static_cast<float>(success_count_) * ideal_cycle_time_ / operating_time;
204  }
205 
206  float quality = 0.0f;
207  auto total_count = success_count_ + failure_count_;
208  if (total_count > 0)
209  {
210  quality = static_cast<float>(success_count_) / static_cast<float>(total_count);
211  }
212 
213  snapshot_out.duration = scheduled_time;
214  snapshot_out.idle_duration = getIdleTime();
215  snapshot_out.exe_duration = getExecuteTime();
216  snapshot_out.held_duration = held_time;
217  snapshot_out.susp_duration = suspend_time;
218  snapshot_out.cmplt_duration = getCompleteTime();
219  snapshot_out.stop_duration = stopped_time;
220  snapshot_out.abort_duration = aborted_time;
221  snapshot_out.success_count = success_count_;
222  snapshot_out.fail_count = failure_count_;
223  snapshot_out.throughput = throughput;
224  snapshot_out.availability = availability;
225  snapshot_out.performance = performance;
226  snapshot_out.quality = quality;
227  snapshot_out.overall_equipment_effectiveness = quality * performance * availability;
230 }
231 
233 {
235 }
236 
238 {
240 }
241 
243 {
245 }
246 
248 {
250 }
251 
253 {
255 }
256 
258 {
260 }
261 
263 {
265 }
266 
268 {
270 }
271 
273 {
275 }
276 
278 {
280 }
281 
283 {
285 }
286 
288 {
290 }
291 
293 {
295 }
296 
298 {
300 }
301 
303 {
305 }
306 
308 {
310 }
311 
313 {
314  std::lock_guard<std::recursive_mutex> lock(stat_mutex_);
315  std::chrono::duration<double> duration = std::chrono::steady_clock::now() - start_time_;
316  auto elapsed_time = duration.count();
317  for (auto iter = duration_map_.begin(); iter != duration_map_.end(); iter++)
318  {
319  elapsed_time += iter->second;
320  }
321 
322  return elapsed_time;
323 }
324 
326 {
327  std::lock_guard<std::recursive_mutex> lock(stat_mutex_);
328  start_time_ = std::chrono::steady_clock::now();
329  duration_map_.clear();
330  failure_count_ = 0;
331  success_count_ = 0;
332 
333  for (auto& itemized_it : itemized_error_map_)
334  {
335  itemized_it.second.count = 0;
336  itemized_it.second.duration = 0;
337  }
338 
339  for (auto& itemized_it : itemized_quality_map_)
340  {
341  itemized_it.second.count = 0;
342  itemized_it.second.duration = 0;
343  }
344 }
345 
346 void AbstractStateMachine::incrementMapStatItem(std::map<int16_t, PackmlStatsItemized>& itemized_map, int16_t id,
347  int32_t count, double duration)
348 {
349  auto itemized_stat_it = itemized_map.find(id);
350  if (itemized_stat_it != itemized_map.end())
351  {
352  itemized_stat_it->second.count += count;
353  itemized_stat_it->second.duration += duration;
354  }
355  else
356  {
357  PackmlStatsItemized new_stats;
358  new_stats.id = id;
359  new_stats.count = count;
360  new_stats.duration = duration;
361  itemized_map.insert(std::pair<int16_t, PackmlStatsItemized>(id, new_stats));
362  }
363 }
364 
365 void AbstractStateMachine::incrementErrorStatItem(int16_t id, int32_t count, double duration)
366 {
367  std::lock_guard<std::recursive_mutex> lock(stat_mutex_);
368  incrementMapStatItem(itemized_error_map_, id, count, duration);
369 }
370 
371 void AbstractStateMachine::incrementQualityStatItem(int16_t id, int32_t count, double duration)
372 {
373  std::lock_guard<std::recursive_mutex> lock(stat_mutex_);
374  incrementMapStatItem(itemized_quality_map_, id, count, duration);
375 }
376 
378 {
379  std::lock_guard<std::recursive_mutex> lock(stat_mutex_);
380  success_count_++;
381 }
382 
384 {
385  std::lock_guard<std::recursive_mutex> lock(stat_mutex_);
386  failure_count_++;
387 }
388 
389 void AbstractStateMachine::setIdealCycleTime(float ideal_cycle_time)
390 {
391  std::lock_guard<std::recursive_mutex> lock(stat_mutex_);
392  ideal_cycle_time_ = ideal_cycle_time;
393 }
394 
395 void AbstractStateMachine::invokeStateChangedEvent(const std::string& name, StatesEnum value)
396 {
397  updateClock(value);
398  stateChangedEvent.invoke(*this, { name, value });
399 }
400 
402 {
403  std::lock_guard<std::recursive_mutex> lock(stat_mutex_);
404  std::chrono::duration<double> duration = std::chrono::steady_clock::now() - start_time_;
405  auto elapsed_time = duration.count();
406  if (duration_map_.find(current_state_) != duration_map_.end())
407  {
408  elapsed_time += duration_map_[current_state_];
409  }
410 
411  duration_map_[current_state_] = elapsed_time;
412 
413  current_state_ = new_state;
414  start_time_ = std::chrono::steady_clock::now();
415 }
416 
418 {
419  std::lock_guard<std::recursive_mutex> lock(stat_mutex_);
420  double elapsed_time = 0;
421  if (state == current_state_)
422  {
423  std::chrono::duration<double> duration = std::chrono::steady_clock::now() - start_time_;
424  elapsed_time += duration.count();
425  }
426 
427  if (duration_map_.find(state) != duration_map_.end())
428  {
429  elapsed_time += duration_map_[state];
430  }
431 
432  return elapsed_time;
433 }
434 } // namespace packml_sm
virtual void _clear()=0
Override to call implementations version of clear command.
void resetStats()
Reset all of the tracked states.
virtual bool unsuspend()
Call to send the unsuspend command.
virtual void _start()=0
Override to call implementations version of start command.
void setIdealCycleTime(float ideal_cycle_time)
Sets the ideal cycle time in operations per second.
void incrementMapStatItem(std::map< int16_t, PackmlStatsItemized > &itemized_map, int16_t id, int32_t count, double duration)
adds or updates the specific itemized map
double getUnsuspendingTime()
Accessor for the duration spent in unsuspending.
std::map< int16_t, PackmlStatsItemized > itemized_error_map_
virtual void _stop()=0
Override to call implementations version of the stop command.
double getHeldTime()
Accessor for the duration spent in held.
void invokeStateChangedEvent(const std::string &name, StatesEnum value)
Call to invoke a state changed event.
void getCurrentStatSnapshot(PackmlStatsSnapshot &snapshot_out)
Fills the reference variable with the current stats snapshot.
double getUnholdingTime()
Accessor for the duration spent in unholding.
Container for the current packml statistics snap shot.
virtual bool start()
Call to send the start command.
std::chrono::steady_clock::time_point start_time_
double getCompleteTime()
Accessor for the duration spent in complete.
std::map< StatesEnum, double > duration_map_
EventHandler< AbstractStateMachine, StateChangedEventArgs > stateChangedEvent
double getSuspendingTime()
Accessor for the duration spent in suspending.
virtual void _unhold()=0
Override to call implementations version of the unhold command.
double getExecuteTime()
Accessor for the duration spent in execute.
virtual void _abort()=0
Override to call implementations version of the abort command.
double calculateTotalTime()
Accessor for the total duration of the state machine.
virtual bool suspend()
Call to send the suspend command.
void incrementQualityStatItem(int16_t id, int32_t count, double duration)
Call to increment or add a specific Itemized quality stat.
double getAbortedTime()
Accessor for the duration spent in aborted.
double getAbortingTime()
Accessor for the duration spent in aborting.
double getResettingTime()
Accessor for the duration spent in resetting.
virtual bool hold()
Call to send the hold command.
double getSuspendedTime()
Accessor for the duration spent in suspended.
double getStoppingTime()
Accessor for the duration spent in stopping.
std::map< int16_t, PackmlStatsItemized > itemized_quality_map
double getStoppedTime()
Accessor for the duration spent in stopped.
std::map< int16_t, PackmlStatsItemized > itemized_error_map
double getStartingTime()
Accessor for the duration spent in starting.
Container for the current packml statistics snap shot.
virtual void _hold()=0
Override to call implementations version of the hold command.
static void LogWarning(const char *format,...)
Definition: dlog.cpp:47
virtual void _unsuspend()=0
Override to call implementations version of the unsuspend command.
virtual bool stop()
Call to send the stop command.
StatesEnum
Definition: common.h:35
void incrementErrorStatItem(int16_t id, int32_t count, double duration)
Call to increment or add a specific Itemized error stat.
double getClearingTime()
Accessor for the duration spent in clearing.
virtual bool abort()
Call to send the abort command.
virtual bool unhold()
Call to send the unhold command.
std::map< int16_t, PackmlStatsItemized > itemized_quality_map_
virtual bool clear()
Call to send the clear command.
void incrementFailureCount()
Call to increment the failed operation count.
double getHoldingTime()
Accessor for the duration spent in holding.
void incrementSuccessCount()
Call to increment the successful operation count.
StatesEnum getCurrentState() const
Accessor for the current state.
virtual void _reset()=0
Override to call implementations version of the reset command.
virtual void _suspend()=0
Override to call implementations version of the suspend command.
double getStateDuration(StatesEnum state)
Accessor for the given state duration.
AbstractStateMachine()
Constructor for AbstractStateMachine.
double getIdleTime()
Accessor for the duration spent in idle time.
virtual bool reset()
Call to send the reset command.
void updateClock(StatesEnum new_state)
Updates all of the durations for the states based on the new state.


packml_sm
Author(s): Shaun Edwards
autogenerated on Fri Jul 12 2019 03:30:55