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 <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 = 0.0;
00058       _sdf->GetElement(_prefix + "Rate")->GetValue()->Get(update_rate);
00059       update_period_ = update_rate > 0.0 ? 1.0/update_rate : 0.0;
00060     }
00061 
00062     if (_sdf->HasElement(_prefix + "Period")) {
00063       _sdf->GetElement(_prefix + "Period")->GetValue()->Get(update_period_);
00064     }
00065 
00066     if (_sdf->HasElement(_prefix + "Offset")) {
00067       _sdf->GetElement(_prefix + "Offset")->GetValue()->Get(update_offset_);
00068     }
00069   }
00070 
00071   virtual event::ConnectionPtr Connect(const boost::function<void()> &_subscriber, bool connectToWorldUpdateBegin = true)
00072   {
00073     if (connectToWorldUpdateBegin && !update_connection_) {
00074       update_connection_ = event::Events::ConnectWorldUpdateBegin(
00075             boost::bind(&UpdateTimer::Update, this));
00076     }
00077     connection_count_++;
00078     return update_event_.Connect(_subscriber);
00079   }
00080 
00081   virtual void Disconnect(event::ConnectionPtr const& _c = event::ConnectionPtr())
00082   {
00083     if (_c) update_event_.Disconnect(_c);
00084 
00085     if (update_connection_ && (!_c || --connection_count_ == 0)) {
00086       event::Events::DisconnectWorldUpdateBegin(update_connection_);
00087       update_connection_.reset();
00088     }
00089   }
00090 
00091   common::Time const& getUpdatePeriod() const {
00092     return update_period_;
00093   }
00094 
00095   void setUpdatePeriod(common::Time const& period) {
00096     update_period_ = period;
00097   }
00098 
00099   double getUpdateRate() const {
00100     double period = update_period_.Double();
00101     return (period > 0.0)  ? (1.0 / period) : 0.0;
00102   }
00103 
00104   void setUpdateRate(double rate) {
00105     update_period_ = (rate > 0.0) ? (1.0 / rate) : 0.0;
00106   }
00107 
00108   common::Time const& getLastUpdate() const {
00109     return last_update_;
00110   }
00111 
00112   common::Time getTimeSinceLastUpdate() const {
00113     if (last_update_ == common::Time()) return common::Time();
00114     return world_->GetSimTime() - last_update_;
00115   }
00116 
00117   virtual bool checkUpdate() const
00118   {
00119     double period = update_period_.Double();
00120     double step = world_->GetPhysicsEngine()->GetMaxStepSize();
00121     if (period == 0) return true;
00122     double fraction = fmod((world_->GetSimTime() - update_offset_).Double() + (step / 2.0), period);
00123     return (fraction >= 0.0) && (fraction < step);
00124   }
00125 
00126   virtual bool update()
00127   {
00128     if (!checkUpdate()) return false;
00129     last_update_ = world_->GetSimTime();
00130     return true;
00131   }
00132 
00133   virtual bool update(double& dt)
00134   {
00135     dt = getTimeSinceLastUpdate().Double();
00136     return update();
00137   }
00138 
00139   virtual void Reset()
00140   {
00141     last_update_ = common::Time();
00142   }
00143 
00144 protected:
00145   virtual bool Update()
00146   {
00147     if (!checkUpdate()) {
00148       return false;
00149     }
00150     update_event_();
00151     last_update_ = world_->GetSimTime();
00152     return true;
00153   }
00154 
00155 private:
00156   physics::WorldPtr world_;
00157   common::Time update_period_;
00158   common::Time update_offset_;
00159   common::Time last_update_;
00160 
00161   event::EventT<void()> update_event_;
00162   unsigned int connection_count_;
00163   event::ConnectionPtr update_connection_;
00164 };
00165 
00166 } // namespace gazebo
00167 
00168 #endif // HECTOR_GAZEBO_PLUGINS_UPDATE_TIMER_H


husky_gazebo_plugins
Author(s):
autogenerated on Wed Aug 26 2015 11:54:19