IceStateConverter.cpp

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* 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::
* @author     Clara Scherer
* @date       2014
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include <QMap>
#include <QString>
 
#include <algorithm>
#include <cassert>
#include <string>
#include <utility>
#include <vector>
 
#include <ArmarXCore/interface/statechart/RemoteStateIce.h>
 
#include "model/stateinstance/StateInstanceFactory.h"
 
#include "IceStateConverter.h"
#include "model/State.h"
#include "model/stateinstance/LocalState.h"
#include "model/stateinstance/StateInstance.h"
 
#include <cmath>
 
armarx::IceStateConverter::IceStateConverter(armarx::statechartmodel::StatePtr state)
{
    topState.reset(new statechartmodel::LocalState(state, state->getStateName()));
}
 
armarx::IceStateConverter::~IceStateConverter()
{
}
 
 
void armarx::IceStateConverter::convert(armarx::StateIceBasePtr iceBase)
{
    convert(iceBase, topState);
}
 
armarx::statechartmodel::StateInstancePtr armarx::IceStateConverter::getTopState()
{
    return topState;
}
 
void armarx::IceStateConverter::setStateWatcher(armarx::StateWatcherPtr watcher)
{
    this->watcher = watcher;
}
 
std::map<std::string, std::pair<armarx::statechartmodel::StateInstancePtr, armarx::StateIceBasePtr>> armarx::IceStateConverter::getCompleteStateMap() const
{
    return completeStateMap;
}
 
void armarx::IceStateConverter::convert(armarx::StateIceBasePtr iceBase, armarx::statechartmodel::StateInstancePtr modelState)
{
    if (watcher && modelState->getStateClass())
    {
        if (!watcher->subscribeToState(iceBase, modelState->getStateClass()))
        {
            ARMARX_WARNING_S << "subscribing failed";
        }
    }
    //    ARMARX_LOG_S << "Converting state " << modelState->getStateName();
 
    if (stateEqual(iceBase, modelState))
    {
        updateState(iceBase, modelState);
    }
    else
    {
        ARMARX_INFO_S << "State " << iceBase->stateName << " could not be recognized - resetting";
        resetState(iceBase, modelState);
    }
 
    StateList iceSubstates = iceBase->subStateList;
    std::sort(iceSubstates.begin(), iceSubstates.end(), &armarx::IceStateConverter::compareIceStates);
 
    statechartmodel::StateInstanceMap modelSubstates;
    if (modelState->getStateClass())
    {
        modelSubstates = modelState->getStateClass()->getSubstates();
    }
 
    assert(iceSubstates.size() == static_cast<size_t>(modelSubstates.size()));
 
    using modelSubstateIter = statechartmodel::StateInstanceMap::const_iterator;
    StateList::const_iterator iceIter = iceSubstates.begin();
    modelSubstateIter modelIter = modelSubstates.begin();
 
    for (; iceIter != iceSubstates.end(); iceIter++, modelIter++)
    {
        convert(StateIceBasePtr::dynamicCast(*iceIter), modelIter.value());
    }
}
 
bool armarx::IceStateConverter::stateEqual(armarx::StateIceBasePtr iceBase, armarx::statechartmodel::StateInstancePtr modelState)
{
    if (!modelState)
    {
        return true;
    }
    //    ARMARX_CHECK_EXPRESSION(modelState) << iceBase->stateName;
    if (!modelState || iceBase->stateName.c_str() != modelState->getInstanceName())
    {
        if (!modelState->getStateClass() && iceBase->transitions.size() > 0)
        {
            return false;
        }
        if (modelState->getStateClass() && !sameTransitions(iceBase, modelState->getStateClass()))
        {
            return false;
        }
    }
 
    return true;
}
 
