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 Rate::Rate(const Duration& d)
00049   : start_(Time::now())
00050   , expected_cycle_time_(1.0 / d.toSec())
00051   , actual_cycle_time_(0.0)
00052 { }
00053 
00054 
00055 
00056 bool Rate::sleep()
00057 {
00058   Time expected_end = start_ + expected_cycle_time_;
00059 
00060   Time actual_end = Time::now();
00061 
00062   // detect backward jumps in time
00063   if (actual_end < start_)
00064   {
00065     expected_end = actual_end + expected_cycle_time_;
00066   }
00067 
00068   //calculate the time we'll sleep for
00069   Duration sleep_time = expected_end - actual_end;
00070 
00071   //set the actual amount of time the loop took in case the user wants to know
00072   actual_cycle_time_ = actual_end - start_;
00073 
00074   //make sure to reset our start time
00075   start_ = expected_end;
00076 
00077   //if we've taken too much time we won't sleep
00078   if(sleep_time <= Duration(0.0))
00079   {
00080     // if we've jumped forward in time, or the loop has taken more than a full extra
00081     // cycle, reset our cycle
00082     if (actual_end > expected_end + expected_cycle_time_)
00083     {
00084       start_ = actual_end;
00085     }
00086     return true;
00087   }
00088 
00089   return sleep_time.sleep();
00090 }
00091 
00092 void Rate::reset()
00093 {
00094   start_ = Time::now();
00095 }
00096 
00097 Duration Rate::cycleTime() const
00098 {
00099   return actual_cycle_time_;
00100 }
00101 
00102 WallRate::WallRate(double frequency)
00103 : start_(WallTime::now())
00104 , expected_cycle_time_(1.0 / frequency)
00105 , actual_cycle_time_(0.0)
00106 {}
00107 
00108 WallRate::WallRate(const Duration& d)
00109 : start_(WallTime::now())
00110 , expected_cycle_time_(1.0 / d.toSec())
00111 , actual_cycle_time_(0.0)
00112 {}
00113 
00114 bool WallRate::sleep()
00115 {
00116   WallTime expected_end = start_ + expected_cycle_time_;
00117 
00118   WallTime actual_end = WallTime::now();
00119 
00120   // detect backward jumps in time
00121   if (actual_end < start_)
00122   {
00123     expected_end = actual_end + expected_cycle_time_;
00124   }
00125 
00126   //calculate the time we'll sleep for
00127   WallDuration sleep_time = expected_end - actual_end;
00128 
00129   //set the actual amount of time the loop took in case the user wants to know
00130   actual_cycle_time_ = actual_end - start_;
00131 
00132   //make sure to reset our start time
00133   start_ = expected_end;
00134 
00135   //if we've taken too much time we won't sleep
00136   if(sleep_time <= WallDuration(0.0))
00137   {
00138     // if we've jumped forward in time, or the loop has taken more than a full extra
00139     // cycle, reset our cycle
00140     if (actual_end > expected_end + expected_cycle_time_)
00141     {
00142       start_ = actual_end;
00143     }
00144     return true;
00145   }
00146 
00147   return sleep_time.sleep();
00148 }
00149 
00150 void WallRate::reset()
00151 {
00152   start_ = WallTime::now();
00153 }
00154 
00155 WallDuration WallRate::cycleTime() const
00156 {
00157   return actual_cycle_time_;
00158 }
00159 
00160 
00161 }


rostime
Author(s): Josh Faust
autogenerated on Fri Jan 3 2014 11:49:59