rtt_rosclock_sim_clock_activity_manager.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Johannes Meyer, TU Darmstadt
00005  *  Copyright (c) 2013, Intermodalics BVBA
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 TU Darmstadt and Intermodalics BVBA
00019  *     nor the names of its contributors may be
00020  *     used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  *  Copyright (c) 2014, Jonathan Bohren, The Johns Hopkins University
00037  *  - Generalized for multiple time sources
00038  *  - Integrated with rtt_rosclock package
00039  *********************************************************************/
00040 
00041 #include <rtt_rosclock/rtt_rosclock_sim_clock_activity.h>
00042 #include <rtt_rosclock/rtt_rosclock_sim_clock_activity_manager.h>
00043 
00044 #include <rtt/base/RunnableInterface.hpp>
00045 #include <rtt/os/TimeService.hpp>
00046 #include <rtt/Logger.hpp>
00047 
00048 #include <boost/weak_ptr.hpp>
00049 
00050 using namespace rtt_rosclock;
00051 
00052 boost::weak_ptr<SimClockActivityManager> SimClockActivityManager::singleton;
00053 
00054 boost::shared_ptr<SimClockActivityManager> SimClockActivityManager::GetInstance()
00055 {
00056   return singleton.lock();
00057 }
00058 
00059 boost::shared_ptr<SimClockActivityManager> SimClockActivityManager::Instance()
00060 {
00061   // Create a new instance, if necessary
00062   boost::shared_ptr<SimClockActivityManager> shared = GetInstance();
00063   if(singleton.expired()) {
00064     shared.reset(new SimClockActivityManager());
00065     singleton = shared;
00066   }
00067 
00068   return shared;
00069 }
00070 
00071 SimClockActivityManager::SimClockActivityManager() 
00072   : simulation_period_(0.0) 
00073 { 
00074 }
00075 
00076 SimClockActivityManager::~SimClockActivityManager()
00077 {
00078 }
00079 
00080 RTT::Seconds SimClockActivityManager::getSimulationPeriod() const
00081 {
00082   return simulation_period_;
00083 }
00084 
00085 void SimClockActivityManager::setSimulationPeriod(RTT::Seconds s)
00086 {
00087   simulation_period_ = s;
00088 }
00089 
00090 void SimClockActivityManager::update()
00091 {
00092   RTT::os::MutexLock lock(modify_activities_mutex_);
00093   RTT::os::TimeService::ticks now = RTT::os::TimeService::Instance()->getTicks();
00094 
00095   // Iterate through all activities
00096   for(std::list<SimClockActivity *>::const_iterator it = activities_.begin(); it != activities_.end(); it++)
00097   {
00098     // Update this activitiy at its desired minimum period
00099     SimClockActivity *activity = *it;
00100     if (RTT::os::TimeService::ticks2nsecs(now - activity->getLastExecutionTicks()) * 1e-9 >= activity->getPeriod())
00101     {
00102       activity->execute();
00103     }
00104   }
00105 }
00106 
00107 void SimClockActivityManager::add(SimClockActivity *activity)
00108 {
00109   RTT::os::MutexLock lock(modify_activities_mutex_);
00110   std::list<SimClockActivity *>::iterator it = std::find(activities_.begin(), activities_.end(), activity);
00111   if (it == activities_.end()) {
00112     activities_.push_back(activity);
00113   }
00114 }
00115 
00116 void SimClockActivityManager::remove(SimClockActivity *activity)
00117 {
00118   RTT::os::MutexLock lock(modify_activities_mutex_);
00119   std::list<SimClockActivity *>::iterator it = std::find(activities_.begin(), activities_.end(), activity);
00120   if (it != activities_.end()) {
00121     activities_.erase(it);
00122   }
00123 }


rtt_rosclock
Author(s):
autogenerated on Thu Jun 6 2019 18:06:39