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  template<> const Duration DurationBase<Duration>::MAX = DURATION_MAX;
87  template<> const Duration DurationBase<Duration>::MIN = DURATION_MIN;
91  template<> const Duration DurationBase<Duration>::MILLISECOND = Duration(0, 1000000);
94  template<> const Duration DurationBase<Duration>::HOUR = Duration(60 * 60, 0);
95  template<> const Duration DurationBase<Duration>::DAY = Duration(60 * 60 * 24, 0);
96 
107 
108  const Time TIME_MAX(std::numeric_limits<uint32_t>::max(), 999999999);
109  const Time TIME_MIN(0, 1);
110 
111  template<> const Time TimeBase<Time, Duration>::MAX = TIME_MAX;
112  template<> const Time TimeBase<Time, Duration>::MIN = TIME_MIN;
113  template<> const Time TimeBase<Time, Duration>::ZERO = Time(0, 0);
115 
120 
125 
126  // This is declared here because it's set from the Time class but read from
127  // the Duration class, and need not be exported to users of either.
128  static bool g_stopped(false);
129 
130  // I assume that this is declared here, instead of time.h, to keep users
131  // of time.h from including boost/thread/mutex.hpp
132  static boost::mutex g_sim_time_mutex;
133 
134  static bool g_initialized(false);
135  static bool g_use_sim_time(true);
136  static Time g_sim_time(0, 0);
137 
138  /*********************************************************************
139  ** Cross Platform Functions
140  *********************************************************************/
141  void ros_walltime(uint32_t& sec, uint32_t& nsec)
142  {
143 #if !defined(_WIN32)
144 #if HAS_CLOCK_GETTIME
145  timespec start;
146  clock_gettime(CLOCK_REALTIME, &start);
147  if (start.tv_sec < 0 || start.tv_sec > std::numeric_limits<uint32_t>::max())
148  throw std::runtime_error("Timespec is out of dual 32-bit range");
149  sec = start.tv_sec;
150  nsec = start.tv_nsec;
151 #else
152  struct timeval timeofday;
153  gettimeofday(&timeofday,NULL);
154  if (timeofday.tv_sec < 0 || timeofday.tv_sec > std::numeric_limits<uint32_t>::max())
155  throw std::runtime_error("Timeofday is out of dual signed 32-bit range");
156  sec = timeofday.tv_sec;
157  nsec = timeofday.tv_usec * 1000;
158 #endif
159 #else
160  uint64_t now_s = 0;
161  uint64_t now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
162 
163  normalizeSecNSec(now_s, now_ns);
164 
165  sec = (uint32_t)now_s;
166  nsec = (uint32_t)now_ns;
167 #endif
168  }
169 
170  void ros_steadytime(uint32_t& sec, uint32_t& nsec)
171  {
172 #if !defined(_WIN32)
173  timespec start;
174 #if defined(__APPLE__)
175  // On macOS use clock_get_time.
176  clock_serv_t cclock;
177  mach_timespec_t mts;
178  host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock);
179  clock_get_time(cclock, &mts);
180  mach_port_deallocate(mach_task_self(), cclock);
181  start.tv_sec = mts.tv_sec;
182  start.tv_nsec = mts.tv_nsec;
183 #else // defined(__APPLE__)
184  // Otherwise use clock_gettime.
185  clock_gettime(CLOCK_MONOTONIC, &start);
186 #endif // defined(__APPLE__)
187  sec = start.tv_sec;
188  nsec = start.tv_nsec;
189 #else
190  uint64_t now_s = 0;
191  uint64_t now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::steady_clock::now().time_since_epoch()).count();
192 
193  normalizeSecNSec(now_s, now_ns);
194 
195  sec = (uint32_t)now_s;
196  nsec = (uint32_t)now_ns;
197 #endif
198  }
199 
200  /*
201  * These have only internal linkage to this translation unit.
202  * (i.e. not exposed to users of the time classes)
203  */
204 
208  int ros_nanosleep(const uint32_t &sec, const uint32_t &nsec)
209  {
210 #if defined(_WIN32)
211  std::this_thread::sleep_for(std::chrono::nanoseconds(static_cast<int64_t>(sec * 1e9 + nsec)));
212  return 0;
213 #else
214  timespec req = { sec, nsec };
215  return nanosleep(&req, NULL);
216 #endif
217  }
218 
224  bool ros_wallsleep(uint32_t sec, uint32_t nsec)
225  {
226 #if defined(_WIN32)
227  ros_nanosleep(sec,nsec);
228 #else
229  timespec req = { sec, nsec };
230  timespec rem = {0, 0};
231  while (nanosleep(&req, &rem) && !g_stopped)
232  {
233  req = rem;
234  }
235 #endif
236  return !g_stopped;
237  }
238 
239  /*********************************************************************
240  ** Class Methods
241  *********************************************************************/
242 
243  bool Time::useSystemTime()
244  {
245  return !g_use_sim_time;
246  }
247 
248  bool Time::isSimTime()
249  {
251  }
252 
253  bool Time::isSystemTime()
254  {
255  return !isSimTime();
256  }
257 
258  Time Time::now()
259  {
261  {
263  }
264 
265  if (g_use_sim_time)
266  {
267  boost::mutex::scoped_lock lock(g_sim_time_mutex);
268  Time t = g_sim_time;
269  return t;
270  }
271 
272  Time t;
273  ros_walltime(t.sec, t.nsec);
274 
275  return t;
276  }
277 
278  void Time::setNow(const Time& new_now)
279  {
280  boost::mutex::scoped_lock lock(g_sim_time_mutex);
281 
282  g_sim_time = new_now;
283  g_use_sim_time = true;
284  }
285 
286  void Time::init()
287  {
288  g_stopped = false;
289  g_use_sim_time = false;
290  g_initialized = true;
291  }
292 
293  void Time::shutdown()
294  {
295  g_stopped = true;
296  }
297 
298  bool Time::isValid()
299  {
301  }
302 
303  bool Time::waitForValid()
304  {
306  }
307 
308  bool Time::waitForValid(const ros::WallDuration& timeout)
309  {
311  while (!isValid() && !g_stopped)
312  {
313  ros::WallDuration(0.01).sleep();
314 
315  if (timeout > ros::WallDuration(0, 0) && (ros::WallTime::now() - start > timeout))
316  {
317  return false;
318  }
319  }
320 
321  if (g_stopped)
322  {
323  return false;
324  }
325 
326  return true;
327  }
328 
329  Time Time::fromBoost(const boost::posix_time::ptime& t)
330  {
331  boost::posix_time::time_duration diff = t - boost::posix_time::from_time_t(0);
332  return Time::fromBoost(diff);
333  }
334 
335  Time Time::fromBoost(const boost::posix_time::time_duration& d)
336  {
337  Time t;
338  int64_t sec64 = d.total_seconds();
339  if (sec64 < 0 || sec64 > std::numeric_limits<uint32_t>::max())
340  throw std::runtime_error("time_duration is out of dual 32-bit range");
341  t.sec = (uint32_t)sec64;
342 #if defined(BOOST_DATE_TIME_HAS_NANOSECONDS)
343  t.nsec = d.fractional_seconds();
344 #else
345  t.nsec = d.fractional_seconds()*1000;
346 #endif
347  return t;
348  }
349 
350  std::ostream& operator<<(std::ostream& os, const Time &rhs)
351  {
352  boost::io::ios_all_saver s(os);
353  os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
354  return os;
355  }
356 
357  std::ostream& operator<<(std::ostream& os, const Duration& rhs)
358  {
359  boost::io::ios_all_saver s(os);
360  if (rhs.sec >= 0 || rhs.nsec == 0)
361  {
362  os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
363  }
364  else
365  {
366  os << (rhs.sec == -1 ? "-" : "") << (rhs.sec + 1) << "." << std::setw(9) << std::setfill('0') << (1000000000 - rhs.nsec);
367  }
368  return os;
369  }
370 
371  bool Time::sleepUntil(const Time& end)
372  {
374  {
375  Duration d(end - Time::now());
376  if (d > Duration(0))
377  {
378  return d.sleep();
379  }
380 
381  return true;
382  }
383  else
384  {
385  Time start = Time::now();
386  while (!g_stopped && (Time::now() < end))
387  {
388  ros_nanosleep(0,1000000);
389  if (Time::now() < start)
390  {
391  return false;
392  }
393  }
394 
395  return true;
396  }
397  }
398 
399  bool WallTime::sleepUntil(const WallTime& end)
400  {
402  if (d > WallDuration(0))
403  {
404  return d.sleep();
405  }
406 
407  return true;
408  }
409 
410  bool SteadyTime::sleepUntil(const SteadyTime& end)
411  {
413  if (d > WallDuration(0))
414  {
415  return d.sleep();
416  }
417 
418  return true;
419  }
420 
421  bool Duration::sleep() const
422  {
424  {
425  return ros_wallsleep(sec, nsec);
426  }
427  else
428  {
429  Time start = Time::now();
430  Time end = start + *this;
431  if (start.isZero())
432  {
433  end = TIME_MAX;
434  }
435 
436  bool rc = false;
437  // This allows sim time to run up to 10x real-time even for very short sleep durations.
438  const uint32_t sleep_nsec = (sec != 0) ? 1000000 : (std::min)(1000000, nsec/10);
439  while (!g_stopped && (Time::now() < end))
440  {
441  ros_wallsleep(0, sleep_nsec);
442  rc = true;
443 
444  // If we started at time 0 wait for the first actual time to arrive before starting the timer on
445  // our sleep
446  if (start.isZero())
447  {
448  start = Time::now();
449  end = start + *this;
450  }
451 
452  // If time jumped backwards from when we started sleeping, return immediately
453  if (Time::now() < start)
454  {
455  return false;
456  }
457  }
458 
459  return rc && !g_stopped;
460  }
461  }
462 
463  std::ostream &operator<<(std::ostream& os, const WallTime &rhs)
464  {
465  boost::io::ios_all_saver s(os);
466  os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
467  return os;
468  }
469 
470  std::ostream &operator<<(std::ostream& os, const SteadyTime &rhs)
471  {
472  boost::io::ios_all_saver s(os);
473  os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
474  return os;
475  }
476 
477  WallTime WallTime::now()
478  {
480  ros_walltime(t.sec, t.nsec);
481 
482  return t;
483  }
484 
486  {
488  ros_steadytime(t.sec, t.nsec);
489 
490  return t;
491  }
492 
493  std::ostream &operator<<(std::ostream& os, const WallDuration& rhs)
494  {
495  boost::io::ios_all_saver s(os);
496  if (rhs.sec >= 0 || rhs.nsec == 0)
497  {
498  os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
499  }
500  else
501  {
502  os << (rhs.sec == -1 ? "-" : "") << (rhs.sec + 1) << "." << std::setw(9) << std::setfill('0') << (1000000000 - rhs.nsec);
503  }
504  return os;
505  }
506 
507  bool WallDuration::sleep() const
508  {
510  }
511 
512  void normalizeSecNSec(uint64_t& sec, uint64_t& nsec)
513  {
514  uint64_t nsec_part = nsec % 1000000000UL;
515  uint64_t sec_part = nsec / 1000000000UL;
516 
517  if (sec + sec_part > std::numeric_limits<uint32_t>::max())
518  throw std::runtime_error("Time is out of dual 32-bit range");
519 
520  sec += sec_part;
521  nsec = nsec_part;
522  }
523 
524  void normalizeSecNSec(uint32_t& sec, uint32_t& nsec)
525  {
526  uint64_t sec64 = sec;
527  uint64_t nsec64 = nsec;
528 
529  normalizeSecNSec(sec64, nsec64);
530 
531  sec = (uint32_t)sec64;
532  nsec = (uint32_t)nsec64;
533  }
534 
535  void normalizeSecNSecUnsigned(int64_t& sec, int64_t& nsec)
536  {
537  int64_t nsec_part = nsec % 1000000000L;
538  int64_t sec_part = sec + nsec / 1000000000L;
539  if (nsec_part < 0)
540  {
541  nsec_part += 1000000000L;
542  --sec_part;
543  }
544 
545  if (sec_part < 0 || sec_part > std::numeric_limits<uint32_t>::max())
546  throw std::runtime_error("Time is out of dual 32-bit range");
547 
548  sec = sec_part;
549  nsec = nsec_part;
550  }
551 
552  template class TimeBase<Time, Duration>;
553  template class TimeBase<WallTime, WallDuration>;
554  template class TimeBase<SteadyTime, WallDuration>;
555 }
ros::g_initialized
static bool g_initialized(false)
ros::ros_steadytime
ROSTIME_DECL void ros_steadytime(uint32_t &sec, uint32_t &nsec)
Definition: src/time.cpp:172
ros::ros_walltime
ROSTIME_DECL void ros_walltime(uint32_t &sec, uint32_t &nsec)
Definition: src/time.cpp:143
ros::DurationBase< Duration >::HOUR
static const Duration HOUR
One hour duration.
Definition: duration.h:102
ros::normalizeSecNSecUnsigned
ROSTIME_DECL void normalizeSecNSecUnsigned(int64_t &sec, int64_t &nsec)
Definition: src/time.cpp:537
ros::WallDuration::sleep
bool sleep() const
sleep for the amount of time specified by this Duration. If a signal interrupts the sleep,...
Definition: src/time.cpp:509
time.h
ros::operator<<
ROSTIME_DECL std::ostream & operator<<(std::ostream &os, const Duration &rhs)
Definition: src/time.cpp:359
ros::Time::isSimTime
static bool isSimTime()
Definition: src/time.cpp:250
ros::DURATION_MIN
const ROSTIME_DECL Duration DURATION_MIN
ros
ros::DurationBase< Duration >::DAY
static const Duration DAY
One day duration.
Definition: duration.h:101
ros::DurationBase< Duration >::nsec
int32_t nsec
Definition: duration.h:75
ros::Time::setNow
static void setNow(const Time &new_now)
Definition: src/time.cpp:280
ros::Time::shutdown
static void shutdown()
Definition: src/time.cpp:295
ros::DURATION_MAX
const ROSTIME_DECL Duration DURATION_MAX
ros::DurationBase< Duration >::MICROSECOND
static const Duration MICROSECOND
One microsecond duration.
Definition: duration.h:106
ros::Time::sleepUntil
static bool sleepUntil(const Time &end)
Sleep until a specific time has been reached.
Definition: src/time.cpp:373
ros::TimeBase< Time, Duration >::MAX
static const Time MAX
Maximum representable time.
Definition: time.h:166
ros::DurationBase< Duration >::MILLISECOND
static const Duration MILLISECOND
One millisecond duration.
Definition: duration.h:105
ros::DurationBase< Duration >::MIN
static const Duration MIN
Minimum representable duration (negative)
Definition: duration.h:98
ros::DurationBase< Duration >::ZERO
static const Duration ZERO
Zero duration.
Definition: duration.h:100
ros::Time::isValid
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:300
ros::SteadyTime::now
static SteadyTime now()
Returns the current steady (monotonic) clock time.
Definition: src/time.cpp:487
ros::Time::useSystemTime
static bool useSystemTime()
Definition: src/time.cpp:245
ros::ros_wallsleep
bool ros_wallsleep(uint32_t sec, uint32_t nsec)
Go to the wall!
Definition: src/time.cpp:226
ros::TimeBase
Base class for Time implementations. Provides storage, common functions and operator overloads....
Definition: time.h:130
ros::g_sim_time_mutex
static boost::mutex g_sim_time_mutex
Definition: src/time.cpp:134
ros::TimeBase::isZero
bool isZero() const
Definition: time.h:161
ros::WallTime::now
static WallTime now()
Returns the current wall clock time.
Definition: src/time.cpp:479
ros::SteadyTime
Time representation. Always steady-clock time.
Definition: time.h:273
time.h
ros::TimeBase< Time, Duration >::ZERO
static const Time ZERO
Zero (invalid) time.
Definition: time.h:167
ros::Time::isSystemTime
static bool isSystemTime()
Definition: src/time.cpp:255
ros::DurationBase
Base class for Duration implementations. Provides storage, common functions and operator overloads....
Definition: duration.h:72
ros::DurationBase< Duration >::SECOND
static const Duration SECOND
One second duration.
Definition: duration.h:104
ros::SteadyTime::sleepUntil
static bool sleepUntil(const SteadyTime &end)
Sleep until a specific time has been reached.
Definition: src/time.cpp:412
ros::WallTime
Time representation. Always wall-clock time.
Definition: time.h:234
ros::DurationBase< Duration >::NANOSECOND
static const Duration NANOSECOND
One nanosecond duration.
Definition: duration.h:107
ros::TimeBase::sec
uint32_t sec
Definition: time.h:135
ros::TimeBase::nsec
uint32_t nsec
Definition: time.h:135
ros::TIME_MIN
const ROSTIME_DECL Time TIME_MIN
ros::TimeNotInitializedException
Thrown if the ros subsystem hasn't been initialised before use.
Definition: time.h:88
ros::ros_nanosleep
int ros_nanosleep(const uint32_t &sec, const uint32_t &nsec)
Simple representation of the rt library nanosleep function.
Definition: src/time.cpp:210
ros::DurationBase< Duration >::MAX
static const Duration MAX
Maximum representable duration.
Definition: duration.h:99
ros::Time::init
static void init()
Definition: src/time.cpp:288
ros::Time::Time
Time()
Definition: time.h:177
ros::TIME_MAX
const ROSTIME_DECL Time TIME_MAX
ros::g_sim_time
static Time g_sim_time(0, 0)
ros::Time
Time representation. May either represent wall clock time or ROS clock time.
Definition: time.h:174
ros::TimeBase< Time, Duration >::MIN
static const Time MIN
Minimum representable time.
Definition: time.h:165
ros::Time::waitForValid
static bool waitForValid()
Wait for time source to become valid.
Definition: src/time.cpp:305
ros::TimeBase< Time, Duration >::UNINITIALIZED
static const Time UNINITIALIZED
Uninitialized time.
Definition: time.h:168
ros::DurationBase< Duration >::MINUTE
static const Duration MINUTE
One minute duration.
Definition: duration.h:103
ros::WallTime::sleepUntil
static bool sleepUntil(const WallTime &end)
Sleep until a specific time has been reached.
Definition: src/time.cpp:401
ros::Duration::sleep
bool sleep() const
sleep for the amount of time specified by this Duration. If a signal interrupts the sleep,...
Definition: src/time.cpp:423
ros::WallDuration
Duration representation for use with the WallTime class.
Definition: duration.h:155
ros::Duration
Duration representation for use with the Time class.
Definition: duration.h:117
ros::normalizeSecNSec
ROSTIME_DECL void normalizeSecNSec(uint64_t &sec, uint64_t &nsec)
Definition: src/time.cpp:514
ros::g_use_sim_time
static bool g_use_sim_time(true)
ros::g_stopped
static bool g_stopped(false)
ros::DurationBase< Duration >::sec
int32_t sec
Definition: duration.h:75
ros::Time::fromBoost
static Time fromBoost(const boost::posix_time::ptime &t)
Definition: src/time.cpp:331
ros::Time::now
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:260


rostime
Author(s): Josh Faust, Dirk Thomas
autogenerated on Sat Jun 17 2023 02:32:37