StateWatcher.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
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "StateWatcher.h"
 
#include <iterator>
 
#include <ArmarXCore/core/services/profiler/Profiler.h> // for ProfilerPtr
#include <ArmarXCore/observers/Event.h>
#include <ArmarXCore/observers/condition/Term.h>
#include <ArmarXCore/observers/variant/StringValueMap.h>
#include <ArmarXCore/observers/variant/VariantContainer.h>
#include <ArmarXCore/statechart/StateUtilFunctions.h>
 
#include "view/TransitionItem.h"
 
namespace armarx
{
    StateWatcher::StateWatcher()
    {
    }
 
    void
    StateWatcher::onInitComponent()
    {
        usingTopic("StateReportingTopic");
    }
 
    void
    StateWatcher::onConnectComponent()
    {
 
        qRegisterMetaType<statechartmodel::TransitionCPtr>("TransitionCPtr");
        qRegisterMetaType<statechartmodel::StateInstancePtr>("StateInstancePtr");
        subscriptionTask =
            new PeriodicTask<StateWatcher>(this, &StateWatcher::subscriptionFunction, 1000);
        subscriptionTask->start();
    }
 
    void
    StateWatcher::onDisconnectComponent()
    {
        subscriptionTask->stop();
        try
        {
            clear();
        }
        catch (...)
        {
        }
    }
 
    void
    StateWatcher::subscriptionFunction()
    {
        SubscriptionMap tempList;
        {
            std::unique_lock lock(mapMutex);
            tempList.swap(subscriptionQueue);
        }
        ARMARX_DEBUG << tempList.size() << " new subscriptions received";
        for (auto& elem : tempList)
        {
            StateIceBasePtr iceState = elem.first;
            statechartmodel::StatePtr state = elem.second.first;
            StateItem* stateItem = elem.second.second;
            StateListenerDataMap::iterator it;
            {
                std::unique_lock lock(mapMutex);
                it = stateMap.find(iceState->globalStateIdentifier);
                if (it != stateMap.end() && state && it->second.states.count(state) > 0)
                {
                    continue;
                }
                if (it != stateMap.end() && stateItem && it->second.stateItems.count(stateItem) > 0)
                {
                    continue;
                }
            }
            ARMARX_VERBOSE << "subscribing : " << iceState->globalStateIdentifier;
            try
            {
 
                StateListenerData data;
                if (it != stateMap.end())
                {
                    data = it->second;
                }
                data.iceState = iceState;
                if (state)
                {
                    data.states.insert(state);
                }
                if (stateItem)
                {
                    data.stateItems.insert(stateItem);
                }
 
                std::unique_lock lock(mapMutex);
                stateMap[iceState->globalStateIdentifier] = data;
                //                ARMARX_IMPORTANT << "subscribed to " << iceState->globalStateIdentifier;
            }
            catch (...)
            {
                handleExceptions();
            }
        }
    }
 
    bool
    StateWatcher::subscribeToState(StateIceBasePtr iceState, statechartmodel::StatePtr state)
    {
        std::unique_lock lock(mapMutex);
        if (!iceState)
        {
            ARMARX_WARNING << "Ice State NULL";
            return false;
        }
        if (!state)
        {
            ARMARX_WARNING << "State NULL: " << iceState->globalStateIdentifier;
            return false;
        }
        StateItem* item = nullptr;
        subscriptionQueue.push_back(std::make_pair(iceState, std::make_pair(state, item)));
        return true;
    }
 
    bool
    StateWatcher::unsubscribeState(StateItem* state)
    {
        std::unique_lock lock(mapMutex);
        for (auto& pair : stateMap)
        {
            for (auto item : pair.second.stateItems)
            {
                if (item == state)
                {
                    pair.second.stateItems.erase(state);
                    return true;
                }
            }
        }
        return false;
    }
 
    bool
    StateWatcher::subscribeToState(StateIceBasePtr iceState, StateItem* state)
    {
        std::unique_lock lock(mapMutex);
        if (!iceState)
        {
            ARMARX_WARNING << "Ice State NULL";
            return false;
        }
        if (!state)
        {
            ARMARX_WARNING << "State NULL: " << iceState->globalStateIdentifier;
            return false;
        }
        subscriptionQueue.push_back(
            std::make_pair(iceState, std::make_pair(statechartmodel::StatePtr(), state)));
        return true;
    }
 
