ObjectReader.cpp

Go to the documentation of this file.

#include "ObjectReader.h"
 
#include <map>
#include <optional>
#include <string>
#include <vector>
 
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Duration.h>
 
#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
#include "RobotAPI/libraries/ArmarXObjects/ObjectPose.h"
#include "RobotAPI/libraries/armem/client/MemoryNameSystem.h"
#include "RobotAPI/libraries/armem/core/error/mns.h"
#include <RobotAPI/interface/ArmarXObjects/ArmarXObjectsTypes.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
 
// #include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
// #include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
// #include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
 
namespace armarx::armem::obj::instance
{
    void
    Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
    {
        const std::string prefix = propertyPrefix;
 
        def->optional(properties.memoryName, prefix + "MemoryName");
    }
 
    void
    Reader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
    {
        // Wait for the memory to become available and add it as dependency.
        ARMARX_IMPORTANT << "Waiting for memory '" << properties.memoryName << "' ...";
        try
        {
            // simply wait until memory is ready. Do nothing with reader but get prx
            auto r = memoryNameSystem.useReader(properties.memoryName);
 
            // cast MemoryPrx to objPoseStoragePrx --> NOT WORKING DUE TO SOME ICE CACHING OR SO.
            // Explanation:
            // When the readingPrx disconnects and reconnects, the cast fails as it uses internally
            // a ice_isA call which results in connection refused. The reason is, that for some
            // reason the mns thinks that the reconnected prx is similar to the old info
            // (check ice_identity) so it returnes the cached prx in its server map. Could be fixed
            // by always querying the mns component for new proxies, but this may slow the system
            // down.
            //this->objPoseStorage =
            //    objpose::ObjectPoseStorageInterfacePrx::uncheckedCast(r.readingPrx);
            // Current fix: Get prx from mns:
            this->readingPrx = r.readingPrx;
 
            ARMARX_INFO << "Connected to Memory '" << properties.memoryName << "'";
        }
        catch (const armem::error::CouldNotResolveMemoryServer& e)
        {
            ARMARX_ERROR << e.what();
            return;
        }
    }
 
    std::map<std::string, bool>
    Reader::requestLocalization(const ObjectID& instanceId,
                                const armarx::core::time::Duration& until)
    {
        std::map<std::string, bool> ret;
        auto providers = getObjectPoseStorage()->getAvailableProvidersInfo();
        for (const auto& [k, p] : providers)
        {
            // TODO: check supported objects?
            ret[k] = this->requestLocalization(instanceId, k, until);
        }
        return ret;
    }
 
    bool
    Reader::requestLocalization(const ObjectID& instanceId,
                                const std::string& provider,
                                const armarx::core::time::Duration& until)
    {
 
        armarx::data::ObjectID requestObject;
        armarx::toIce(requestObject, instanceId);
 
        armarx::objpose::observer::RequestObjectsInput req;
        req.provider = provider;
        req.request.objectIDs = {requestObject};
        req.request.relativeTimeoutMS = until.toMilliSeconds();
 
        auto requestResult = getObjectPoseStorage()->requestObjects(req);
 
        if (requestResult.results.count(requestObject))
        {
            return requestResult.results.at(requestObject).result.success;
        }
        return false;
    }
 
    std::optional<objpose::ObjectPose>
    Reader::queryLatestObjectInstance(const ObjectID& instanceId)
    {
        // TODO: Shall we throw an exception if no instance index is set?
 
        auto objectPoses = getObjectPoseStorage()->getObjectPoses();
        for (const auto& pose : objectPoses)
        {
            ObjectID oid;
            fromIce(pose.objectID, oid);
 
            // fully specified with instance name?
            if (not instanceId.instanceName().empty())
            {
                if (oid == instanceId)
                {
                    objpose::ObjectPose oi;
                    fromIce(pose, oi);
                    return oi;
                }
            }
            else // only class name provided => take the first matching instance
            {
                if (oid.equalClass(instanceId))
                {
                    objpose::ObjectPose oi;
                    fromIce(pose, oi);
                    return oi;
                }
            }
        }
        return std::nullopt;
    }
 
    std::map<std::string, objpose::ObjectPose>
    Reader::queryLatestObjectInstances()
    {
        std::map<std::string, objpose::ObjectPose> ret;
        auto objectPoses = getObjectPoseStorage()->getObjectPoses();
        for (const auto& pose : objectPoses)
        {
            ObjectID oid;
            fromIce(pose.objectID, oid);
            fromIce(pose, ret[oid.str()]);
        }
        return ret;
    }
 
    std::map<std::string, objpose::ObjectPose>
    Reader::queryLatestObjectInstances(const ObjectID& classId)
    {
        std::map<std::string, objpose::ObjectPose> ret;
        auto objectPoses = getObjectPoseStorage()->getObjectPoses();
        for (const auto& pose : objectPoses)
        {
            ObjectID oid;
            fromIce(pose.objectID, oid);
 
            if (oid.equalClass(classId))
            {
                fromIce(pose, ret[oid.str()]);
            }
        }
        return ret;
    }
 
    std::map<std::string, std::vector<objpose::ObjectPose>>
    Reader::queryAttachedObjectInstancesForProviders()
    {
 
        std::map<std::string, std::vector<objpose::ObjectPose>> ret;
 
        // Not working: does not return all provider segment names
        // const auto providers = getObjectPoseStorage()->getAvailableProviderNames();
        // ARMARX_VERBOSE << "Found providers: " << providers;
 
 
        for (const auto& providerName : {"--placeholder--"})
        {
            // const auto objects = getObjectPoseStorage()->getObjectPosesByProvider(providerName);
 
            const auto objects = getObjectPoseStorage()->getObjectPoses();
 
            // filter attached objects
            const auto isAttached = [](const objpose::ObjectPose& objectPose) noexcept -> bool
            { return objectPose.attachment.has_value(); };
 
            std::vector<objpose::ObjectPose> attachedObjects = {};
            for (const auto& obj : objects)
            {
                ARMARX_VERBOSE << obj.objectID;
                if (isAttached(obj))
                {
                    attachedObjects.emplace_back(obj);
                }
            }
 
 
            if (not attachedObjects.empty())
            {
                ARMARX_VERBOSE << "Provider " << QUOTED(providerName) << " has "
                               << attachedObjects.size() << " attached objects";
                ret[providerName] = attachedObjects;
            }
        }
 
        return ret;
    }
 
 
} // namespace armarx::armem::obj::instance