RequestedObjects.cpp

Go to the documentation of this file.

#include "RequestedObjects.h"
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/TimeUtil.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)
    {
        ARMARX_INFO << VAROUT(relativeTimeout.toMilliSeconds());
 
        if (relativeTimeout.toMilliSeconds() < 0)
        {
            ARMARX_INFO << "Infite localization request for object ids :" << objectIDs;
            for (const auto& id : objectIDs)
            {
                infiniteRequests.push_back(id);
            }
        }
        else
        {
            ARMARX_INFO << "Localization request for " << relativeTimeout
                        << " for object ids :" << objectIDs;
 
            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.
 
        if (not currentRequests.empty())
        {
            ARMARX_INFO << currentRequests.begin()->first - now;
        }
 
        while (not currentRequests.empty() and currentRequests.begin()->first <= now)
        {
            ARMARX_INFO << "No longer localizing objects: ";
            for (const Request& request : currentRequests.begin()->second)
            {
                ARMARX_INFO << request.objectIDs;
            }
            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;
    }
 
} // namespace armarx::objpose