实现周期控制的两种方案

sleep方案

思路&＃xff1a;循环中执行任务&＃xff0c;执行完任务后&＃xff0c;计算该周期剩余的时间&＃xff0c;通过sleep休眠该时间

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

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


定时器方案

思路&＃xff1a;定义一个定时器&＃xff0c;使其到达下一周期时自动触发要执行的任务

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

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 &＃61; start_ &＃43; expected_cycle_time_;
00051
00052 Time actual_end &＃61; Time::now();
00053
00054 // detect backward jumps in time
00055 if (actual_end < start_)
00056 {
00057 expected_end &＃61; actual_end &＃43; expected_cycle_time_;
00058 }
00059
00060 //calculate the time we&＃39;ll sleep for
00061 Duration sleep_time &＃61; 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_ &＃61; actual_end - start_;
00065
00066 //make sure to reset our start time
00067 start_ &＃61; expected_end;
00068
00069 //if we&＃39;ve taken too much time we won&＃39;t sleep
00070 if(sleep_time <&＃61; Duration(0.0))
00071 {
00072 // if we&＃39;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 &＃43; expected_cycle_time_)
00075 {
00076 start_ &＃61; actual_end;
00077 }
00078 return true;
00079 }
00080
00081 return sleep_time.sleep();
00082 }
00083
00084 void Rate::reset()
00085 {
00086 start_ &＃61; 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 &＃61; start_ &＃43; expected_cycle_time_;
00103
00104 WallTime actual_end &＃61; WallTime::now();
00105
00106 // detect backward jumps in time
00107 if (actual_end < start_)
00108 {
00109 expected_end &＃61; actual_end &＃43; expected_cycle_time_;
00110 }
00111
00112 //calculate the time we&＃39;ll sleep for
00113 WallDuration sleep_time &＃61; 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_ &＃61; actual_end - start_;
00117
00118 //make sure to reset our start time
00119 start_ &＃61; expected_end;
00120
00121 //if we&＃39;ve taken too much time we won&＃39;t sleep
00122 if(sleep_time <&＃61; WallDuration(0.0))
00123 {
00124 // if we&＃39;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 &＃43; expected_cycle_time_)
00127 {
00128 start_ &＃61; actual_end;
00129 }
00130 return true;
00131 }
00132
00133 return sleep_time.sleep();
00134 }
00135
00136 void WallRate::reset()
00137 {
00138 start_ &＃61; WallTime::now();
00139 }
00140
00141 WallDuration WallRate::cycleTime()
00142 {
00143 return actual_cycle_time_;
00144 }
00145
00146 }