State.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 "State.h"
 
#include <mutex>
 
#include <IceUtil/UUID.h>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include "DynamicRemoteState.h" //needed for substate copying
#include "RemoteState.h" //needed for substate copying
#include "StateBaseImpl.h"
#include "StateUtilFunctions.h"
 
namespace armarx
{
    // TODO: This should probably be somewhere in SimoxUtility?
    std::string
    joinStrings(std::vector<std::string> const& input, std::string const& seperator)
    {
        return simox::alg::join(input, seperator);
    }
 
    struct State::Impl
    {
        std::mutex inputParameterMutex;
        std::mutex localParameterMutex;
        std::mutex outputParameterMutex;
    };
 
    State::State() : simpl(new Impl)
    {
    }
 
    State::State(const State& source) :
        IceUtil::Shared(),
        Ice::Object(source),
        StateIceBase(source),
        Logging(source),
        StateBase(source),
        StateController(source),
        StateUtility(source),
        simpl(new Impl)
    {
    }
 
    State&
    State::operator=(const State& source)
    {
        assert(0);
        StateBase::operator=(source);
        StateBase::impl->context = source.impl->context;
        return *this;
    }
 
    State::~State()
    {
    }
 
    bool
    State::init(StatechartContextInterface* context, StatechartManager* manager)
    {
        __checkPhase(ePreDefined, __PRETTY_FUNCTION__);
 
        return StateBase::init(context, manager);
    }
 
    unsigned int
    State::getId() const
    {
        return StateBase::impl->localUniqueId;
    }
 
    std::string
    State::getGlobalIdString() const
    {
        return globalStateIdentifier;
    }
 
    StateBasePtr
    State::getInitState() const
    {
        return StateBasePtr::dynamicCast(initState);
    }
 
    bool
    State::isUnbreakable() const
    {
        return unbreakable;
    }
 
    StateParameterMap&
    State::getOutputParameters()
    {
        return outputParameters;
    }
 
    StatePtr
    State::operator[](std::string const& stateName)
    {
        for (size_t i = 0; i < subStateList.size(); ++i)
        {
            if (stateName == StateBasePtr::dynamicCast(subStateList[i])->stateName ||
                stateName == StateBasePtr::dynamicCast(subStateList[i])->globalStateIdentifier)
            {
                return StatePtr::dynamicCast(subStateList[i]);
            }
        }
 
        return nullptr;
    }
 
    StateBasePtr
    State::addState(StateBasePtr pNewState)
    {
 
        if (findSubstateByName(pNewState->getStateName()))
        {
            throw exceptions::local::eStatechartLogicError(
                "There exists already a substate with name '" + stateName +
                "' in this hierarchy level. In one hierarchy level (aka one substatelist) the "
                "names must be unique.");
        }
 
        pNewState->__setParentState(this);
        subStateList.push_back(pNewState);
        pNewState->init(StateBase::impl->context, StateBase::impl->manager);
        return pNewState;
    }
 
    RemoteStatePtr
    State::addRemoteState(std::string stateName, std::string proxyName, std::string instanceName)
    {
        __checkPhase(eSubstatesDefinitions, __PRETTY_FUNCTION__);
 
        if (instanceName.empty())
        {
            instanceName = stateName;
        }
 
        if (findSubstateByName(instanceName))
        {
            throw LocalException("There exists already a substate with name '" + instanceName +
                                 "' in this hierarchy level. In one hierarchy level aka one "
                                 "substatelist the names must be unique.");
        }
 
        RemoteStatePtr pNewState = new RemoteState();
        pNewState->setStateName(instanceName);
        pNewState->setStateClassName(stateName);
        pNewState->__setParentState(this);
        pNewState->setProxyName(proxyName);
 
        subStateList.push_back(pNewState);
        StatechartContext* context = StateBase::getContext<StatechartContext>();
 
        pNewState->StateBase::init(StateBase::impl->context, StateBase::impl->manager);
 
        if (!ManagedIceObjectPtr::dynamicCast(pNewState))
        {
            throw exceptions::local::eNullPointerException(
                "could not cast RemoteStatePtr to ManagedIceObjectPtr");
        }
 
        if (!context)
        {
            throw exceptions::local::eNullPointerException(
                "statechart context ptr is NULL -> could not add state as component");
        }
        else
        {
 
            context->getArmarXManager()->addObject(ManagedIceObjectPtr::dynamicCast(pNewState),
                                                   false);
 
            //            ARMARX_DEBUG << "Waiting for RemoteState '" << pNewState->getName() << "'";
 
            //            pNewState->getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted, 500);
 
            //            ARMARX_DEBUG << "RemoteState '" << pNewState->getName() << "' started.";
        }
 
        return pNewState;
    }
 
