ParameterMapping.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 "ParameterMapping.h"
 
#include <ArmarXCore/observers/variant/DataFieldIdentifier.h>
 
#include "Exception.h"
#include "StateParameter.h"
#include "StateUtilFunctions.h"
#include "StatechartContext.h"
 
namespace armarx
{
 
    StatechartContext* ParameterMapping::__context = nullptr;
 
    ParameterMapping::ParameterMapping(const ParameterMapping& source) :
        IceUtil::Shared(source),
        Ice::Object(source),
        ParameterMappingIceBase(source),
        Logging(source)
    {
        *this = source;
    }
 
    void
    ParameterMapping::_applyMapping(StateParameterMap& targetDictionary)
    {
        if (priorities.size() == 0)
        {
            _addMissingSources(priorities);
        }
 
        if (__greedyInput)
        {
            PriorityMap::reverse_iterator it = priorities.rbegin();
 
            for (; it != priorities.rend(); ++it)
            {
                SourceDictionaryMap::iterator itSourceDict = sourceDictionaries.find(it->second);
 
                if (itSourceDict != sourceDictionaries.end())
                {
                    _greedyMapping(targetDictionary, itSourceDict->second);
                }
            }
        }
 
        PriorityMap::iterator it = priorities.begin();
 
        // iterate from start (low priority) and overwrite values later if found in higher
        // priority dictionaries as well
        for (; it != priorities.end(); ++it)
        {
            __fillFromMappingSource(it->second, targetDictionary);
        }
    }
 
    void
    ParameterMapping::__fillFromMappingSource(MappingSource mappingSource,
                                              StateParameterMap& targetDictionary)
    {
        if (mappingSource == eDataField)
        {
            // get data directly from the observer
            _fillFromDataField(targetDictionary);
        }
        else if (mappingSource == eValue)
        {
            // get direct value mappings
            _fillFromValues(targetDictionary);
        }
        else
        {
            // get data from source dictionaries
 
            // select which source dict to use
            SourceDictionaryMap::iterator itSourceDict = sourceDictionaries.find(mappingSource);
 
            if (itSourceDict != sourceDictionaries.end())
            {
                for (StateParameterMap::iterator itTargetDict = targetDictionary.begin();
                     itTargetDict != targetDictionary.end();
                     itTargetDict++)
                {
                    // get the specific entry (if existing) in the source dict for the selected target dict entry
                    StringVariantContainerBaseMap::const_iterator itSourceEntry = _hasMappingEntry(
                        itTargetDict->first, itSourceDict->second, itSourceDict->first);
 
                    if (itSourceEntry != itSourceDict->second.end())
                    {
                        __copyDataFromSourceDict(itSourceEntry, itTargetDict);
                    }
                }
            }
        }
    }
 
    void
    ParameterMapping::__copyDataFromSourceDict(
        StringVariantContainerBaseMap::const_iterator& itSourceEntry,
        StateParameterMap::iterator itTargetEntry)
    {
        if (itSourceEntry->second->getSize() > 0)
        {
            if (!VariantContainerType::compare(itTargetEntry->second->value->getContainerType(),
                                               itSourceEntry->second->getContainerType()))
            {
                throw exceptions::local::eStatechartLogicError(
                    "Cannot map different types: " +
                    VariantContainerType::allTypesToString(
                        itTargetEntry->second->value->getContainerType()) +
                    " (" + itTargetEntry->first + ") vs. " +
                    VariantContainerType::allTypesToString(
                        itSourceEntry->second->getContainerType()) +
                    " (" + itSourceEntry->first + ")");
            }
        }
 
        {
            //            // copy data from source dictionary
            //            if (!VariantContainerType::compare(itTargetEntry->second->value->getContainerType(), itSourceEntry->second->getContainerType()))
            //                throw exceptions::local::eStatechartLogicError("Cannot map parameter! The variant type of targetkey '" + itTargetEntry->first + "' does not match sourcekey '" + itSourceEntry->first + "' ("
            //                        + VariantContainerType::allTypesToString(itTargetEntry->second->value->getContainerType()) + " vs. " + VariantContainerType::allTypesToString(itSourceEntry->second->getContainerType()) + ")");
 
            itTargetEntry->second->value = itSourceEntry->second; //->cloneContainer();
        }
 
 
        itTargetEntry->second->set = true;
    }
 
    void
    ParameterMapping::_greedyMapping(StateParameterMap& targetDictionary,
                                     StringVariantContainerBaseMap& sourceDictionary)
    {
        for (StringVariantContainerBaseMap::const_iterator it = sourceDictionary.begin();
             it != sourceDictionary.end();
             it++)
        {
            StateParameterPtr param = StateParameter::create();
            param->value = it->second->cloneContainer();
            param->optionalParam = false;
            param->set = true;
 
            targetDictionary[it->first] = param;
        }
    }
 
