StateUtilFunctions.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    ArmarX::
* @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 "StateUtilFunctions.h"
 
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/observers/variant/ChannelRef.h>
#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
#include <ArmarXCore/util/json/JSONObject.h>
 
#include "Exception.h"
#include "StateParameter.h"
 
using namespace armarx;
 
std::string
StateUtilFunctions::transitionErrorToString(TransitionErrorType type)
{
    switch (type)
    {
        case eTransitionErrorUnexpectedEvent:
            return "Unexpected Event";
            break;
 
        case eTransitionErrorInput:
            return "Invalid input parameters";
            break;
 
        default:
            return "undefined";
    }
}
 
StringVariantContainerBaseMap
StateUtilFunctions::getSetValues(const StateParameterMap& paramMap)
{
    StringVariantContainerBaseMap setValueMap;
 
    for (StateParameterMap::const_iterator it = paramMap.begin(); it != paramMap.end(); it++)
    {
        if (it->second->set)
        {
            setValueMap[it->first] = it->second->value; //->cloneContainer();
        }
    }
 
    return setValueMap;
}
 
StringVariantContainerBaseMap
StateUtilFunctions::getValues(const StateParameterMap& paramMap)
{
    StringVariantContainerBaseMap ValueMap;
 
    for (StateParameterMap::const_iterator it = paramMap.begin(); it != paramMap.end(); it++)
    {
        ValueMap[it->first] = it->second->value->cloneContainer();
    }
 
    return ValueMap;
}
 
bool
StateUtilFunctions::addToDictionary(EventPtr event, const std::string key, const Variant& value)
{
    bool result = true;
    StringVariantContainerBaseMap& dictionary = event->properties;
    StringVariantContainerBaseMap::iterator it = dictionary.find(key);
 
    if (it != dictionary.end())
    {
        result = false;
    }
 
    dictionary[key] = new SingleVariant(value);
    return result;
}
 
bool
StateUtilFunctions::addToDictionary(EventPtr event,
                                    const std::string key,
                                    const SingleTypeVariantListPtr& valueList)
{
    bool result = true;
    StringVariantContainerBaseMap& dictionary = event->properties;
    StringVariantContainerBaseMap::iterator it = dictionary.find(key);
 
    if (it != dictionary.end())
    {
        result = false;
    }
 
    dictionary[key] = valueList->cloneContainer();
 
    return result;
}
 
bool
StateUtilFunctions::equalKeys(const StateParameterMap& dict1, const StateParameterMap& dict2)
{
    //        ARMARX_INFO << "dict1" << dict1.size() << " dict2: " << dict2.size() << std::endl;
    bool equal = true;
 
    for (StateParameterMap::const_iterator it = dict1.begin(); it != dict1.end(); it++)
    {
        StateParameterMap::const_iterator it2 = dict2.find(it->first);
 
        if (it2 == dict2.end() ||
            VariantContainerType::compare(it2->second->value->getContainerType(),
                                          it->second->value->getContainerType()))
        {
            equal = false;
        }
 
        break;
    }
 
    return equal;
}
 
bool
StateUtilFunctions::equalKeys(const StringVariantContainerBaseMap& dict1,
                              const StringVariantContainerBaseMap& dict2)
{
    bool equal = true;
 
    for (StringVariantContainerBaseMap::const_iterator it = dict1.begin(); it != dict1.end(); it++)
    {
        StringVariantContainerBaseMap::const_iterator it2 = dict2.find(it->first);
 
        if (it2 == dict2.end() || VariantContainerType::compare(it2->second->getContainerType(),
                                                                it->second->getContainerType()))
        {
            equal = false;
            break;
        }
    }
 
    return equal;
}
 
void
StateUtilFunctions::fillDictionary(const StringVariantContainerBaseMap& source,
                                   StringVariantContainerBaseMap& destination)
{
    for (StringVariantContainerBaseMap::const_iterator it = source.begin(); it != source.end();
         it++)
    {
        StringVariantContainerBaseMap::iterator itDest = destination.find(it->first);
 
        if (itDest != destination.end())
        {
            *itDest->second = *it->second;
        }
    }
}
 
