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