ConditionCheckEqualsWithTolerance.h

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     Mirko Waechter < waechter at kit dot edu >
* @date       2012
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
 
#pragma once
 
#include <ArmarXCore/core/system/ImportExport.h>
#include <ArmarXCore/observers/ConditionCheck.h>
 
namespace armarx
{
    /**
     * \ingroup ConditionChecks
     *
     * Checks if the numbers published in the relevant data fields equal reference value with respect to a tolerance.
     *
     * Parameters: The reference value and the tolerance
     * Supported types: Int, Float, Double
     */
    class ARMARXCORE_IMPORT_EXPORT ConditionCheckEqualsWithTolerance :
        public ConditionCheck
    {
    public:
        ConditionCheckEqualsWithTolerance()
        {
            setNumberParameters(2);
 
            addSupportedType(VariantType::Int, createParameterTypeList(2, VariantType::Int, VariantType::Int));
            addSupportedType(VariantType::Long, createParameterTypeList(2, VariantType::Long, VariantType::Long));
            addSupportedType(VariantType::Float, createParameterTypeList(2, VariantType::Float, VariantType::Float));
            addSupportedType(VariantType::Double, createParameterTypeList(2, VariantType::Double, VariantType::Double));
        }
 
        ConditionCheck* clone() override
        {
            return new ConditionCheckEqualsWithTolerance(*this);
        }
 
        bool evaluate(const StringVariantMap& dataFields) override
        {
            if (dataFields.size() != 1)
            {
                printf("Size of dataFields: %d\n", (int)dataFields.size());
                throw InvalidConditionException("Wrong number of datafields for condition equals ");
            }
 
            const Variant& value = dataFields.begin()->second;
            VariantTypeId type = value.getType();
 
            if (type == VariantType::Int)
            {
                return value.getInt() >= getParameter(0).getInt() - getParameter(1).getInt() &&  value.getInt() <= getParameter(0).getInt() + getParameter(1).getInt();
            }
 
            if (type == VariantType::Long)
            {
                return value.getLong() >= getParameter(0).getLong() - getParameter(1).getLong() &&  value.getLong() <= getParameter(0).getLong() + getParameter(1).getLong();
            }
 
            if (type == VariantType::Float)
            {
                return value.getFloat() >= getParameter(0).getFloat() - getParameter(1).getFloat() &&  value.getFloat() <= getParameter(0).getFloat() + getParameter(1).getFloat();
            }
 
            if (type == VariantType::Double)
            {
                return value.getDouble() >= getParameter(0).getDouble() - getParameter(1).getDouble() &&  value.getDouble() <= getParameter(0).getDouble() + getParameter(1).getDouble();
            }
 
            return false;
        }
    };
}