    RemoteStatePtr
    State::addDynamicRemoteState(std::string instanceName)
    {
        __checkPhase(eSubstatesDefinitions, __PRETTY_FUNCTION__);
 
        if (findSubstateByName(instanceName))
        {
            throw LocalException("There exists already a substate with name '" + stateName +
                                 "' in this hierarchy level. In one hierarchy level aka one "
                                 "substatelist the names must be unique.");
        }
 
        RemoteStatePtr pNewState = new DynamicRemoteState();
 
        pNewState->__setParentState(this);
        pNewState->setStateName(instanceName);
        subStateList.push_back(pNewState);
        StatechartContext* context = StateBase::getContext<StatechartContext>();
 
        pNewState->StateBase::init(StateBase::impl->context, StateBase::impl->manager);
 
        ARMARX_CHECK_EXPRESSION(ManagedIceObjectPtr::dynamicCast(pNewState));
        ARMARX_CHECK_EXPRESSION(context);
 
        context->getArmarXManager()->addObject(ManagedIceObjectPtr::dynamicCast(pNewState),
                                               false,
                                               pNewState->getName() + IceUtil::generateUUID());
 
        ARMARX_DEBUG << "Waiting for RemoteState " << pNewState->getName();
 
        pNewState->getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
 
        ARMARX_DEBUG << "RemoteState " << pNewState->getName() << "started.";
 
        return pNewState;
    }
 
    TransitionIceBase&
    State::addTransition(EventPtr event,
                         StateIceBasePtr sourceState,
                         StateIceBasePtr destinationState,
                         ParameterMappingIceBasePtr mappingToNextStatesInput,
                         ParameterMappingIceBasePtr mappingToParentStatesLocal,
                         ParameterMappingIceBasePtr mappingToParentStatesOutput)
    {
        __checkPhase(eSubstatesDefinitions, __PRETTY_FUNCTION__);
        TransitionIceBase t;
 
        assert(sourceState._ptr);
        assert(destinationState._ptr);
        t.sourceState = sourceState;
        t.evt = createEvent(event->eventName, event->properties);
        t.destinationState = destinationState;
 
        if (mappingToNextStatesInput)
        {
            t.mappingToNextStatesInput = PMPtr::dynamicCast(mappingToNextStatesInput)->clone();
        }
 
        if (mappingToParentStatesLocal)
        {
            t.mappingToParentStatesLocal = PMPtr::dynamicCast(mappingToParentStatesLocal)->clone();
        }
 
        if (mappingToParentStatesOutput)
        {
            t.mappingToParentStatesOutput =
                PMPtr::dynamicCast(mappingToParentStatesOutput)->clone();
        }
 
        if (__checkExistenceOfTransition(t))
            throw exceptions::local::eStatechartLogicError(
                "There already exists a transition on event '" + t.evt->eventName + "' from '" +
                sourceState->stateName + "' to '" + destinationState->stateName + "'");
 
        transitions.push_back(t);
        return *transitions.rbegin();
    }
 
    void
    State::inheritInputFromSubstate(std::string stateName)
    {
        __checkPhase(eParametersDefinitions, __PRETTY_FUNCTION__);
        StateBase::impl->inputInheritance.push_back(stateName);
    }
 
    bool
    State::addToInput(const std::string& key,
                      VariantTypeId type,
                      bool optional,
                      VariantPtr defaultValue)
    {
        __checkPhase(eParametersDefinitions, __PRETTY_FUNCTION__);
        return addParameter(inputParameters, key, type, optional);
    }
 