    void
    ParameterMapping::_fillFromDataField(StateParameterMap& targetDictionary)
    {
        std::map<std::string,
                 std::pair<DataFieldIdentifierBaseList, std::vector<StateParameterPtr>>>
            observerSplittedMap;
 
        //first split required parameters by observer
        MappingList::const_iterator itMapping;
 
        for (itMapping = mappings.begin(); itMapping != mappings.end(); ++itMapping)
        {
            if (itMapping->mappingSrc == eDataField)
            {
                StateParameterMap::iterator itTargetDict =
                    targetDictionary.find(itMapping->targetKey);
 
                if (itTargetDict != targetDictionary.end())
                {
                    DataFieldIdentifierBasePtr id = new DataFieldIdentifier(itMapping->sourceKey);
 
                    observerSplittedMap[id->observerName].first.push_back(id);
                    observerSplittedMap[id->observerName].second.push_back(
                        StateParameterPtr::dynamicCast(itTargetDict->second));
                }
            }
        }
 
        //now get all the data from each observer in a bulk and set the stateparameterpointers
        if (!__context && observerSplittedMap.size() != 0)
        {
            throw LocalException("StatechartContext Pointer is NULL");
        }
 
        std::map<std::string,
                 std::pair<DataFieldIdentifierBaseList, std::vector<StateParameterPtr>>>::iterator
            itObservers = observerSplittedMap.begin();
 
        for (; itObservers != observerSplittedMap.end(); ++itObservers)
        {
            std::string observerName = itObservers->first;
            std::vector<StateParameterPtr>& paramList = itObservers->second.second;
            TimedVariantBaseList variants =
                __context->getDataListFromObserver(observerName, itObservers->second.first);
 
            for (unsigned int i = 0; i < variants.size() && i < paramList.size(); ++i)
            {
                if (!variants.at(i)->getInitialized())
                {
                    continue;
                }
                const Variant& data = *VariantPtr::dynamicCast(variants.at(i));
                paramList.at(i)->value = new SingleVariant(data);
                paramList.at(i)->set = true;
            }
        }
    }
 
    void
    ParameterMapping::_fillFromValues(StateParameterMap& targetDictionary)
    {
        MappingList::const_iterator itMapping;
 
        for (itMapping = mappings.begin(); itMapping != mappings.end(); ++itMapping)
        {
            if (itMapping->mappingSrc == eValue)
            {
                StateParameterMap::iterator itTargetDict =
                    targetDictionary.find(itMapping->targetKey);
 
                if (itTargetDict != targetDictionary.end())
                {
                    itTargetDict->second->value = itMapping->value;
                    itTargetDict->second->set = true;
                }
            }
        }
    }
 
    ParameterMappingPtr
    createMapping()
    {
        return PM::createMapping();
    }
 
    Ice::ObjectPtr
    ParameterMapping::ice_clone() const
    {
        Ice::ObjectPtr ptr = new ParameterMapping(*this);
        return ptr;
    }
 
    ParameterMappingPtr
    ParameterMapping::clone() const
    {
        return ParameterMappingPtr::dynamicCast(ice_clone());
        //        ParameterMappingPtr ptr = new ParameterMapping(*this);
        //        return ptr;
    }
 
    ParameterMapping&
    ParameterMapping::operator=(const ParameterMapping& rhs)
    {
        useParentsInput = rhs.useParentsInput;
        __greedyInput = rhs.__greedyInput;
        priorities = rhs.priorities;
        mappings = rhs.mappings;
        sourceDictionaries.clear();
 
        for (SourceDictionaryMap::const_iterator it = rhs.sourceDictionaries.begin();
             it != rhs.sourceDictionaries.end();
             ++it)
        {
            StateUtilFunctions::copyDictionary(it->second, sourceDictionaries[it->first]);
        }
 
        return *this;
    }
 
    std::string
    ParameterMapping::MappingSourceToString(MappingSource mappingSource)
    {
        switch (mappingSource)
        {
            case eParent:
                return "Parent";
 
            case eOutput:
                return "Output";
 
            case eDataField:
                return "DataField";
 
            case eEvent:
                return "Event";
 
            case eValue:
                return "Value";
 
            default:
                return "Undefined";
        }
    }
 
    MappingSource
    ParameterMapping::StringToMappingSource(const std::string& mappingSourceString)
    {
        if (mappingSourceString == "Parent")
        {
            return eParent;
        }
        else if (mappingSourceString == "Output")
        {
            return eOutput;
        }
        else if (mappingSourceString == "DataField")
        {
            return eDataField;
        }
        else if (mappingSourceString == "Event")
        {
            return eEvent;
        }
        else if (mappingSourceString == "Value")
        {
            return eValue;
        }
        else
        {
            return eMappingSourcesCount;
        }
    }
 