bool armarx::IceStateConverter::sameTransitions(armarx::StateIceBasePtr iceBase, armarx::statechartmodel::StatePtr modelState)
{
    if (static_cast<size_t>(modelState->getTransitions().size()) != iceBase->transitions.size())
    {
        return false;
    }
 
    assert(static_cast<size_t>(modelState->getTransitions().size()) == iceBase->transitions.size());
 
    std::pair<stringVector, stringVector> sortedTransitions = sortTransitionNames(iceBase->transitions, modelState->getTransitions());
    stringVector iceTransitions = sortedTransitions.first;
    stringVector modelTransitions = sortedTransitions.second;
 
    using stringIter = stringVector::const_iterator;
    std::pair<stringIter, stringIter> mismatch;
    mismatch = std::mismatch<stringIter, stringIter>(iceTransitions.begin(), iceTransitions.end(), modelTransitions.begin());
 
    if (mismatch.first == iceTransitions.end()) //all transitions have matched
    {
        return true;
    }
 
    return false;
}
 
 
void armarx::IceStateConverter::updateTransitions(armarx::StateIceBasePtr iceBase, armarx::statechartmodel::StatePtr modelState)
{
    if (modelState->getTransitions().empty())
    {
        resetTransitions(iceBase, modelState);
        return;
    }
 
    TransitionTable iceTransitions = iceBase ->transitions;
    statechartmodel::CTransitionList modelTransitions = modelState->getTransitions();
 
    //check for new transitions
    foreach (TransitionIceBase newTrans, iceTransitions)
    {
        //transitions from all don't have a source state specified
        if (!newTrans.sourceState)
        {
            updateTransitionFromAll(newTrans, modelState);
        }
        else
        {
            updateSingleTransition(newTrans, modelState);
        }
    }
 
    //check for old transitions
    foreach (auto oldTrans, modelTransitions)
    {
        bool found = false;
 
        foreach (auto newTrans, iceTransitions)
        {
            if (newTrans.evt->eventName == oldTrans->eventName.toStdString())
            {
                found = true;
                break;
            }
        }
 
        if (!found)
        {
            //remove the transition from the model
            modelState->removeTransition(modelState->findTransition(oldTrans));
        }
    }
}
 
void armarx::IceStateConverter::updateSubstates(armarx::StateIceBasePtr iceBase, armarx::statechartmodel::StatePtr modelState)
{
    using modelSubstateIter = statechartmodel::StateInstanceMap::const_iterator;
 
    statechartmodel::StateInstanceMap modelSubstates = modelState->getSubstates();
    if (modelSubstates.empty())
    {
        resetSubstates(iceBase, modelState);
        return;
    }
 
    StateList iceSubstates = iceBase->subStateList;
    std::sort(iceSubstates.begin(), iceSubstates.end(), &armarx::IceStateConverter::compareIceStates);
 
 
    StateList::const_iterator iceIter = iceSubstates.begin();
    modelSubstateIter modelIter = modelSubstates.begin();
 
    while ((iceIter != iceSubstates.end()) || (modelIter != modelSubstates.end()))
    {
        //        ARMARX_CHECK_EXPRESSION(modelIter.value()->getStateClass()) << (*iceIter)->stateName;
        //TODO:StateInstance name oder Statename für Identifikation benutzen?
        //momentan wird Name des States benutzt
        if ((iceIter != iceSubstates.end()) && ((modelIter == modelSubstates.end()) || ((*iceIter)->stateName.c_str() < modelIter.value()->getInstanceName())))
            //a substate from ice is missing in the model
        {
            //insert new substate
            statechartmodel::StatePtr newSubstate(new statechartmodel::State);
            newSubstate->setStateName((*iceIter)->stateName.c_str());
            //            newSubstate->setActive(false);
            statechartmodel::StateInstancePtr instance = statechartmodel::StateInstanceFactory::CreateFromIceType((*iceIter)->stateType, newSubstate, (*iceIter)->stateName.c_str(), modelState);
            modelState->addSubstate(instance);
            //name of stateInstance is initialized to be the same as the correspoding state's name
 
            iceIter++;
            continue;
        }
 
        if ((modelIter != modelSubstates.end()) && ((iceIter == iceSubstates.end()) || (*iceIter)->stateName.c_str() > modelIter.value()->getInstanceName()))
            //a substate in the model is not found in ice anymore
        {
            //substate löschen
            modelState->removeSubstate(modelIter.key());
 
            modelIter++;
            continue;
        }
 
        //        if (watcher /*&& (*iceIter)->subStateList.size() > 0*/)
        //        {
        //            if (!watcher->subscribeToState(*iceIter, (*modelIter)->getStateClass()))
        //            {
        //                ARMARX_WARNING_S << "subscribing failed";
        //            }
        //        }
 
        iceIter++;
        modelIter++;
        continue;
    }
}
 