void
StateUtilFunctions::fillDictionary(const StringVariantContainerBaseMap& source,
                                   StateParameterMap& destination)
{
    for (StringVariantContainerBaseMap::const_iterator it = source.begin(); it != source.end();
         it++)
    {
        StateParameterMap::iterator itDest = destination.find(it->first);
 
        if (itDest != destination.end())
        {
            itDest->second->value = it->second->cloneContainer();
            itDest->second->set = true;
        }
    }
}
 
void
StateUtilFunctions::copyDictionary(const StringVariantContainerBaseMap& source,
                                   StringVariantContainerBaseMap& destination)
{
    destination.clear();
 
    for (StringVariantContainerBaseMap::const_iterator it = source.begin(); it != source.end();
         it++)
    {
        destination[it->first] = it->second->cloneContainer();
    }
}
 
void
StateUtilFunctions::copyDictionary(const StateParameterMap& source, StateParameterMap& destination)
{
    destination.clear();
 
    for (StateParameterMap::const_iterator it = source.begin(); it != source.end(); it++)
    {
        destination[it->first] = StateParameterIceBasePtr::dynamicCast(it->second->ice_clone());
    }
}
 
void
StateUtilFunctions::copyDictionary(const StateParameterMap& source,
                                   StringVariantContainerBaseMap& destination)
{
    destination.clear();
 
    for (StateParameterMap::const_iterator it = source.begin(); it != source.end(); it++)
    {
        destination[it->first] = it->second->value->cloneContainer();
    }
}
 
void
StateUtilFunctions::copyDictionary(const StringVariantContainerBaseMap& source,
                                   StateParameterMap& destination)
{
    assert(0);
    destination.clear();
 
    for (StringVariantContainerBaseMap::const_iterator it = source.begin(); it != source.end();
         it++)
    {
        StateParameterPtr param = StateParameter::create();
        param->value = it->second->cloneContainer();
        param->optionalParam = false;
        param->set = true;
        destination[it->first] = param;
    }
}
 
std::string
StateUtilFunctions::getDictionaryString(const StringVariantContainerBaseMap& mymap)
{
    std::stringstream result;
    int i = 0;
 
    for (StringVariantContainerBaseMap::const_iterator it = mymap.begin(); it != mymap.end(); it++)
    {
        VariantPtr var;
        result << "\t" << it->first << " ("
               << VariantContainerType::allTypesToString(it->second->getContainerType()) << "): ";
 
        if (!it->second->getContainerType()->subType)
        {
            SingleVariantPtr sVar = SingleVariantPtr::dynamicCast(it->second);
 
            if (!sVar)
            {
                throw LocalException("Could not cast to SingleVariant");
            }
 
            var = sVar->get();
            result << var->getOutputValueOnly() << "\n";
        }
        else
        {
            JSONObjectPtr json = new JSONObject();
            json->serializeIceObject(it->second);
            result << json->asString(true) << "\n";
        }
 
        //            break;
        //        case eVariantListParam:
        //        {
        //            SingleTypeVariantListBasePtr listPtr = it->second->getVariantList();
        //            result <<  "\t#" << i << it->first.c_str() << " (VariantList)" <<std::endl;
        //            for(int j = 0; j < listPtr->getSize(); ++j )
        //            {
        //                var = VariantPtr::dynamicCast(listPtr->getElementBase(j));
        //                result << "\t\t#" << j << " " << getVariantString(var) << "\n";
        //            }
        //            break;
        //        }
        //        default:
        //            break;
        //        }
        i++;
    }
 
    return result.str();
}
 
std::string
StateUtilFunctions::getDictionaryString(const StateParameterMap& mymap)
{
 
    std::stringstream result;
    int i = 0;
 
    for (StateParameterMap::const_iterator it = mymap.begin(); it != mymap.end(); it++)
    {
        VariantPtr var;
        std::string setStr;
 
        if (it->second->set)
        {
            setStr = "set";
        }
        else
        {
            setStr = "not set";
        }
 
        if (!it->second->value->getContainerType()->subType)
        {
            //        case eVariantParam:
            SingleVariantPtr sVar = SingleVariantPtr::dynamicCast(it->second->value);
 
            if (!sVar)
            {
                throw LocalException("Could not cast to SingleVariant");
            }
 
            var = sVar->get();
 
            if (!var)
            {
                ARMARX_WARNING_S << "Variant is null";
            }
 
            result << "\t#" << i << " " << getVariantString(var, it->first) << " (" << setStr
                   << ")\n";
            //            break;
        }
        else
        {
            JSONObjectPtr json = new JSONObject();
            json->serializeIceObject(it->second->value);
            result << "\t#" << i << " " << it->first << ": " << json->asString(true) << "\n";
        }
 
        //        case eVariantListParam:
        //        {
        //            SingleTypeVariantListBasePtr listPtr = it->second->value->getVariantList();
        //            result <<  "\t#" << i << it->first.c_str() << " (VariantList)" << " (" << setStr << ")" <<std::endl;
        //            for(int j = 0; j < listPtr->getSize(); ++j)
        //            {
        //                var = VariantPtr::dynamicCast(listPtr->getElementBase(j));
        //                result << "\t\t#" << j << " " << getVariantString(var) << "\n";
        //            }
        //            break;
        //        }
        //        default:
        //            break;
        //        }
        i++;
    }
 
    return result.str();
}
 
