realtime_clock.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013 hiDOF, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031  * Publishing ROS messages is difficult, as the publish function is
00032  * not realtime safe.  This class provides the proper locking so that
00033  * you can call publish in realtime and a separate (non-realtime)
00034  * thread will ensure that the message gets published over ROS.
00035  *
00036  * Author: Wim Meeussen
00037  */
00038 
00039 #include <realtime_tools/realtime_clock.h>
00040 
00041 namespace realtime_tools
00042 {
00043 
00044   
00045   RealtimeClock::RealtimeClock()
00046     :lock_misses_(0), 
00047      system_time_(ros::Time()), 
00048      last_realtime_time_(ros::Time()),
00049      running_(true),
00050      initialized_(false)
00051   {
00052     // thread for time loop
00053     thread_ = boost::thread(boost::bind(&RealtimeClock::loop, this));
00054   }
00055   
00056 
00057   RealtimeClock::~RealtimeClock()
00058   {
00059     running_ = false;
00060     thread_.join();
00061   }
00062 
00063 
00064 
00065   ros::Time RealtimeClock::getSystemTime(const ros::Time& realtime_time)
00066   {
00067     if (mutex_.try_lock())
00068     {
00069       // update time offset when we have a new system time measurement in the last cycle
00070       if (lock_misses_ == 0 && system_time_ != ros::Time())
00071       {
00072         // get additional offset caused by period of realtime loop
00073         ros::Duration period_offset;
00074         if (last_realtime_time_ != ros::Time())
00075           period_offset = ros::Duration((realtime_time - last_realtime_time_).toSec()/2.0);
00076 
00077         if (!initialized_)
00078         {
00079           clock_offset_ = system_time_ + period_offset - realtime_time;
00080           initialized_ = true;
00081         }
00082         else
00083           clock_offset_ = clock_offset_*0.9999 + (system_time_ + period_offset - realtime_time)*0.0001;
00084       }
00085       system_time_ = ros::Time();
00086       lock_misses_ = 0;
00087       mutex_.unlock();
00088     }
00089         
00090     else
00091       lock_misses_++;
00092 
00093     last_realtime_time_ = realtime_time;
00094 
00095     // return time
00096     return realtime_time + clock_offset_;
00097   }
00098 
00099 
00100 
00101   void RealtimeClock::loop()
00102   {
00103     ros::Rate r(750);
00104     while (running_)
00105     {
00106       // get lock
00107       lock();
00108 
00109       // store system time
00110       system_time_ = ros::Time::now();
00111       
00112       // warning, using non-locked 'lock_misses_', but it's just for debugging
00113       if (lock_misses_ > 100)
00114         ROS_WARN_THROTTLE(1.0, "Time estimator has trouble transferring data between non-RT and RT");
00115 
00116       // release lock
00117       mutex_.unlock();
00118       r.sleep();
00119     }
00120   }
00121 
00122 
00123   void RealtimeClock::lock()
00124   {
00125 #ifdef NON_POLLING
00126     mutex_.lock();
00127 #else
00128     while (!mutex_.try_lock())
00129       usleep(500);
00130 #endif
00131   }
00132 
00133 }// namespace
00134 


realtime_tools
Author(s): Stuart Glaser
autogenerated on Thu Aug 27 2015 14:38:05