    VariantPtr
    StateWatcher::getStateParameter(statechartmodel::StatePtr state,
                                    const std::string paramMapType) const
    {
        std::unique_lock lock(mapMutex);
        try
        {
            for (const auto& stateData : stateMap)
            {
                if (stateData.second.states.count(state))
                {
                    //                    ARMARX_INFO << "Checking: " << "Found";
                    StringValueMap map(false);
                    for (auto& entry : stateData.second.parameters.at(paramMapType))
                    {
                        map.addElement(entry.first, entry.second->value);
                    }
                    return new Variant(map);
                }
            }
        }
        catch (...)
        {
            handleExceptions();
        }
 
        return VariantPtr();
    }
 
    ::armarx::StateParameterMap
    StateWatcher::getStateParameterMap(statechartmodel::StatePtr state,
                                       const std::string paramMapType) const
    {
        std::unique_lock lock(mapMutex);
        //        try
        {
            for (const auto& stateData : stateMap)
            {
                if (stateData.second.states.count(state))
                {
                    //                    ARMARX_INFO << "Checking: " << "Found";
                    ARMARX_INFO << "query state: "
                                << stateData.second.iceState->globalStateIdentifier;
 
                    auto it = stateData.second.parameters.find(paramMapType);
                    if (it == stateData.second.parameters.end())
                    {
                        std::stringstream s;
                        for (auto& e : stateData.second.parameters)
                        {
                            s << e.first << ", ";
                        }
                        throw LocalException()
                            << "could not find " << paramMapType << " available: " << s.str();
                    }
                    return it->second;
                }
            }
        }
        //        catch (...)
        //        {
        //            handleExceptions();
        //        }
 
        return StateParameterMap();
    }
 
    void
    StateWatcher::clear()
    {
        std::unique_lock lock(mapMutex);
 
        stateMap.clear();
    }
 
    std::string
    StateWatcher::ExtractStateName(const std::string globalIdString)
    {
        if (globalIdString.find("->") == std::string::npos)
        {
            return globalIdString;
        }
        return globalIdString.substr(globalIdString.find_last_of("->") + 1);
    }
 
    std::string
    StateWatcher::getDefaultName() const
    {
        return "StateWatcher";
    }
 
    void
    StateWatcher::reportNetworkTraffic(const std::string&,
                                       const std::string&,
                                       Ice::Int,
                                       Ice::Int,
                                       const Ice::Current&)
    {
    }
 
    void
    StateWatcher::reportEvent(const ProfilerEvent& event, const Ice::Current&)
    {
        try
        {
            std::string stateIdentifier = event.parentName;
            if (event.functionName !=
                Profiler::Profiler::GetEventName(Profiler::Profiler::eFunctionStart))
            {
                std::unique_lock lock(mapMutex);
                auto it = stateMap.find(stateIdentifier);
                if (it == stateMap.end())
                {
                    ARMARX_VERBOSE << "Could not find state with id " << stateIdentifier;
                    return;
                }
                const StateListenerData& data = it->second;
                for (auto& state : data.states)
                {
                    if (state)
                    {
                        QMetaObject::invokeMethod(state.get(),
                                                  "setActiveSubstate",
                                                  Q_ARG(statechartmodel::StateInstancePtr,
                                                        statechartmodel::StateInstancePtr()));
                        //                        auto subState = state->getActiveSubstate();
                        //                        if (subState)
                        //                        {
                        //                            QMetaObject::invokeMethod(subState.get(), "setActive", Q_ARG(bool, false));
                        //                        }
                    }
                }
            }
        }
        catch (...)
        {
            handleExceptions();
        }
    }
 
