SystemObserver.cpp

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package    ArmarX::Core
* @author     Kai Welke (welke _at_ kit _dot_ edu)
* @date       2012
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txtg
*             GNU General Public License
*/
 
#include "SystemObserver.h"
 
#include <IceUtil/Time.h>
 
#include <ArmarXCore/core/exceptions/Exception.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/ObserverObjectFactories.h>
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
#include <ArmarXCore/observers/checks/ConditionCheckValid.h>
#include <ArmarXCore/observers/variant/ChannelRef.h>
 
namespace armarx
{
    // ********************************************************************
    // observer framework hooks
    // ********************************************************************
    void
    SystemObserver::onInitObserver()
    {
        maxTimerId = 0;
 
        // register all checks
        offerConditionCheck(checks::SystemObserver::updated->getCheckStr(),
                            new ConditionCheckUpdated());
        offerConditionCheck(checks::SystemObserver::valid->getCheckStr(),
                            new ConditionCheckValid());
        offerConditionCheck(checks::SystemObserver::equals->getCheckStr(),
                            new ConditionCheckEquals());
        offerConditionCheck(checks::SystemObserver::inrange->getCheckStr(),
                            new ConditionCheckInRange());
        offerConditionCheck(checks::SystemObserver::larger->getCheckStr(),
                            new ConditionCheckLarger());
        offerConditionCheck(checks::SystemObserver::smaller->getCheckStr(),
                            new ConditionCheckSmaller());
    }
 
    void
    SystemObserver::postWorkerJobs()
    {
        Observer::postWorkerJobs();
        std::unique_lock lock(timersMutex);
        SystemObserverTimerMap::iterator iter = timers.begin();
 
        while (iter != timers.end())
        {
            try
            {
                // update the timer
                updateTimer(iter->second);
 
                // update data field and channel
                setDataField(iter->second.timerName, "elapsedMs", int(iter->second.elapsedMs));
                updateChannel(iter->second.timerName);
            }
            catch (armarx::LocalException& e)
            {
                ARMARX_FATAL << e.what();
            }
            catch (armarx::UserException& e)
            {
                ARMARX_FATAL << e.reason;
            }
            iter++;
        }
    }
 
    // ********************************************************************
    // timers interface implementation
    // ********************************************************************
    void
    SystemObserver::startTimer_async(const AMD_SystemObserverInterface_startTimerPtr& amd,
                                     const std::string& timerBaseName,
                                     const Ice::Current& c)
    {
        addWorkerJob("SystemObserver::startTimer",
                     [this, amd, timerBaseName]
                     {
                         std::stringstream temp;
                         temp << ++maxTimerId << "_" << timerBaseName;
                         std::string timerName = temp.str();
 
                         std::unique_lock lock(timersMutex);
 
                         if (timers.find(timerName) != timers.end())
                         {
                             std::string reason = "Timer " + timerName + " already exists";
                             amd->ice_exception(InvalidTimerException(reason.c_str()));
                         }
 
                         // create new timer
                         SystemObserverTimer timer;
                         timer.timerName = timerName;
                         resetTimer(timer);
 
                         std::pair<std::string, SystemObserverTimer> entry;
                         entry.first = timerName;
                         entry.second = timer;
 
                         timers.insert(entry);
 
                         std::unique_lock registryLock(channelsMutex);
 
                         // add channel and datafield
                         offerChannel(timerName, "Timer " + timerName);
                         offerDataFieldWithDefault(timerName,
                                                   "elapsedMs",
                                                   int(0),
                                                   "Elapsed milliseconds of Timer " + timerName);
                         updateChannel(timerName);
 
                         // return datafield identifier for easy condition installation
                         amd->ice_response(new ChannelRef(this, timerName));
                     });
    }
 
    void
    SystemObserver::resetTimer_async(const AMD_SystemObserverInterface_resetTimerPtr& amd,
                                     const ChannelRefBasePtr& timer,
                                     const Ice::Current& c)
    {
        addWorkerJob("SystemObserver::resetTimer",
                     [this, amd, timer]
                     {
                         std::unique_lock lock(timersMutex);
                         std::string timerName =
                             ChannelRefPtr::dynamicCast(timer)->getChannelName();
 
                         SystemObserverTimerMap::iterator iter = timers.find(timerName);
 
                         if (iter != timers.end())
                         {
                             resetTimer(iter->second);
                         }
                         else
                         {
                             std::string reason = "Timer " + timerName + " unknown";
                             amd->ice_exception(InvalidTimerException(reason.c_str()));
                         }
                         amd->ice_response();
                     });
    }
 