armarx::statechartmodel::StateParameterMap armarx::IceStateConverter::convertToModelParameterMap(armarx::StateParameterMap iceMap)
{
    statechartmodel::StateParameterMap modelMap;
 
 
    for (StateParameterMap::const_iterator iter = iceMap.begin(); iter != iceMap.end(); iter++)
    {
        statechartmodel::StateParameterPtr param = statechartmodel::StateParameter::FromIceStateParameter((*iter).second);
 
        modelMap.insert((*iter).first.c_str(), param);
    }
 
    return modelMap;
}
 
std::pair<armarx::IceStateConverter::stringVector, armarx::IceStateConverter::stringVector> armarx::IceStateConverter::sortTransitionNames(armarx::TransitionTable iceTransitions,
        armarx::statechartmodel::CTransitionList modelTransitions)
{
    stringVector eventsOfIceTransitions;
 
    for (TransitionTable::const_iterator iceIter = iceTransitions.begin() ; iceIter != iceTransitions.end(); iceIter++)
    {
        eventsOfIceTransitions.push_back((*iceIter).evt->eventName);
    }
 
    std::sort(eventsOfIceTransitions.begin(), eventsOfIceTransitions.end());
 
    stringVector eventsOfModelTransitions;
 
    for (statechartmodel::CTransitionList::const_iterator modelIter = modelTransitions.begin() ; modelIter != modelTransitions.end(); modelIter++)
    {
        eventsOfModelTransitions.push_back((*modelIter)->eventName.toStdString());
    }
 
    std::sort(eventsOfModelTransitions.begin(), eventsOfModelTransitions.end());
 
    return make_pair(eventsOfIceTransitions, eventsOfModelTransitions);
}
 
bool armarx::IceStateConverter::compareIceStates(armarx::AbstractStateIceBasePtr l, armarx::AbstractStateIceBasePtr r)
{
    return l->stateName < r->stateName;
}
 
armarx::statechartmodel::StateInstancePtr armarx::IceStateConverter::findSubstateByName(statechartmodel::StatePtr state, QString name)
{
    //search for stateinstance with this name among the substates of modelState
    statechartmodel::StateInstanceMap::const_iterator substateIter = state->getSubstates().find(name);
 
    if (substateIter != state->getSubstates().end())
    {
        return *substateIter;
    }
 
    ARMARX_ERROR_S << "substate " << name << " provided by ice is not found among the substates of "
                   << state->getStateName().toStdString();
    return statechartmodel::StateInstancePtr();
}
 
void armarx::IceStateConverter::updateStartState(armarx::StateIceBasePtr iceBase, armarx::statechartmodel::StatePtr modelState)
{
    //requires that names of substates are distinct for one state
    if (iceBase->initState && modelState->getStartState()
        && (iceBase->initState->stateName.c_str() == modelState->getStartState()->getInstanceName()))
    {
        return;
        //no need for change if both model- and ice-startState exist and have the same name.
    }
    else
    {
        resetStartState(iceBase, modelState);
    }
}
 
void armarx::IceStateConverter::updateActiveSubstate(armarx::StateIceBasePtr iceBase, armarx::statechartmodel::StatePtr modelState)
{
    if (!modelState || !iceBase)
    {
        return;
    }
    if (iceBase->activeSubstate && modelState->getActiveSubstate()
        && (iceBase->activeSubstate->stateName.c_str() == modelState->getActiveSubstate()->getInstanceName()))
    {
        return;
        //no need to change anything if both active substates exist and are the same
    }
    else
    {
        //        ARMARX_INFO_S << "Resetting ActiveSubstate of" << iceBase->stateName;
        resetActiveSubstate(iceBase, modelState);
    }
}
 
