ConditionCheck.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::Core
* @author     Kai Welke (welke _at_ kit _dot_ edu)
* @date       2011
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include <ArmarXCore/observers/ConditionCheck.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
#include <ArmarXCore/core/exceptions/Exception.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <algorithm>
#include <cstdarg>
 
template class ::IceInternal::Handle<::armarx::ConditionCheck>;
 
namespace armarx
{
    ConditionCheck::ConditionCheck()
    {
 
        reset();
    }
 
    ConditionCheckPtr ConditionCheck::createInstance(const CheckConfiguration& configuration, const ChannelRegistry& channelRegistry)
    {
        // channel available?
        DataFieldIdentifierBasePtr dataFieldIdentifier = configuration.dataFieldIdentifier;
        ChannelRegistry::const_iterator iterChannels = channelRegistry.find(dataFieldIdentifier->channelName);
 
        if (iterChannels == channelRegistry.end())
        {
            throw InvalidConditionException("Invalid channel \"" + dataFieldIdentifier->channelName + "\" in check " + configuration.checkName);
        }
 
        // datafield available?
        const DataFieldRegistry& dataFieldRegistry = iterChannels->second.dataFields;
        DataFieldRegistry::const_iterator iterData = dataFieldRegistry.find(dataFieldIdentifier->datafieldName);
 
        if (iterData == dataFieldRegistry.end())
        {
            throw InvalidConditionException("Invalid datafield \"" + dataFieldIdentifier->channelName + "." + dataFieldIdentifier->datafieldName + "\" in check " + configuration.checkName);
        }
 
        // types supported by check?
        VariantPtr dataField = VariantPtr::dynamicCast(iterData->second.value);
        assureTypeSupported(configuration, dataField->getType());
 
        // create new instance of check
        ConditionCheckPtr check = this->clone();
        check->configuration = configuration;
 
        return check;
    }
 
    void ConditionCheck::reset()
    {
        firstEval = true;
        numberParameters = 0;
        fulFilled = false;
    }
 
    void ConditionCheck::evaluateCondition(const DataFieldRegistry& dataFields)
    {
        // find data fields that correspond to the current check
        //    DataFieldRegistry::const_iterator iter = dataFields.begin();
 
        StringVariantMap checkDataFields;
 
        // TODO: this loop should be skipped, computational to heavy, is it even possible to install a condition over several datafields? (I dont think so)
        //    while (iter != dataFields.end())
        //    {
        //        DatafieldRefPtr ref = DatafieldRefPtr::dynamicCast(iter->second.identifier);
        //        ARMARX_CHECK_EXPRESSION(ref);
 
        //        if (ref->getDataFieldIdentifier()->beginsWith(*DataFieldIdentifierPtr::dynamicCast(configuration.dataFieldIdentifier)))
        //        {
        //            checkDataFields[iter->second.identifier->datafieldName] = *VariantPtr::dynamicCast(iter->second.value);
        //        }
 
        //        iter++;
        //    }
 
        auto iter = dataFields.find(configuration.dataFieldIdentifier->datafieldName);
        if (iter != dataFields.end())
        {
            checkDataFields[iter->second.identifier->datafieldName] = *VariantPtr::dynamicCast(iter->second.value);
        }
        if (checkDataFields.size() == 0)
        {
            return;
        }
 
        // evaluate condition
        bool oldFulFilled = fulFilled;
        fulFilled = evaluate(checkDataFields);
 
        // if status changed, inform listener
        if (configuration.reportAlways || oldFulFilled != fulFilled || firstEval || configuration.reportDatafieldValues)
        {
            if (oldFulFilled != fulFilled)
            {
                ARMARX_DEBUG_S << "Evaluating check: '" << configuration.checkName << "' - status changed (" << oldFulFilled << " -> " << fulFilled << ")" << std::endl;
            }
            else if (firstEval)
            {
                ARMARX_DEBUG_S << "First evaluating of check: '" << configuration.checkName << "' is " <<  fulFilled;
            }
            else
            {
                ARMARX_DEBUG_S << "Unchanged value of check: '" << configuration.checkName << "' is " <<  fulFilled;
            }
 
 
            firstEval = false;
            if (configuration.reportDatafieldValues)
            {
                configuration.listener->setValueAndData(fulFilled, configuration.dataFieldIdentifier, iter->second.value);
            }
            else
            {
                configuration.listener->setValue(fulFilled);
            }
        }
    }
 
