OffsetFilter.cpp

Go to the documentation of this file.

#include "OffsetFilter.h"
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
 
 
namespace armarx::filters
{
 
    OffsetFilter::OffsetFilter()
    {
        firstRun = true;
        windowFilterSize = 1;
    }
 
 
    VariantBasePtr OffsetFilter::calculate(const Ice::Current&) const
    {
        std::unique_lock lock(historyMutex);
 
        VariantPtr newVariant;
 
        if (initialValue && dataHistory.size() > 0)
        {
            VariantTypeId type = dataHistory.begin()->second->getType();
            VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
 
            if (currentValue->getType() != initialValue->getType())
            {
                ARMARX_ERROR_S << "Types in OffsetFilter are different: " << Variant::typeToString(currentValue->getType()) << " and " << Variant::typeToString(initialValue->getType());
                return nullptr;
            }
 
            if (type == VariantType::Int)
            {
                int newValue = dataHistory.rbegin()->second->getInt() - initialValue->getInt();
                newVariant = new Variant(newValue);
            }
            else if (type == VariantType::Long)
            {
                long newValue = dataHistory.rbegin()->second->getLong() - initialValue->getLong();
                newVariant = new Variant(newValue);
            }
            else if (type == VariantType::Float)
            {
                float newValue = dataHistory.rbegin()->second->getFloat() - initialValue->getFloat();
                newVariant = new Variant(newValue);
            }
            else if (type == VariantType::Double)
            {
                double newValue = dataHistory.rbegin()->second->getDouble() - initialValue->getDouble();
                newVariant = new Variant(newValue);
            }
            else if (type == VariantType::FramedDirection)
            {
                FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(currentValue->get<FramedDirection>());
                FramedDirectionPtr intialVec = FramedDirectionPtr::dynamicCast(initialValue->get<FramedDirection>());
                Eigen::Vector3f newValue =  vec->toEigen() - intialVec->toEigen();
 
                newVariant = new Variant(new FramedDirection(newValue, vec->frame, vec->agent));
            }
            else if (type == VariantType::FramedPosition)
            {
                FramedPositionPtr pos = FramedPositionPtr::dynamicCast(currentValue->get<FramedPosition>());
                FramedPositionPtr intialPos = FramedPositionPtr::dynamicCast(initialValue->get<FramedPosition>());
                Eigen::Vector3f newValue =  pos->toEigen() - intialPos->toEigen();
                newVariant = new Variant(new FramedPosition(newValue, pos->frame, pos->agent));
            }
            else if (type == VariantType::MatrixFloat)
            {
                MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
                MatrixFloatPtr initialMatrix = MatrixFloatPtr::dynamicCast(initialValue->get<MatrixFloat>());
                Eigen::MatrixXf newMatrix = matrix->toEigen() - initialMatrix->toEigen();
                newVariant = new Variant(new MatrixFloat(newMatrix));
            }
        }
 
        return newVariant;
 
    }
 
    ParameterTypeList OffsetFilter::getSupportedTypes(const Ice::Current&) const
    {
        ParameterTypeList result;
        result.push_back(VariantType::Int);
        result.push_back(VariantType::Long);
        result.push_back(VariantType::Float);
        result.push_back(VariantType::Double);
        result.push_back(VariantType::FramedDirection);
        result.push_back(VariantType::FramedPosition);
        result.push_back(VariantType::MatrixFloat);
        return result;
    }
 
    void OffsetFilter::update(Ice::Long timestamp, const VariantBasePtr& value, const Ice::Current& c)
    {
        DatafieldFilter::update(timestamp, value, c);
 
        if (firstRun)
        {
            std::unique_lock lock(historyMutex);
 
            if (dataHistory.size() == 0)
            {
                return;
            }
 
            initialValue = VariantPtr::dynamicCast(dataHistory.begin()->second);
            firstRun = false;
        }
    }
 
}