    bool
    State::addToInput(const std::string& key,
                      const ContainerType& type,
                      bool optional,
                      VariantContainerBasePtr defaultValue)
    {
        __checkPhase(eParametersDefinitions, __PRETTY_FUNCTION__);
        return addParameterContainer(inputParameters, key, type, optional, defaultValue);
    }
 
    bool
    State::addToOutput(const std::string& key, VariantTypeId type, bool optional)
    {
        __checkPhase(eParametersDefinitions, __PRETTY_FUNCTION__);
        return addParameter(outputParameters, key, type, optional);
    }
 
    bool
    State::addToOutput(const std::string& key, const ContainerType& type, bool optional)
    {
        __checkPhase(eParametersDefinitions, __PRETTY_FUNCTION__);
 
        return addParameterContainer(outputParameters, key, type, optional);
    }
 
    bool
    State::addToLocal(const std::string& key,
                      const ContainerType& type,
                      VariantContainerBasePtr defaultValue)
    {
        __checkPhase(eParametersDefinitions, __PRETTY_FUNCTION__);
        std::unique_lock lock(simpl->localParameterMutex);
 
        return addParameterContainer(localParameters, key, type, true, defaultValue);
    }
 
    bool
    State::addToLocal(const std::string& key, VariantTypeId type, VariantPtr defaultValue)
    {
        __checkPhase(eParametersDefinitions, __PRETTY_FUNCTION__);
        std::unique_lock lock(simpl->localParameterMutex);
 
        return addParameter(localParameters, key, type, true, defaultValue);
    }
 
    StateBasePtr
    State::setInitState(StateBasePtr initState, ParameterMappingPtr initialStateMapping)
    {
        __checkPhase(eSubstatesDefinitions, __PRETTY_FUNCTION__);
 
        if (initialStateMapping)
        {
            this->initialStateMapping = initialStateMapping->clone();
        }
 
        if (!StateIceBasePtr::dynamicCast(initState))
        {
            throw LocalException("Couldnt cast initstate");
        }
 
        this->initState = initState;
 
        return initState;
    }
 
    void
    State::setUnbreakable(bool setUnbreakable)
    {
        __checkPhase(eStatechartDefinitions, __PRETTY_FUNCTION__);
        unbreakable = setUnbreakable;
    }
 
    void
    State::setUseRunFunction(bool useRunFuntion)
    {
        StateBase::impl->__useRunFunction = useRunFuntion;
    }
 
    void
    State::cancelSubstates()
    {
        __checkPhase(eEntering, __PRETTY_FUNCTION__);
        StateBase::impl->cancelEnteringSubstates = true;
    }
 
    void
    State::setStateName(const std::string& newName)
    {
        stateName = newName;
    }
 
    void
    State::setStateClassNameFromTypeName(const std::string& typeName)
    {
        std::string className = simox::alg::replace_all(typeName, ":", "_");
        setStateClassName(className);
    }
 
    StateParameterMap
    State::getInputParameters()
    {
        StateParameterMap result;
        std::unique_lock lock(simpl->inputParameterMutex);
        StateUtilFunctions::copyDictionary(inputParameters, result);
        return result;
    }
 
    VariantPtr
    State::getInput(const std::string& key) const
    {
        __checkPhaseMin(eDefined, __PRETTY_FUNCTION__);
        std::unique_lock lock(simpl->inputParameterMutex);
 
 
        VariantPtr var;
        getParameter(inputParameters, key, var);
        return var->clone();
    }
 
    StateParameterMap&
    State::getLocalParameters()
    {
        return localParameters;
    }
 
    void
    State::setInput(const std::string& key, const Variant& value)
    {
        __checkPhaseMin(eDefined, __PRETTY_FUNCTION__);
        std::unique_lock lock(simpl->inputParameterMutex);
 
        if (getStatePhase() >= eEntering && isInputParameterSet(key))
        {
            throw LocalException("Input parameter '" + key + "' in state '" + stateName +
                                 "'' is already set and cannot be changed.");
        }
 
        setParameter(inputParameters, key, value);
    }
 
