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 <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
#include <ArmarXCore/observers/checks/ConditionCheckValid.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/variant/ChannelRef.h>
#include <ArmarXCore/observers/ObserverObjectFactories.h>
#include <ArmarXCore/core/exceptions/Exception.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <IceUtil/Time.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);
    }
}