void armarx::IceStateConverter::updateStateAttributes(armarx::StateIceBasePtr iceBase, armarx::statechartmodel::StateInstancePtr modelState)
{
    if (iceBase->stateName.c_str() != modelState->getInstanceName())
    {
        resetStateAttributes(iceBase, modelState);
    }
}
 
void armarx::IceStateConverter::updateTransitionFromAll(armarx::TransitionIceBase newTrans, armarx::statechartmodel::StatePtr modelState)
{
    //            ARMARX_INFO_S << newTrans.evt->eventName << " is a transition from all.";
 
    statechartmodel::CTransitionList modelTransitions = modelState->getTransitions();
 
    if (!newTrans.destinationState)
    {
        ARMARX_ERROR_S << "Transitions from all cannot be detached";
        return;
    }
    statechartmodel::StateInstancePtr destination = findSubstateByName(modelState, newTrans.destinationState->stateName.c_str());
 
    //iterate over all substates and test whether they have a fitting transition
    foreach (statechartmodel::StateInstancePtr substate, modelState->getSubstates())
    {
        bool transFound = false;
        foreach (statechartmodel::TransitionCPtr oldTrans, modelTransitions)
        {
            if ((newTrans.evt->eventName == oldTrans->eventName.toStdString())
                && (substate == oldTrans->sourceState))
            {
                transFound = true;
 
                //update destination state if necessary
                if (!(oldTrans->destinationState) || (newTrans.destinationState->stateName != oldTrans->destinationState->getInstanceName().toStdString()))
                {
                    modelState->updateTransitionDestination(oldTrans, destination);
                }
 
                break;
            }
        }
 
        if (!transFound)
        {
            //create new model Transition to insert
            statechartmodel::TransitionPtr transition(new statechartmodel::Transition());
 
            transition->eventName = newTrans.evt->eventName.c_str();
            transition->sourceState = substate;
            transition->destinationState = destination;
 
            modelState->addTransition(transition);
        }
    }
}
 
void armarx::IceStateConverter::updateSingleTransition(armarx::TransitionIceBase newTrans, armarx::statechartmodel::StatePtr modelState)
{
    statechartmodel::CTransitionList modelTransitions = modelState->getTransitions();
 
    bool found = false;
 
    foreach (statechartmodel::TransitionCPtr oldTrans, modelTransitions)
    {
        //it is assumed that a transition is uniquely identified by its source and event
        if ((newTrans.evt->eventName == oldTrans->eventName.toStdString()))
        {
            found = true;
 
            //update destination state if necessary
            if (newTrans.destinationState)
            {
                if (!(oldTrans->destinationState) || (newTrans.destinationState->stateName != oldTrans->destinationState->getInstanceName().toStdString()))
                {
                    modelState->updateTransitionDestination(oldTrans, findSubstateByName(modelState, newTrans.destinationState->stateName.c_str()));
                }
            }
            else
            {
                if (oldTrans->destinationState)
                {
                    modelState->detachTransitionDestination(oldTrans);
                }
            }
 
            break;
        }
    }
 
    if (!found)
    {
        //create new model Transition to insert
        statechartmodel::TransitionPtr transition(new statechartmodel::Transition());
        transition->eventName = newTrans.evt->eventName.c_str();
 
        transition->sourceState = findSubstateByName(modelState, newTrans.sourceState->stateName.c_str());
 
        if (newTrans.destinationState) //transitions can be detached, i.e. not have a destination state
        {
            transition->destinationState = findSubstateByName(modelState, newTrans.destinationState->stateName.c_str());
            modelState->addTransition(transition);
        }
        else
        {
            modelState->addDetachedTransition(transition->eventName, transition->sourceState);
        }
    }
}
 
