src/time.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 #ifdef _MSC_VER
35  #ifndef NOMINMAX
36  #define NOMINMAX
37  #endif
38 #endif
39 
40 #include "ros/time.h"
41 #include "ros/impl/time.h"
42 #include <cmath>
43 #include <ctime>
44 #include <iomanip>
45 #include <stdexcept>
46 #include <limits>
47 
48 // time related includes for macOS
49 #if defined(__APPLE__)
50 #include <mach/clock.h>
51 #include <mach/mach.h>
52 #endif // defined(__APPLE__)
53 
54 #include <boost/thread/mutex.hpp>
55 #include <boost/io/ios_state.hpp>
56 #include <boost/date_time/posix_time/ptime.hpp>
57 
58 /*********************************************************************
59  ** Preprocessor
60  *********************************************************************/
61 
62 // Could probably do some better and more elaborate checking
63 // and definition here.
64 #define HAS_CLOCK_GETTIME (_POSIX_C_SOURCE >= 199309L)
65 
66 /*********************************************************************
67  ** Namespaces
68  *********************************************************************/
69 
70 namespace ros
71 {
72 
73  /*********************************************************************
74  ** Variables
75  *********************************************************************/
76 
77  const Duration DURATION_MAX(std::numeric_limits<int32_t>::max(), 999999999);
78  const Duration DURATION_MIN(std::numeric_limits<int32_t>::min(), 0);
79 
80  const Time TIME_MAX(std::numeric_limits<uint32_t>::max(), 999999999);
81  const Time TIME_MIN(0, 1);
82 
83  // This is declared here because it's set from the Time class but read from
84  // the Duration class, and need not be exported to users of either.
85  static bool g_stopped(false);
86 
87  // I assume that this is declared here, instead of time.h, to keep users
88  // of time.h from including boost/thread/mutex.hpp
89  static boost::mutex g_sim_time_mutex;
90 
91  static bool g_initialized(false);
92  static bool g_use_sim_time(true);
93  static Time g_sim_time(0, 0);
94 
95  /*********************************************************************
96  ** Cross Platform Functions
97  *********************************************************************/
98  /*
99  * These have only internal linkage to this translation unit.
100  * (i.e. not exposed to users of the time classes)
101  */
102  void ros_walltime(uint32_t& sec, uint32_t& nsec)
103  {
104 #ifndef WIN32
105 #if HAS_CLOCK_GETTIME
106  timespec start;
107  clock_gettime(CLOCK_REALTIME, &start);
108  if (start.tv_sec < 0 || start.tv_sec > UINT_MAX)
109  throw std::runtime_error("Timespec is out of dual 32-bit range");
110  sec = start.tv_sec;
111  nsec = start.tv_nsec;
112 #else
113  struct timeval timeofday;
114  gettimeofday(&timeofday,NULL);
115  if (timeofday.tv_sec < 0 || timeofday.tv_sec > UINT_MAX)
116  throw std::runtime_error("Timeofday is out of dual signed 32-bit range");
117  sec = timeofday.tv_sec;
118  nsec = timeofday.tv_usec * 1000;
119 #endif
120 #else
121  // Win32 implementation
122  // unless I've missed something obvious, the only way to get high-precision
123  // time on Windows is via the QueryPerformanceCounter() call. However,
124  // this is somewhat problematic in Windows XP on some processors, especially
125  // AMD, because the Windows implementation can freak out when the CPU clocks
126  // down to save power. Time can jump or even go backwards. Microsoft has
127  // fixed this bug for most systems now, but it can still show up if you have
128  // not installed the latest CPU drivers (an oxymoron). They fixed all these
129  // problems in Windows Vista, and this API is by far the most accurate that
130  // I know of in Windows, so I'll use it here despite all these caveats
131  static LARGE_INTEGER cpu_freq, init_cpu_time;
132  static uint32_t start_sec = 0;
133  static uint32_t start_nsec = 0;
134  if ( ( start_sec == 0 ) && ( start_nsec == 0 ) )
135  {
136  QueryPerformanceFrequency(&cpu_freq);
137  if (cpu_freq.QuadPart == 0) {
139  }
140  QueryPerformanceCounter(&init_cpu_time);
141  // compute an offset from the Epoch using the lower-performance timer API
142  FILETIME ft;
143  GetSystemTimeAsFileTime(&ft);
144  LARGE_INTEGER start_li;
145  start_li.LowPart = ft.dwLowDateTime;
146  start_li.HighPart = ft.dwHighDateTime;
147  // why did they choose 1601 as the time zero, instead of 1970?
148  // there were no outstanding hard rock bands in 1601.
149 #ifdef _MSC_VER
150  start_li.QuadPart -= 116444736000000000Ui64;
151 #else
152  start_li.QuadPart -= 116444736000000000ULL;
153 #endif
154  int64_t start_sec64 = start_li.QuadPart / 10000000; // 100-ns units
155  if (start_sec64 < 0 || start_sec64 > UINT_MAX)
156  throw std::runtime_error("SystemTime is out of dual 32-bit range");
157  start_sec = (uint32_t)start_sec64;
158  start_nsec = (start_li.LowPart % 10000000) * 100;
159  }
160  LARGE_INTEGER cur_time;
161  QueryPerformanceCounter(&cur_time);
162  LARGE_INTEGER delta_cpu_time;
163  delta_cpu_time.QuadPart = cur_time.QuadPart - init_cpu_time.QuadPart;
164  // todo: how to handle cpu clock drift. not sure it's a big deal for us.
165  // also, think about clock wraparound. seems extremely unlikey, but possible
166  double d_delta_cpu_time = delta_cpu_time.QuadPart / (double) cpu_freq.QuadPart;
167  uint32_t delta_sec = (uint32_t) floor(d_delta_cpu_time);
168  uint32_t delta_nsec = (uint32_t) boost::math::round((d_delta_cpu_time-delta_sec) * 1e9);
169 
170  int64_t sec_sum = (int64_t)start_sec + (int64_t)delta_sec;
171  int64_t nsec_sum = (int64_t)start_nsec + (int64_t)delta_nsec;
172 
173  // Throws an exception if we go out of 32-bit range
174  normalizeSecNSecUnsigned(sec_sum, nsec_sum);
175 
176  sec = sec_sum;
177  nsec = nsec_sum;
178 #endif
179  }
180 
181  void ros_steadytime(uint32_t& sec, uint32_t& nsec)
182  {
183 #ifndef WIN32
184  timespec start;
185 #if defined(__APPLE__)
186  // On macOS use clock_get_time.
187  clock_serv_t cclock;
188  mach_timespec_t mts;
189  host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock);
190  clock_get_time(cclock, &mts);
191  mach_port_deallocate(mach_task_self(), cclock);
192  start.tv_sec = mts.tv_sec;
193  start.tv_nsec = mts.tv_nsec;
194 #else // defined(__APPLE__)
195  // Otherwise use clock_gettime.
196  clock_gettime(CLOCK_MONOTONIC, &start);
197 #endif // defined(__APPLE__)
198  sec = start.tv_sec;
199  nsec = start.tv_nsec;
200 #else
201  static LARGE_INTEGER cpu_frequency, performance_count;
202  // These should not ever fail since XP is already end of life:
203  // From https://msdn.microsoft.com/en-us/library/windows/desktop/ms644905(v=vs.85).aspx and
204  // https://msdn.microsoft.com/en-us/library/windows/desktop/ms644904(v=vs.85).aspx:
205  // "On systems that run Windows XP or later, the function will always succeed and will
206  // thus never return zero."
207  QueryPerformanceFrequency(&cpu_frequency);
208  if (cpu_frequency.QuadPart == 0) {
210  }
211  QueryPerformanceCounter(&performance_count);
212  double steady_time = performance_count.QuadPart / (double) cpu_frequency.QuadPart;
213  int64_t steady_sec = floor(steady_time);
214  int64_t steady_nsec = boost::math::round((steady_time - steady_sec) * 1e9);
215 
216  // Throws an exception if we go out of 32-bit range
217  normalizeSecNSecUnsigned(steady_sec, steady_nsec);
218 
219  sec = steady_sec;
220  nsec = steady_nsec;
221 #endif
222  }
223 
227  int ros_nanosleep(const uint32_t &sec, const uint32_t &nsec)
228  {
229 #if defined(WIN32)
230  HANDLE timer = NULL;
231  LARGE_INTEGER sleepTime;
232  sleepTime.QuadPart = -
233  static_cast<int64_t>(sec)*10000000LL -
234  static_cast<int64_t>(nsec) / 100LL;
235 
236  timer = CreateWaitableTimer(NULL, TRUE, NULL);
237  if (timer == NULL)
238  {
239  return -1;
240  }
241 
242  if (!SetWaitableTimer (timer, &sleepTime, 0, NULL, NULL, 0))
243  {
244  return -1;
245  }
246 
247  if (WaitForSingleObject (timer, INFINITE) != WAIT_OBJECT_0)
248  {
249  return -1;
250  }
251  return 0;
252 #else
253  timespec req = { sec, nsec };
254  return nanosleep(&req, NULL);
255 #endif
256  }
257 
263  bool ros_wallsleep(uint32_t sec, uint32_t nsec)
264  {
265 #if defined(WIN32)
266  ros_nanosleep(sec,nsec);
267 #else
268  timespec req = { sec, nsec };
269  timespec rem = {0, 0};
270  while (nanosleep(&req, &rem) && !g_stopped)
271  {
272  req = rem;
273  }
274 #endif
275  return !g_stopped;
276  }
277 
278  /*********************************************************************
279  ** Class Methods
280  *********************************************************************/
281 
283  {
284  return !g_use_sim_time;
285  }
286 
288  {
289  return g_use_sim_time;
290  }
291 
293  {
294  return !isSimTime();
295  }
296 
298  {
299  if (!g_initialized)
300  {
302  }
303 
304  if (g_use_sim_time)
305  {
306  boost::mutex::scoped_lock lock(g_sim_time_mutex);
307  Time t = g_sim_time;
308  return t;
309  }
310 
311  Time t;
312  ros_walltime(t.sec, t.nsec);
313 
314  return t;
315  }
316 
317  void Time::setNow(const Time& new_now)
318  {
319  boost::mutex::scoped_lock lock(g_sim_time_mutex);
320 
321  g_sim_time = new_now;
322  g_use_sim_time = true;
323  }
324 
325  void Time::init()
326  {
327  g_stopped = false;
328  g_use_sim_time = false;
329  g_initialized = true;
330  }
331 
333  {
334  g_stopped = true;
335  }
336 
338  {
339  return (!g_use_sim_time) || !g_sim_time.isZero();
340  }
341 
343  {
345  }
346 
348  {
350  while (!isValid() && !g_stopped)
351  {
352  ros::WallDuration(0.01).sleep();
353 
354  if (timeout > ros::WallDuration(0, 0) && (ros::WallTime::now() - start > timeout))
355  {
356  return false;
357  }
358  }
359 
360  if (g_stopped)
361  {
362  return false;
363  }
364 
365  return true;
366  }
367 
368  Time Time::fromBoost(const boost::posix_time::ptime& t)
369  {
370  boost::posix_time::time_duration diff = t - boost::posix_time::from_time_t(0);
371  return Time::fromBoost(diff);
372  }
373 
374  Time Time::fromBoost(const boost::posix_time::time_duration& d)
375  {
376  Time t;
377  int64_t sec64 = d.total_seconds();
378  if (sec64 < 0 || sec64 > UINT_MAX)
379  throw std::runtime_error("time_duration is out of dual 32-bit range");
380  t.sec = (uint32_t)sec64;
381 #if defined(BOOST_DATE_TIME_HAS_NANOSECONDS)
382  t.nsec = d.fractional_seconds();
383 #else
384  t.nsec = d.fractional_seconds()*1000;
385 #endif
386  return t;
387  }
388 
389  std::ostream& operator<<(std::ostream& os, const Time &rhs)
390  {
391  boost::io::ios_all_saver s(os);
392  os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
393  return os;
394  }
395 
396  std::ostream& operator<<(std::ostream& os, const Duration& rhs)
397  {
398  boost::io::ios_all_saver s(os);
399  if (rhs.sec >= 0 || rhs.nsec == 0)
400  {
401  os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
402  }
403  else
404  {
405  os << (rhs.sec == -1 ? "-" : "") << (rhs.sec + 1) << "." << std::setw(9) << std::setfill('0') << (1000000000 - rhs.nsec);
406  }
407  return os;
408  }
409 
410  bool Time::sleepUntil(const Time& end)
411  {
412  if (Time::useSystemTime())
413  {
414  Duration d(end - Time::now());
415  if (d > Duration(0))
416  {
417  return d.sleep();
418  }
419 
420  return true;
421  }
422  else
423  {
424  Time start = Time::now();
425  while (!g_stopped && (Time::now() < end))
426  {
427  ros_nanosleep(0,1000000);
428  if (Time::now() < start)
429  {
430  return false;
431  }
432  }
433 
434  return true;
435  }
436  }
437 
439  {
440  WallDuration d(end - WallTime::now());
441  if (d > WallDuration(0))
442  {
443  return d.sleep();
444  }
445 
446  return true;
447  }
448 
450  {
451  WallDuration d(end - SteadyTime::now());
452  if (d > WallDuration(0))
453  {
454  return d.sleep();
455  }
456 
457  return true;
458  }
459 
460  bool Duration::sleep() const
461  {
462  if (Time::useSystemTime())
463  {
464  return ros_wallsleep(sec, nsec);
465  }
466  else
467  {
468  Time start = Time::now();
469  Time end = start + *this;
470  if (start.isZero())
471  {
472  end = TIME_MAX;
473  }
474 
475  bool rc = false;
476  while (!g_stopped && (Time::now() < end))
477  {
478  ros_wallsleep(0, 1000000);
479  rc = true;
480 
481  // If we started at time 0 wait for the first actual time to arrive before starting the timer on
482  // our sleep
483  if (start.isZero())
484  {
485  start = Time::now();
486  end = start + *this;
487  }
488 
489  // If time jumped backwards from when we started sleeping, return immediately
490  if (Time::now() < start)
491  {
492  return false;
493  }
494  }
495 
496  return rc && !g_stopped;
497  }
498  }
499 
500  std::ostream &operator<<(std::ostream& os, const WallTime &rhs)
501  {
502  boost::io::ios_all_saver s(os);
503  os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
504  return os;
505  }
506 
507  std::ostream &operator<<(std::ostream& os, const SteadyTime &rhs)
508  {
509  boost::io::ios_all_saver s(os);
510  os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
511  return os;
512  }
513 
515  {
516  WallTime t;
517  ros_walltime(t.sec, t.nsec);
518 
519  return t;
520  }
521 
523  {
524  SteadyTime t;
525  ros_steadytime(t.sec, t.nsec);
526 
527  return t;
528  }
529 
530  std::ostream &operator<<(std::ostream& os, const WallDuration& rhs)
531  {
532  boost::io::ios_all_saver s(os);
533  if (rhs.sec >= 0 || rhs.nsec == 0)
534  {
535  os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
536  }
537  else
538  {
539  os << (rhs.sec == -1 ? "-" : "") << (rhs.sec + 1) << "." << std::setw(9) << std::setfill('0') << (1000000000 - rhs.nsec);
540  }
541  return os;
542  }
543 
544  bool WallDuration::sleep() const
545  {
546  return ros_wallsleep(sec, nsec);
547  }
548 
549  void normalizeSecNSec(uint64_t& sec, uint64_t& nsec)
550  {
551  uint64_t nsec_part = nsec % 1000000000UL;
552  uint64_t sec_part = nsec / 1000000000UL;
553 
554  if (sec + sec_part > UINT_MAX)
555  throw std::runtime_error("Time is out of dual 32-bit range");
556 
557  sec += sec_part;
558  nsec = nsec_part;
559  }
560 
561  void normalizeSecNSec(uint32_t& sec, uint32_t& nsec)
562  {
563  uint64_t sec64 = sec;
564  uint64_t nsec64 = nsec;
565 
566  normalizeSecNSec(sec64, nsec64);
567 
568  sec = (uint32_t)sec64;
569  nsec = (uint32_t)nsec64;
570  }
571 
572  void normalizeSecNSecUnsigned(int64_t& sec, int64_t& nsec)
573  {
574  int64_t nsec_part = nsec % 1000000000L;
575  int64_t sec_part = sec + nsec / 1000000000L;
576  if (nsec_part < 0)
577  {
578  nsec_part += 1000000000L;
579  --sec_part;
580  }
581 
582  if (sec_part < 0 || sec_part > UINT_MAX)
583  throw std::runtime_error("Time is out of dual 32-bit range");
584 
585  sec = sec_part;
586  nsec = nsec_part;
587  }
588 
589  template class TimeBase<Time, Duration>;
590  template class TimeBase<WallTime, WallDuration>;
591  template class TimeBase<SteadyTime, WallDuration>;
592 }
593 
594 
bool ros_wallsleep(uint32_t sec, uint32_t nsec)
Go to the wall!
Definition: src/time.cpp:263
static Time g_sim_time(0, 0)
void ros_steadytime(uint32_t &sec, uint32_t &nsec)
Definition: src/time.cpp:181
Time representation. May either represent wall clock time or ROS clock time.
Definition: time.h:179
uint32_t sec
Definition: time.h:131
ROSTIME_DECL void normalizeSecNSecUnsigned(int64_t &sec, int64_t &nsec)
Definition: src/time.cpp:572
static bool sleepUntil(const Time &end)
Sleep until a specific time has been reached.
Definition: src/time.cpp:410
bool sleep() const
sleep for the amount of time specified by this Duration. If a signal interrupts the sleep...
Definition: src/time.cpp:460
Duration representation for use with the WallTime class.
Definition: duration.h:136
Thrown if the ros subsystem hasn&#39;t been initialised before use.
Definition: time.h:88
std::ostream & operator<<(std::ostream &os, const Duration &rhs)
Definition: src/time.cpp:396
static bool useSystemTime()
Definition: src/time.cpp:282
static bool sleepUntil(const WallTime &end)
Sleep until a specific time has been reached.
Definition: src/time.cpp:438
void ros_walltime(uint32_t &sec, uint32_t &nsec)
Definition: src/time.cpp:102
static void setNow(const Time &new_now)
Definition: src/time.cpp:317
uint32_t nsec
Definition: time.h:131
int32_t nsec
Definition: duration.h:75
static bool g_stopped(false)
static bool isSystemTime()
Definition: src/time.cpp:292
Thrown if windoze high perf. timestamping is unavailable.
Definition: time.h:102
static void shutdown()
Definition: src/time.cpp:332
static bool g_initialized(false)
Time representation. Always wall-clock time.
Definition: time.h:235
bool sleep() const
sleep for the amount of time specified by this Duration. If a signal interrupts the sleep...
Definition: src/time.cpp:544
static bool sleepUntil(const SteadyTime &end)
Sleep until a specific time has been reached.
Definition: src/time.cpp:449
static SteadyTime now()
Returns the current steady (monotonic) clock time.
Definition: src/time.cpp:522
static bool isValid()
Returns whether or not the current time is valid. Time is valid if it is non-zero.
Definition: src/time.cpp:337
static void init()
Definition: src/time.cpp:325
ROSTIME_DECL const Time TIME_MAX
static boost::mutex g_sim_time_mutex
Definition: src/time.cpp:89
int ros_nanosleep(const uint32_t &sec, const uint32_t &nsec)
Simple representation of the rt library nanosleep function.
Definition: src/time.cpp:227
Time representation. Always steady-clock time.
Definition: time.h:269
ROSTIME_DECL const Time TIME_MIN
ROSTIME_DECL void normalizeSecNSec(uint64_t &sec, uint64_t &nsec)
Definition: src/time.cpp:549
Duration representation for use with the Time class.
Definition: duration.h:108
static WallTime now()
Returns the current wall clock time.
Definition: src/time.cpp:514
static bool g_use_sim_time(true)
static Time now()
Retrieve the current time. If ROS clock time is in use, this returns the time according to the ROS cl...
Definition: src/time.cpp:297
static Time fromBoost(const boost::posix_time::ptime &t)
Definition: src/time.cpp:368
ROSTIME_DECL const Duration DURATION_MIN
static bool waitForValid()
Wait for time to become valid.
Definition: src/time.cpp:342
ROSTIME_DECL const Duration DURATION_MAX
static bool isSimTime()
Definition: src/time.cpp:287
bool isZero() const
Definition: time.h:168


rostime
Author(s): Josh Faust
autogenerated on Thu Dec 21 2017 03:22:01