    StringVariantContainerBaseMap::const_iterator
    ParameterMapping::_hasMappingEntry(const std::string& keyDestination,
                                       const StringVariantContainerBaseMap& sourceDict,
                                       MappingSource allowedMappingSource)
    {
        StringVariantContainerBaseMap::const_iterator result;
        Ice::StringSeq fieldsDest = _getFields(keyDestination);
 
 
        // first: find entry in mapping that fits somehow to keyDestination
        //        e.g. 'State1.*' fits to 'State1.angle', '*' fits to 'State1.angle', 'angle' does NOT fit to 'State1.angle'
        //        Note: '*.*' is same as '*'
        int destWildcardIndex = -1;
        MappingList::const_iterator it;
 
        for (it = mappings.begin(); it != mappings.end(); ++it)
        {
            if (it->mappingSrc != allowedMappingSource)
            {
                continue;
            }
 
            bool found = true;
            Ice::StringSeq mappingFields = _getFields(it->targetKey);
 
            for (unsigned int i = 0; i < fieldsDest.size(); ++i)
            {
                if (i >= mappingFields.size())
                {
                    break;
                }
 
                if (mappingFields.at(i) == "*")
                {
                    destWildcardIndex = i;
                }
 
                // check if mapping fields fit to fields of key of destination map
                if (!(mappingFields.at(i) == "*" || mappingFields.at(i) == fieldsDest.at(i)))
                {
                    found = false;
                    break;
                }
            }
 
            if (found)
            {
                // second: find key in SourceDict that fits to the 2nd parameter of the selected mapping entry
                result = _findSourceEntry(it->sourceKey, sourceDict, destWildcardIndex, fieldsDest);
 
                if (result != sourceDict.end())
                {
                    return result;
                }
            }
        }
 
        if (it == mappings.end())
        {
            result = sourceDict.end(); // not found
        }
 
        return result;
    }
 
    StringVariantContainerBaseMap::const_iterator
    ParameterMapping::_findSourceEntry(const std::string sourceKey,
                                       const StringVariantContainerBaseMap& sourceDict,
                                       int destWildcardIndex,
                                       const Ice::StringSeq& fieldsDest)
    {
        StringVariantContainerBaseMap::const_iterator result = sourceDict.end();
        Ice::StringSeq mappingFields = _getFields(sourceKey);
 
        for (result = sourceDict.begin(); result != sourceDict.end(); ++result)
        {
            Ice::StringSeq fieldsSrcDict = _getFields(result->first);
            bool found = true;
 
            for (unsigned int i = 0; i < fieldsSrcDict.size(); ++i)
            {
                if (i >= mappingFields.size())
                {
                    break;
                }
 
                assert(i < fieldsSrcDict.size());
 
                if (mappingFields.at(i) == "*")
                {
                    // check if all fields match after the wildcard
                    for (int j = 0; j < int(fieldsSrcDict.size()) - int(i); ++j)
                    {
                        assert(i + j < fieldsSrcDict.size());
                        assert(destWildcardIndex + j < (int)fieldsDest.size());
 
                        if (fieldsSrcDict.at(i + j) != fieldsDest.at(destWildcardIndex + j))
                        {
                            found = false;
                            break;
                        }
                    }
 
                    if (!found)
                    {
                        break;
                    }
                }
                // check if the fields do match exactly
                else if (mappingFields.at(i) != fieldsSrcDict.at(i))
                {
                    found = false;
                    break;
                }
            }
 
            if (found)
            {
                return result;
            }
        }
 
        return result;
    }
 
    Ice::StringSeq
    ParameterMapping::_getFields(std::string source, char seperator)
    {
        std::vector<std::string> result;
        size_t pos;
 
        while ((pos = source.find(seperator)) != std::string::npos)
        {
            result.push_back(source.substr(0, pos));
            source = source.substr(pos + 1);
        }
 
        result.push_back(source);
        return result;
    }
 
    void
    ParameterMapping::_addMissingSources(PriorityMap& priorityMap)
    {
        int lowestPriority = 0;
 
        if (priorityMap.begin() != priorityMap.end())
        {
            lowestPriority = priorityMap.begin()->first;
        }
 
        bool found = false;
 
        for (int i = 0; i < (int)eMappingSourcesCount; ++i)
        {
            PriorityMap::iterator it = priorityMap.begin();
 
            for (; it != priorityMap.end(); ++it)
            {
                if (int(it->second) == i)
                {
                    found = true;
                }
            }
 
            if (!found)
            {
                priorityMap[--lowestPriority] = MappingSource(i);
            }
        }
    }
 