void armarx::IceStateConverter::resetTransitions(armarx::StateIceBasePtr iceBase, armarx::statechartmodel::StatePtr modelState)
{
    statechartmodel::TransitionList newTransitionList;
    statechartmodel::TransitionList detachedTransitionList;
 
    for (TransitionTable::const_iterator iceIter = iceBase->transitions.begin(); iceIter != iceBase->transitions.end(); iceIter++)
    {
        statechartmodel::TransitionPtr transition(new statechartmodel::Transition());
        transition->eventName = (*iceIter).evt->eventName.c_str();
 
        bool detached = false;
 
        //transitions from all have no sourceState
        if (!((*iceIter).sourceState))
        {
            if (!((*iceIter).destinationState))
            {
                ARMARX_ERROR_S << "Transition from all states: " << transition->eventName << " doesn't have a destination state.";
                continue;
            }
            statechartmodel::StateInstancePtr destination = findSubstateByName(modelState, (*iceIter).destinationState->stateName.c_str());
 
            foreach (statechartmodel::StateInstancePtr substate, modelState->getSubstates())
            {
                statechartmodel::TransitionPtr newTransition(new statechartmodel::Transition());
                newTransition->eventName = transition->eventName;
                newTransition->sourceState = substate;
                newTransition->destinationState = destination;
 
                newTransitionList.push_back(newTransition);
            }
        }
        else
        {
            transition->sourceState = findSubstateByName(modelState, (*iceIter).sourceState->stateName.c_str());
 
            if ((*iceIter).destinationState)
            {
                transition->destinationState = findSubstateByName(modelState, (*iceIter).destinationState->stateName.c_str());
            }
            else
            {
                detached = true;
            }
 
            //TODO: also set parameter mapping
            if (detached)
            {
                detachedTransitionList.push_back(transition);
            }
            else
            {
                newTransitionList.push_back(transition);
            }
        }
    }
 
    modelState->replaceTransitions(newTransitionList);
 
    foreach (statechartmodel::TransitionPtr trans, detachedTransitionList)
    {
        modelState->addDetachedTransition(trans->eventName, trans->sourceState);
    }
}
 
void armarx::IceStateConverter::resetSubstates(armarx::StateIceBasePtr iceBase, armarx::statechartmodel::StatePtr modelState)
{
    statechartmodel::StateInstanceMap newSubstateList;
 
    for (StateList::const_iterator iceIter = iceBase->subStateList.begin(); iceIter != iceBase->subStateList.end(); iceIter++)
    {
        statechartmodel::StatePtr newSubstate(new statechartmodel::State());
        newSubstate->setStateName((*iceIter)->stateName.c_str());
        //        newSubstate->setActive(false);
        QString proxyName;
        armarx::RemoteStateIceBasePtr remoteState = armarx::RemoteStateIceBasePtr::dynamicCast(*iceIter);
        if (remoteState)
        {
            proxyName = QString::fromStdString(remoteState->proxyName);
        }
        statechartmodel::StateInstancePtr instance = statechartmodel::StateInstanceFactory::CreateFromIceType((*iceIter)->stateType, newSubstate, (*iceIter)->stateName.c_str(), modelState, proxyName);
        //        statechartmodel::StateInstancePtr instance(new statechartmodel::LocalState(newSubstate, (*iceIter)->stateName.c_str(), modelState));
 
        instance->inputParameters = (*iceIter)->inputParameters;
        newSubstateList.insert((*iceIter)->stateName.c_str(), instance);
 
        if (watcher && StateIceBasePtr::dynamicCast(*iceIter)->subStateList.size() > 0)
        {
            if (!watcher->subscribeToState(StateIceBasePtr::dynamicCast(*iceIter), newSubstate))
            {
                ARMARX_INFO_S << "subscribing failed";
            }
        }
        resetState(StateIceBasePtr::dynamicCast(*iceIter), instance);
        //name of stateInstance is initialized to be the same as the correspoding state's name
    }
 
    modelState->replaceSubstates(newSubstateList);
}
 