    void
    State::setInput(std::string const& key, const VariantContainerBase& valueList)
    {
        __checkPhaseMin(eDefined, __PRETTY_FUNCTION__);
        std::unique_lock lock(simpl->inputParameterMutex);
 
        if (getStatePhase() >= eEntering && isInputParameterSet(key))
        {
            throw LocalException("Input parameter '" + key + "' in state '" + stateName +
                                 "'' is already set and cannot be changed.");
        }
 
        setParameterContainer(inputParameters, key, valueList);
    }
 
    void
    State::setLocal(std::string const& key, const Variant& value)
    {
        __checkPhaseMin(eDefined, __PRETTY_FUNCTION__);
        std::unique_lock lock(simpl->localParameterMutex);
 
        setParameter(getLocalParameters(), key, value);
    }
 
    void
    State::setLocal(std::string const& key, const VariantContainerBase& valueList)
    {
        __checkPhaseMin(eDefined, __PRETTY_FUNCTION__);
        std::unique_lock lock(simpl->localParameterMutex);
 
        setParameterContainer(getLocalParameters(), key, valueList);
    }
 
    void
    State::setOutput(std::string const& key, const Variant& value)
    {
        __checkPhaseMin(eDefined, __PRETTY_FUNCTION__);
        std::unique_lock lock(simpl->outputParameterMutex);
 
        setParameter(getOutputParameters(), key, value);
    }
 
    void
    State::setOutput(std::string const& key, const VariantContainerBase& valueList)
    {
        __checkPhaseMin(eDefined, __PRETTY_FUNCTION__);
        std::unique_lock lock(simpl->outputParameterMutex);
 
        setParameterContainer(getOutputParameters(), key, valueList);
    }
 
    StateBasePtr
    State::clone() const
    {
        StateBasePtr result = new State(*this);
        //result->deepCopy(shared_from_this,false);
        return result;
    }
 
    StateBasePtr
    State::createEmptyCopy() const
    {
        StateBasePtr result = new State();
        //result->deepCopy(shared_from_this,false);
        return result;
    }
 
    //    SingleTypeVariantListPtr
    //    State::getInputList(std::string key){
    //        __checkPhaseMin(eDefined, __PRETTY_FUNCTION__);
    //        std::unique_lock lock(inputParameterMutex);
    //        SingleTypeVariantListPtr varList;
    //        getParameter(inputParameters, key, varList);
    //        return varList;
    //    }
 
    VariantContainerBasePtr
    State::getLocalContainer(std::string const& key)
    {
        __checkPhaseMin(eDefined, __PRETTY_FUNCTION__);
        std::unique_lock lock(simpl->localParameterMutex);
 
        VariantContainerBasePtr varList;
        getParameterContainer(localParameters, key, varList);
        return VariantContainerBasePtr::dynamicCast(varList);
    }
 
    bool
    State::isInputParameterSet(const std::string& key) const
    {
        return isParameterSet(inputParameters, key);
    }
 
    bool
    State::isLocalParameterSet(const std::string& key) const
    {
        return isParameterSet(localParameters, key);
    }
 
    bool
    State::isOutputParameterSet(const std::string& key) const
    {
        return isParameterSet(outputParameters, key);
    }
 
    template <>
    StatePtr
    State::addState<State>(std::string const& stateName)
    {
        __checkPhase(eSubstatesDefinitions, __PRETTY_FUNCTION__);
 
        if (findSubstateByName(stateName))
        {
            throw exceptions::local::eStatechartLogicError(
                "There already exists a substate with name '" + stateName +
                "' in this hierarchy level. In one hierarchy level (aka one substatelist) the "
                "names must be unique.");
        }
 
        if (stateName.empty())
        {
            throw exceptions::local::eStatechartLogicError("The statename must not be empty");
        }
 
        StatePtr pNewState = new State();
        pNewState->stateName = stateName;
        pNewState->__setParentState(this);
        subStateList.push_back(pNewState);
        StatechartContext* context = StateBase::getContext<StatechartContext>();
        pNewState->init(context, StateBase::impl->manager);
 
        return pNewState;
    }
 
 
} // namespace armarx