Go to the documentation of this file.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 #ifdef _MSC_VER
00035 #ifndef NOMINMAX
00036 #define NOMINMAX
00037 #endif
00038 #endif
00039
00040 #include "ros/time.h"
00041 #include "ros/impl/time.h"
00042 #include <cmath>
00043 #include <ctime>
00044 #include <iomanip>
00045 #include <stdexcept>
00046 #include <limits>
00047
00048 #include <boost/thread/mutex.hpp>
00049 #include <boost/io/ios_state.hpp>
00050 #include <boost/date_time/posix_time/ptime.hpp>
00051
00052
00053
00054
00055
00056
00057
00058 #define HAS_CLOCK_GETTIME (_POSIX_C_SOURCE >= 199309L)
00059
00060
00061
00062
00063
00064 namespace ros
00065 {
00066
00067
00068
00069
00070
00071 const Duration DURATION_MAX(std::numeric_limits<int32_t>::max(), 999999999);
00072 const Duration DURATION_MIN(std::numeric_limits<int32_t>::min(), 0);
00073
00074 const Time TIME_MAX(std::numeric_limits<uint32_t>::max(), 999999999);
00075 const Time TIME_MIN(0, 1);
00076
00077
00078
00079 static bool g_stopped(false);
00080
00081
00082
00083 static boost::mutex g_sim_time_mutex;
00084
00085 static bool g_initialized(false);
00086 static bool g_use_sim_time(true);
00087 static Time g_sim_time(0, 0);
00088
00089
00090
00091
00092
00093
00094
00095
00096 void ros_walltime(uint32_t& sec, uint32_t& nsec)
00097 #ifndef WIN32
00098 throw(NoHighPerformanceTimersException)
00099 #endif
00100 {
00101 #ifndef WIN32
00102 #if HAS_CLOCK_GETTIME
00103 timespec start;
00104 clock_gettime(CLOCK_REALTIME, &start);
00105 sec = start.tv_sec;
00106 nsec = start.tv_nsec;
00107 #else
00108 struct timeval timeofday;
00109 gettimeofday(&timeofday,NULL);
00110 sec = timeofday.tv_sec;
00111 nsec = timeofday.tv_usec * 1000;
00112 #endif
00113 #else
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124 static LARGE_INTEGER cpu_freq, init_cpu_time;
00125 static uint32_t start_sec = 0;
00126 static uint32_t start_nsec = 0;
00127 if ( ( start_sec == 0 ) && ( start_nsec == 0 ) )
00128 {
00129 QueryPerformanceFrequency(&cpu_freq);
00130 if (cpu_freq.QuadPart == 0) {
00131 throw NoHighPerformanceTimersException();
00132 }
00133 QueryPerformanceCounter(&init_cpu_time);
00134
00135 FILETIME ft;
00136 GetSystemTimeAsFileTime(&ft);
00137 LARGE_INTEGER start_li;
00138 start_li.LowPart = ft.dwLowDateTime;
00139 start_li.HighPart = ft.dwHighDateTime;
00140
00141
00142 #ifdef _MSC_VER
00143 start_li.QuadPart -= 116444736000000000Ui64;
00144 #else
00145 start_li.QuadPart -= 116444736000000000ULL;
00146 #endif
00147 start_sec = (uint32_t)(start_li.QuadPart / 10000000);
00148 start_nsec = (start_li.LowPart % 10000000) * 100;
00149 }
00150 LARGE_INTEGER cur_time;
00151 QueryPerformanceCounter(&cur_time);
00152 LARGE_INTEGER delta_cpu_time;
00153 delta_cpu_time.QuadPart = cur_time.QuadPart - init_cpu_time.QuadPart;
00154
00155
00156 double d_delta_cpu_time = delta_cpu_time.QuadPart / (double) cpu_freq.QuadPart;
00157 uint32_t delta_sec = (uint32_t) floor(d_delta_cpu_time);
00158 uint32_t delta_nsec = (uint32_t) boost::math::round((d_delta_cpu_time-delta_sec) * 1e9);
00159
00160 int64_t sec_sum = (int64_t)start_sec + (int64_t)delta_sec;
00161 int64_t nsec_sum = (int64_t)start_nsec + (int64_t)delta_nsec;
00162
00163
00164 normalizeSecNSecUnsigned(sec_sum, nsec_sum);
00165
00166 sec = sec_sum;
00167 nsec = nsec_sum;
00168 #endif
00169 }
00173 int ros_nanosleep(const uint32_t &sec, const uint32_t &nsec)
00174 {
00175 #if defined(WIN32)
00176 HANDLE timer = NULL;
00177 LARGE_INTEGER sleepTime;
00178 sleepTime.QuadPart = -
00179 static_cast<int64_t>(sec)*10000000LL -
00180 static_cast<int64_t>(nsec) / 100LL;
00181
00182 timer = CreateWaitableTimer(NULL, TRUE, NULL);
00183 if (timer == NULL)
00184 {
00185 return -1;
00186 }
00187
00188 if (!SetWaitableTimer (timer, &sleepTime, 0, NULL, NULL, 0))
00189 {
00190 return -1;
00191 }
00192
00193 if (WaitForSingleObject (timer, INFINITE) != WAIT_OBJECT_0)
00194 {
00195 return -1;
00196 }
00197 return 0;
00198 #else
00199 timespec req = { sec, nsec };
00200 return nanosleep(&req, NULL);
00201 #endif
00202 }
00203
00209 bool ros_wallsleep(uint32_t sec, uint32_t nsec)
00210 {
00211 #if defined(WIN32)
00212 ros_nanosleep(sec,nsec);
00213 #else
00214 timespec req = { sec, nsec };
00215 timespec rem = {0, 0};
00216 while (nanosleep(&req, &rem) && !g_stopped)
00217 {
00218 req = rem;
00219 }
00220 #endif
00221 return !g_stopped;
00222 }
00223
00224
00225
00226
00227
00228 bool Time::useSystemTime()
00229 {
00230 return !g_use_sim_time;
00231 }
00232
00233 bool Time::isSimTime()
00234 {
00235 return g_use_sim_time;
00236 }
00237
00238 bool Time::isSystemTime()
00239 {
00240 return !isSimTime();
00241 }
00242
00243 Time Time::now()
00244 {
00245 if (!g_initialized)
00246 {
00247 throw TimeNotInitializedException();
00248 }
00249
00250 if (g_use_sim_time)
00251 {
00252 boost::mutex::scoped_lock lock(g_sim_time_mutex);
00253 Time t = g_sim_time;
00254 return t;
00255 }
00256
00257 Time t;
00258 ros_walltime(t.sec, t.nsec);
00259
00260 return t;
00261 }
00262
00263 void Time::setNow(const Time& new_now)
00264 {
00265 boost::mutex::scoped_lock lock(g_sim_time_mutex);
00266
00267 g_sim_time = new_now;
00268 g_use_sim_time = true;
00269 }
00270
00271 void Time::init()
00272 {
00273 g_stopped = false;
00274 g_use_sim_time = false;
00275 g_initialized = true;
00276 }
00277
00278 void Time::shutdown()
00279 {
00280 g_stopped = true;
00281 }
00282
00283 bool Time::isValid()
00284 {
00285 return (!g_use_sim_time) || !g_sim_time.isZero();
00286 }
00287
00288 bool Time::waitForValid()
00289 {
00290 return waitForValid(ros::WallDuration());
00291 }
00292
00293 bool Time::waitForValid(const ros::WallDuration& timeout)
00294 {
00295 ros::WallTime start = ros::WallTime::now();
00296 while (!isValid() && !g_stopped)
00297 {
00298 ros::WallDuration(0.01).sleep();
00299
00300 if (timeout > ros::WallDuration(0, 0) && (ros::WallTime::now() - start > timeout))
00301 {
00302 return false;
00303 }
00304 }
00305
00306 if (g_stopped)
00307 {
00308 return false;
00309 }
00310
00311 return true;
00312 }
00313
00314 Time Time::fromBoost(const boost::posix_time::ptime& t)
00315 {
00316 boost::posix_time::time_duration diff = t - boost::posix_time::from_time_t(0);
00317 return Time::fromBoost(diff);
00318 }
00319
00320 Time Time::fromBoost(const boost::posix_time::time_duration& d)
00321 {
00322 Time t;
00323 t.sec = d.total_seconds();
00324 #if defined(BOOST_DATE_TIME_HAS_NANOSECONDS)
00325 t.nsec = d.fractional_seconds();
00326 #else
00327 t.nsec = d.fractional_seconds()*1000;
00328 #endif
00329 return t;
00330 }
00331
00332 std::ostream& operator<<(std::ostream& os, const Time &rhs)
00333 {
00334 boost::io::ios_all_saver s(os);
00335 os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
00336 return os;
00337 }
00338
00339 std::ostream& operator<<(std::ostream& os, const Duration& rhs)
00340 {
00341 boost::io::ios_all_saver s(os);
00342 if (rhs.sec >= 0 || rhs.nsec == 0)
00343 {
00344 os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
00345 }
00346 else
00347 {
00348 os << (rhs.sec == -1 ? "-" : "") << (rhs.sec + 1) << "." << std::setw(9) << std::setfill('0') << (1000000000 - rhs.nsec);
00349 }
00350 return os;
00351 }
00352
00353 bool Time::sleepUntil(const Time& end)
00354 {
00355 if (Time::useSystemTime())
00356 {
00357 Duration d(end - Time::now());
00358 if (d > Duration(0))
00359 {
00360 return d.sleep();
00361 }
00362
00363 return true;
00364 }
00365 else
00366 {
00367 Time start = Time::now();
00368 while (!g_stopped && (Time::now() < end))
00369 {
00370 ros_nanosleep(0,1000000);
00371 if (Time::now() < start)
00372 {
00373 return false;
00374 }
00375 }
00376
00377 return true;
00378 }
00379 }
00380
00381 bool WallTime::sleepUntil(const WallTime& end)
00382 {
00383 WallDuration d(end - WallTime::now());
00384 if (d > WallDuration(0))
00385 {
00386 return d.sleep();
00387 }
00388
00389 return true;
00390 }
00391
00392 bool Duration::sleep() const
00393 {
00394 if (Time::useSystemTime())
00395 {
00396 return ros_wallsleep(sec, nsec);
00397 }
00398 else
00399 {
00400 Time start = Time::now();
00401 Time end = start + *this;
00402 if (start.isZero())
00403 {
00404 end = TIME_MAX;
00405 }
00406
00407 while (!g_stopped && (Time::now() < end))
00408 {
00409 ros_wallsleep(0, 1000000);
00410
00411
00412
00413 if (start.isZero())
00414 {
00415 start = Time::now();
00416 end = start + *this;
00417 }
00418
00419
00420 if (Time::now() < start)
00421 {
00422 return false;
00423 }
00424 }
00425
00426 return true;
00427 }
00428 }
00429
00430 std::ostream &operator<<(std::ostream& os, const WallTime &rhs)
00431 {
00432 boost::io::ios_all_saver s(os);
00433 os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
00434 return os;
00435 }
00436
00437 WallTime WallTime::now()
00438 {
00439 WallTime t;
00440 ros_walltime(t.sec, t.nsec);
00441
00442 return t;
00443 }
00444
00445 std::ostream &operator<<(std::ostream& os, const WallDuration& rhs)
00446 {
00447 boost::io::ios_all_saver s(os);
00448 if (rhs.sec >= 0 || rhs.nsec == 0)
00449 {
00450 os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
00451 }
00452 else
00453 {
00454 os << (rhs.sec == -1 ? "-" : "") << (rhs.sec + 1) << "." << std::setw(9) << std::setfill('0') << (1000000000 - rhs.nsec);
00455 }
00456 return os;
00457 }
00458
00459 bool WallDuration::sleep() const
00460 {
00461 return ros_wallsleep(sec, nsec);
00462 }
00463
00464 void normalizeSecNSec(uint64_t& sec, uint64_t& nsec)
00465 {
00466 uint64_t nsec_part = nsec % 1000000000UL;
00467 uint64_t sec_part = nsec / 1000000000UL;
00468
00469 if (sec + sec_part > UINT_MAX)
00470 throw std::runtime_error("Time is out of dual 32-bit range");
00471
00472 sec += sec_part;
00473 nsec = nsec_part;
00474 }
00475
00476 void normalizeSecNSec(uint32_t& sec, uint32_t& nsec)
00477 {
00478 uint64_t sec64 = sec;
00479 uint64_t nsec64 = nsec;
00480
00481 normalizeSecNSec(sec64, nsec64);
00482
00483 sec = (uint32_t)sec64;
00484 nsec = (uint32_t)nsec64;
00485 }
00486
00487 void normalizeSecNSecUnsigned(int64_t& sec, int64_t& nsec)
00488 {
00489 int64_t nsec_part = nsec % 1000000000L;
00490 int64_t sec_part = sec + nsec / 1000000000L;
00491 if (nsec_part < 0)
00492 {
00493 nsec_part += 1000000000L;
00494 --sec_part;
00495 }
00496
00497 if (sec_part < 0 || sec_part > UINT_MAX)
00498 throw std::runtime_error("Time is out of dual 32-bit range");
00499
00500 sec = sec_part;
00501 nsec = nsec_part;
00502 }
00503
00504 template class TimeBase<Time, Duration>;
00505 template class TimeBase<WallTime, WallDuration>;
00506 }
00507
00508