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  void ros_walltime(uint32_t& sec, uint32_t& nsec)
99  {
100 #ifndef WIN32
101 #if HAS_CLOCK_GETTIME
102  timespec start;
103  clock_gettime(CLOCK_REALTIME, &start);
104  if (start.tv_sec < 0 || start.tv_sec > UINT_MAX)
105  throw std::runtime_error("Timespec is out of dual 32-bit range");
106  sec = start.tv_sec;
107  nsec = start.tv_nsec;
108 #else
109  struct timeval timeofday;
110  gettimeofday(&timeofday,NULL);
111  if (timeofday.tv_sec < 0 || timeofday.tv_sec > UINT_MAX)
112  throw std::runtime_error("Timeofday is out of dual signed 32-bit range");
113  sec = timeofday.tv_sec;
114  nsec = timeofday.tv_usec * 1000;
115 #endif
116 #else
117  // Win32 implementation
118  // unless I've missed something obvious, the only way to get high-precision
119  // time on Windows is via the QueryPerformanceCounter() call. However,
120  // this is somewhat problematic in Windows XP on some processors, especially
121  // AMD, because the Windows implementation can freak out when the CPU clocks
122  // down to save power. Time can jump or even go backwards. Microsoft has
123  // fixed this bug for most systems now, but it can still show up if you have
124  // not installed the latest CPU drivers (an oxymoron). They fixed all these
125  // problems in Windows Vista, and this API is by far the most accurate that
126  // I know of in Windows, so I'll use it here despite all these caveats
127  static LARGE_INTEGER cpu_freq, init_cpu_time;
128  static uint32_t start_sec = 0;
129  static uint32_t start_nsec = 0;
130  if ( ( start_sec == 0 ) && ( start_nsec == 0 ) )
131  {
132  QueryPerformanceFrequency(&cpu_freq);
133  if (cpu_freq.QuadPart == 0) {
135  }
136  QueryPerformanceCounter(&init_cpu_time);
137  // compute an offset from the Epoch using the lower-performance timer API
138  FILETIME ft;
139  GetSystemTimeAsFileTime(&ft);
140  LARGE_INTEGER start_li;
141  start_li.LowPart = ft.dwLowDateTime;
142  start_li.HighPart = ft.dwHighDateTime;
143  // why did they choose 1601 as the time zero, instead of 1970?
144  // there were no outstanding hard rock bands in 1601.
145 #ifdef _MSC_VER
146  start_li.QuadPart -= 116444736000000000Ui64;
147 #else
148  start_li.QuadPart -= 116444736000000000ULL;
149 #endif
150  int64_t start_sec64 = start_li.QuadPart / 10000000; // 100-ns units
151  if (start_sec64 < 0 || start_sec64 > UINT_MAX)
152  throw std::runtime_error("SystemTime is out of dual 32-bit range");
153  start_sec = (uint32_t)start_sec64;
154  start_nsec = (start_li.LowPart % 10000000) * 100;
155  }
156  LARGE_INTEGER cur_time;
157  QueryPerformanceCounter(&cur_time);
158  LARGE_INTEGER delta_cpu_time;
159  delta_cpu_time.QuadPart = cur_time.QuadPart - init_cpu_time.QuadPart;
160  // todo: how to handle cpu clock drift. not sure it's a big deal for us.
161  // also, think about clock wraparound. seems extremely unlikey, but possible
162  double d_delta_cpu_time = delta_cpu_time.QuadPart / (double) cpu_freq.QuadPart;
163  uint32_t delta_sec = (uint32_t) floor(d_delta_cpu_time);
164  uint32_t delta_nsec = (uint32_t) boost::math::round((d_delta_cpu_time-delta_sec) * 1e9);
165 
166  int64_t sec_sum = (int64_t)start_sec + (int64_t)delta_sec;
167  int64_t nsec_sum = (int64_t)start_nsec + (int64_t)delta_nsec;
168 
169  // Throws an exception if we go out of 32-bit range
170  normalizeSecNSecUnsigned(sec_sum, nsec_sum);
171 
172  sec = sec_sum;
173  nsec = nsec_sum;
174 #endif
175  }
176 
177  void ros_steadytime(uint32_t& sec, uint32_t& nsec)
178  {
179 #ifndef WIN32
180  timespec start;
181 #if defined(__APPLE__)
182  // On macOS use clock_get_time.
183  clock_serv_t cclock;
184  mach_timespec_t mts;
185  host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock);
186  clock_get_time(cclock, &mts);
187  mach_port_deallocate(mach_task_self(), cclock);
188  start.tv_sec = mts.tv_sec;
189  start.tv_nsec = mts.tv_nsec;
190 #else // defined(__APPLE__)
191  // Otherwise use clock_gettime.
192  clock_gettime(CLOCK_MONOTONIC, &start);
193 #endif // defined(__APPLE__)
194  sec = start.tv_sec;
195  nsec = start.tv_nsec;
196 #else
197  static LARGE_INTEGER cpu_frequency, performance_count;
198  // These should not ever fail since XP is already end of life:
199  // From https://msdn.microsoft.com/en-us/library/windows/desktop/ms644905(v=vs.85).aspx and
200  // https://msdn.microsoft.com/en-us/library/windows/desktop/ms644904(v=vs.85).aspx:
201  // "On systems that run Windows XP or later, the function will always succeed and will
202  // thus never return zero."
203  QueryPerformanceFrequency(&cpu_frequency);
204  if (cpu_frequency.QuadPart == 0) {
206  }
207  QueryPerformanceCounter(&performance_count);
208  double steady_time = performance_count.QuadPart / (double) cpu_frequency.QuadPart;
209  int64_t steady_sec = floor(steady_time);
210  int64_t steady_nsec = boost::math::round((steady_time - steady_sec) * 1e9);
211 
212  // Throws an exception if we go out of 32-bit range
213  normalizeSecNSecUnsigned(steady_sec, steady_nsec);
214 
215  sec = steady_sec;
216  nsec = steady_nsec;
217 #endif
218  }
219 
220  /*
221  * These have only internal linkage to this translation unit.
222  * (i.e. not exposed to users of the time classes)
223  */
224 
228  int ros_nanosleep(const uint32_t &sec, const uint32_t &nsec)
229  {
230 #if defined(WIN32)
231  HANDLE timer = NULL;
232  LARGE_INTEGER sleepTime;
233  sleepTime.QuadPart = -
234  static_cast<int64_t>(sec)*10000000LL -
235  static_cast<int64_t>(nsec) / 100LL;
236 
237  timer = CreateWaitableTimer(NULL, TRUE, NULL);
238  if (timer == NULL)
239  {
240  return -1;
241  }
242 
243  if (!SetWaitableTimer (timer, &sleepTime, 0, NULL, NULL, 0))
244  {
245  return -1;
246  }
247 
248  if (WaitForSingleObject (timer, INFINITE) != WAIT_OBJECT_0)
249  {
250  return -1;
251  }
252  return 0;
253 #else
254  timespec req = { sec, nsec };
255  return nanosleep(&req, NULL);
256 #endif
257  }
258 
264  bool ros_wallsleep(uint32_t sec, uint32_t nsec)
265  {
266 #if defined(WIN32)
267  ros_nanosleep(sec,nsec);
268 #else
269  timespec req = { sec, nsec };
270  timespec rem = {0, 0};
271  while (nanosleep(&req, &rem) && !g_stopped)
272  {
273  req = rem;
274  }
275 #endif
276  return !g_stopped;
277  }
278 
279  /*********************************************************************
280  ** Class Methods
281  *********************************************************************/
282 
284  {
285  return !g_use_sim_time;
286  }
287 
289  {
290  return g_use_sim_time;
291  }
292 
294  {
295  return !isSimTime();
296  }
297 
299  {
300  if (!g_initialized)
301  {
303  }
304 
305  if (g_use_sim_time)
306  {
307  boost::mutex::scoped_lock lock(g_sim_time_mutex);
308  Time t = g_sim_time;
309  return t;
310  }
311 
312  Time t;
313  ros_walltime(t.sec, t.nsec);
314 
315  return t;
316  }
317 
318  void Time::setNow(const Time& new_now)
319  {
320  boost::mutex::scoped_lock lock(g_sim_time_mutex);
321 
322  g_sim_time = new_now;
323  g_use_sim_time = true;
324  }
325 
326  void Time::init()
327  {
328  g_stopped = false;
329  g_use_sim_time = false;
330  g_initialized = true;
331  }
332 
334  {
335  g_stopped = true;
336  }
337 
339  {
340  return (!g_use_sim_time) || !g_sim_time.isZero();
341  }
342 
344  {
346  }
347 
349  {
351  while (!isValid() && !g_stopped)
352  {
353  ros::WallDuration(0.01).sleep();
354 
355  if (timeout > ros::WallDuration(0, 0) && (ros::WallTime::now() - start > timeout))
356  {
357  return false;
358  }
359  }
360 
361  if (g_stopped)
362  {
363  return false;
364  }
365 
366  return true;
367  }
368 
369  Time Time::fromBoost(const boost::posix_time::ptime& t)
370  {
371  boost::posix_time::time_duration diff = t - boost::posix_time::from_time_t(0);
372  return Time::fromBoost(diff);
373  }
374 
375  Time Time::fromBoost(const boost::posix_time::time_duration& d)
376  {
377  Time t;
378  int64_t sec64 = d.total_seconds();
379  if (sec64 < 0 || sec64 > UINT_MAX)
380  throw std::runtime_error("time_duration is out of dual 32-bit range");
381  t.sec = (uint32_t)sec64;
382 #if defined(BOOST_DATE_TIME_HAS_NANOSECONDS)
383  t.nsec = d.fractional_seconds();
384 #else
385  t.nsec = d.fractional_seconds()*1000;
386 #endif
387  return t;
388  }
389 
390  std::ostream& operator<<(std::ostream& os, const Time &rhs)
391  {
392  boost::io::ios_all_saver s(os);
393  os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
394  return os;
395  }
396 
397  std::ostream& operator<<(std::ostream& os, const Duration& rhs)
398  {
399  boost::io::ios_all_saver s(os);
400  if (rhs.sec >= 0 || rhs.nsec == 0)
401  {
402  os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
403  }
404  else
405  {
406  os << (rhs.sec == -1 ? "-" : "") << (rhs.sec + 1) << "." << std::setw(9) << std::setfill('0') << (1000000000 - rhs.nsec);
407  }
408  return os;
409  }
410 
411  bool Time::sleepUntil(const Time& end)
412  {
413  if (Time::useSystemTime())
414  {
415  Duration d(end - Time::now());
416  if (d > Duration(0))
417  {
418  return d.sleep();
419  }
420 
421  return true;
422  }
423  else
424  {
425  Time start = Time::now();
426  while (!g_stopped && (Time::now() < end))
427  {
428  ros_nanosleep(0,1000000);
429  if (Time::now() < start)
430  {
431  return false;
432  }
433  }
434 
435  return true;
436  }
437  }
438 
440  {
441  WallDuration d(end - WallTime::now());
442  if (d > WallDuration(0))
443  {
444  return d.sleep();
445  }
446 
447  return true;
448  }
449 
451  {
452  WallDuration d(end - SteadyTime::now());
453  if (d > WallDuration(0))
454  {
455  return d.sleep();
456  }
457 
458  return true;
459  }
460 
461  bool Duration::sleep() const
462  {
463  if (Time::useSystemTime())
464  {
465  return ros_wallsleep(sec, nsec);
466  }
467  else
468  {
469  Time start = Time::now();
470  Time end = start + *this;
471  if (start.isZero())
472  {
473  end = TIME_MAX;
474  }
475 
476  bool rc = false;
477  while (!g_stopped && (Time::now() < end))
478  {
479  ros_wallsleep(0, 1000000);
480  rc = true;
481 
482  // If we started at time 0 wait for the first actual time to arrive before starting the timer on
483  // our sleep
484  if (start.isZero())
485  {
486  start = Time::now();
487  end = start + *this;
488  }
489 
490  // If time jumped backwards from when we started sleeping, return immediately
491  if (Time::now() < start)
492  {
493  return false;
494  }
495  }
496 
497  return rc && !g_stopped;
498  }
499  }
500 
501  std::ostream &operator<<(std::ostream& os, const WallTime &rhs)
502  {
503  boost::io::ios_all_saver s(os);
504  os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
505  return os;
506  }
507 
508  std::ostream &operator<<(std::ostream& os, const SteadyTime &rhs)
509  {
510  boost::io::ios_all_saver s(os);
511  os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
512  return os;
513  }
514 
516  {
517  WallTime t;
518  ros_walltime(t.sec, t.nsec);
519 
520  return t;
521  }
522 
524  {
525  SteadyTime t;
526  ros_steadytime(t.sec, t.nsec);
527 
528  return t;
529  }
530 
531  std::ostream &operator<<(std::ostream& os, const WallDuration& rhs)
532  {
533  boost::io::ios_all_saver s(os);
534  if (rhs.sec >= 0 || rhs.nsec == 0)
535  {
536  os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
537  }
538  else
539  {
540  os << (rhs.sec == -1 ? "-" : "") << (rhs.sec + 1) << "." << std::setw(9) << std::setfill('0') << (1000000000 - rhs.nsec);
541  }
542  return os;
543  }
544 
545  bool WallDuration::sleep() const
546  {
547  return ros_wallsleep(sec, nsec);
548  }
549 
550  void normalizeSecNSec(uint64_t& sec, uint64_t& nsec)
551  {
552  uint64_t nsec_part = nsec % 1000000000UL;
553  uint64_t sec_part = nsec / 1000000000UL;
554 
555  if (sec + sec_part > UINT_MAX)
556  throw std::runtime_error("Time is out of dual 32-bit range");
557 
558  sec += sec_part;
559  nsec = nsec_part;
560  }
561 
562  void normalizeSecNSec(uint32_t& sec, uint32_t& nsec)
563  {
564  uint64_t sec64 = sec;
565  uint64_t nsec64 = nsec;
566 
567  normalizeSecNSec(sec64, nsec64);
568 
569  sec = (uint32_t)sec64;
570  nsec = (uint32_t)nsec64;
571  }
572 
573  void normalizeSecNSecUnsigned(int64_t& sec, int64_t& nsec)
574  {
575  int64_t nsec_part = nsec % 1000000000L;
576  int64_t sec_part = sec + nsec / 1000000000L;
577  if (nsec_part < 0)
578  {
579  nsec_part += 1000000000L;
580  --sec_part;
581  }
582 
583  if (sec_part < 0 || sec_part > UINT_MAX)
584  throw std::runtime_error("Time is out of dual 32-bit range");
585 
586  sec = sec_part;
587  nsec = nsec_part;
588  }
589 
590  template class TimeBase<Time, Duration>;
591  template class TimeBase<WallTime, WallDuration>;
592  template class TimeBase<SteadyTime, WallDuration>;
593 }
594 
595 
bool ros_wallsleep(uint32_t sec, uint32_t nsec)
Go to the wall!
Definition: src/time.cpp:264
static Time g_sim_time(0, 0)
ROSTIME_DECL void ros_steadytime(uint32_t &sec, uint32_t &nsec)
Definition: src/time.cpp:177
Time representation. May either represent wall clock time or ROS clock time.
Definition: time.h:180
uint32_t sec
Definition: time.h:133
ROSTIME_DECL void normalizeSecNSecUnsigned(int64_t &sec, int64_t &nsec)
Definition: src/time.cpp:573
static bool sleepUntil(const Time &end)
Sleep until a specific time has been reached.
Definition: src/time.cpp:411
bool sleep() const
sleep for the amount of time specified by this Duration. If a signal interrupts the sleep...
Definition: src/time.cpp:461
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
ROSTIME_DECL std::ostream & operator<<(std::ostream &os, const Duration &rhs)
Definition: src/time.cpp:397
static bool useSystemTime()
Definition: src/time.cpp:283
static bool sleepUntil(const WallTime &end)
Sleep until a specific time has been reached.
Definition: src/time.cpp:439
ROSTIME_DECL void ros_walltime(uint32_t &sec, uint32_t &nsec)
Definition: src/time.cpp:98
static void setNow(const Time &new_now)
Definition: src/time.cpp:318
uint32_t nsec
Definition: time.h:133
int32_t nsec
Definition: duration.h:75
static bool g_stopped(false)
static bool isSystemTime()
Definition: src/time.cpp:293
Thrown if windows high perf. timestamping is unavailable.
Definition: time.h:102
static void shutdown()
Definition: src/time.cpp:333
static bool g_initialized(false)
Time representation. Always wall-clock time.
Definition: time.h:236
bool sleep() const
sleep for the amount of time specified by this Duration. If a signal interrupts the sleep...
Definition: src/time.cpp:545
static bool sleepUntil(const SteadyTime &end)
Sleep until a specific time has been reached.
Definition: src/time.cpp:450
static SteadyTime now()
Returns the current steady (monotonic) clock time.
Definition: src/time.cpp:523
static bool isValid()
Returns whether or not the current time is valid. Time is valid if it is non-zero.
Definition: src/time.cpp:338
static void init()
Definition: src/time.cpp:326
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:228
Time representation. Always steady-clock time.
Definition: time.h:270
ROSTIME_DECL const Time TIME_MIN
ROSTIME_DECL void normalizeSecNSec(uint64_t &sec, uint64_t &nsec)
Definition: src/time.cpp:550
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:515
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:298
static Time fromBoost(const boost::posix_time::ptime &t)
Definition: src/time.cpp:369
ROSTIME_DECL const Duration DURATION_MIN
static bool waitForValid()
Wait for time to become valid.
Definition: src/time.cpp:343
ROSTIME_DECL const Duration DURATION_MAX
static bool isSimTime()
Definition: src/time.cpp:288
bool isZero() const
Definition: time.h:169


rostime
Author(s): Josh Faust
autogenerated on Mon Aug 6 2018 02:22:28