实现周期控制的两种方案

sleep方案

思路：循环中执行任务，执行完任务后，计算该周期剩余的时间，通过sleep休眠该时间

例如，ros中的Rate就是实现的这种方案，文章末尾附上Rate的源码

ros::Rate r(10);//10HZ
while(ros::ok())
{
	// 其他任务
	
    r.sleep(); //使用Rate对象调整休眠时间，考虑循环中其他任务占用的时间，确保让整个循环的频率是 10hz 
}

定时器方案

思路：定义一个定时器，使其到达下一周期时自动触发要执行的任务

Note：
无论哪一种方案，执行任务的时间都必须小于控制周期，否则将毫无意义，因此在写周期控制时，首先要测试任务的执行时间

---

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 #ifndef ROSLIB_RATE_H
00038 #define ROSLIB_RATE_H
00039 
00040 #include "ros/time.h"
00041 
00042 namespace ros
00043 {
00048 class Rate
00049 {
00050 public:
00055   Rate(double frequency);
00056 
00061   bool sleep();
00062 
00066   void reset();
00067 
00072   Duration cycleTime();
00073 
00077   Duration expectedCycleTime() { return expected_cycle_time_; }
00078 
00079 private:
00080   Time start_;
00081   Duration expected_cycle_time_, actual_cycle_time_;
00082 };
00083 
00088 class WallRate
00089 {
00090 public:
00095   WallRate(double frequency);
00096 
00101   bool sleep();
00102 
00106   void reset();
00107 
00112   WallDuration cycleTime();
00113 
00117   WallDuration expectedCycleTime() { return expected_cycle_time_; }
00118 
00119 private:
00120   WallTime start_;
00121   WallDuration expected_cycle_time_, actual_cycle_time_;
00122 };
00123 
00124 }
00125 
00126 #endif

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 }