StateUtil.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    ArmarXCore::Statechart
* @author     Mirko Waechter( mirko.waechter at kit dot edu)
* @date       2012
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
#include "StateUtil.h"
#include "StatechartContext.h"
#include "StateParameter.h"
#include "StatechartManager.h"
#include "StatechartEventDistributor.h"
#include "StateBaseImpl.h"
#include <ArmarXCore/observers/variant/ChannelRef.h>
 
#include <ArmarXCore/core/exceptions/Exception.h>
#include <sstream>
 
 
namespace armarx
{
    void StateUtility::sendEvent(const EventPtr event, StateBasePtr eventProcessor)
    {
        //        __checkPhaseMin(eEntering, __PRETTY_FUNCTION__);
 
        if (!eventProcessor._ptr)
        {
            eventProcessor = impl->__parentState;
        }
 
        // if sendEvent was called and this state is not active anymore, omit event
        if (
            (eventProcessor.get() == impl->__parentState && (getStatePhase() < eEntering ||  getStatePhase() >= eBreaking))
            || (impl->__parentState && impl->__parentState->activeSubstate.get() != this))
        {
            ARMARX_VERBOSE << "The state " << stateName << " is not active anymore - event '" << event->eventName << "' omitted"  << std::endl;
 
            if (impl->__parentState->activeSubstate)
            {
                ARMARX_VERBOSE << "Active state is now: " << StateBasePtr::dynamicCast(impl->__parentState->activeSubstate)->stateName;
            }
 
            return;
        }
 
        //        if(!checkEventIsHandled(event))
        //        {
        //            throw exceptions::local::eStatechartLogicError("The event " + event->eventName + " is not handled by the parent state. Please insert a transition for this event in the parent state of state '" + getGlobalHierarchyString() + "'");
        //        }
        StateControllerPtr eventController = StateControllerPtr::dynamicCast(eventProcessor); // cast here to make sure cast works
 
        if (eventController._ptr)
        {
            if (impl->manager)
            {
                impl->manager->addEvent(event, eventController);
            }
 
            //            ScopedRecursiveLock lock(__eventBufferMutex);
            //            ARMARX_VERBOSE << "NEW event inserted";
            //            __eventBuffer.push_back(pair<StateControllerPtr, EventPtr>(eventController, event));
        }
 
        //        if(__stateMutex.try_lock()){// if locked, this function was called from onEnter()
        //            __stateMutex.unlock();
        //            if(functionPhase != eRunning)
        //                __processEvents();
        //        }
 
    }
 
    //    std::string StateUtility::getScope() const
    //    {
    //        std::stringstream str;
    //        str << context->getName() << "_"  << stateName << "_" << localUniqueId;
    //        return str.str();
    //    }
 
 
 
    ConditionIdentifier StateUtility::installCondition(const Term& condition, const EventPtr evt, const std::string& desc)
    {
        __checkPhase({eEntering, eEntered}, __PRETTY_FUNCTION__);
        StatechartContext* context = getContext<StatechartContext>();
        if (!context)
        {
            throw exceptions::local::eNullPointerException();
        }
 
        if (!impl->__parentState)
        {
            throw exceptions::local::eNullPointerException();
        }
 
        //        if (!checkEventIsHandled(evt))
        //        {
        //            throw exceptions::local::eStatechartLogicError("The event " + evt->eventName + " is not handled by the parent state. Please insert a transition for this event in the parent state of state '" + getGlobalHierarchyString() + "'");
        //        }
 
        StateBasePtr eventProcessor = impl->__parentState;
        ConditionHandlerInterfacePrx conditionHandlerPrx = context->conditionHandlerPrx;
 
        std::string uniqueEventIdentifier = context->eventDistributor->registerEvent(evt, eventProcessor, impl->visitCounter);
 
        EventPtr  sentEvent = new Event(*evt);
        sentEvent->eventName = uniqueEventIdentifier;
 
        ARMARX_DEBUG << "registered event: " << sentEvent->eventName << ", EventRecv:" << sentEvent->eventReceiverName << flush;
        installedConditions.push_back(condition.getImpl());
        ConditionIdentifier condId = conditionHandlerPrx->installConditionWithDescription(context->eventDistributor->getListener(), condition.getImpl(), sentEvent, desc, true, false, {});
        __installedConditionIdentifiers.push_back(condId);
        return condId;
    }
 
    void StateUtility::removeCondition(ConditionIdentifier conditionId)
    {
        auto context = getContext<StatechartContext>();
        if (!context)
        {
            throw exceptions::local::eNullPointerException();
        }
 
        // remove from eventmap?
        ARMARX_DEBUG << "removing condition with Id: " << conditionId.uniqueId << flush;
        context->conditionHandlerPrx->begin_removeCondition(conditionId);
    }
 