    //    ParameterMappingPtr
    //    ParameterMapping::addTuple(const std::string & sourceKey,
    //                               const std::string & targetKey){
    //        Ice::StringSeq sourceKeySeq = getFields(sourceKey);
    //        Ice::StringSeq targetKeySeq = getFields(targetKey);
    //        if((*sourceKeySeq.rbegin()=="*" && *targetKeySeq.rbegin() != "*")
    //            || (*sourceKeySeq.rbegin()!="*" && *targetKeySeq.rbegin() == "*"))
    //            throw exceptions::local::eLogicError("A parameter mapping cannot map from a wildcard to a specific key or vice versa. Either both keys must be wildcards or none.");
    //        paramMapping[targetKey] = sourceKey;
    //        return this;
    //    }
 
    ParameterMappingPtr
    ParameterMapping::setSourcePriority(int priorityLevel,
                                        MappingSource mappingSrc,
                                        const Ice::Current& c)
    {
        if (priorities.find(priorityLevel) != priorities.end())
        {
            std::stringstream stream;
            stream << "Priority level " << priorityLevel << " already exists!";
            throw exceptions::local::eStatechartLogicError(stream.str());
        }
 
        priorities[priorityLevel] = mappingSrc;
        return this;
    }
 
    ParameterMappingPtr
    ParameterMapping::_addSourceDictionary(MappingSource mappingSrc,
                                           const StringVariantContainerBaseMap& sourceDict,
                                           const Ice::Current& c)
    {
        //        StateUtilFunctions::copyDictionary(sourceDict, sourceDictionaries[mappingSrc]);
        sourceDictionaries[mappingSrc] = sourceDict;
        return this;
    }
 
    void
    ParameterMapping::addMappingEntry(MappingSource mappingSource,
                                      const std::string& sourceKey,
                                      const std::string& targetKey,
                                      VariantContainerBasePtr value)
    {
        if (!value && mappingSource == eValue)
        {
            return;
        }
        MappingEntry entry;
        entry.mappingSrc = mappingSource;
        entry.targetKey = targetKey;
        entry.sourceKey = sourceKey;
        entry.value = value;
        mappings.push_back(entry);
    }
 
    ParameterMappingPtr
    ParameterMapping::createMapping()
    {
        ParameterMappingPtr ptr = new ParameterMapping();
        return ptr;
    }
 
    ParameterMappingPtr
    ParameterMapping::mapFromOutput(const std::string& sourceKey,
                                    const std::string& targetKey,
                                    const Ice::Current& c)
    {
        addMappingEntry(eOutput, sourceKey, targetKey);
        return this;
    }
 
    ParameterMappingPtr
    ParameterMapping::mapFromOutput(const std::string& bothKeys, const Ice::Current& c)
    {
        addMappingEntry(eOutput, bothKeys, bothKeys);
        return this;
    }
 
    ParameterMappingPtr
    ParameterMapping::mapFromParent(const std::string& parentKey,
                                    const std::string& targetKey,
                                    const Ice::Current& c)
    {
        addMappingEntry(eParent, parentKey, targetKey);
        return this;
    }
 
    ParameterMappingPtr
    ParameterMapping::mapFromParent(const std::string& bothKeys, const Ice::Current& c)
    {
        addMappingEntry(eParent, bothKeys, bothKeys);
        return this;
    }
 
    ParameterMappingPtr
    ParameterMapping::mapFromEvent(const std::string& eventKey,
                                   const std::string& targetKey,
                                   const Ice::Current& c)
    {
        addMappingEntry(eEvent, eventKey, targetKey);
        return this;
    }
 
    ParameterMappingPtr
    ParameterMapping::mapFromEvent(const std::string& bothKeys, const Ice::Current& c)
    {
        addMappingEntry(eEvent, bothKeys, bothKeys);
        return this;
    }
 
    ParameterMappingPtr
    ParameterMapping::mapFromDataField(const DataFieldIdentifierBasePtr& dataFieldIdentifier,
                                       const std::string& targetKey,
                                       const Ice::Current& c)
    {
        DataFieldIdentifierPtr id = DataFieldIdentifierPtr::dynamicCast(dataFieldIdentifier);
        addMappingEntry(eDataField, id->getIdentifierStr(), targetKey);
        return this;
    }
 
    ParameterMappingPtr
    ParameterMapping::setTargetDictToGreedy(bool on)
    {
        __greedyInput = on;
        return this;
    }
 
    void
    ParameterMapping::_setStatechartContext(StatechartContext* context)
    {
        ParameterMapping::__context = context;
    }
} // namespace armarx
 
std::ostream&
std::operator<<(std::ostream& stream, const armarx::ParameterMapping& mapping)
{
    for (const ::armarx::MappingEntry& elem : mapping.mappings)
    {
        stream << armarx::ParameterMapping::MappingSourceToString(elem.mappingSrc) << ": "
               << elem.sourceKey << " -> " << elem.targetKey << "\n";
    }
 
    return stream;
}