rtt_rosclock_sim_clock_activity_manager.cpp

Go to the documentation of this file.

/*
 * (C) 2013, Johannes Meyer, TU Darmstadt
 * (C) 2013, Intermodalics BVBA
 * (C) 2014, Jonathan Bohren, The Johns Hopkins University
 *     - Generalized for multiple time sources
 *     - Integrated with rtt_rosclock package
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name of the copyright holder nor the names of its contributors
 *    may be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <rtt_rosclock/rtt_rosclock_sim_clock_activity.h>
#include <rtt_rosclock/rtt_rosclock_sim_clock_activity_manager.h>

#include <rtt/base/RunnableInterface.hpp>
#include <rtt/os/TimeService.hpp>
#include <rtt/Logger.hpp>

#include <boost/weak_ptr.hpp>

using namespace rtt_rosclock;

boost::weak_ptr<SimClockActivityManager> SimClockActivityManager::singleton;

boost::shared_ptr<SimClockActivityManager> SimClockActivityManager::GetInstance()
{
  return singleton.lock();
}

boost::shared_ptr<SimClockActivityManager> SimClockActivityManager::Instance()
{
  // Create a new instance, if necessary
  boost::shared_ptr<SimClockActivityManager> shared = GetInstance();
  if(singleton.expired()) {
    shared.reset(new SimClockActivityManager());
    singleton = shared;
  }

  return shared;
}

SimClockActivityManager::SimClockActivityManager() 
  : simulation_period_(0.0) 
{ 
}

SimClockActivityManager::~SimClockActivityManager()
{
}

RTT::Seconds SimClockActivityManager::getSimulationPeriod() const
{
  return simulation_period_;
}

void SimClockActivityManager::setSimulationPeriod(RTT::Seconds s)
{
  simulation_period_ = s;
}

void SimClockActivityManager::update()
{
  RTT::os::MutexLock lock(modify_activities_mutex_);
  RTT::os::TimeService::ticks now = RTT::os::TimeService::Instance()->getTicks();

  // Iterate through all activities
  for(std::list<SimClockActivity *>::const_iterator it = activities_.begin(); it != activities_.end(); it++)
  {
    // Update this activitiy at its desired minimum period
    SimClockActivity *activity = *it;
    if (RTT::os::TimeService::ticks2nsecs(now - activity->getLastExecutionTicks()) * 1e-9 >= activity->getPeriod())
    {
      activity->execute();
    }
  }
}

void SimClockActivityManager::add(SimClockActivity *activity)
{
  RTT::os::MutexLock lock(modify_activities_mutex_);
  std::list<SimClockActivity *>::iterator it = std::find(activities_.begin(), activities_.end(), activity);
  if (it == activities_.end()) {
    activities_.push_back(activity);
  }
}

void SimClockActivityManager::remove(SimClockActivity *activity)
{
  RTT::os::MutexLock lock(modify_activities_mutex_);
  std::list<SimClockActivity *>::iterator it = std::find(activities_.begin(), activities_.end(), activity);
  if (it != activities_.end()) {
    activities_.erase(it);
  }
}