urg_time.c
Go to the documentation of this file.
00001 
00009 #include "urg_c/urg_time.h"
00010 
00011 // Portable time function borrowed from ros::Time
00012 void urg_walltime(unsigned long *sec, unsigned long *nsec) 
00013   {
00014 #ifndef WIN32
00015 #if HAS_CLOCK_GETTIME
00016     struct  timespec start;
00017     clock_gettime(CLOCK_REALTIME, &start);
00018     *sec  = start.tv_sec;
00019     *nsec = start.tv_nsec;
00020 #else
00021     struct timeval timeofday;
00022     gettimeofday(&timeofday,NULL);
00023     sec  = timeofday.tv_sec;
00024     nsec = timeofday.tv_usec * 1000;
00025 #endif
00026 #else
00027     // Win32 implementation
00028     // unless I've missed something obvious, the only way to get high-precision
00029     // time on Windows is via the QueryPerformanceCounter() call. However,
00030     // this is somewhat problematic in Windows XP on some processors, especially
00031     // AMD, because the Windows implementation can freak out when the CPU clocks
00032     // down to save power. Time can jump or even go backwards. Microsoft has
00033     // fixed this bug for most systems now, but it can still show up if you have
00034     // not installed the latest CPU drivers (an oxymoron). They fixed all these
00035     // problems in Windows Vista, and this API is by far the most accurate that
00036     // I know of in Windows, so I'll use it here despite all these caveats
00037     static LARGE_INTEGER cpu_freq, init_cpu_time;
00038     static uint32_t start_sec = 0;
00039     static uint32_t start_nsec = 0;
00040     if ( ( start_sec == 0 ) && ( start_nsec == 0 ) )
00041       {
00042         QueryPerformanceFrequency(&cpu_freq);
00043         if (cpu_freq.QuadPart == 0) {
00044           throw NoHighPerformanceTimersException();
00045         }
00046         QueryPerformanceCounter(&init_cpu_time);
00047         // compute an offset from the Epoch using the lower-performance timer API
00048         FILETIME ft;
00049         GetSystemTimeAsFileTime(&ft);
00050         LARGE_INTEGER start_li;
00051         start_li.LowPart = ft.dwLowDateTime;
00052         start_li.HighPart = ft.dwHighDateTime;
00053         // why did they choose 1601 as the time zero, instead of 1970?
00054         // there were no outstanding hard rock bands in 1601.
00055 #ifdef _MSC_VER
00056         start_li.QuadPart -= 116444736000000000Ui64;
00057 #else
00058         start_li.QuadPart -= 116444736000000000ULL;
00059 #endif
00060         start_sec = (uint32_t)(start_li.QuadPart / 10000000); // 100-ns units. odd.
00061         start_nsec = (start_li.LowPart % 10000000) * 100;
00062       }
00063     LARGE_INTEGER cur_time;
00064     QueryPerformanceCounter(&cur_time);
00065     LARGE_INTEGER delta_cpu_time;
00066     delta_cpu_time.QuadPart = cur_time.QuadPart - init_cpu_time.QuadPart;
00067     // todo: how to handle cpu clock drift. not sure it's a big deal for us.
00068     // also, think about clock wraparound. seems extremely unlikey, but possible
00069     double d_delta_cpu_time = delta_cpu_time.QuadPart / (double) cpu_freq.QuadPart;
00070     uint32_t delta_sec = (uint32_t) floor(d_delta_cpu_time);
00071     uint32_t delta_nsec = (uint32_t) boost::math::round((d_delta_cpu_time-delta_sec) * 1e9);
00072 
00073     int64_t sec_sum  = (int64_t)start_sec  + (int64_t)delta_sec;
00074     int64_t nsec_sum = (int64_t)start_nsec + (int64_t)delta_nsec;
00075 
00076     // Throws an exception if we go out of 32-bit range
00077     normalizeSecNSecUnsigned(sec_sum, nsec_sum);
00078 
00079     sec = sec_sum;
00080     nsec = nsec_sum;
00081 #endif
00082   }
00083 
00084 void urg_get_walltime(unsigned long long *nsecs){
00085     unsigned long sec;
00086     unsigned long nsec;
00087     urg_walltime(&sec, &nsec);
00088     *nsecs = (unsigned long long)sec*1000000000ll + (unsigned long long)nsec;
00089 }


urg_c
Author(s): Satofumi Kamimura , Katsumi Kimoto, Adrian Boeing
autogenerated on Wed Aug 26 2015 16:38:27