MRPT  2.0.4
CRateTimer.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "system-precomp.h" // Precompiled headers
11 
12 #include <mrpt/config.h>
13 #include <mrpt/core/exceptions.h>
14 #include <mrpt/system/CRateTimer.h>
15 #include <chrono>
16 #include <thread>
17 
18 #if defined(MRPT_OS_LINUX)
19 #include <time.h> // clock_nanosleep()
20 #endif
21 
22 using namespace mrpt::system;
23 
24 CRateTimer::CRateTimer(const double rate_hz) { setRate(rate_hz); }
25 void CRateTimer::setRate(const double rate_hz)
26 {
27  ASSERT_ABOVE_(rate_hz, 0.0);
28  m_rate_hz = rate_hz;
29 }
31 {
32  const double elapsed_tim = m_tictac.Tac();
33  const double period = 1.0 / m_rate_hz;
34  const int64_t wait_tim_ns =
35  static_cast<int64_t>(1.0e9 * (period - elapsed_tim));
36  if (elapsed_tim > period || wait_tim_ns <= 0)
37  {
38  m_tictac.Tic();
39  return false;
40  }
41 
42 #if !defined(MRPT_OS_LINUX)
43  std::this_thread::sleep_for(std::chrono::nanoseconds(wait_tim_ns));
44 #else
45  timespec ts{0, 0}, ts_remainer{0, 0};
46  ts.tv_sec = wait_tim_ns / 1000000000;
47  ts.tv_nsec = wait_tim_ns % 1000000000;
48 
49  for (;;)
50  {
51  ASSERT_ABOVEEQ_(ts.tv_sec, 0);
52  ASSERT_ABOVE_(ts.tv_nsec, 0);
53  ASSERT_BELOWEQ_(ts.tv_nsec, 999999999);
54 
55  int err = clock_nanosleep(CLOCK_MONOTONIC, 0, &ts, &ts_remainer);
56  if (err == 0) break; // all good
57  if (err == EINTR)
58  {
59  ts = ts_remainer;
60  continue;
61  }
62  fprintf(
63  stderr, "[CRateTimer::sleep] Error %i in clock_nanosleep()\n", err);
64  break;
65  }
66 
67 #endif
68  m_tictac.Tic();
69  return true;
70 }
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:87
bool sleep()
Sleeps for some time, such as the return of this method is 1/rate (seconds) after the return of the p...
Definition: CRateTimer.cpp:30
CRateTimer(const double rate_hz=1.0)
Ctor: specifies the desired rate (Hz)
Definition: CRateTimer.cpp:24
mrpt::system::CTicTac m_tictac
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:167
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:415
void setRate(const double rate_hz)
Changes the object loop rate (Hz)
Definition: CRateTimer.cpp:25
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
#define ASSERT_BELOWEQ_(__A, __B)
Definition: exceptions.h:161
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:76



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 7b5ddf9de Fri May 29 14:02:56 2020 +0200 at vie may 29 14:15:09 CEST 2020