YARP
Yet Another Robot Platform
Timer.cpp
Go to the documentation of this file.
1 /*
2  * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * SPDX-License-Identifier: BSD-3-Clause
4  */
5 
6 #include <yarp/os/Timer.h>
7 
9 #include <yarp/os/Time.h>
10 
11 #include <cmath>
12 #include <map>
13 #include <mutex>
14 #include <utility>
15 
16 using namespace yarp::os;
17 
18 //disclaimer: the following inheritance little madness is for avoiding critical copy and paste code and
19 //avoiding data inconsistence(example: RateThread::GetIterations() and runTimes)
20 
22 {
23 protected:
24  yarp::os::YarpTimerEvent getEventNow(unsigned int iteration);
25 
26  bool runTimer(unsigned int iteration, YarpTimerEvent event);
27 
28 public:
30 
31 #ifndef YARP_NO_DEPRECATED // since YARP 3.3
35  TimerCallback call,
36  yarp::os::Mutex* mutex) :
37  m_settings(sett),
38  m_callback(std::move(call)),
39  m_old_mutex(mutex)
40  {
41  }
43 #endif // YARP_NO_DEPRECATED
44 
46  TimerCallback call,
47  std::mutex* mutex = nullptr) :
48  m_settings(sett),
49  m_callback(std::move(call)),
50  m_mutex(mutex)
51  {
52  }
53 
54  virtual ~PrivateImpl() = default;
55 
56  virtual bool startTimer() = 0;
57 
58  virtual void stopTimer() = 0;
59 
60  virtual bool stepTimer() = 0;
61 
62  virtual bool timerIsRunning() = 0;
63 
64 
67  double m_startStamp{0.0};
68  double m_lastReal{0.0};
69  std::mutex* m_mutex{nullptr};
70 #ifndef YARP_NO_DEPRECATED // since YARP 3.3
75 #endif // YARP_NO_DEPRECATED
76 };
77 
79 {
80 public:
81 
82 #ifndef YARP_NO_DEPRECATED // since YARP 3.3
85  MonoThreadTimer(const TimerSettings& sett,
86  const TimerCallback& call,
87  yarp::os::Mutex* mutex);
89 #endif // YARP_NO_DEPRECATED
90 
91  MonoThreadTimer(const TimerSettings& sett,
92  const TimerCallback& call,
93  std::mutex* mutex = nullptr);
94 
95  ~MonoThreadTimer() override;
96  bool m_active{false};
97  unsigned int m_runTimes{1};
98  size_t m_id{(size_t)-1};
99 
101  {
102  return PrivateImpl::getEventNow(m_runTimes);
103  }
104 
105  bool startTimer() override
106  {
107  m_startStamp = yarp::os::Time::now();
108  m_active = true;
109  return true;
110  }
111 
112  void stopTimer() override
113  {
114  m_active = false;
115  }
116 
117  bool stepTimer() override
118  {
119  return step(getEventNow(), true);
120  }
121 
122  virtual bool step(YarpTimerEvent event, bool singleStep)
123  {
124  bool m_active = runTimer(m_runTimes, event);
125  if (!singleStep) {
126  m_runTimes++;
127  }
128  return m_active;
129  }
130 
131  bool timerIsRunning() override
132  {
133  return m_active;
134  }
135 };
136 
137 double gcd(double a, double b)
138 {
139  if (a < b) {
140  return gcd(b, a);
141  }
142 
143  // base case
144  if (fabs(b) < 0.001) {
145  return a;
146  } else {
147  return (gcd(b, a - floor(a / b) * b));
148  }
149 }
150 
152 {
153  std::mutex mu;
154  std::map<size_t, MonoThreadTimer*> timers;
155  TimerSingleton() :
156  PeriodicThread(10)
157  {
158  }
159 
160  void run() override;
161 
162  ~TimerSingleton() override
163  {
164  stop();
165  }
166 
167 public:
168  //reminder: int c++11 static variables'inside function are guaranteed to be lazy initialized and atomic
169  static TimerSingleton& self()
170  {
171  static TimerSingleton instance;
172  return instance;
173  }
174 
176  {
177  if (timers.size()) {
178  setPeriod(gcd(getPeriod(), t->m_settings.period));
179  } else {
180  setPeriod(t->m_settings.period);
181  }
182  mu.lock();
183  timers[timers.size()] = t;
184  mu.unlock();
185 
186  return timers.size() - 1;
187  }
188 
189  void removeTimer(size_t id)
190  {
191  mu.lock();
192  timers.erase(id);
193  mu.unlock();
194  if (!timers.size()) {
195  return;
196  }
197 
198  double new_gcd = timers.begin()->second->m_settings.period;
199  for (auto& i : timers) {
200  new_gcd = gcd(new_gcd, i.second->m_settings.period);
201  }
202  setPeriod(new_gcd);
203  }
204 
205  size_t getTimerCount()
206  {
207  return timers.size();
208  }
209 };
210 
211 #ifndef YARP_NO_DEPRECATED // since YARP 3.3
215  const TimerCallback& call,
216  yarp::os::Mutex* mutex) :
217  PrivateImpl(sett, call, mutex)
218 {
219  TimerSingleton& singlInstance = TimerSingleton::self();
220  m_id = singlInstance.addTimer(this);
221  if (!singlInstance.isRunning()) {
222  singlInstance.start();
223  }
224 }
226 #endif // YARP_NO_DEPRECATED
227 
229  const TimerCallback& call,
230  std::mutex* mutex) :
231  PrivateImpl(sett, call, mutex)
232 {
233  TimerSingleton& singlInstance = TimerSingleton::self();
234  m_id = singlInstance.addTimer(this);
235  if (!singlInstance.isRunning()) {
236  singlInstance.start();
237  }
238 }
239 
241 {
242  TimerSingleton& singlInstance = TimerSingleton::self();
243  singlInstance.removeTimer(m_id);
244  if (singlInstance.getTimerCount() == 0) {
245  singlInstance.stop();
246  }
247 }
248 
249 void TimerSingleton::run()
250 {
251  mu.lock();
252  for (auto t : timers) {
253  MonoThreadTimer& timer = *t.second;
254  YarpTimerEvent tEvent = timer.getEventNow();
255  if (timer.m_active && tEvent.currentReal > tEvent.currentExpected) {
256  timer.m_active = timer.step(tEvent, false);
257  timer.m_lastReal = tEvent.currentReal;
258  }
259  }
260  mu.unlock();
261 }
262 
266 {
268  void run() override;
269  bool threadInit() override;
270  bool singleStep{false};
271 
272 public:
273 #ifndef YARP_NO_DEPRECATED // since YARP 3.3
277  const TimerCallback& call,
278  yarp::os::Mutex* mutex) :
279  PrivateImpl(sett, call, mutex),
280  PeriodicThread(sett.period)
281  {
282  }
284 #endif // YARP_NO_DEPRECATED
285 
287  const TimerCallback& call,
288  std::mutex* mutex = nullptr) :
289  PrivateImpl(sett, call, mutex),
290  PeriodicThread(sett.period)
291  {
292  }
293 
294  ~ThreadedTimer() override
295  {
296  stop();
297  }
298 
299  bool startTimer() override
300  {
303  return start();
304  }
305 
306  bool stepTimer() override
307  {
308  singleStep = true;
309  step();
310  return true;
311  }
312 
313  void stopTimer() override
314  {
315  return askToStop();
316  }
317 
318  bool timerIsRunning() override
319  {
320  return isRunning();
321  }
322 };
323 
324 bool ThreadedTimer::threadInit()
325 {
327  return true;
328 }
329 
330 #ifndef YARP_NO_DEPRECATED // since YARP 3.3
333 //the newThread parameter is not in the settings for it to be unmutable and only checked by the constructor
334 Timer::Timer(const TimerSettings& settings, const TimerCallback& callback, bool newThread, Mutex* mutex) :
335  //added cast for incompatible operand error
336  impl(newThread ? dynamic_cast<PrivateImpl*>(new ThreadedTimer(settings, callback, mutex))
337  : dynamic_cast<PrivateImpl*>(new MonoThreadTimer(settings, callback, mutex)))
338 {
339 }
341 #endif // YARP_NO_DEPRECATED
342 
343 //the newThread parameter is not in the settings for it to be unmutable and only checked by the constructor
344 Timer::Timer(const TimerSettings& settings, const TimerCallback& callback, bool newThread, std::mutex* mutex) :
345  //added cast for incompatible operand error
346  impl(newThread ? dynamic_cast<PrivateImpl*>(new ThreadedTimer(settings, callback, mutex))
347  : dynamic_cast<PrivateImpl*>(new MonoThreadTimer(settings, callback, mutex)))
348 {
349 }
350 
352 {
353 
354  return impl->startTimer();
355 }
356 
358 {
359  return impl->stepTimer();
360 }
361 
363 {
364  impl->stopTimer();
365 }
366 
368 {
369  YarpTimerEvent event;
370 
372  event.currentExpected = m_startStamp + iteration * m_settings.period;
373  event.lastExpected = event.currentExpected - m_settings.period;
374  event.lastReal = m_lastReal;
375  event.lastDuration = event.currentReal - m_lastReal;
376  event.runCount = iteration;
377  return event;
378 }
379 
380 bool yarp::os::Timer::PrivateImpl::runTimer(unsigned int iteration, YarpTimerEvent event)
381 {
382  if (m_mutex != nullptr) {
383  m_mutex->lock();
384  }
385 
386 #ifndef YARP_NO_DEPRECATED // since YARP 3.3
387  if (m_old_mutex != nullptr) {
388  m_old_mutex->lock();
389  }
390 #endif // YARP_NO_DEPRECATED
391 
392  bool ret = m_callback(event);
393 
394 #ifndef YARP_NO_DEPRECATED // since YARP 3.3
395  if (m_old_mutex != nullptr) {
396  m_old_mutex->unlock();
397  }
398 #endif // YARP_NO_DEPRECATED
399 
400  if (m_mutex != nullptr) {
401  m_mutex->unlock();
402  }
403 
404  if (!ret) {
405  return false;
406  }
407 
408  m_lastReal = event.currentReal;
409 
410  double timerAge = (yarp::os::Time::now() - m_startStamp);
411 
412  //totalRunCount == 0 ----> infinite run count. follows the run count of the timer
413  bool stop(m_settings.totalRunCount != 0 && m_settings.totalRunCount <= iteration);
414 
415  //totalTime == 0 ----> infinite time. follows the age check for the timer
416  stop |= m_settings.totalTime > 0.00001 && (m_settings.totalTime - timerAge) < m_settings.tolerance;
417 
418  return !stop;
419 }
420 
421 void ThreadedTimer::run()
422 {
423  if (getIterations() == 0 && !singleStep) {
424  return;
425  }
426  singleStep = false;
427  YarpTimerEvent event = getEventNow(this->getIterations());
428  if (!runTimer(this->getIterations(), event)) {
429  askToStop();
430  }
431 }
432 
433 void Timer::setSettings(const TimerSettings& settings)
434 {
435  impl->m_settings = settings;
436 }
437 
439 {
440  return impl->m_settings;
441 }
442 
444 {
445  return impl->timerIsRunning();
446 }
447 
449 {
450  delete impl;
451 }
float t
bool ret
double gcd(double a, double b)
Definition: Timer.cpp:137
void stopTimer() override
Definition: Timer.cpp:112
MonoThreadTimer(const TimerSettings &sett, const TimerCallback &call, yarp::os::Mutex *mutex)
Definition: Timer.cpp:214
~MonoThreadTimer() override
Definition: Timer.cpp:240
virtual yarp::os::YarpTimerEvent getEventNow()
Definition: Timer.cpp:100
bool stepTimer() override
Definition: Timer.cpp:117
size_t m_id
Definition: Timer.cpp:98
virtual bool step(YarpTimerEvent event, bool singleStep)
Definition: Timer.cpp:122
bool m_active
Definition: Timer.cpp:96
bool startTimer() override
Definition: Timer.cpp:105
bool timerIsRunning() override
Definition: Timer.cpp:131
void stopTimer() override
Definition: Timer.cpp:313
ThreadedTimer(const TimerSettings &sett, const TimerCallback &call, std::mutex *mutex=nullptr)
Definition: Timer.cpp:286
bool stepTimer() override
Definition: Timer.cpp:306
~ThreadedTimer() override
Definition: Timer.cpp:294
bool startTimer() override
Definition: Timer.cpp:299
ThreadedTimer(const TimerSettings &sett, const TimerCallback &call, yarp::os::Mutex *mutex)
Definition: Timer.cpp:276
bool timerIsRunning() override
Definition: Timer.cpp:318
static TimerSingleton & self()
Definition: Timer.cpp:169
void removeTimer(size_t id)
Definition: Timer.cpp:189
size_t addTimer(MonoThreadTimer *t)
Definition: Timer.cpp:175
size_t getTimerCount()
Definition: Timer.cpp:205
Basic wrapper for mutual exclusion.
Definition: Mutex.h:32
An abstraction for a periodic thread.
bool setPeriod(double period)
Set the (new) period of the thread.
bool isRunning() const
Returns true when the thread is started, false otherwise.
void askToStop()
Stop the thread.
bool start()
Call this to start the thread.
void step()
Call this to "step" the thread rather than starting it.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
TimerCallback m_callback
Definition: Timer.cpp:66
yarp::os::YarpTimerEvent getEventNow(unsigned int iteration)
Definition: Timer.cpp:367
virtual bool startTimer()=0
yarp::os::Mutex * m_old_mutex
Definition: Timer.cpp:73
bool runTimer(unsigned int iteration, YarpTimerEvent event)
Definition: Timer.cpp:380
PrivateImpl(const TimerSettings &sett, TimerCallback call, std::mutex *mutex=nullptr)
Definition: Timer.cpp:45
virtual ~PrivateImpl()=default
virtual void stopTimer()=0
virtual bool timerIsRunning()=0
TimerSettings m_settings
Definition: Timer.cpp:65
virtual bool stepTimer()=0
yarp::os::Timer::TimerCallback TimerCallback
Definition: Timer.cpp:29
PrivateImpl(const TimerSettings &sett, TimerCallback call, yarp::os::Mutex *mutex)
Definition: Timer.cpp:34
virtual ~Timer()
Definition: Timer.cpp:448
void setSettings(const yarp::os::TimerSettings &settings)
setSettings
Definition: Timer.cpp:433
virtual bool isRunning()
Definition: Timer.cpp:443
virtual bool start()
Definition: Timer.cpp:351
virtual void stop()
Definition: Timer.cpp:362
const yarp::os::TimerSettings getSettings()
getSettings
Definition: Timer.cpp:438
virtual bool step()
Definition: Timer.cpp:357
Timer(const Timer &)=delete
std::function< bool(const yarp::os::YarpTimerEvent &)> TimerCallback
Definition: Timer.h:106
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition: Time.cpp:121
An interface to the operating system, including Port based communication.
double currentReal
currentReal When the current callback is actually being called
Definition: Timer.h:44
double currentExpected
currentExpected this is when the current callback should have been called
Definition: Timer.h:39
#define YARP_WARNING_POP
Ends a temporary alteration of the enabled warnings.
Definition: system.h:332
#define YARP_WARNING_PUSH
Starts a temporary alteration of the enabled warnings.
Definition: system.h:331
#define YARP_DISABLE_DEPRECATED_WARNING
Disable deprecated warnings in the following code.
Definition: system.h:333