RobotReader.cpp

Go to the documentation of this file.

#include "RobotReader.h"
 
#include <chrono>
#include <mutex>
#include <optional>
#include <thread>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/Duration.h>
 
#include "RobotAPI/libraries/armem_robot_state/client/common/constants.h"
#include "RobotAPI/libraries/armem_robot_state/common/localization/types.h"
#include "RobotAPI/libraries/core/FramedPose.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_robot_state/aron/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.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/armem_robot_state/types.h>
 
 
namespace fs = ::std::filesystem;
 
namespace armarx::armem::robot_state
{
    RobotReader::RobotReader(const RobotReader& r) :
        properties(r.properties),
        propertyPrefix(r.propertyPrefix),
        memoryReader(r.memoryReader),
        transformReader(r.transformReader)
    {
    }
 
    void
    RobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
    {
        transformReader.registerPropertyDefinitions(def);
    }
 
    void
    RobotReader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
    {
        transformReader.connect(memoryNameSystem);
 
        armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(200));
 
        // Wait for the memory to become available and add it as dependency.
        ARMARX_INFO << "RobotReader: Waiting for memory '" << constants::memoryName << "' ...";
        try
        {
            // memoryReader = memoryNameSystem.useReader(constants::memoryName);
            memoryReader = transformReader.getMemoryReader();
            ARMARX_INFO << "RobotReader: Connected to memory '" << constants::memoryName << "'";
        }
        catch (const armem::error::CouldNotResolveMemoryServer& e)
        {
            ARMARX_ERROR << e.what();
            return;
        }
    }
 
    std::optional<Robot>
    RobotReader::get(const std::string& name, const armem::Time& timestamp) const
    {
        const auto description = queryDescription(name, timestamp);
 
        if (not description)
        {
            ARMARX_ERROR << "Unknown object " << name;
            return std::nullopt;
        }
 
        return get(*description, timestamp);
    }
 
    Robot
    RobotReader::get(const description::RobotDescription& description,
                     const armem::Time& timestamp) const
    {
        Robot robot{.description = description,
                    .instance = "", // TODO(fabian.reister):
                    .config = {}, // will be populated by synchronize
                    .timestamp = timestamp};
 
        ARMARX_CHECK(synchronize(robot, timestamp));
 
        return robot;
    }
 
    void
    RobotReader::setSyncTimeout(const armem::Duration& duration)
    {
        syncTimeout = duration;
    }
 
    void
    RobotReader::setSleepAfterSyncFailure(const armem::Duration& duration)
    {
        ARMARX_CHECK_NONNEGATIVE(duration.toMicroSeconds());
        sleepAfterFailure = duration;
    }
 
    bool
    RobotReader::synchronize(Robot& robot, const armem::Time& timestamp) const
    {
        const auto tsStartFunctionInvokation = armem::Time::Now();
 
        while (true)
        {
            auto state = queryState(robot.description.name, timestamp);
 
            if (not state) /* c++20 [[unlikely]] */
            {
                ARMARX_VERBOSE << "Could not synchronize object " << robot.description.name;
 
                // if the syncTime is non-positive there will be no retry
                const auto elapsedTime = armem::Time::Now() - tsStartFunctionInvokation;
                if (elapsedTime > syncTimeout)
                {
                    ARMARX_VERBOSE << "Could not synchronize object " << robot.description.name;
                    return false;
                }
 
                ARMARX_INFO << "Retrying to query robot state after failure";
                Clock::WaitFor(sleepAfterFailure);
                // Without this the retry fell through and dereferenced the empty optional below.
                // Only setSyncTimeout() makes that reachable -- with the default timeout of 0 the
                // branch above always returns first -- so the retry path has never actually run.
                continue;
            }
 
            robot.config = std::move(*state);
            return true;
        }
    }
 
    std::optional<description::RobotDescription>
    RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp) const
    {
 
        const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
 
        // Query all entities from provider.
        armem::client::query::Builder qb;
 
        // clang-format off
        qb
        .coreSegments().withName(constants::descriptionCoreSegment)
        .providerSegments().withName(name)
        .entities().all()
        .snapshots().beforeOrAtTime(sanitizedTimestamp);
        // clang-format on
 
        ARMARX_DEBUG << "Lookup query in reader";
 
        if (not memoryReader)
        {
            ARMARX_WARNING << "Memory reader is null. Did you forget to call "
                              "RobotReader::connect() in onConnectComponent()?";
            return std::nullopt;
        }
 
        try
        {
            std::scoped_lock l(memoryReaderMutex);
            const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
            ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
            if (not qResult.success) /* c++20 [[unlikely]] */
            {
                return std::nullopt;
            }
 
            return getRobotDescription(qResult.memory, name);
        }
        catch (...)
        {
            ARMARX_VERBOSE << "Query description failure" << GetHandledExceptionString();
        }
 
        return std::nullopt;
    }
 
    std::optional<RobotState>
    RobotReader::queryState(const std::string& robotName, const armem::Time& timestamp) const
    {
        std::optional<RobotState> robotState = queryJointState(robotName, timestamp);
 
        if (robotState)
        {
            const auto globalPose = queryGlobalPose(robotName, timestamp);
            if (not globalPose)
            {
                ARMARX_VERBOSE << "Failed to query global pose for robot " << robotName;
                return std::nullopt;
            }
            robotState->globalPose = *globalPose;
        }
 
        return robotState;
    }
 
    std::optional<RobotState>
    RobotReader::queryJointState(const std::string& robotName, const armem::Time& timestamp) const
    {
        // Seeded with the query time so that a provider which never set referencedTime cannot
        // leave this at an invalid time; getRobotProprioception() overwrites it on success.
        armem::Time referencedTime = timestamp;
        const auto proprioception = queryProprioception(robotName, timestamp, &referencedTime);
 
        if (not proprioception.has_value())
        {
            ARMARX_VERBOSE << "Failed to query proprioception for robot '" << robotName << "'.";
            return std::nullopt;
        }
        const auto jointMap = proprioception->joints.position;
 
        // The time the data refers to, not the time it was asked for. Returning the latter made
        // every result look freshly measured, so a caller had no way to tell how stale it was --
        // and a caller comparing successive timestamps to spot unchanged data never saw a match.
        return RobotState{.timestamp = referencedTime,
                          .globalPose = RobotState::Pose::Identity(),
                          .jointMap = jointMap,
                          .proprioception = proprioception};
    }
 
    std::optional<::armarx::armem::robot_state::localization::Transform>
    RobotReader::queryOdometryPose(const std::string& robotName, const armem::Time& timestamp) const
    {
 
        robot_state::localization::TransformQuery query{
            .header = {.parentFrame = OdometryFrame,
                       .frame = constants::robotRootNodeName,
                       .agent = robotName,
                       .timestamp = timestamp}};
 
        // try
        {
            const auto result = transformReader.lookupTransform(query);
            if (not result)
            {
                return std::nullopt;
            }
            return result.transform;
        }
        // catch (...)
        // {
        //     ARMARX_VERBOSE << GetHandledExceptionString();
        //     return std::nullopt;
        // }
    }
 
    std::optional<armarx::armem::arondto::Proprioception>
    RobotReader::queryProprioception(const std::string& robotName,
                                     const armem::Time& timestamp,
                                     armem::Time* referencedTime) const // Why timestamp?!?!
    {
        // TODO(fabian.reister): how to deal with multiple providers?
 
        // Query all entities from provider.
        armem::client::query::Builder qb;
 
        ARMARX_DEBUG << "Querying robot description for robot: " << robotName;
 
        // clang-format off
        qb
        .coreSegments().withName(constants::proprioceptionCoreSegment)
        .providerSegments().withName(robotName) // agent
        .entities().all() // TODO
        .snapshots().beforeOrAtTime(timestamp);
        // clang-format on
 
        try
        {
            std::scoped_lock l(memoryReaderMutex);
            const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
            ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
            if (not qResult.success) /* c++20 [[unlikely]] */
            {
                ARMARX_VERBOSE << qResult.errorMessage;
                return std::nullopt;
            }
 
            return getRobotProprioception(qResult.memory, robotName, referencedTime);
        }
        catch (...)
        {
            ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query joint state. Reason: "
                           << GetHandledExceptionString();
            return std::nullopt;
        }
    }
 
    RobotReader::JointTrajectory
    RobotReader::queryJointStates(const std::string& robotName,
                                  const armem::Time& begin,
                                  const armem::Time& end) const
    {
        armem::client::query::Builder qb;
 
        ARMARX_DEBUG << "Querying robot joint states for robot: `" << robotName
                     << "` on time interval [" << begin << "," << end << "]";
 
        // clang-format off
        qb
        .coreSegments().withName(constants::proprioceptionCoreSegment)
        .providerSegments().withName(robotName) // agent
        .entities().all() // TODO
        .snapshots().timeRange(begin, end);
        // clang-format on
 
        try
        {
            std::scoped_lock l(memoryReaderMutex);
            const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
            ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
            if (not qResult.success) /* c++20 [[unlikely]] */
            {
                ARMARX_VERBOSE << qResult.errorMessage;
                return {};
            }
 
            return getRobotJointStates(qResult.memory, robotName);
        }
        catch (...)
        {
            ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query joint states. Reason: "
                           << GetHandledExceptionString();
            return {};
        }
    }
 
    std::optional<PlatformState>
    RobotReader::queryPlatformState(const std::string& robotName,
                                    const armem::Time& timestamp) const
    {
        // TODO(fabian.reister): how to deal with multiple providers?
 
        // Query all entities from provider.
        armem::client::query::Builder qb;
 
        ARMARX_DEBUG << "Querying robot description for robot: " << robotName;
 
        // clang-format off
        qb
        .coreSegments().withName(constants::proprioceptionCoreSegment)
        .providerSegments().withName(robotName) // agent
        .entities().all() // TODO
        .snapshots().beforeOrAtTime(timestamp);
        // clang-format on
 
        try
        {
            std::scoped_lock l(memoryReaderMutex);
            const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
            ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
            if (not qResult.success) /* c++20 [[unlikely]] */
            {
                ARMARX_VERBOSE << qResult.errorMessage;
                return std::nullopt;
            }
 
            return getRobotPlatformState(qResult.memory, robotName);
        }
        catch (...)
        {
            ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query platform state. Reason: "
                           << GetHandledExceptionString();
            return std::nullopt;
        }
    }
 
    std::optional<RobotState::Pose>
    RobotReader::queryGlobalPose(const std::string& robotName, const armem::Time& timestamp) const
    {
        try
        {
            const auto result =
                transformReader.getGlobalPose(robotName, constants::robotRootNodeName, timestamp);
            if (not result)
            {
                return std::nullopt;
            }
 
            return result.transform.transform;
        }
        catch (...)
        {
            ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query global pose. Reason: "
                           << GetHandledExceptionString();
            return std::nullopt;
        }
    }
 
    std::optional<RobotState>
    RobotReader::getRobotState(const armarx::armem::wm::Memory& memory,
                               const std::string& name) const
    {
        // clang-format off
        const armem::wm::ProviderSegment& providerSegment = memory
                .getCoreSegment(constants::proprioceptionCoreSegment)
                .getProviderSegment(name);
 
        // TODO entitiesToRobotState()
 
        if (providerSegment.empty())
        {
            ARMARX_VERBOSE << "No entity found";
            return std::nullopt;
        }
 
        // clang-format on
 
        const armem::wm::EntityInstance* instance = nullptr;
        providerSegment.forEachInstance(
            [&instance](const wm::EntityInstance& i)
            {
                instance = &i;
                return false; // break
            });
 
        if (instance == nullptr)
        {
            ARMARX_VERBOSE << "No entity snapshots found";
            return std::nullopt;
        }
 
        // Here, we access the RobotUnit streaming data stored in the proprioception segment.
        return convertRobotState(*instance);
    }
 
    // FIXME remove this, use armem/util/util.h
    template <typename AronClass>
    std::optional<AronClass>
    tryCast(const wm::EntityInstance& item)
    {
        static_assert(std::is_base_of<armarx::aron::codegenerator::cpp::AronGeneratedClass,
                                      AronClass>::value);
 
        try
        {
            return AronClass::FromAron(item.data());
        }
        catch (const armarx::aron::error::AronException&)
        {
            return std::nullopt;
        }
    }
 
    std::optional<armarx::armem::arondto::Proprioception>
    RobotReader::getRobotProprioception(const armarx::armem::wm::Memory& memory,
                                        const std::string& name,
                                        armem::Time* referencedTime) const
    {
        // clang-format off
        const armem::wm::CoreSegment& coreSegment = memory
                .getCoreSegment(constants::proprioceptionCoreSegment);
        // clang-format on
 
        std::optional<armarx::armem::arondto::Proprioception> proprioception;
 
        coreSegment.forEachEntity(
            [&proprioception, referencedTime](const wm::Entity& entity)
            {
                const auto& snapshot = entity.getLatestSnapshot();
                if (not snapshot.hasInstance(0))
                {
                    return;
                }
 
                const auto& entityInstance = snapshot.getInstance(0);
 
                proprioception = tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
 
                if (proprioception and referencedTime != nullptr)
                {
                    *referencedTime = entityInstance.metadata().referencedTime;
                }
            });
 
        return proprioception;
    }
 
    RobotReader::JointTrajectory
    RobotReader::getRobotJointStates(const armarx::armem::wm::Memory& memory,
                                     const std::string& name) const
    {
 
        RobotReader::JointTrajectory jointTrajectory;
 
        // clang-format off
        const armem::wm::CoreSegment& coreSegment = memory
                .getCoreSegment(constants::proprioceptionCoreSegment);
        // clang-format on
 
        coreSegment.forEachEntity(
            [&jointTrajectory](const wm::Entity& entity)
            {
                entity.forEachSnapshot(
                    [&](const auto& snapshot)
                    {
                        if (not snapshot.hasInstance(0))
                        {
                            return;
                        }
 
                        const auto& entityInstance = snapshot.getInstance(0);
 
                        const auto proprioception =
                            tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
                        ARMARX_CHECK(proprioception.has_value());
 
                        const armarx::armem::prop::arondto::Joints& joints = proprioception->joints;
 
                        jointTrajectory.emplace(entityInstance.id().timestamp, joints.position);
                    });
            });
 
        ARMARX_INFO << "Joint trajectory with " << jointTrajectory.size() << " elements";
 
        return jointTrajectory;
    }
 
    // force torque for left and right
    std::optional<std::map<RobotReader::Hand, proprioception::ForceTorque>>
    RobotReader::queryForceTorque(const std::string& robotName, const armem::Time& timestamp) const
    {
 
        // Query all entities from provider.
        armem::client::query::Builder qb;
 
        ARMARX_DEBUG << "Querying force torques description for robot: " << robotName;
 
        // clang-format off
        qb
        .coreSegments().withName(constants::proprioceptionCoreSegment)
        .providerSegments().withName(robotName) // agent
        .entities().all() // TODO
        .snapshots().beforeOrAtTime(timestamp);
        // clang-format on
 
        try
        {
            std::scoped_lock l(memoryReaderMutex);
            const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
            ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
            if (not qResult.success) /* c++20 [[unlikely]] */
            {
                ARMARX_VERBOSE << qResult.errorMessage;
                return std::nullopt;
            }
 
            return getForceTorque(qResult.memory, robotName);
        }
        catch (...)
        {
            ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query force torque. Reason: "
                           << GetHandledExceptionString();
            return std::nullopt;
        }
    }
 
    // force torque for left and right
    std::optional<std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>>
    RobotReader::queryForceTorques(const std::string& robotName,
                                   const armem::Time& start,
                                   const armem::Time& end) const
    {
 
        // Query all entities from provider.
        armem::client::query::Builder qb;
 
        ARMARX_DEBUG << "Querying force torques description for robot: " << robotName;
 
        // clang-format off
        qb
        .coreSegments().withName(constants::proprioceptionCoreSegment)
        .providerSegments().withName(robotName) // agent
        .entities().all() // TODO
        .snapshots().timeRange(start, end);
        // clang-format on
 
        try
        {
            std::scoped_lock l(memoryReaderMutex);
            const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
            ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
            if (not qResult.success) /* c++20 [[unlikely]] */
            {
                ARMARX_VERBOSE << qResult.errorMessage;
                return std::nullopt;
            }
 
            return getForceTorques(qResult.memory, robotName);
        }
        catch (...)
        {
            ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query force torques. Reason: "
                           << GetHandledExceptionString();
            return std::nullopt;
        }
    }
 
    std::optional<std::map<RobotReader::Hand, exteroception::ToF>>
    RobotReader::queryToF(const std::string& robotName, const armem::Time& timestamp) const
    {
        // Query all entities from provider.
        armem::client::query::Builder qb;
 
        ARMARX_DEBUG << "Querying ToF data for robot: " << robotName;
 
        // clang-format off
        qb
        .coreSegments().withName(constants::exteroceptionCoreSegment)
        .providerSegments().withName(robotName) // agent
        .entities().all() // TODO
        .snapshots().beforeOrAtTime(timestamp);
        // clang-format on
 
        try
        {
            std::scoped_lock l(memoryReaderMutex);
            const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
            ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
            if (not qResult.success) /* c++20 [[unlikely]] */
            {
                ARMARX_VERBOSE << qResult.errorMessage;
                return std::nullopt;
            }
 
            return getToF(qResult.memory, robotName);
        }
        catch (...)
        {
            ARMARX_VERBOSE << deactivateSpam(1)
                           << "Failed to query ToF. Reason: " << GetHandledExceptionString();
            return std::nullopt;
        }
    }
 
    std::optional<PlatformState>
    RobotReader::getRobotPlatformState(const armarx::armem::wm::Memory& memory,
                                       const std::string& name) const
    {
        std::optional<PlatformState> platformState;
 
        // clang-format off
        const armem::wm::CoreSegment& coreSegment = memory
                .getCoreSegment(constants::proprioceptionCoreSegment);
        // clang-format on
 
        coreSegment.forEachEntity(
            [&platformState](const wm::Entity& entity)
            {
                if (not entity.getLatestSnapshot().hasInstance(0))
                {
                    return;
                }
 
                const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
 
                const auto proprioception =
                    tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
                ARMARX_CHECK(proprioception.has_value());
 
                platformState = PlatformState(); // initialize optional
                fromAron(proprioception->platform, platformState.value());
            });
 
        return platformState;
    }
 
    inline RobotReader::Hand
    fromHandName(const std::string& name)
    {
        if (name == "Left")
        {
            return RobotReader::Hand::Left;
        }
 
        if (name == "Right")
        {
            return RobotReader::Hand::Right;
        }
 
        throw LocalException("Unknown hand name `" + name + "`!");
    }
 
    std::map<RobotReader::Hand, proprioception::ForceTorque>
    RobotReader::getForceTorque(const armarx::armem::wm::Memory& memory,
                                const std::string& name) const
    {
        std::map<RobotReader::Hand, proprioception::ForceTorque> forceTorques;
 
        // clang-format off
        const armem::wm::CoreSegment& coreSegment = memory
                .getCoreSegment(constants::proprioceptionCoreSegment);
        // clang-format on
 
        coreSegment.forEachEntity(
            [&forceTorques](const wm::Entity& entity)
            {
                if (not entity.getLatestSnapshot().hasInstance(0))
                {
                    return;
                }
 
                const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
 
                const auto proprioception =
                    tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
                ARMARX_CHECK(proprioception.has_value());
 
                for (const auto& [handName, dtoFt] : proprioception->forceTorque)
                {
                    proprioception::ForceTorque forceTorque;
                    fromAron(dtoFt, forceTorque);
 
                    const auto hand = fromHandName(handName);
                    forceTorques.emplace(hand, forceTorque);
                }
            });
 
        return forceTorques;
    }
 
    std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>
    RobotReader::getForceTorques(const armarx::armem::wm::Memory& memory,
                                 const std::string& name) const
    {
        std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>
            forceTorques;
 
        // clang-format off
        const armem::wm::CoreSegment& coreSegment = memory
                .getCoreSegment(constants::proprioceptionCoreSegment);
        // clang-format on
 
        coreSegment.forEachInstance(
            [&forceTorques](const wm::EntityInstance& entityInstance)
            {
                const auto proprioception =
                    tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
                ARMARX_CHECK(proprioception.has_value());
 
 
                for (const auto& [handName, dtoFt] : proprioception->forceTorque)
                {
                    proprioception::ForceTorque forceTorque;
                    fromAron(dtoFt, forceTorque);
 
                    const auto hand = fromHandName(handName);
                    forceTorques[hand].emplace(entityInstance.id().timestamp, forceTorque);
                }
            });
 
        return forceTorques;
    }
 
    std::map<RobotReader::Hand, exteroception::ToF>
    RobotReader::getToF(const armarx::armem::wm::Memory& memory, const std::string& name) const
    {
        std::map<RobotReader::Hand, exteroception::ToF> tofs;
 
        // clang-format off
        const armem::wm::CoreSegment& coreSegment = memory
                .getCoreSegment(constants::exteroceptionCoreSegment);
        // clang-format on
 
        coreSegment.forEachEntity(
            [&tofs](const wm::Entity& entity)
            {
                ARMARX_DEBUG << "Processing ToF element";
 
                if (not entity.getLatestSnapshot().hasInstance(0))
                {
                    return;
                }
                const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
 
                const auto exteroception =
                    tryCast<::armarx::armem::arondto::Exteroception>(entityInstance);
                ARMARX_CHECK(exteroception.has_value());
 
 
                for (const auto& [handName, dtoFt] : exteroception->tof)
                {
                    ARMARX_DEBUG << "Processing ToF element for hand `" << handName << "`";
 
                    exteroception::ToF tof;
                    fromAron(dtoFt, tof);
 
                    const auto hand = fromHandName(handName);
                    tofs.emplace(hand, tof);
                }
            });
 
        return tofs;
    }
 
    std::optional<description::RobotDescription>
    RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory,
                                     const std::string& name) const
    {
        // clang-format off
        const armem::wm::ProviderSegment& providerSegment = memory
                .getCoreSegment(constants::descriptionCoreSegment)
                .getProviderSegment(name);
        // clang-format on
 
        const armem::wm::EntityInstance* instance = nullptr;
        providerSegment.forEachInstance([&instance](const wm::EntityInstance& i)
                                        { instance = &i; });
        if (instance == nullptr)
        {
            ARMARX_VERBOSE << "No entity snapshots found in provider segment `" << name << "`";
            return std::nullopt;
        }
 
        return convertRobotDescription(*instance);
    }
 
    std::vector<description::RobotDescription>
    RobotReader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
    {
        const armem::wm::CoreSegment& coreSegment =
            memory.getCoreSegment(constants::descriptionCoreSegment);
 
        std::vector<description::RobotDescription> descriptions;
 
        coreSegment.forEachInstance(
            [&descriptions](const wm::EntityInstance& instance)
            {
                if (const std::optional<description::RobotDescription> desc =
                        convertRobotDescription(instance))
                {
                    descriptions.push_back(desc.value());
                }
            });
 
        return descriptions;
    }
 
    std::vector<description::RobotDescription>
    RobotReader::queryDescriptions(const armem::Time& timestamp) const
    {
        const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
 
        // Query all entities from provider.
        armem::client::query::Builder qb;
 
        // clang-format off
        qb
        .coreSegments().withName(constants::descriptionCoreSegment)
        .providerSegments().all()
        .entities().all()
        .snapshots().beforeOrAtTime(sanitizedTimestamp);
        // clang-format on
 
        ARMARX_DEBUG << "Lookup query in reader";
 
        if (not memoryReader)
        {
            ARMARX_WARNING << "Memory reader is null. Did you forget to call "
                              "RobotReader::connect() in onConnectComponent()?";
            return {};
        }
 
        try
        {
            std::scoped_lock l(memoryReaderMutex);
            const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
            ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
            if (not qResult.success) /* c++20 [[unlikely]] */
            {
                return {};
            }
 
            return getRobotDescriptions(qResult.memory);
        }
        catch (...)
        {
            ARMARX_VERBOSE << "Query description failure" << GetHandledExceptionString();
        }
 
        return {};
    }
} // namespace armarx::armem::robot_state