EqualityFilter.cpp

Go to the documentation of this file.

#include "EqualityFilter.h"
 
#include <list>
 
#include <IceUtil/Time.h>
 
#include "RobotAPI/libraries/armem/server/ltm/EntitySnapshot.h"
 
#include "RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h"
#include "RobotAPI/libraries/aron/core/data/variant/primitive/Float.h"
#include "RobotAPI/libraries/aron/core/data/variant/primitive/Long.h"
#include "RobotAPI/libraries/aron/core/data/variant/primitive/Double.h"
#include "RobotAPI/libraries/aron/core/data/variant/primitive/String.h"
#include "RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h"
#include "RobotAPI/libraries/aron/core/data/variant/container/List.h"
#include "RobotAPI/libraries/aron/core/data/variant/primitive/Int.h"
 
#include "RobotAPI/libraries/aron/core/Descriptor.h"
 
namespace armarx::armem::server::ltm::processor::filter
{
    bool
    SnapshotEqualityFilter::accept(const armem::wm::EntitySnapshot& e, bool simulatedVersion)
    {
        // Thread safety: lock the filter mutex for all mutable state access
        std::lock_guard<std::mutex> lock(filterMutex_);
 
        //ARMARX_INFO << "Equality filtering!!!";
 
        auto start = std::chrono::high_resolution_clock::now();
 
        const std::string entityId = e.id().getEntityID().str();
        auto it = lastAcceptedSnapshots.find(entityId);
 
        // accepting if entityId occurs for the first time
        if (it == lastAcceptedSnapshots.end()) 
        {
            lastAcceptedSnapshots.emplace(entityId, e);
            stats.accepted++;
            auto end = std::chrono::high_resolution_clock::now();
            stats.end_time = end;
            stats.additional_time += (end - start);
            return true;
        }
 
        const armem::wm::EntitySnapshot& oldSnapshot = it->second;
        bool isEqual = checkSnapshotEquality(oldSnapshot, e);
 
        // equalsDeep() can instead be used to compare the snapshots with less flexibility
        // bool isEqual = it->second.equalsDeep(e);
 
        if (isEqual)
        {
            stats.rejected++;
        }
        else
        {
            it->second = e;
            stats.accepted++;
        }
        auto end = std::chrono::high_resolution_clock::now();
        stats.end_time = end;
        stats.additional_time += (end - start);
 
        return !isEqual;
    }
 
    bool
    SnapshotEqualityFilter::checkSnapshotEquality(
        const armem::wm::EntitySnapshot& oldSnapshot, 
        const armem::wm::EntitySnapshot& newSnapshot)
    {
        // load instances into vectors and compare each assuming order of instances
        // and data inside them stays the same
        std::vector<aron::data::VariantPtr> oldInstances;
        std::vector<aron::data::VariantPtr> newInstances;
 
        oldSnapshot.forEachInstance(
            [&oldInstances](armem::wm::EntityInstance& i)
            {
                oldInstances.push_back(i.data());
            }
        );
 
        newSnapshot.forEachInstance(
            [&newInstances](armem::wm::EntityInstance& i)
            {
                newInstances.push_back(i.data());
            }
        );
 
        if (oldInstances.size() != newInstances.size())
        {
            return false;
        }
 
        for (size_t i = 0; i < newInstances.size(); i++)
        {
            if (!checkInstanceEquality(oldInstances[i], newInstances[i]))
            {
                return false;
            }
        }
        return true;
    }
 