void armarx::IceStateConverter::resetParameters(armarx::StateIceBasePtr iceBase, armarx::statechartmodel::StatePtr modelState)
{
    modelState->setInputParameters(convertToModelParameterMap(iceBase->inputParameters));
    modelState->setLocalParameters(convertToModelParameterMap(iceBase->localParameters));
    modelState->setOutputParameters(convertToModelParameterMap(iceBase->outputParameters));
}
 
void armarx::IceStateConverter::resetStartState(armarx::StateIceBasePtr iceBase, armarx::statechartmodel::StatePtr modelState)
{
    if (iceBase->initState) //test whether ice state has an active substate
    {
        modelState->setStartState(findSubstateByName(modelState, iceBase->initState->stateName.c_str()));
    }
    else   //modelState still has an active substate but iceBase doesn't
    {
        modelState->setStartState(statechartmodel::StateInstancePtr());
    }
}
 
void armarx::IceStateConverter::resetActiveSubstate(armarx::StateIceBasePtr iceBase, armarx::statechartmodel::StatePtr modelState)
{
    if (iceBase->activeSubstate) //test whether ice state has an active substate
    {
 
        statechartmodel::StateInstancePtr activeSubstate = findSubstateByName(modelState, iceBase->activeSubstate->stateName.c_str());
        modelState->setActiveSubstate(activeSubstate);
 
    }
    else   //modelState still has an active substate but iceBase doesn't
    {
        modelState->setActiveSubstate(statechartmodel::StateInstancePtr());
    }
}
 
void armarx::IceStateConverter::resetStateAttributes(armarx::StateIceBasePtr iceBase, armarx::statechartmodel::StateInstancePtr modelState)
{
    modelState->setInstanceName(iceBase->stateName.c_str());
    if (modelState->getStateClass())
    {
        modelState->getStateClass()->setStateName(iceBase->stateClassName.c_str());
    }
    int subStateCount = modelState->getStateClass() ? modelState->getStateClass()->getSubstates().size() : 0;
    float sizeFactor = pow(subStateCount, 0.7);
    sizeFactor = std::max(sizeFactor, 1.0f);
    modelState->setBoundingBox(sizeFactor * modelState->defaultBoundingSquareSize);
}
 
void armarx::IceStateConverter::resetState(armarx::StateIceBasePtr iceBase, armarx::statechartmodel::StateInstancePtr modelState)
{
    completeStateMap[iceBase->globalStateIdentifier] = std::make_pair(modelState, iceBase);
 
 
    if (modelState->getStateClass())
    {
        auto stateClass = modelState->getStateClass();
        resetParameters(iceBase, stateClass);
        resetSubstates(iceBase, stateClass);
        resetTransitions(iceBase, stateClass);
        resetActiveSubstate(iceBase, stateClass);
        resetStartState(iceBase, stateClass);
    }
 
    resetStateAttributes(iceBase, modelState);
 
}
 
void armarx::IceStateConverter::updateState(armarx::StateIceBasePtr iceBase, statechartmodel::StateInstancePtr modelState)
{
    updateStateAttributes(iceBase, modelState);
 
 
    if (modelState->getStateClass())
    {
        auto stateClass = modelState->getStateClass();
        updateSubstates(iceBase, stateClass);
        if (!sameTransitions(iceBase, stateClass))
        {
            //        ARMARX_INFO_S << "Updating transitions of" << iceBase->stateName;
            updateTransitions(iceBase, stateClass);
        }
 
        resetParameters(iceBase, stateClass);
        updateActiveSubstate(iceBase, stateClass);
        updateStartState(iceBase, stateClass);
    }
}
 
bool armarx::IceStateConverter::transitionEqual(armarx::TransitionIceBase iceTransition,
        armarx::statechartmodel::TransitionPtr modelTransition)
{
    if (iceTransition.evt->eventName.c_str() == modelTransition->eventName)
    {
        return false;
    }
 
    return true;
}