    StateUtility::ActionEventIdentifier StateUtility::setTimeoutEvent(int timeoutDurationMs, const EventPtr& evt)
    {
        __checkPhase(eEntering, __PRETTY_FUNCTION__);
 
        StatechartContext* context = getContext<StatechartContext>();
        ARMARX_INFO << "Installing Timeout Event (Timeout: " << timeoutDurationMs << "ms)" << flush;
 
        if (!context)
        {
            throw exceptions::local::eNullPointerException("context pointer is NULL");
        }
 
        if (!context->eventDistributor->getProxy())
        {
            throw exceptions::local::eNullPointerException("eventWrapper->myproxy was not set");
        }
 
        if (!impl->__parentState)
        {
            throw exceptions::local::eNullPointerException("parent pointer is NULL");
        }
 
 
        std::stringstream timerName;
        timerName  << "Timer_" << context->getName() << "_" << stateName << "_" << impl->localUniqueId << "_" << evt->eventName << "_" << timeoutDurationMs << "ms";
 
        ActionEventIdentifier result;
        result.actionId = ChannelRefPtr::dynamicCast(context->systemObserverPrx->startTimer(timerName.str()));
        __installedTimerChannelRefs.push_back(result.actionId);
        Literal expression(ChannelRefPtr::dynamicCast(result.actionId)->getDataFieldIdentifier("elapsedMs"), "larger", Literal::createParameterList(timeoutDurationMs));
        result.conditionId = installCondition(expression, evt, "Timeout condition");
        return result;
    }
 
    void StateUtility::removeTimeoutEvent(const ActionEventIdentifier& id)
    {
        StatechartContext* context = getContext<StatechartContext>();
        if (!context)
        {
            throw exceptions::local::eNullPointerException();
        }
 
        context->systemObserverPrx->begin_removeTimer(id.actionId);
        removeCondition(id.conditionId);
    }
 
 
 
    StateUtility::ActionEventIdentifier StateUtility::setCounterEvent(int counterThreshold, const EventPtr& evt, int initialValue)
    {
        __checkPhaseMin(eEntering, __PRETTY_FUNCTION__);
        StatechartContext* context = getContext<StatechartContext>();
        ARMARX_INFO << "Installing Counter Event (Counter Threshold: " << counterThreshold << ", InitialValue: " << initialValue << ")" << flush;
 
        if (!context)
        {
            throw exceptions::local::eNullPointerException("context pointer is NULL");
        }
 
        if (!context->eventDistributor->getListener())
        {
            throw exceptions::local::eNullPointerException("eventWrapper->myproxy was not set");
        }
 
        if (!impl->__parentState)
        {
            throw exceptions::local::eNullPointerException("parentState pointer is NULL");
        }
 
 
        std::stringstream counterName;
        counterName  << "Counter_" << stateName << "_" << impl->localUniqueId << "_" << evt->eventName;
 
        ActionEventIdentifier result;
 
        result.actionId = ChannelRefPtr::dynamicCast(context->systemObserverPrx->startCounter(initialValue, counterName.str()));
        __installedCounterChannelRefs.push_back(result.actionId);
        Literal expression(ChannelRefPtr::dynamicCast(result.actionId)->getDataFieldIdentifier("value"), "equals", Literal::createParameterList(counterThreshold));
        result.conditionId = installCondition(expression, evt, "Counter condition");
        return result;
    }
 
    void StateUtility::removeCounterEvent(const StateUtility::ActionEventIdentifier& id)
    {
        StatechartContext* context = getContext<StatechartContext>();
        if (!context)
        {
            throw exceptions::local::eNullPointerException();
        }
 
        context->systemObserverPrx->begin_removeCounter(id.actionId);
        removeCondition(id.conditionId);
    }
 
    bool
    StateUtility::checkEventIsHandled(const EventPtr& event) const
    {
        __checkPhaseMin(eDefined, __PRETTY_FUNCTION__);
 
        if (!impl->__parentState)
        {
            throw exceptions::local::eNullPointerException("parentState pointer is NULL");
        }
 
        for (unsigned int i = 0; i < impl->__parentState->transitions.size(); ++i)
        {
            TransitionIceBase& t = impl->__parentState->transitions.at(i);
 
            if (t.evt->eventName == event->eventName)
            {
                return true;
            }
        }
 
        return false;
    }
 
    EventPtr StateUtility::createEvent(const std::string& eventName, const StringVariantContainerBaseMap& properties)
    {
        EventPtr evt = new Event(this->stateName, eventName);
        evt->properties = properties;
        return evt;
    }
 
    void StateUtility::_removeInstalledConditions()
    {
        StatechartContext* context = dynamic_cast<StatechartContext*>(getContext(false));
        if (!context)
        {
            ARMARX_INFO << "Statechart Context is NULL - cannot clear installed conditions";
            return;
        }
        for (auto c : __installedConditionIdentifiers)
        {
            removeCondition(c);
        }
 
        __installedConditionIdentifiers.clear();
 
        for (auto c : __installedTimerChannelRefs)
        {
            context->systemObserverPrx->removeTimer(c);
        }
 
        __installedTimerChannelRefs.clear();
 
        for (auto c : __installedCounterChannelRefs)
        {
            context->systemObserverPrx->begin_removeCounter(c);
        }
 
        __installedCounterChannelRefs.clear();
    }
 
 
}