Reader.cpp

Go to the documentation of this file.

#include "Reader.h"
 
#include <SimoxUtility/algorithm/string/string_tools.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 "../frame_conversions.h"
 
namespace armarx::armem::locations::client
{
    void
    Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
    {
        const std::string prefix = propertyPrefix;
 
        def->optional(p.memoryName, prefix + "MemoryName");
        objReader.registerPropertyDefinitions(def);
        robotReader.registerPropertyDefinitions(def);
    }
 
    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
            locationReader = memoryNameSystem.useReader(p.memoryName);
 
            ARMARX_IMPORTANT << "Connected to Memory '" << p.memoryName << "'";
        }
        catch (const armem::error::CouldNotResolveMemoryServer& e)
        {
            ARMARX_ERROR << e.what();
            return;
        }
 
        objReader.connect(memoryNameSystem);
        robotReader.connect(memoryNameSystem);
    }
 
    std::map<std::string, armarx::navigation::location::arondto::Location>
    Reader::getAllLocations()
    {
        auto mId = armarx::armem::MemoryID{p.memoryName, "Location"};
        auto res = locationReader.getLatestSnapshotsIn(mId);
 
        if (res.success)
        {
            std::map<std::string, armarx::navigation::location::arondto::Location> ret;
            res.memory.forEachEntity(
                [&ret](const armarx::armem::wm::Entity& e)
                {
                    auto i = e.findLatestInstance();
                    if (i)
                    {
                        auto loc = i->dataAs<armarx::navigation::location::arondto::Location>();
                        ret[i->id().providerSegmentName + "/" + i->id().entityName] = loc;
                    }
                });
            return ret;
        }
 
        throw error::ArMemError(res.errorMessage);
    }
 
    std::map<std::string, armarx::navigation::location::arondto::Location>
    Reader::getAllLocationsInGlobalFrame()
    {
        auto locations = this->getAllLocations();
        auto objects = objReader.queryLatestObjectInstances();
        auto robotDecs = robotReader.queryDescriptions(armarx::core::time::DateTime::Now());
 
        for (auto& [locName, location] : locations)
        {
            (void)locName;
            if (location.framedPose.header.frame == armarx::GlobalFrame)
            {
                location.framedPose.header.agent = ""; //sanity set
                continue;
            }
            // check if we have an object that matches the relative location
            for (const auto& [objectInstanceId, object] : objects)
            {
                if (simox::alg::starts_with(objectInstanceId, location.framedPose.header.agent))
                {
                    toGlobal(location, object);
                }
            }
 
            // TODO: check if we have an robot that matches the relative location
        }
 
        return locations;
    }
 
 
} // namespace armarx::armem::locations::client