00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef ROS_DURATION_H
00036 #define ROS_DURATION_H
00037
00038 #include <iostream>
00039 #include <math.h>
00040 #include <stdexcept>
00041 #include <climits>
00042 #include <stdint.h>
00043
00044 namespace ros
00045 {
00046
00047 inline void normalizeSecNSecSigned(int64_t& sec, int64_t& nsec)
00048 {
00049 int64_t nsec_part = nsec;
00050 int64_t sec_part = sec;
00051
00052 while (nsec_part > 1000000000L)
00053 {
00054 nsec_part -= 1000000000L;
00055 ++sec_part;
00056 }
00057 while (nsec_part < 0)
00058 {
00059 nsec_part += 1000000000L;
00060 --sec_part;
00061 }
00062
00063 if (sec_part < INT_MIN || sec_part > INT_MAX)
00064 throw std::runtime_error("Duration is out of dual 32-bit range");
00065
00066 sec = sec_part;
00067 nsec = nsec_part;
00068 }
00069
00070 inline void normalizeSecNSecSigned(int32_t& sec, int32_t& nsec)
00071 {
00072 int64_t sec64 = sec;
00073 int64_t nsec64 = nsec;
00074
00075 normalizeSecNSecSigned(sec64, nsec64);
00076
00077 sec = (int32_t)sec64;
00078 nsec = (int32_t)nsec64;
00079 }
00080
00085 template<class T>
00086 class DurationBase
00087 {
00088 public:
00089 int32_t sec, nsec;
00090 DurationBase() : sec(0), nsec(0) { }
00091 DurationBase(int32_t _sec, int32_t _nsec);
00092 explicit DurationBase(double t){fromSec(t);};
00093 ~DurationBase() {}
00094 T operator+(const T &rhs) const;
00095 T operator-(const T &rhs) const;
00096 T operator-() const;
00097 T operator*(double scale) const;
00098 T& operator+=(const T &rhs);
00099 T& operator-=(const T &rhs);
00100 T& operator*=(double scale);
00101 bool operator==(const T &rhs) const;
00102 inline bool operator!=(const T &rhs) const { return !(*static_cast<const T*>(this) == rhs); }
00103 bool operator>(const T &rhs) const;
00104 bool operator<(const T &rhs) const;
00105 bool operator>=(const T &rhs) const;
00106 bool operator<=(const T &rhs) const;
00107 double toSec() const { return (double)sec + 1e-9*(double)nsec; };
00108 int64_t toNSec() const {return (int64_t)sec*1000000000ll + (int64_t)nsec; };
00109 T& fromSec(double t);
00110 T& fromNSec(int64_t t);
00111 bool isZero();
00112 };
00113
00119 class Duration : public DurationBase<Duration>
00120 {
00121 public:
00122 Duration()
00123 : DurationBase<Duration>()
00124 { }
00125
00126 Duration(int32_t _sec, int32_t _nsec)
00127 : DurationBase<Duration>(_sec, _nsec)
00128 {}
00129
00130 explicit Duration(double t) { fromSec(t); }
00131
00135 bool sleep() const;
00136 };
00137
00138 extern const Duration DURATION_MAX;
00139 extern const Duration DURATION_MIN;
00140
00146 class WallDuration : public DurationBase<WallDuration>
00147 {
00148 public:
00149 WallDuration()
00150 : DurationBase<WallDuration>()
00151 { }
00152
00153 WallDuration(int32_t _sec, int32_t _nsec)
00154 : DurationBase<WallDuration>(_sec, _nsec)
00155 {}
00156
00157 explicit WallDuration(double t) { fromSec(t); }
00158
00162 bool sleep() const;
00163 };
00164
00165 std::ostream &operator <<(std::ostream &os, const Duration &rhs);
00166 std::ostream &operator <<(std::ostream &os, const WallDuration &rhs);
00167
00168
00169
00170
00171 template<class T>
00172 DurationBase<T>::DurationBase(int32_t _sec, int32_t _nsec)
00173 : sec(_sec), nsec(_nsec)
00174 {
00175 normalizeSecNSecSigned(sec, nsec);
00176 }
00177
00178 template<class T>
00179 T& DurationBase<T>::fromSec(double d)
00180 {
00181 #ifdef HAVE_TRUNC
00182 sec = (int32_t)trunc(d);
00183 #else
00184
00185
00186 if (d >= 0.0)
00187 sec = (int32_t)floor(d);
00188 else
00189 sec = (int32_t)floor(d) + 1;
00190 #endif
00191 nsec = (int32_t)((d - (double)sec)*1000000000);
00192 return *static_cast<T*>(this);
00193 }
00194
00195 template<class T>
00196 T& DurationBase<T>::fromNSec(int64_t t)
00197 {
00198 sec = (int32_t)(t / 1000000000);
00199 nsec = (int32_t)(t % 1000000000);
00200
00201 normalizeSecNSecSigned(sec, nsec);
00202
00203 return *static_cast<T*>(this);
00204 }
00205
00206 template<class T>
00207 T DurationBase<T>::operator+(const T &rhs) const
00208 {
00209 return T(sec + rhs.sec, nsec + rhs.nsec);
00210 }
00211
00212 template<class T>
00213 T DurationBase<T>::operator*(double scale) const
00214 {
00215 return T(toSec() * scale);
00216 }
00217
00218 template<class T>
00219 T DurationBase<T>::operator-(const T &rhs) const
00220 {
00221 return T(sec - rhs.sec, nsec - rhs.nsec);
00222 }
00223
00224 template<class T>
00225 T DurationBase<T>::operator-() const
00226 {
00227 return T(-sec , -nsec);
00228 }
00229
00230 template<class T>
00231 T& DurationBase<T>::operator+=(const T &rhs)
00232 {
00233 *this = *this + rhs;
00234 return *static_cast<T*>(this);
00235 }
00236
00237 template<class T>
00238 T& DurationBase<T>::operator-=(const T &rhs)
00239 {
00240 *this += (-rhs);
00241 return *static_cast<T*>(this);
00242 }
00243
00244 template<class T>
00245 T& DurationBase<T>::operator*=(double scale)
00246 {
00247 fromSec(toSec() * scale);
00248 return *static_cast<T*>(this);
00249 }
00250
00251 template<class T>
00252 bool DurationBase<T>::operator<(const T &rhs) const
00253 {
00254 if (sec < rhs.sec)
00255 return true;
00256 else if (sec == rhs.sec && nsec < rhs.nsec)
00257 return true;
00258 return false;
00259 }
00260
00261 template<class T>
00262 bool DurationBase<T>::operator>(const T &rhs) const
00263 {
00264 if (sec > rhs.sec)
00265 return true;
00266 else if (sec == rhs.sec && nsec > rhs.nsec)
00267 return true;
00268 return false;
00269 }
00270
00271 template<class T>
00272 bool DurationBase<T>::operator<=(const T &rhs) const
00273 {
00274 if (sec < rhs.sec)
00275 return true;
00276 else if (sec == rhs.sec && nsec <= rhs.nsec)
00277 return true;
00278 return false;
00279 }
00280
00281 template<class T>
00282 bool DurationBase<T>::operator>=(const T &rhs) const
00283 {
00284 if (sec > rhs.sec)
00285 return true;
00286 else if (sec == rhs.sec && nsec >= rhs.nsec)
00287 return true;
00288 return false;
00289 }
00290
00291 template<class T>
00292 bool DurationBase<T>::operator==(const T &rhs) const
00293 {
00294 return sec == rhs.sec && nsec == rhs.nsec;
00295 }
00296
00297 template<class T>
00298 bool DurationBase<T>::isZero()
00299 {
00300 return sec == 0 && nsec == 0;
00301 }
00302
00303 }
00304
00305 #endif // ROS_DURATION_H
00306
00307