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_(d.sec, d.nsec)
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 false to show that the desired rate was not met
00087     return false;
00088   }
00089 
00090   return sleep_time.sleep();
00091 }
00092 
00093 void Rate::reset()
00094 {
00095   start_ = Time::now();
00096 }
00097 
00098 Duration Rate::cycleTime() const
00099 {
00100   return actual_cycle_time_;
00101 }
00102 
00103 WallRate::WallRate(double frequency)
00104 : start_(WallTime::now())
00105 , expected_cycle_time_(1.0 / frequency)
00106 , actual_cycle_time_(0.0)
00107 {}
00108 
00109 WallRate::WallRate(const Duration& d)
00110 : start_(WallTime::now())
00111 , expected_cycle_time_(d.sec, d.nsec)
00112 , actual_cycle_time_(0.0)
00113 {}
00114 
00115 bool WallRate::sleep()
00116 {
00117   WallTime expected_end = start_ + expected_cycle_time_;
00118 
00119   WallTime actual_end = WallTime::now();
00120 
00121   // detect backward jumps in time
00122   if (actual_end < start_)
00123   {
00124     expected_end = actual_end + expected_cycle_time_;
00125   }
00126 
00127   //calculate the time we'll sleep for
00128   WallDuration sleep_time = expected_end - actual_end;
00129 
00130   //set the actual amount of time the loop took in case the user wants to know
00131   actual_cycle_time_ = actual_end - start_;
00132 
00133   //make sure to reset our start time
00134   start_ = expected_end;
00135 
00136   //if we've taken too much time we won't sleep
00137   if(sleep_time <= WallDuration(0.0))
00138   {
00139     // if we've jumped forward in time, or the loop has taken more than a full extra
00140     // cycle, reset our cycle
00141     if (actual_end > expected_end + expected_cycle_time_)
00142     {
00143       start_ = actual_end;
00144     }
00145     return true;
00146   }
00147 
00148   return sleep_time.sleep();
00149 }
00150 
00151 void WallRate::reset()
00152 {
00153   start_ = WallTime::now();
00154 }
00155 
00156 WallDuration WallRate::cycleTime() const
00157 {
00158   return actual_cycle_time_;
00159 }
00160 
00161 
00162 }


rostime
Author(s): Josh Faust
autogenerated on Wed Mar 8 2017 03:55:05