    void
    SystemObserver::pauseTimer_async(const AMD_SystemObserverInterface_pauseTimerPtr& amd,
                                     const ChannelRefBasePtr& timer,
                                     const Ice::Current& c)
    {
        addWorkerJob("SystemObserver::pauseTimer",
                     [this, amd, timer]
                     {
                         std::unique_lock lock(timersMutex);
                         std::string timerName =
                             ChannelRefPtr::dynamicCast(timer)->getChannelName();
 
                         SystemObserverTimerMap::iterator iter = timers.find(timerName);
 
                         if (iter != timers.end())
                         {
                             iter->second.paused = true;
                         }
                         else
                         {
                             std::string reason = "Timer " + timerName + " unknown";
                             amd->ice_exception(InvalidTimerException(reason.c_str()));
                         }
                         amd->ice_response();
                     });
    }
 
    void
    SystemObserver::unpauseTimer_async(const AMD_SystemObserverInterface_unpauseTimerPtr& amd,
                                       const ChannelRefBasePtr& timer,
                                       const Ice::Current& c)
    {
        addWorkerJob("SystemObserver::unpauseTimer",
                     [this, amd, timer]
                     {
                         std::unique_lock lock(timersMutex);
                         std::string timerName =
                             ChannelRefPtr::dynamicCast(timer)->getChannelName();
 
                         SystemObserverTimerMap::iterator iter = timers.find(timerName);
 
                         if (iter != timers.end())
                         {
                             iter->second.paused = false;
                             iter->second.startTimeMs += getElapsedTimeMs(iter->second.startTimeMs +
                                                                          iter->second.elapsedMs);
                         }
                         else
                         {
                             std::string reason = "Timer " + timerName + " unknown";
                             amd->ice_exception(InvalidTimerException(reason.c_str()));
                         }
                         amd->ice_response();
                     });
    }
 
    void
    SystemObserver::removeTimer_async(const AMD_SystemObserverInterface_removeTimerPtr& amd,
                                      const ChannelRefBasePtr& timer,
                                      const Ice::Current& c)
    {
        addWorkerJob("SystemObserver::removeTimer",
                     [this, amd, timer]
                     {
                         std::unique_lock lock(timersMutex);
                         std::string timerName =
                             ChannelRefPtr::dynamicCast(timer)->getChannelName();
 
                         SystemObserverTimerMap::iterator iter = timers.find(timerName);
 
                         if (iter != timers.end())
                         {
                             removeChannel(timerName);
                             timers.erase(iter);
                         }
                         amd->ice_response();
                     });
    }
 
    // ********************************************************************
    // counters implementation
    // ********************************************************************
    void
    SystemObserver::startCounter_async(const AMD_SystemObserverInterface_startCounterPtr& amd,
                                       int initialValue,
                                       const std::string& counterBaseName,
                                       const Ice::Current& c)
    {
        addWorkerJob("SystemObserver::startCounter",
                     [this, amd, initialValue, counterBaseName]
                     {
                         std::stringstream temp;
                         temp << ++maxCounterId << "_" << counterBaseName;
                         std::string counterName = temp.str();
 
                         std::unique_lock lock(countersMutex);
 
                         if (counters.find(counterName) != counters.end())
                         {
                             std::string reason = "Counter " + counterName + " already exists";
                             amd->ice_exception(InvalidCounterException(reason.c_str()));
                         }
 
 
                         // create new counter
                         SystemObserverCounter counter;
                         counter.counterName = counterName;
                         counter.value = initialValue;
 
                         std::pair<std::string, SystemObserverCounter> entry;
                         entry.first = counterName;
                         entry.second = counter;
 
 
                         SystemObserverCounterMap::iterator iter;
                         iter = (counters.insert(entry)).first;
 
 
                         // add channel and datafield
                         offerChannel(counterName, "Counter " + counterName);
                         offerDataField(counterName,
                                        "value",
                                        VariantType::Int,
                                        "Current value of counter " + counterName);
 
                         updateCounter(iter);
 
                         // return datafield identifier for easy condition installation
                         amd->ice_response(new ChannelRef(this, counterName));
                     });
    }
 
    void
    SystemObserver::incrementCounter_async(
        const AMD_SystemObserverInterface_incrementCounterPtr& amd,
        const ChannelRefBasePtr& counter,
        const Ice::Current& c)
    {
        addWorkerJob("SystemObserver::incrementCounter",
                     [this, amd, counter]
                     {
                         std::unique_lock lock(countersMutex);
                         std::string counterName =
                             ChannelRefPtr::dynamicCast(counter)->getChannelName();
 
                         SystemObserverCounterMap::iterator iter = counters.find(counterName);
 
                         if (iter != counters.end())
                         {
                             iter->second.value++;
                         }
                         else
                         {
                             std::string reason = "Counter " + counterName + " unknown";
                             amd->ice_exception(InvalidCounterException(reason.c_str()));
                         }
 
                         updateCounter(iter);
                         amd->ice_response(iter->second.value);
                     });
    }
 
