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 #if !defined(_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  uint64_t now_s = 0;
124  uint64_t now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
125 
126  normalizeSecNSec(now_s, now_ns);
127 
128  sec = (uint32_t)now_s;
129  nsec = (uint32_t)now_ns;
130 #endif
131  }
132 
133  void ros_steadytime(uint32_t& sec, uint32_t& nsec)
134  {
135 #if !defined(_WIN32)
136  timespec start;
137 #if defined(__APPLE__)
138  // On macOS use clock_get_time.
139  clock_serv_t cclock;
140  mach_timespec_t mts;
141  host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock);
142  clock_get_time(cclock, &mts);
143  mach_port_deallocate(mach_task_self(), cclock);
144  start.tv_sec = mts.tv_sec;
145  start.tv_nsec = mts.tv_nsec;
146 #else // defined(__APPLE__)
147  // Otherwise use clock_gettime.
148  clock_gettime(CLOCK_MONOTONIC, &start);
149 #endif // defined(__APPLE__)
150  sec = start.tv_sec;
151  nsec = start.tv_nsec;
152 #else
153  uint64_t now_s = 0;
154  uint64_t now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::steady_clock::now().time_since_epoch()).count();
155 
156  normalizeSecNSec(now_s, now_ns);
157 
158  sec = (uint32_t)now_s;
159  nsec = (uint32_t)now_ns;
160 #endif
161  }
162 
163  /*
164  * These have only internal linkage to this translation unit.
165  * (i.e. not exposed to users of the time classes)
166  */
167 
171  int ros_nanosleep(const uint32_t &sec, const uint32_t &nsec)
172  {
173 #if defined(_WIN32)
174  std::this_thread::sleep_for(std::chrono::nanoseconds(static_cast<int64_t>(sec * 1e9 + nsec)));
175  return 0;
176 #else
177  timespec req = { sec, nsec };
178  return nanosleep(&req, NULL);
179 #endif
180  }
181 
187  bool ros_wallsleep(uint32_t sec, uint32_t nsec)
188  {
189 #if defined(_WIN32)
190  ros_nanosleep(sec,nsec);
191 #else
192  timespec req = { sec, nsec };
193  timespec rem = {0, 0};
194  while (nanosleep(&req, &rem) && !g_stopped)
195  {
196  req = rem;
197  }
198 #endif
199  return !g_stopped;
200  }
201 
202  /*********************************************************************
203  ** Class Methods
204  *********************************************************************/
205 
207  {
208  return !g_use_sim_time;
209  }
210 
212  {
213  return g_use_sim_time;
214  }
215 
217  {
218  return !isSimTime();
219  }
220 
222  {
223  if (!g_initialized)
224  {
226  }
227 
228  if (g_use_sim_time)
229  {
230  boost::mutex::scoped_lock lock(g_sim_time_mutex);
231  Time t = g_sim_time;
232  return t;
233  }
234 
235  Time t;
236  ros_walltime(t.sec, t.nsec);
237 
238  return t;
239  }
240 
241  void Time::setNow(const Time& new_now)
242  {
243  boost::mutex::scoped_lock lock(g_sim_time_mutex);
244 
245  g_sim_time = new_now;
246  g_use_sim_time = true;
247  }
248 
249  void Time::init()
250  {
251  g_stopped = false;
252  g_use_sim_time = false;
253  g_initialized = true;
254  }
255 
257  {
258  g_stopped = true;
259  }
260 
262  {
263  return (!g_use_sim_time) || !g_sim_time.isZero();
264  }
265 
267  {
269  }
270 
272  {
274  while (!isValid() && !g_stopped)
275  {
276  ros::WallDuration(0.01).sleep();
277 
278  if (timeout > ros::WallDuration(0, 0) && (ros::WallTime::now() - start > timeout))
279  {
280  return false;
281  }
282  }
283 
284  if (g_stopped)
285  {
286  return false;
287  }
288 
289  return true;
290  }
291 
292  Time Time::fromBoost(const boost::posix_time::ptime& t)
293  {
294  boost::posix_time::time_duration diff = t - boost::posix_time::from_time_t(0);
295  return Time::fromBoost(diff);
296  }
297 
298  Time Time::fromBoost(const boost::posix_time::time_duration& d)
299  {
300  Time t;
301  int64_t sec64 = d.total_seconds();
302  if (sec64 < 0 || sec64 > std::numeric_limits<uint32_t>::max())
303  throw std::runtime_error("time_duration is out of dual 32-bit range");
304  t.sec = (uint32_t)sec64;
305 #if defined(BOOST_DATE_TIME_HAS_NANOSECONDS)
306  t.nsec = d.fractional_seconds();
307 #else
308  t.nsec = d.fractional_seconds()*1000;
309 #endif
310  return t;
311  }
312 
313  std::ostream& operator<<(std::ostream& os, const Time &rhs)
314  {
315  boost::io::ios_all_saver s(os);
316  os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
317  return os;
318  }
319 
320  std::ostream& operator<<(std::ostream& os, const Duration& rhs)
321  {
322  boost::io::ios_all_saver s(os);
323  if (rhs.sec >= 0 || rhs.nsec == 0)
324  {
325  os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
326  }
327  else
328  {
329  os << (rhs.sec == -1 ? "-" : "") << (rhs.sec + 1) << "." << std::setw(9) << std::setfill('0') << (1000000000 - rhs.nsec);
330  }
331  return os;
332  }
333 
334  bool Time::sleepUntil(const Time& end)
335  {
336  if (Time::useSystemTime())
337  {
338  Duration d(end - Time::now());
339  if (d > Duration(0))
340  {
341  return d.sleep();
342  }
343 
344  return true;
345  }
346  else
347  {
348  Time start = Time::now();
349  while (!g_stopped && (Time::now() < end))
350  {
351  ros_nanosleep(0,1000000);
352  if (Time::now() < start)
353  {
354  return false;
355  }
356  }
357 
358  return true;
359  }
360  }
361 
363  {
364  WallDuration d(end - WallTime::now());
365  if (d > WallDuration(0))
366  {
367  return d.sleep();
368  }
369 
370  return true;
371  }
372 
374  {
375  WallDuration d(end - SteadyTime::now());
376  if (d > WallDuration(0))
377  {
378  return d.sleep();
379  }
380 
381  return true;
382  }
383 
384  bool Duration::sleep() const
385  {
386  if (Time::useSystemTime())
387  {
388  return ros_wallsleep(sec, nsec);
389  }
390  else
391  {
392  Time start = Time::now();
393  Time end = start + *this;
394  if (start.isZero())
395  {
396  end = TIME_MAX;
397  }
398 
399  bool rc = false;
400  while (!g_stopped && (Time::now() < end))
401  {
402  ros_wallsleep(0, 1000000);
403  rc = true;
404 
405  // If we started at time 0 wait for the first actual time to arrive before starting the timer on
406  // our sleep
407  if (start.isZero())
408  {
409  start = Time::now();
410  end = start + *this;
411  }
412 
413  // If time jumped backwards from when we started sleeping, return immediately
414  if (Time::now() < start)
415  {
416  return false;
417  }
418  }
419 
420  return rc && !g_stopped;
421  }
422  }
423 
424  std::ostream &operator<<(std::ostream& os, const WallTime &rhs)
425  {
426  boost::io::ios_all_saver s(os);
427  os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
428  return os;
429  }
430 
431  std::ostream &operator<<(std::ostream& os, const SteadyTime &rhs)
432  {
433  boost::io::ios_all_saver s(os);
434  os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
435  return os;
436  }
437 
439  {
440  WallTime t;
441  ros_walltime(t.sec, t.nsec);
442 
443  return t;
444  }
445 
447  {
448  SteadyTime t;
449  ros_steadytime(t.sec, t.nsec);
450 
451  return t;
452  }
453 
454  std::ostream &operator<<(std::ostream& os, const WallDuration& rhs)
455  {
456  boost::io::ios_all_saver s(os);
457  if (rhs.sec >= 0 || rhs.nsec == 0)
458  {
459  os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
460  }
461  else
462  {
463  os << (rhs.sec == -1 ? "-" : "") << (rhs.sec + 1) << "." << std::setw(9) << std::setfill('0') << (1000000000 - rhs.nsec);
464  }
465  return os;
466  }
467 
468  bool WallDuration::sleep() const
469  {
470  return ros_wallsleep(sec, nsec);
471  }
472 
473  void normalizeSecNSec(uint64_t& sec, uint64_t& nsec)
474  {
475  uint64_t nsec_part = nsec % 1000000000UL;
476  uint64_t sec_part = nsec / 1000000000UL;
477 
478  if (sec + sec_part > std::numeric_limits<uint32_t>::max())
479  throw std::runtime_error("Time is out of dual 32-bit range");
480 
481  sec += sec_part;
482  nsec = nsec_part;
483  }
484 
485  void normalizeSecNSec(uint32_t& sec, uint32_t& nsec)
486  {
487  uint64_t sec64 = sec;
488  uint64_t nsec64 = nsec;
489 
490  normalizeSecNSec(sec64, nsec64);
491 
492  sec = (uint32_t)sec64;
493  nsec = (uint32_t)nsec64;
494  }
495 
496  void normalizeSecNSecUnsigned(int64_t& sec, int64_t& nsec)
497  {
498  int64_t nsec_part = nsec % 1000000000L;
499  int64_t sec_part = sec + nsec / 1000000000L;
500  if (nsec_part < 0)
501  {
502  nsec_part += 1000000000L;
503  --sec_part;
504  }
505 
506  if (sec_part < 0 || sec_part > std::numeric_limits<uint32_t>::max())
507  throw std::runtime_error("Time is out of dual 32-bit range");
508 
509  sec = sec_part;
510  nsec = nsec_part;
511  }
512 
513  template class TimeBase<Time, Duration>;
514  template class TimeBase<WallTime, WallDuration>;
515  template class TimeBase<SteadyTime, WallDuration>;
516 }
bool ros_wallsleep(uint32_t sec, uint32_t nsec)
Go to the wall!
Definition: src/time.cpp:187
static Time g_sim_time(0, 0)
ROSTIME_DECL void ros_steadytime(uint32_t &sec, uint32_t &nsec)
Definition: src/time.cpp:133
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:496
static bool sleepUntil(const Time &end)
Sleep until a specific time has been reached.
Definition: src/time.cpp:334
bool sleep() const
sleep for the amount of time specified by this Duration. If a signal interrupts the sleep...
Definition: src/time.cpp:384
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:320
static bool useSystemTime()
Definition: src/time.cpp:206
static bool sleepUntil(const WallTime &end)
Sleep until a specific time has been reached.
Definition: src/time.cpp:362
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:241
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:216
static void shutdown()
Definition: src/time.cpp:256
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:468
static bool sleepUntil(const SteadyTime &end)
Sleep until a specific time has been reached.
Definition: src/time.cpp:373
static SteadyTime now()
Returns the current steady (monotonic) clock time.
Definition: src/time.cpp:446
static bool isValid()
Returns whether or not the current time source is valid. Simulation time is valid if it is non-zero...
Definition: src/time.cpp:261
static void init()
Definition: src/time.cpp:249
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:171
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:473
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:438
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:221
static Time fromBoost(const boost::posix_time::ptime &t)
Definition: src/time.cpp:292
ROSTIME_DECL const Duration DURATION_MIN
static bool waitForValid()
Wait for time source to become valid.
Definition: src/time.cpp:266
ROSTIME_DECL const Duration DURATION_MAX
static bool isSimTime()
Definition: src/time.cpp:211
bool isZero() const
Definition: time.h:159


rostime
Author(s): Josh Faust
autogenerated on Mon Jul 27 2020 03:22:42