time.h
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 
35 #ifndef ROS_TIME_H_INCLUDED
36 #define ROS_TIME_H_INCLUDED
37 
38 /*********************************************************************
39  ** Pragmas
40  *********************************************************************/
41 
42 #ifdef _MSC_VER
43  // Rostime has some magic interface that doesn't directly include
44  // its implementation, this just disables those warnings.
45  #pragma warning(disable: 4244)
46  #pragma warning(disable: 4661)
47 #endif
48 
49 /*********************************************************************
50  ** Headers
51  *********************************************************************/
52 
53 #include <ros/platform.h>
54 #include <iostream>
55 #include <cmath>
56 #include <ros/exception.h>
57 #include "duration.h"
58 #include <boost/math/special_functions/round.hpp>
59 #include "rostime_decl.h"
60 
61 /*********************************************************************
62  ** Cross Platform Headers
63  *********************************************************************/
64 
65 #ifdef WIN32
66  #include <sys/timeb.h>
67 #else
68  #include <sys/time.h>
69 #endif
70 
71 namespace boost {
72  namespace posix_time {
73  class ptime;
74  class time_duration;
75  }
76 }
77 
78 namespace ros
79 {
80 
81  /*********************************************************************
82  ** Exceptions
83  *********************************************************************/
84 
89  {
90  public:
92  : Exception("Cannot use ros::Time::now() before the first NodeHandle has been created or ros::start() has been called. "
93  "If this is a standalone app or test that just uses ros::Time and does not communicate over ROS, you may also call ros::Time::init()")
94  {}
95  };
96 
103  {
104  public:
106  : Exception("This windows platform does not "
107  "support the high-performance timing api.")
108  {}
109  };
110 
111  /*********************************************************************
112  ** Functions
113  *********************************************************************/
114 
115  ROSTIME_DECL void normalizeSecNSec(uint64_t& sec, uint64_t& nsec);
116  ROSTIME_DECL void normalizeSecNSec(uint32_t& sec, uint32_t& nsec);
117  ROSTIME_DECL void normalizeSecNSecUnsigned(int64_t& sec, int64_t& nsec);
118 
119  /*********************************************************************
120  ** Time Classes
121  *********************************************************************/
122 
127  template<class T, class D>
128  class TimeBase
129  {
130  public:
131  uint32_t sec, nsec;
132 
133  TimeBase() : sec(0), nsec(0) { }
134  TimeBase(uint32_t _sec, uint32_t _nsec) : sec(_sec), nsec(_nsec)
135  {
136  normalizeSecNSec(sec, nsec);
137  }
138  explicit TimeBase(double t) { fromSec(t); }
140  D operator-(const T &rhs) const;
141  T operator+(const D &rhs) const;
142  T operator-(const D &rhs) const;
143  T& operator+=(const D &rhs);
144  T& operator-=(const D &rhs);
145  bool operator==(const T &rhs) const;
146  inline bool operator!=(const T &rhs) const { return !(*static_cast<const T*>(this) == rhs); }
147  bool operator>(const T &rhs) const;
148  bool operator<(const T &rhs) const;
149  bool operator>=(const T &rhs) const;
150  bool operator<=(const T &rhs) const;
151 
152  double toSec() const { return (double)sec + 1e-9*(double)nsec; };
153  T& fromSec(double t) {
154  int64_t sec64 = (int64_t)floor(t);
155  if (sec64 < 0 || sec64 > UINT_MAX)
156  throw std::runtime_error("Time is out of dual 32-bit range");
157  sec = (uint32_t)sec64;
158  nsec = (uint32_t)boost::math::round((t-sec) * 1e9);
159  // avoid rounding errors
160  sec += (nsec / 1000000000ul);
161  nsec %= 1000000000ul;
162  return *static_cast<T*>(this);
163  }
164 
165  uint64_t toNSec() const {return (uint64_t)sec*1000000000ull + (uint64_t)nsec; }
166  T& fromNSec(uint64_t t);
167 
168  inline bool isZero() const { return sec == 0 && nsec == 0; }
169  inline bool is_zero() const { return isZero(); }
170  boost::posix_time::ptime toBoost() const;
171 
172  };
173 
179  class ROSTIME_DECL Time : public TimeBase<Time, Duration>
180  {
181  public:
183  : TimeBase<Time, Duration>()
184  {}
185 
186  Time(uint32_t _sec, uint32_t _nsec)
187  : TimeBase<Time, Duration>(_sec, _nsec)
188  {}
189 
190  explicit Time(double t) { fromSec(t); }
191 
196  static Time now();
201  static bool sleepUntil(const Time& end);
202 
203  static void init();
204  static void shutdown();
205  static void setNow(const Time& new_now);
206  static bool useSystemTime();
207  static bool isSimTime();
208  static bool isSystemTime();
209 
213  static bool isValid();
217  static bool waitForValid();
221  static bool waitForValid(const ros::WallDuration& timeout);
222 
223  static Time fromBoost(const boost::posix_time::ptime& t);
224  static Time fromBoost(const boost::posix_time::time_duration& d);
225  };
226 
227  extern ROSTIME_DECL const Time TIME_MAX;
228  extern ROSTIME_DECL const Time TIME_MIN;
229 
235  class ROSTIME_DECL WallTime : public TimeBase<WallTime, WallDuration>
236  {
237  public:
240  {}
241 
242  WallTime(uint32_t _sec, uint32_t _nsec)
243  : TimeBase<WallTime, WallDuration>(_sec, _nsec)
244  {}
245 
246  explicit WallTime(double t) { fromSec(t); }
247 
251  static WallTime now();
252 
257  static bool sleepUntil(const WallTime& end);
258 
259  static bool isSystemTime() { return true; }
260  };
261 
269  class ROSTIME_DECL SteadyTime : public TimeBase<SteadyTime, WallDuration>
270  {
271  public:
274  {}
275 
276  SteadyTime(uint32_t _sec, uint32_t _nsec)
277  : TimeBase<SteadyTime, WallDuration>(_sec, _nsec)
278  {}
279 
280  explicit SteadyTime(double t) { fromSec(t); }
281 
285  static SteadyTime now();
286 
291  static bool sleepUntil(const SteadyTime& end);
292 
293  static bool isSystemTime() { return true; }
294  };
295 
296  ROSTIME_DECL std::ostream &operator <<(std::ostream &os, const Time &rhs);
297  ROSTIME_DECL std::ostream &operator <<(std::ostream &os, const WallTime &rhs);
298  ROSTIME_DECL std::ostream &operator <<(std::ostream &os, const SteadyTime &rhs);
299 }
300 
301 #endif // ROS_TIME_H
302 
ROSTIME_DECL std::ostream & operator<<(std::ostream &os, const SteadyTime &rhs)
Definition: src/time.cpp:507
Definition: duration.h:56
TimeBase()
Definition: time.h:133
static bool isSystemTime()
Definition: time.h:293
Time representation. May either represent wall clock time or ROS clock time.
Definition: time.h:179
uint32_t sec
Definition: time.h:131
bool is_zero() const
Definition: time.h:169
WallTime(uint32_t _sec, uint32_t _nsec)
Definition: time.h:242
ROSTIME_DECL void normalizeSecNSecUnsigned(int64_t &sec, int64_t &nsec)
Definition: src/time.cpp:572
bool operator!=(const T &rhs) const
Definition: time.h:146
SteadyTime(uint32_t _sec, uint32_t _nsec)
Definition: time.h:276
Time(double t)
Definition: time.h:190
TimeBase(uint32_t _sec, uint32_t _nsec)
Definition: time.h:134
WallTime(double t)
Definition: time.h:246
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
Time(uint32_t _sec, uint32_t _nsec)
Definition: time.h:186
Base class for Time implementations. Provides storage, common functions and operator overloads...
Definition: time.h:128
Thrown if windoze high perf. timestamping is unavailable.
Definition: time.h:102
SteadyTime(double t)
Definition: time.h:280
~TimeBase()
Definition: time.h:139
Time representation. Always wall-clock time.
Definition: time.h:235
TimeBase(double t)
Definition: time.h:138
Time()
Definition: time.h:182
ROSTIME_DECL void normalizeSecNSec(uint32_t &sec, uint32_t &nsec)
Definition: src/time.cpp:561
uint64_t toNSec() const
Definition: time.h:165
ROSTIME_DECL const Time TIME_MAX
Time representation. Always steady-clock time.
Definition: time.h:269
static bool isSystemTime()
Definition: time.h:259
ROSTIME_DECL const Time TIME_MIN
Duration representation for use with the Time class.
Definition: duration.h:108
T & fromSec(double t)
Definition: time.h:153
WallTime()
Definition: time.h:238
double toSec() const
Definition: time.h:152
bool isZero() const
Definition: time.h:168
#define ROSTIME_DECL
Definition: rostime_decl.h:52


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