    void
    SystemObserver::decrementCounter_async(
        const AMD_SystemObserverInterface_decrementCounterPtr& amd,
        const ChannelRefBasePtr& counter,
        const Ice::Current& c)
    {
        addWorkerJob("SystemObserver::decrementCounter",
                     [this, amd, counter]
                     {
                         std::unique_lock lock(countersMutex);
                         std::string counterName =
                             ChannelRefPtr::dynamicCast(counter)->getChannelName();
 
                         SystemObserverCounterMap::iterator iter = counters.find(counterName);
 
                         if (iter != counters.end())
                         {
                             iter->second.value--;
                         }
                         else
                         {
                             std::string reason = "Counter " + counterName + " unknown";
                             amd->ice_exception(InvalidCounterException(reason.c_str()));
                         }
 
                         updateCounter(iter);
                         amd->ice_response(iter->second.value);
                     });
    }
 
    void
    SystemObserver::resetCounter_async(const AMD_SystemObserverInterface_resetCounterPtr& amd,
                                       const ChannelRefBasePtr& counter,
                                       const Ice::Current& c)
    {
        addWorkerJob("SystemObserver::resetCounter",
                     [this, amd, counter]
                     {
                         std::unique_lock lock(countersMutex);
                         std::string counterName =
                             ChannelRefPtr::dynamicCast(counter)->getChannelName();
 
                         SystemObserverCounterMap::iterator iter = counters.find(counterName);
 
                         if (iter != counters.end())
                         {
                             iter->second.value = 0;
                         }
                         else
                         {
                             std::string reason = "Counter " + counterName + " unknown";
                             amd->ice_exception(InvalidCounterException(reason.c_str()));
                         }
 
                         updateCounter(iter);
                         amd->ice_response();
                     });
    }
 
    void
    SystemObserver::setCounter_async(const AMD_SystemObserverInterface_setCounterPtr& amd,
                                     const ChannelRefBasePtr& counter,
                                     int counterValue,
                                     const Ice::Current& c)
    {
        addWorkerJob("SystemObserver::setCounter",
                     [this, amd, counter, counterValue]
                     {
                         std::unique_lock lock(countersMutex);
                         std::string counterName =
                             ChannelRefPtr::dynamicCast(counter)->getChannelName();
 
                         SystemObserverCounterMap::iterator iter = counters.find(counterName);
 
                         if (iter != counters.end())
                         {
                             iter->second.value = counterValue;
                         }
                         else
                         {
                             std::string reason = "Counter " + counterName + " unknown";
                             amd->ice_exception(InvalidCounterException(reason.c_str()));
                         }
 
                         updateCounter(iter);
                         amd->ice_response();
                     });
    }
 
    void
    SystemObserver::removeCounter_async(const AMD_SystemObserverInterface_removeCounterPtr& amd,
                                        const ChannelRefBasePtr& counter,
                                        const Ice::Current& c)
    {
        addWorkerJob("SystemObserver::removeCounter",
                     [this, amd, counter]
                     {
                         std::unique_lock lock(countersMutex);
                         std::string counterName =
                             ChannelRefPtr::dynamicCast(counter)->getChannelName();
 
                         SystemObserverCounterMap::iterator iter = counters.find(counterName);
 
                         if (iter != counters.end())
                         {
                             removeChannel(counterName);
                             counters.erase(iter);
                         }
                         amd->ice_response();
                     });
    }
 
    // ********************************************************************
    // private methods
    // ********************************************************************
    void
    SystemObserver::resetTimer(SystemObserverTimer& timer)
    {
        timer.startTimeMs = getCurrentTimeMs();
        timer.elapsedMs = getElapsedTimeMs(timer.startTimeMs);
        timer.paused = false;
    }
 
    void
    SystemObserver::updateTimer(SystemObserverTimer& timer)
    {
        if (!timer.paused)
        {
            timer.elapsedMs = getElapsedTimeMs(timer.startTimeMs);
        }
    }
 
    int
    SystemObserver::getCurrentTimeMs()
    {
        IceUtil::Time current = TimeUtil::GetTime();
 
        return current.toMilliSeconds();
    }
 
    int
    SystemObserver::getElapsedTimeMs(int referenceTimeMs)
    {
        IceUtil::Time current = TimeUtil::GetTime();
        IceUtil::Time reference = IceUtil::Time::milliSeconds(referenceTimeMs);
        IceUtil::Time interval = current - reference;
 
        return interval.toMilliSeconds();
    }
 
    void
    SystemObserver::updateCounter(SystemObserverCounterMap::iterator& iterCounter)
    {
        // update data field and channel
        setDataField(iterCounter->second.counterName, "value", iterCounter->second.value);
        updateChannel(iterCounter->second.counterName);
    }
} // namespace armarx