time.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
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  ** Preprocessor
00054  *********************************************************************/
00055 
00056 // Could probably do some better and more elaborate checking
00057 // and definition here.
00058 #define HAS_CLOCK_GETTIME (_POSIX_C_SOURCE >= 199309L)
00059 
00060 /*********************************************************************
00061  ** Namespaces
00062  *********************************************************************/
00063 
00064 namespace ros
00065 {
00066 
00067   /*********************************************************************
00068    ** Variables
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   // This is declared here because it's set from the Time class but read from
00078   // the Duration class, and need not be exported to users of either.
00079   static bool g_stopped(false);
00080 
00081   // I assume that this is declared here, instead of time.h, to keep users
00082   // of time.h from including boost/thread/mutex.hpp
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    ** Cross Platform Functions
00091    *********************************************************************/
00092   /*
00093    * These have only internal linkage to this translation unit.
00094    * (i.e. not exposed to users of the time classes)
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     // Win32 implementation
00115     // unless I've missed something obvious, the only way to get high-precision
00116     // time on Windows is via the QueryPerformanceCounter() call. However,
00117     // this is somewhat problematic in Windows XP on some processors, especially
00118     // AMD, because the Windows implementation can freak out when the CPU clocks
00119     // down to save power. Time can jump or even go backwards. Microsoft has
00120     // fixed this bug for most systems now, but it can still show up if you have
00121     // not installed the latest CPU drivers (an oxymoron). They fixed all these
00122     // problems in Windows Vista, and this API is by far the most accurate that
00123     // I know of in Windows, so I'll use it here despite all these caveats
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         // compute an offset from the Epoch using the lower-performance timer API
00135         FILETIME ft;
00136         GetSystemTimeAsFileTime(&ft);
00137         LARGE_INTEGER start_li;
00138         start_li.LowPart = ft.dwLowDateTime;
00139         start_li.HighPart = ft.dwHighDateTime;
00140         // why did they choose 1601 as the time zero, instead of 1970?
00141         // there were no outstanding hard rock bands in 1601.
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); // 100-ns units. odd.
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     // todo: how to handle cpu clock drift. not sure it's a big deal for us.
00155     // also, think about clock wraparound. seems extremely unlikey, but possible
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     // Throws an exception if we go out of 32-bit range
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    ** Class Methods
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             // If we started at time 0 wait for the first actual time to arrive before starting the timer on
00412             // our sleep
00413             if (start.isZero())
00414               {
00415                 start = Time::now();
00416                 end = start + *this;
00417               }
00418 
00419             // If time jumped backwards from when we started sleeping, return immediately
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 


rostime
Author(s): Josh Faust
autogenerated on Mon Oct 6 2014 07:00:39