ObjectReader.cpp

Go to the documentation of this file.

#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(p.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 '" << p.memoryName << "' ...";
        try
        {
            // simply wait until memory is ready. Do nothing with reader but get prx
            auto r = memoryNameSystem.useReader(p.memoryName);
 
            // cast MemoryPrx to objPoseStoragePrx
            this->objPoseStorage =
                objpose::ObjectPoseStorageInterfacePrx::checkedCast(r.readingPrx);
 
            ARMARX_IMPORTANT << "Connected to Memory and ObjectPoseStorage '" << p.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 = objPoseStorage->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 = objPoseStorage->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 = objPoseStorage->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 = objPoseStorage->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 = objPoseStorage->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