    bool ConditionCheck::getFulFilled()
    {
        return fulFilled;
    }
 
    const Variant& ConditionCheck::getParameter(int index)
    {
        if (index >= numberParameters)
        {
            throw LocalException("Illegal parameter requested\n");
        }
 
        VariantPtr var = VariantPtr::dynamicCast(configuration.checkParameters.at(index));
 
        if (var->getType() == VariantType::DatafieldRef) // get datafield content
        {
            DatafieldRefPtr dfr = var->getClass<DatafieldRef>();
            //        ARMARX_VERBOSE_S << "Turning Datafield " << dfr->getDataFieldIdentifier()->getIdentifierStr() << "into variant";
            var = dfr->getDataField();
        }
 
        return *var;
    }
 
    void ConditionCheck::assureTypeSupported(const CheckConfiguration& configuration, VariantTypeId dataFieldType)
    {
 
        SupportedTypeList::iterator iter = supportedTypes.begin();
 
        bool bValidType = false;
 
        while (iter != supportedTypes.end())
        {
            VariantTypeId supportedType = iter->dataFieldType;
 
            // 0 means: all types are supported
            if (supportedType == 0)
            {
                bValidType = true;
                break;
            }
            else if (supportedType == dataFieldType)
            {
                if (configuration.checkParameters.size() != iter->parameterTypes.size())
                {
                    throw InvalidConditionException("Wrong number of parameters for condition in check  " + configuration.checkName);
                }
 
                ParameterTypeList::iterator iter_param = iter->parameterTypes.begin();
                bool bParameterValid = true;
                int p = 0;
 
                while (iter_param != iter->parameterTypes.end())
                {
 
 
                    VariantTypeId paramType = configuration.checkParameters.at(p)->getType();
 
                    if (paramType == VariantType::DatafieldRef) // get datafield and check for type of the datafield
                    {
                        VariantPtr var = VariantPtr::dynamicCast(configuration.checkParameters.at(p));
                        DatafieldRefPtr dfr = var->getClass<DatafieldRef>();
 
                        if (!dfr->validate())
                        {
                            bParameterValid = false;
                            break;
                        }
 
                        //                    ARMARX_VERBOSE_S << dfr->getDataFieldIdentifier()->getIdentifierStr() << " has type: " << dfr->getDataField()->getType();
                        paramType = dfr->getDataField()->getType();
                    }
 
                    if (*iter_param != paramType)
                    {
                        bParameterValid = false;
                        break;
                    }
 
                    p++;
                    iter_param++;
                }
 
                if (bParameterValid)
                {
                    bValidType = true;
                    break;
                }
            }
 
            iter++;
        }
 
        if (!bValidType)
        {
            throw InvalidConditionException("Invalid types for check " + configuration.checkName + " Type: " + Variant::typeToString(dataFieldType));
        }
    }
 
    void ConditionCheck::setNumberParameters(int numberParameters)
    {
        this->numberParameters = numberParameters;
    }
 
    void ConditionCheck::addSupportedType(VariantTypeId dataFieldType, ParameterTypeList parameterTypes)
    {
        SupportedType type;
        type.dataFieldType = dataFieldType;
        type.parameterTypes = parameterTypes;
 
        supportedTypes.push_back(type);
    }
 
    ParameterTypeList ConditionCheck::createParameterTypeList(int numberTypes, ...)
    {
        va_list ap;
        va_start(ap, numberTypes);
 
        ParameterTypeList params;
 
        for (int p = 0 ; p < numberTypes; p++)
        {
            params.push_back((VariantTypeId) va_arg(ap, int));
        }
 
        va_end(ap);
 
        return params;
    }
}