ObjectReader.cpp

#include "ObjectReader.h"

#include <mutex>
#include <optional>

#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/logging/Logging.h>

#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_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>
#include <RobotAPI/libraries/aron/common/aron_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);
            if (oid == 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;
    }

} // namespace armarx::armem::obj::instance