RequestedObjects.cpp

Go to the documentation of this file.

#include "RequestedObjects.h"
 
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
 
namespace armarx::objpose
{
    RequestedObjects::RequestedObjects()
    {
    }
 
    void RequestedObjects::requestObjects(const std::vector<armarx::data::ObjectID>& objectIDs, long relativeTimeOutMS)
    {
        requestObjects(::armarx::fromIce(objectIDs), relativeTimeOutMS);
    }
 
    void RequestedObjects::requestObjects(const std::vector<armarx::ObjectID>& objectIDs, long relativeTimeOutMS)
    {
        requestObjects(objectIDs, IceUtil::Time::milliSeconds(relativeTimeOutMS));
    }
 
    void RequestedObjects::requestObjects(
        const std::vector<armarx::ObjectID>& objectIDs, IceUtil::Time relativeTimeout)
    {
        if (relativeTimeout.toMilliSeconds() < 0)
        {
            for (const auto& id : objectIDs)
            {
                infiniteRequests.push_back(id);
            }
        }
        else
        {
            IceUtil::Time absoluteTimeout = TimeUtil::GetTime() + relativeTimeout;
            Request req;
            req.objectIDs = objectIDs;
            currentRequests[absoluteTimeout].push_back(req);
        }
    }
 
    RequestedObjects::Update RequestedObjects::updateRequestedObjects()
    {
        return updateRequestedObjects(TimeUtil::GetTime());
    }
 
    RequestedObjects::Update RequestedObjects::updateRequestedObjects(IceUtil::Time now)
    {
        // Remove requests with timeout.
        while (currentRequests.size() > 0 && currentRequests.begin()->first <= now)
        {
            currentRequests.erase(currentRequests.begin());
        }
 
        std::set<armarx::ObjectID> current;
        current.insert(infiniteRequests.begin(), infiniteRequests.end());
 
        // Process current requests.
        for (const auto& [timeout, requests] : currentRequests)
        {
            ARMARX_CHECK_LESS(now, timeout);
            for (const auto& request : requests)
            {
                current.insert(request.objectIDs.begin(), request.objectIDs.end());
            }
        }
 
        Update update;
        update.current = { current.begin(), current.end() };
 
        // added = current - last
        std::set_difference(update.current.begin(), update.current.end(),
                            lastCurrent.begin(), lastCurrent.end(),
                            std::inserter(update.added, update.added.begin()));
        // removed = last - current
        std::set_difference(lastCurrent.begin(), lastCurrent.end(),
                            update.current.begin(), update.current.end(),
                            std::inserter(update.removed, update.removed.begin()));
 
        this->lastCurrent = update.current;
        return update;
    }
 
}