    bool
    SnapshotEqualityFilter::checkInstanceEquality(
        const aron::data::VariantPtr& oldData, 
        const aron::data::VariantPtr& newData)
    {
        if (!oldData && !newData)
        {
            return true;
        }
        if (!oldData || !newData)
        {
            //ARMARX_INFO << "nullptr in filtering";
            return false;
        }
 
        // if importance is set and zero, ignore value changes
        bool zeroImportanceOld = oldData
            && oldData->getImportance().has_value()
            && *oldData->getImportance() == 0.f;
        bool zeroImportanceNew = newData
            && newData->getImportance().has_value()
            && *newData->getImportance() == 0.f;
        if (zeroImportanceOld || zeroImportanceNew)
        {
            return true;
        }
 
        auto oldDesc = oldData->getDescriptor();
        auto newDesc = newData->getDescriptor();
        auto desc_string = aron::data::defaultconversion::string::Descriptor2String.at(oldDesc);
        //ARMARX_INFO << desc_string;
 
        if (oldDesc != newDesc)
        {
            return false;
        }
 
        // comparison can be extended for new types
        // recursive in case of containers, LIST is order-sensitive
        switch(newDesc)
        {
        case aron::data::Descriptor::INT:
        {
            auto oldInt = aron::data::Int::DynamicCastAndCheck(oldData);
            auto newInt = aron::data::Int::DynamicCastAndCheck(newData);
            return *oldInt == *newInt;
        }
        case aron::data::Descriptor::FLOAT:
        {
            auto oldFloat = aron::data::Float::DynamicCastAndCheck(oldData);
            auto newFloat = aron::data::Float::DynamicCastAndCheck(newData);
            return *oldFloat == *newFloat;
        }
        case aron::data::Descriptor::DOUBLE:
        {
            auto oldDouble = aron::data::Double::DynamicCastAndCheck(oldData);
            auto newDouble = aron::data::Double::DynamicCastAndCheck(newData);
            return *oldDouble == *newDouble;
        }
        case aron::data::Descriptor::LONG:
        {
            auto oldLong = aron::data::Long::DynamicCastAndCheck(oldData);
            auto newLong = aron::data::Long::DynamicCastAndCheck(newData);
            return *oldLong == *newLong;
        }
        case aron::data::Descriptor::STRING:
        {
            auto oldString = aron::data::String::DynamicCastAndCheck(oldData);
            auto newString = aron::data::String::DynamicCastAndCheck(newData);
            return *oldString == *newString;
        }
        case aron::data::Descriptor::BOOL:
        {
            auto oldBool = aron::data::Bool::DynamicCastAndCheck(oldData);
            auto newBool = aron::data::Bool::DynamicCastAndCheck(newData);
            return *oldBool == *newBool;
        }
        case aron::data::Descriptor::NDARRAY:
        {
            auto oldNdarr = aron::data::NDArray::DynamicCastAndCheck(oldData);
            auto newNdarr = aron::data::NDArray::DynamicCastAndCheck(newData);
            return *oldNdarr == *newNdarr;
        }
        case aron::data::Descriptor::LIST:
        {
            auto oldList = aron::data::List::DynamicCastAndCheck(oldData);
            auto newList = aron::data::List::DynamicCastAndCheck(newData);
            const auto& oldElems = oldList->getElements();
            const auto& newElems = newList->getElements();
 
            if (oldElems.size() != newElems.size())
            {
                return false;
            }
 
            for (size_t i = 0; i < newElems.size(); i++)
            {
                if (!checkInstanceEquality(oldElems[i], newElems[i]))
                {
                    return false;
                }
            }
            return true;
        }
        case aron::data::Descriptor::DICT:
        {
            auto oldDict = aron::data::Dict::DynamicCastAndCheck(oldData);
            auto newDict = aron::data::Dict::DynamicCastAndCheck(newData);
            auto oldKeys = oldDict->getAllKeys();
            auto newKeys = newDict->getAllKeys();
 
            if (oldKeys.size() != newKeys.size())
            {
                return false;
            }
 
            for (const auto& key : newKeys)
            {
                if (!oldDict->hasElement(key))
                {
                    return false;
                }
                if (!checkInstanceEquality(oldDict->at(key), newDict->at(key)))
                {
                    return false;
                }
            }
            return true;
        }
        default:
        {
            ARMARX_INFO << "data-type not yet supported.";
            return false; // maybe adjust
        }
        }
        return false;
 
    }
 
    
    void
    SnapshotEqualityFilter::configure(const nlohmann::json& json)
    {
        std::lock_guard<std::mutex> lock(filterMutex_);
 
        stats.start_time = std::chrono::high_resolution_clock::now();
    }
 
    SnapshotFilter::FilterStatistics
    SnapshotEqualityFilter::getFilterStatistics()
    {
        std::lock_guard<std::mutex> lock(filterMutex_);
        return stats;
    }
 
    std::string
    SnapshotEqualityFilter::getName()
    {
        return this->NAME;
    }
 
} // namespace armarx::armem::server::ltm::processor::filter