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


rostime
Author(s): Josh Faust
autogenerated on Sat Apr 6 2019 02:52:21