    void
    StateWatcher::reportStatechartTransition(const ProfilerStatechartTransition& event,
                                             const Ice::Current&)
    {
        try
        {
            std::unique_lock lock(mapMutex);
 
            {
                ARMARX_DEBUG << "Got event reported for active state changed: "
                             << event.parentStateIdentifier;
                auto it = stateMap.find(event.parentStateIdentifier);
                if (it == stateMap.end())
                {
                    ARMARX_VERBOSE << "Could not find state with id "
                                   << event.parentStateIdentifier;
                    return;
                }
                const StateListenerData& data = it->second;
                for (const statechartmodel::StatePtr& state : data.states)
                {
                    auto activeSubstate = state->getActiveSubstate();
                    if (activeSubstate)
                    {
                        //                        QMetaObject::invokeMethod(activeSubstate.get(), "setActive", Q_ARG(bool, false));
                    }
                    std::string activeSubstateName = ExtractStateName(event.targetStateIdentifier);
                    if (!activeSubstateName.empty())
                    {
                        auto it =
                            state->getSubstates().find(QString::fromStdString(activeSubstateName));
                        if (it != state->getSubstates().end())
                        {
                            statechartmodel::StateInstancePtr substate = *it;
                            if (substate)
                            {
                                ARMARX_DEBUG << "Invoking setActive for " << activeSubstateName;
                                QMetaObject::invokeMethod(
                                    state.get(),
                                    "setActiveSubstate",
                                    Q_ARG(statechartmodel::StateInstancePtr, substate));
                                //                                QMetaObject::invokeMethod(substate.get(), "setActive", Q_ARG(bool, true));
                            }
                        }
                        else
                        {
                            ARMARX_INFO << "could not find state " << activeSubstateName;
                        }
                    }
                    else
                    {
                        QMetaObject::invokeMethod(state.get(),
                                                  "setActiveSubstate",
                                                  Q_ARG(statechartmodel::StateInstancePtr,
                                                        statechartmodel::StateInstancePtr()));
                    }
                }
                for (auto item : data.stateItems)
                {
                    std::string activeSubstateName = ExtractStateName(event.targetStateIdentifier);
                    QMetaObject::invokeMethod(
                        item,
                        "setActiveSubstate",
                        Q_ARG(QString, QString::fromStdString(activeSubstateName)));
                    //                    QMetaObject::invokeMethod(item->getParentStateItem()->setActiveSubstate(QString::fromStdString(activeSubstateName));
                }
            }
 
            {
                //            ARMARX_IMPORTANT << "Got event reported for event: " << event->eventReceiverName;
                auto it = stateMap.find(event.parentStateIdentifier);
                if (it == stateMap.end())
                {
                    ARMARX_VERBOSE << "Could not find state with id "
                                   << event.parentStateIdentifier;
                    return;
                }
                const StateListenerData& data = it->second;
                for (const statechartmodel::StatePtr& state : data.states)
                {
                    statechartmodel::StateCPtr cstate = state;
 
                    std::string eventName = event.eventName;
                    std::string sourceStateName = ExtractStateName(event.sourceStateIdentifier);
                    std::string destinationStateName =
                        ExtractStateName(event.targetStateIdentifier);
 
                    if (state && !eventName.empty() && !destinationStateName.empty())
                    {
                        if (auto t =
                                state->findTransition(QString::fromStdString(eventName),
                                                      QString::fromStdString(sourceStateName),
                                                      QString::fromStdString(destinationStateName)))
                        {
                            ARMARX_DEBUG << "Invoking transition highlight for " << eventName
                                         << " from " << sourceStateName << " to "
                                         << destinationStateName;
                            QMetaObject::invokeMethod(state.get(),
                                                      "setTransitionActivated",
                                                      Q_ARG(statechartmodel::TransitionCPtr, t));
                        }
                        else
                        {
                            auto ct = cstate->getStartTransition();
                            if (ct && eventName == "InitialTransition")
                            {
                                ARMARX_DEBUG << "Invoking inital transition highlight for "
                                             << state->getStateName();
                                QMetaObject::invokeMethod(
                                    state.get(),
                                    "setTransitionActivated",
                                    Q_ARG(statechartmodel::TransitionCPtr, ct));
                            }
                            else
                            {
                                ARMARX_VERBOSE << "Cannot find transition " << eventName
                                               << " for highlighting "
                                               << QString::fromStdString(sourceStateName) << " -> "
                                               << QString::fromStdString(destinationStateName);
                            }
                        }
                    }
                }
                for (auto item : data.stateItems)
                {
                    QString eventName = QString::fromStdString(event.eventName);
                    QString sourceStateName =
                        QString::fromStdString(ExtractStateName(event.sourceStateIdentifier));
                    QString destinationStateName =
                        QString::fromStdString(ExtractStateName(event.targetStateIdentifier));
                    if (item && !eventName.isEmpty() && !destinationStateName.isEmpty())
                    {
                        for (auto t : item->getTransitionItems())
                        {
                            statechartmodel::StateCPtr stateclass =
                                item->getStateInstance()->getStateClass();
                            if ((t->getTransition()->eventName == eventName &&
                                 t->getTransition()->sourceState->getInstanceName() ==
                                     sourceStateName) ||
                                (stateclass &&
                                 t->getTransition() == stateclass->getStartTransition() &&
                                 eventName == "InitialTransition"))
                            {
                                QMetaObject::invokeMethod(
                                    t, "highlightAnimation", Q_ARG(int, 10000));
                                //                                t->highlightAnimation(10000);
                            }
                        }
                    }
                }
            }
        }
        catch (...)
        {
            ARMARX_DEBUG << "updating state " << event.parentStateIdentifier << " failed";
        }
    }
 
