00001 /****************************************************************************** 00002 * * 00003 * pcdfilter_pa_ros_throttle.cpp * 00004 * ============================= * 00005 * * 00006 ******************************************************************************* 00007 * * 00008 * github repository * 00009 * https://github.com/peterweissig/ros_pcdfilter * 00010 * * 00011 * Chair of Automation Technology, Technische Universität Chemnitz * 00012 * https://www.tu-chemnitz.de/etit/proaut * 00013 * * 00014 ******************************************************************************* 00015 * * 00016 * New BSD License * 00017 * * 00018 * Copyright (c) 2015-2017, Peter Weissig, Technische Universität Chemnitz * 00019 * All rights reserved. * 00020 * * 00021 * Redistribution and use in source and binary forms, with or without * 00022 * modification, are permitted provided that the following conditions are met: * 00023 * * Redistributions of source code must retain the above copyright * 00024 * notice, this list of conditions and the following disclaimer. * 00025 * * Redistributions in binary form must reproduce the above copyright * 00026 * notice, this list of conditions and the following disclaimer in the * 00027 * documentation and/or other materials provided with the distribution. * 00028 * * Neither the name of the Technische Universität Chemnitz nor the * 00029 * names of its contributors may be used to endorse or promote products * 00030 * derived from this software without specific prior written permission. * 00031 * * 00032 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * 00033 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * 00034 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 00035 * ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY * 00036 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * 00037 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR * 00038 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * 00039 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * 00040 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 00041 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH * 00042 * DAMAGE. * 00043 * * 00044 ******************************************************************************/ 00045 00046 // local headers 00047 #include "pcdfilter_pa/pcdfilter_pa_ros_throttle.h" 00048 00049 //**************************[cPcdFilterPaRosThrottle]************************** 00050 cPcdFilterPaRosThrottle::cPcdFilterPaRosThrottle() { 00051 skip_count_ = 0; 00052 skip_time_ = 0.1; 00053 minimum_time_before_reset_ = 60; 00054 00055 block_all_input_ = false; 00056 00057 resetThrottle(); 00058 } 00059 00060 //**************************[~cPcdFilterPaRosThrottle]************************* 00061 cPcdFilterPaRosThrottle::~cPcdFilterPaRosThrottle() { 00062 } 00063 00064 //**************************[checkNewInput]************************************ 00065 bool cPcdFilterPaRosThrottle::checkNewInput(ros::Time time) { 00066 00067 bool result = false; 00068 00069 if (block_all_input_) { 00070 return false; 00071 } 00072 00073 // check for time discontinuities 00074 if (time_ > ros::Time(1)) { 00075 if (time_ - time > ros::Duration(minimum_time_before_reset_)) { 00076 resetThrottle(); 00077 } 00078 } 00079 00080 // check counter 00081 count_input_++; 00082 if (skip_count_ > 0) { 00083 if (count_input_ % (skip_count_ + 1) != 1) { 00084 return false; 00085 } 00086 } 00087 00088 // check time 00089 if ((skip_time_ > 0) && (time_ > ros::Time(1))) { 00090 if (time - time_ < ros::Duration(skip_time_)) { 00091 return false; 00092 } 00093 } 00094 00095 time_ = time; 00096 return true; 00097 } 00098 //**************************[resetThrottle]************************************ 00099 void cPcdFilterPaRosThrottle::resetThrottle() { 00100 00101 count_input_ = 0; 00102 time_ = ros::Time(0); 00103 block_all_input_ = false; 00104 }