bool
StateUtilFunctions::checkForCompleteParameters(const StateParameterMap& paramMap,
                                               std::string* logOutput)
{
    bool result = true;
 
    for (StateParameterMap::const_iterator it = paramMap.begin(); it != paramMap.end(); it++)
    {
        if (!it->second->optionalParam && !it->second->set)
        {
            if (logOutput)
            {
                *logOutput += "\tRequired parameter '" + it->first + "' was not set.\n";
            }
 
            result = false;
        }
        else if (!it->second->optionalParam || it->second->set)
        {
            try
            {
                result &= it->second->value->validateElements();
            }
            catch (exceptions::local::IncompleteTypeException& e)
            {
                ARMARX_DEBUG_S << "Incomplete type for key: " << it->first << " - " << e.what();
            }
        }
    }
 
    //    result &= waitForChannelRefs(paramMap);
 
 
    return result;
}
 
bool
StateUtilFunctions::waitForChannelRefs(const StateParameterMap& paramMap)
{
    IceUtil::Int64 timeoutDura = 4000;
 
 
    bool allInitialized = true;
    int waitIntervallMs = 10;
    IceUtil::Time waitStartTime = IceUtil::Time::now();
 
    IceUtil::Time timeout = IceUtil::Time::milliSeconds(timeoutDura);
    std::vector<std::string> notInitList;
 
    IceUtil::Time lastOutput = IceUtil::Time::milliSeconds(0);
 
    while (!allInitialized &&
           IceUtil::Time(waitStartTime + timeout - IceUtil::Time::now()).toMilliSeconds() > 0)
    {
        allInitialized = true;
        notInitList.clear();
 
        for (StateParameterMap::const_iterator it = paramMap.begin(); it != paramMap.end(); it++)
        {
            StateParameterPtr param = StateParameterPtr::dynamicCast(it->second);
 
            if (param->value->getContainerType()->typeId ==
                Variant::typeToString(VariantType::ChannelRef))
            {
                if (!VariantPtr::dynamicCast(param->value)
                         ->getClass<ChannelRef>()
                         ->getInitialized())
                {
                    allInitialized = false;
                    notInitList.push_back(it->first);
                }
            }
        }
 
        if (!allInitialized && IceUtil::Time(IceUtil::Time::now() - lastOutput).toSeconds() >=
                                   5) // output only every 5 seconds
        {
            ARMARX_VERBOSE_S << "Waiting for ChannelRef(s):  " << notInitList << flush;
        }
 
        usleep(waitIntervallMs * 1000);
 
        if (waitIntervallMs < timeoutDura * 0.5f) // increase wait time for lower cpu usage
        {
            waitIntervallMs *= 2;
        }
    }
 
    if (allInitialized)
    {
        return true;
    }
    else
    {
        return false;
    }
}
 
void
StateUtilFunctions::unsetParameters(StateParameterMap& paramMap)
{
    StateParameterMap::iterator it = paramMap.begin();
 
    for (; it != paramMap.end(); ++it)
    {
        it->second->set = false;
    }
}
 
std::string
StateUtilFunctions::getVariantString(const VariantBasePtr& var, const std::string& name)
{
    return getVariantString(VariantPtr::dynamicCast(var), name);
}
 
std::string
StateUtilFunctions::getVariantString(const VariantPtr& var, const std::string& name)
{
    std::stringstream str;
    str << name << ":\n" << var;
    return str.str();
}