    void
    StateWatcher::reportStatechartInputParameters(const ProfilerStatechartParameters& event,
                                                  const Ice::Current&)
    {
        std::unique_lock lock(mapMutex);
        //        ARMARX_INFO << "input update for " << event.stateIdentifier << " content: " << StateUtilFunctions::getDictionaryString(event.parameterMap);
        stateMap[event.stateIdentifier].parameters["inputParameters"] = event.parameterMap;
    }
 
    void
    StateWatcher::reportStatechartLocalParameters(const ProfilerStatechartParameters& event,
                                                  const Ice::Current&)
    {
        std::unique_lock lock(mapMutex);
        //        ARMARX_INFO << "local update for " << event.stateIdentifier << " content: " << StateUtilFunctions::getDictionaryString(event.parameterMap);
        stateMap[event.stateIdentifier].parameters["localParameters"] = event.parameterMap;
    }
 
    void
    StateWatcher::reportStatechartOutputParameters(const ProfilerStatechartParameters& event,
                                                   const Ice::Current&)
    {
        std::unique_lock lock(mapMutex);
        //        ARMARX_INFO << "output update for " << event.stateIdentifier << " content: " << StateUtilFunctions::getDictionaryString(event.parameterMap);
        stateMap[event.stateIdentifier].parameters["outputParameters"] = event.parameterMap;
    }
 
    void
    StateWatcher::reportProcessCpuUsage(const ProfilerProcessCpuUsage&, const Ice::Current&)
    {
    }
 
    void
    StateWatcher::reportProcessMemoryUsage(const ProfilerProcessMemoryUsage&, const Ice::Current&)
    {
    }
 
    void
    StateWatcher::reportEventList(const ProfilerEventList&, const Ice::Current&)
    {
    }
 
    void
    StateWatcher::reportStatechartTransitionList(const ProfilerStatechartTransitionList& data,
                                                 const Ice::Current& c)
    {
        for (auto& elem : data)
        {
            reportStatechartTransition(elem, c);
        }
    }
 
    void
    StateWatcher::reportStatechartInputParametersList(const ProfilerStatechartParametersList& data,
                                                      const Ice::Current& c)
    {
        for (auto& elem : data)
        {
            reportStatechartInputParameters(elem, c);
        }
    }
 
    void
    StateWatcher::reportStatechartLocalParametersList(const ProfilerStatechartParametersList& data,
                                                      const Ice::Current& c)
    {
        for (auto& elem : data)
        {
            reportStatechartLocalParameters(elem, c);
        }
    }
 
    void
    StateWatcher::reportStatechartOutputParametersList(const ProfilerStatechartParametersList& data,
                                                       const Ice::Current& c)
    {
        for (auto& elem : data)
        {
            reportStatechartOutputParameters(elem, c);
        }
    }
 
    void
    StateWatcher::reportProcessCpuUsageList(const ProfilerProcessCpuUsageList&, const Ice::Current&)
    {
    }
 
    void
    StateWatcher::reportProcessMemoryUsageList(const ProfilerProcessMemoryUsageList&,
                                               const Ice::Current&)
    {
    }
} // namespace armarx