49 #if defined(__APPLE__) 50 #include <mach/clock.h> 51 #include <mach/mach.h> 52 #endif // defined(__APPLE__) 60 #include <boost/thread/mutex.hpp> 61 #include <boost/io/ios_state.hpp> 62 #include <boost/date_time/posix_time/ptime.hpp> 70 #define HAS_CLOCK_GETTIME (_POSIX_C_SOURCE >= 199309L) 83 const Duration
DURATION_MAX(std::numeric_limits<int32_t>::max(), 999999999);
84 const Duration
DURATION_MIN(std::numeric_limits<int32_t>::min(), 0);
86 const Time
TIME_MAX(std::numeric_limits<uint32_t>::max(), 999999999);
107 #if HAS_CLOCK_GETTIME 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");
113 nsec = start.tv_nsec;
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;
133 static LARGE_INTEGER cpu_freq, init_cpu_time;
134 static uint32_t start_sec = 0;
135 static uint32_t start_nsec = 0;
136 if ( ( start_sec == 0 ) && ( start_nsec == 0 ) )
138 QueryPerformanceFrequency(&cpu_freq);
139 if (cpu_freq.QuadPart == 0) {
142 QueryPerformanceCounter(&init_cpu_time);
145 GetSystemTimeAsFileTime(&ft);
146 LARGE_INTEGER start_li;
147 start_li.LowPart = ft.dwLowDateTime;
148 start_li.HighPart = ft.dwHighDateTime;
152 start_li.QuadPart -= 116444736000000000Ui64;
154 start_li.QuadPart -= 116444736000000000ULL;
156 int64_t start_sec64 = start_li.QuadPart / 10000000;
157 if (start_sec64 < 0 || start_sec64 > std::numeric_limits<uint32_t>::max())
158 throw std::runtime_error(
"SystemTime is out of dual 32-bit range");
159 start_sec = (uint32_t)start_sec64;
160 start_nsec = (start_li.LowPart % 10000000) * 100;
162 LARGE_INTEGER cur_time;
163 QueryPerformanceCounter(&cur_time);
164 LARGE_INTEGER delta_cpu_time;
165 delta_cpu_time.QuadPart = cur_time.QuadPart - init_cpu_time.QuadPart;
168 double d_delta_cpu_time = delta_cpu_time.QuadPart / (double) cpu_freq.QuadPart;
169 uint32_t delta_sec = (uint32_t) floor(d_delta_cpu_time);
170 uint32_t delta_nsec = (uint32_t) boost::math::round((d_delta_cpu_time-delta_sec) * 1e9);
172 int64_t sec_sum = (int64_t)start_sec + (int64_t)delta_sec;
173 int64_t nsec_sum = (int64_t)start_nsec + (int64_t)delta_nsec;
187 #if defined(__APPLE__) 191 host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock);
192 clock_get_time(cclock, &mts);
193 mach_port_deallocate(mach_task_self(), cclock);
194 start.tv_sec = mts.tv_sec;
195 start.tv_nsec = mts.tv_nsec;
196 #else // defined(__APPLE__) 198 clock_gettime(CLOCK_MONOTONIC, &start);
199 #endif // defined(__APPLE__) 201 nsec = start.tv_nsec;
203 static LARGE_INTEGER cpu_frequency, performance_count;
209 QueryPerformanceFrequency(&cpu_frequency);
210 if (cpu_frequency.QuadPart == 0) {
213 QueryPerformanceCounter(&performance_count);
214 double steady_time = performance_count.QuadPart / (double) cpu_frequency.QuadPart;
215 int64_t steady_sec = floor(steady_time);
216 int64_t steady_nsec = boost::math::round((steady_time - steady_sec) * 1e9);
237 std::this_thread::sleep_for(std::chrono::nanoseconds(static_cast<int64_t>(sec * 1e9 + nsec)));
240 timespec req = { sec, nsec };
241 return nanosleep(&req, NULL);
255 timespec req = { sec, nsec };
256 timespec rem = {0, 0};
257 while (nanosleep(&req, &rem) && !g_stopped)
293 boost::mutex::scoped_lock lock(g_sim_time_mutex);
306 boost::mutex::scoped_lock lock(g_sim_time_mutex);
337 while (!
isValid() && !g_stopped)
357 boost::posix_time::time_duration diff = t - boost::posix_time::from_time_t(0);
364 int64_t sec64 = d.total_seconds();
365 if (sec64 < 0 || sec64 > std::numeric_limits<uint32_t>::max())
366 throw std::runtime_error(
"time_duration is out of dual 32-bit range");
367 t.
sec = (uint32_t)sec64;
368 #if defined(BOOST_DATE_TIME_HAS_NANOSECONDS) 369 t.
nsec = d.fractional_seconds();
371 t.
nsec = d.fractional_seconds()*1000;
378 boost::io::ios_all_saver s(os);
379 os << rhs.
sec <<
"." << std::setw(9) << std::setfill(
'0') << rhs.
nsec;
385 boost::io::ios_all_saver s(os);
386 if (rhs.
sec >= 0 || rhs.
nsec == 0)
388 os << rhs.
sec <<
"." << std::setw(9) << std::setfill(
'0') << rhs.
nsec;
392 os << (rhs.
sec == -1 ?
"-" :
"") << (rhs.
sec + 1) <<
"." << std::setw(9) << std::setfill(
'0') << (1000000000 - rhs.
nsec);
412 while (!g_stopped && (
Time::now() < end))
456 Time end = start + *
this;
463 while (!g_stopped && (
Time::now() < end))
489 boost::io::ios_all_saver s(os);
490 os << rhs.
sec <<
"." << std::setw(9) << std::setfill(
'0') << rhs.
nsec;
496 boost::io::ios_all_saver s(os);
497 os << rhs.
sec <<
"." << std::setw(9) << std::setfill(
'0') << rhs.
nsec;
519 boost::io::ios_all_saver s(os);
520 if (rhs.
sec >= 0 || rhs.
nsec == 0)
522 os << rhs.
sec <<
"." << std::setw(9) << std::setfill(
'0') << rhs.
nsec;
526 os << (rhs.
sec == -1 ?
"-" :
"") << (rhs.
sec + 1) <<
"." << std::setw(9) << std::setfill(
'0') << (1000000000 - rhs.
nsec);
538 uint64_t nsec_part = nsec % 1000000000UL;
539 uint64_t sec_part = nsec / 1000000000UL;
541 if (sec + sec_part > std::numeric_limits<uint32_t>::max())
542 throw std::runtime_error(
"Time is out of dual 32-bit range");
550 uint64_t sec64 =
sec;
551 uint64_t nsec64 =
nsec;
555 sec = (uint32_t)sec64;
556 nsec = (uint32_t)nsec64;
561 int64_t nsec_part = nsec % 1000000000L;
562 int64_t sec_part = sec + nsec / 1000000000L;
565 nsec_part += 1000000000L;
569 if (sec_part < 0 || sec_part > std::numeric_limits<uint32_t>::max())
570 throw std::runtime_error(
"Time is out of dual 32-bit range");
bool ros_wallsleep(uint32_t sec, uint32_t nsec)
Go to the wall!
static Time g_sim_time(0, 0)
ROSTIME_DECL void ros_steadytime(uint32_t &sec, uint32_t &nsec)
Time representation. May either represent wall clock time or ROS clock time.
ROSTIME_DECL void normalizeSecNSecUnsigned(int64_t &sec, int64_t &nsec)
static bool sleepUntil(const Time &end)
Sleep until a specific time has been reached.
bool sleep() const
sleep for the amount of time specified by this Duration. If a signal interrupts the sleep...
Duration representation for use with the WallTime class.
Thrown if the ros subsystem hasn't been initialised before use.
ROSTIME_DECL std::ostream & operator<<(std::ostream &os, const Duration &rhs)
static bool useSystemTime()
static bool sleepUntil(const WallTime &end)
Sleep until a specific time has been reached.
ROSTIME_DECL void ros_walltime(uint32_t &sec, uint32_t &nsec)
static void setNow(const Time &new_now)
static bool g_stopped(false)
static bool isSystemTime()
static bool g_initialized(false)
Time representation. Always wall-clock time.
bool sleep() const
sleep for the amount of time specified by this Duration. If a signal interrupts the sleep...
static bool sleepUntil(const SteadyTime &end)
Sleep until a specific time has been reached.
static SteadyTime now()
Returns the current steady (monotonic) clock time.
static bool isValid()
Returns whether or not the current time is valid. Time is valid if it is non-zero.
ROSTIME_DECL const Time TIME_MAX
static boost::mutex g_sim_time_mutex
int ros_nanosleep(const uint32_t &sec, const uint32_t &nsec)
Simple representation of the rt library nanosleep function.
Time representation. Always steady-clock time.
ROSTIME_DECL const Time TIME_MIN
ROSTIME_DECL void normalizeSecNSec(uint64_t &sec, uint64_t &nsec)
Duration representation for use with the Time class.
static WallTime now()
Returns the current wall clock time.
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...
static Time fromBoost(const boost::posix_time::ptime &t)
ROSTIME_DECL const Duration DURATION_MIN
static bool waitForValid()
Wait for time to become valid.
ROSTIME_DECL const Duration DURATION_MAX