rate.cpp
Go to the documentation of this file.00001 /********************************************************************* 00002 * 00003 * Software License Agreement (BSD License) 00004 * 00005 * Copyright (c) 2009, Willow Garage, Inc. 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of the Willow Garage nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 * Author: Eitan Marder-Eppstein 00036 *********************************************************************/ 00037 #include <ros/rate.h> 00038 00039 namespace ros 00040 { 00041 00042 Rate::Rate(double frequency) 00043 : start_(Time::now()) 00044 , expected_cycle_time_(1.0 / frequency) 00045 , actual_cycle_time_(0.0) 00046 {} 00047 00048 bool Rate::sleep() 00049 { 00050 Time expected_end = start_ + expected_cycle_time_; 00051 00052 Time actual_end = Time::now(); 00053 00054 // detect backward jumps in time 00055 if (actual_end < start_) 00056 { 00057 expected_end = actual_end + expected_cycle_time_; 00058 } 00059 00060 //calculate the time we'll sleep for 00061 Duration sleep_time = expected_end - actual_end; 00062 00063 //set the actual amount of time the loop took in case the user wants to know 00064 actual_cycle_time_ = actual_end - start_; 00065 00066 //make sure to reset our start time 00067 start_ = expected_end; 00068 00069 //if we've taken too much time we won't sleep 00070 if(sleep_time <= Duration(0.0)) 00071 { 00072 // if we've jumped forward in time, or the loop has taken more than a full extra 00073 // cycle, reset our cycle 00074 if (actual_end > expected_end + expected_cycle_time_) 00075 { 00076 start_ = actual_end; 00077 } 00078 return true; 00079 } 00080 00081 return sleep_time.sleep(); 00082 } 00083 00084 void Rate::reset() 00085 { 00086 start_ = Time::now(); 00087 } 00088 00089 Duration Rate::cycleTime() 00090 { 00091 return actual_cycle_time_; 00092 } 00093 00094 WallRate::WallRate(double frequency) 00095 : start_(WallTime::now()) 00096 , expected_cycle_time_(1.0 / frequency) 00097 , actual_cycle_time_(0.0) 00098 {} 00099 00100 bool WallRate::sleep() 00101 { 00102 WallTime expected_end = start_ + expected_cycle_time_; 00103 00104 WallTime actual_end = WallTime::now(); 00105 00106 // detect backward jumps in time 00107 if (actual_end < start_) 00108 { 00109 expected_end = actual_end + expected_cycle_time_; 00110 } 00111 00112 //calculate the time we'll sleep for 00113 WallDuration sleep_time = expected_end - actual_end; 00114 00115 //set the actual amount of time the loop took in case the user wants to know 00116 actual_cycle_time_ = actual_end - start_; 00117 00118 //make sure to reset our start time 00119 start_ = expected_end; 00120 00121 //if we've taken too much time we won't sleep 00122 if(sleep_time <= WallDuration(0.0)) 00123 { 00124 // if we've jumped forward in time, or the loop has taken more than a full extra 00125 // cycle, reset our cycle 00126 if (actual_end > expected_end + expected_cycle_time_) 00127 { 00128 start_ = actual_end; 00129 } 00130 return true; 00131 } 00132 00133 return sleep_time.sleep(); 00134 } 00135 00136 void WallRate::reset() 00137 { 00138 start_ = WallTime::now(); 00139 } 00140 00141 WallDuration WallRate::cycleTime() 00142 { 00143 return actual_cycle_time_; 00144 } 00145 00146 }