update_timer.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2013, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #ifndef HECTOR_GAZEBO_PLUGINS_UPDATE_TIMER_H
00030 #define HECTOR_GAZEBO_PLUGINS_UPDATE_TIMER_H
00031 
00032 #include <gazebo/sdf/sdf.hh>
00033 #include <gazebo/physics/World.hh>
00034 #include <gazebo/physics/PhysicsEngine.hh>
00035 
00036 #include <gazebo/common/Event.hh>
00037 #include <gazebo/common/Events.hh>
00038 
00039 namespace gazebo {
00040 
00041 class UpdateTimer {
00042 public:
00043   UpdateTimer()
00044     : connection_count_(0)
00045   {
00046   }
00047 
00048   virtual ~UpdateTimer()
00049   {
00050   }
00051 
00052   virtual void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf, const std::string& _prefix = "update")
00053   {
00054     this->world_ = _world;
00055 
00056     if (_sdf->HasElement(_prefix + "Rate")) {
00057       double update_rate = _sdf->GetElement(_prefix + "Rate")->GetValueDouble();
00058       update_period_ = update_rate > 0.0 ? 1.0/update_rate : 0.0;
00059     }
00060 
00061     if (_sdf->HasElement(_prefix + "Period")) {
00062       update_period_ = _sdf->GetElement(_prefix + "Period")->GetValueTime();
00063     }
00064 
00065     if (_sdf->HasElement(_prefix + "Offset")) {
00066       update_offset_ = _sdf->GetElement(_prefix + "Offset")->GetValueTime();
00067     }
00068   }
00069 
00070   virtual event::ConnectionPtr Connect(const boost::function<void()> &_subscriber, bool connectToWorldUpdateStart = true)
00071   {
00072     if (connectToWorldUpdateStart && !update_connection_) {
00073       update_connection_ = event::Events::ConnectWorldUpdateStart(
00074             boost::bind(&UpdateTimer::Update, this));
00075     }
00076     connection_count_++;
00077     return update_event_.Connect(_subscriber);
00078   }
00079 
00080   virtual void Disconnect(event::ConnectionPtr const& _c = event::ConnectionPtr())
00081   {
00082     if (_c) update_event_.Disconnect(_c);
00083 
00084     if (update_connection_ && (!_c || --connection_count_ == 0)) {
00085       event::Events::DisconnectWorldUpdateStart(update_connection_);
00086       update_connection_.reset();
00087     }
00088   }
00089 
00090   common::Time const& getUpdatePeriod() const {
00091     return update_period_;
00092   }
00093 
00094   void setUpdatePeriod(common::Time const& period) {
00095     update_period_ = period;
00096   }
00097 
00098   double getUpdateRate() const {
00099     double period = update_period_.Double();
00100     return (period > 0.0)  ? (1.0 / period) : 0.0;
00101   }
00102 
00103   void setUpdateRate(double rate) {
00104     update_period_ = (rate > 0.0) ? (1.0 / rate) : 0.0;
00105   }
00106 
00107   common::Time const& getLastUpdate() const {
00108     return last_update_;
00109   }
00110 
00111   common::Time getTimeSinceLastUpdate() const {
00112     if (last_update_ == common::Time()) return common::Time();
00113     return world_->GetSimTime() - last_update_;
00114   }
00115 
00116   virtual bool checkUpdate() const
00117   {
00118     double period = update_period_.Double();
00119     double step = world_->GetPhysicsEngine()->GetStepTime();
00120     if (period == 0) return true;
00121     double fraction = fmod((world_->GetSimTime() - update_offset_).Double() + (step / 2.0), period);
00122     return (fraction >= 0.0) && (fraction < step);
00123   }
00124 
00125   virtual bool update()
00126   {
00127     if (!checkUpdate()) return false;
00128     last_update_ = world_->GetSimTime();
00129     return true;
00130   }
00131 
00132   virtual bool update(double& dt)
00133   {
00134     dt = getTimeSinceLastUpdate().Double();
00135     return update();
00136   }
00137 
00138   virtual void Reset()
00139   {
00140     last_update_ = common::Time();
00141   }
00142 
00143 protected:
00144   virtual bool Update()
00145   {
00146     if (!checkUpdate()) {
00147       return false;
00148     }
00149     update_event_();
00150     last_update_ = world_->GetSimTime();
00151     return true;
00152   }
00153 
00154 private:
00155   physics::WorldPtr world_;
00156   common::Time update_period_;
00157   common::Time update_offset_;
00158   common::Time last_update_;
00159 
00160   event::EventT<void()> update_event_;
00161   unsigned int connection_count_;
00162   event::ConnectionPtr update_connection_;
00163 };
00164 
00165 } // namespace gazebo
00166 
00167 #endif // HECTOR_GAZEBO_PLUGINS_UPDATE_TIMER_H
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher and Johannes Meyer
autogenerated on Mon Jul 15 2013 16:46:30