Reader.cpp

Go to the documentation of this file.

#include "Reader.h"
 
#include <mutex>
#include <optional>
 
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include "RobotAPI/libraries/armem/core/Commit.h"
#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.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_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/constants.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 "utils.h"
 
namespace armarx::armem::articulated_object
{
 
    void
    Reader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
    {
        ARMARX_TRACE;
 
        // Wait for the memory to become available and add it as dependency.
        ARMARX_IMPORTANT << "Reader: Waiting for memory '" << objects::constants::MemoryName
                         << "' ...";
        try
        {
            memoryReader = memoryNameSystem.useReader(objects::constants::MemoryName);
            ARMARX_IMPORTANT << "Reader: Connected to memory '" << objects::constants::MemoryName
                             << "'";
        }
        catch (const armem::error::CouldNotResolveMemoryServer& e)
        {
            ARMARX_ERROR << e.what();
            return;
        }
 
        armem::MemoryID id = armem::MemoryID();
        id.memoryName = objects::constants::MemoryName;
        id.coreSegmentName = objects::constants::CoreClassSegmentName;
        // listen to all provider segments!
 
        memoryNameSystem.subscribe(id, this, &Reader::updateKnownObjects);
    }
 
    void
    Reader::updateKnownObject(const armem::MemoryID& snapshotId)
    {
        // const std::string& nameWithDataset = snapshotId.providerSegmentName;
 
        // arondto::RobotDescription aronArticulatedObjectDescription;
        // aronArticulatedObjectDescription.fromAron(snapshotId.);
 
        // TODO(fabian.reister): implement
    }
 
    void
    Reader::updateKnownObjects(const armem::MemoryID& subscriptionID,
                               const std::vector<armem::MemoryID>& snapshotIDs)
    {
        ARMARX_TRACE;
 
        ARMARX_INFO << "New objects available!";
 
        // // Query all entities from provider.
        // armem::client::query::Builder qb;
 
        // // clang-format off
        // qb
        // .coreSegments().withName(properties.coreClassSegmentName)
        // .providerSegments().all() // TODO(fabian.reister): think about this: which authority is trustworthy?
        // .entities().withName(name)
        // .snapshots().atTime(timestamp);
        // // clang-format on
 
        // const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
        // std::for_each(snapshotIDs.begin(), snapshotIDs.end(), [&](const auto & snapshotID)
        // {
        //     updateKnownObject(snapshotID);
        // });
    }
 
    std::optional<ArticulatedObject>
    Reader::get(const std::string& name,
                const armem::Time& timestamp,
                const std::optional<std::string>& providerName) const
    {
        ARMARX_TRACE;
 
        const auto splits = simox::alg::split(name, "/");
        ARMARX_CHECK_EQUAL(splits.size(), 3) << "`name` must be of form `DATASET/NAME/INSTANCE`";
 
        const std::string className = splits.at(0) + "/" + splits.at(1); // `DATASET/NAME`
        const std::string instanceName = splits.at(2);
 
        const auto description = queryDescription(className, timestamp, providerName);
 
        if (not description)
        {
            ARMARX_WARNING << "Unknown object " << className;
            return std::nullopt;
        }
 
        return get(*description, timestamp, instanceName, providerName);
    }
 
    ArticulatedObject
    Reader::get(const ArticulatedObjectDescription& description,
                const armem::Time& timestamp,
                const std::string& instanceName,
                const std::optional<std::string>& providerName) const
    {
        ARMARX_TRACE;
 
        ArticulatedObject obj{.description = description,
                              .instance = instanceName,
                              .config = {}, // will be populated by `synchronize()`
                              .timestamp = timestamp};
 
        ARMARX_CHECK(synchronize(obj, timestamp, providerName));
 
        return obj;
    }
 
    bool
    Reader::synchronize(ArticulatedObject& obj,
                        const armem::Time& timestamp,
                        const std::optional<std::string>& providerName) const
    {
        ARMARX_TRACE;
 
        ARMARX_CHECK_NOT_EMPTY(obj.instance) << "An instance name must be provided!";
 
        auto state = queryState(obj.name(), timestamp, providerName);
 
        if (not state) /* c++20 [[unlikely]] */
        {
            ARMARX_WARNING << "Could not synchronize object " << obj.name() << " with instance "
                           << obj.instance << ". Provider is `" << providerName.value_or("~unset~")
                           << "`.";
            return false;
        }
 
        obj.config = std::move(*state);
        return true;
    }
 
    std::vector<robot_state::description::RobotDescription>
    Reader::queryDescriptions(const armem::Time& timestamp,
                              const std::optional<std::string>& providerName) const
    {
        ARMARX_TRACE;
        // Query all entities from provider.
        armem::client::query::Builder qb;
 
 
        if (providerName.has_value()) // query single provider
        {
            ARMARX_CHECK_NOT_EMPTY(providerName.value());
 
            // clang-format off
            qb
            .coreSegments().withName(objects::constants::CoreClassSegmentName)
            .providerSegments().withName(providerName.value())
            .entities().all()
            .snapshots().latest(); // TODO beforeTime(timestamp);
            // clang-format on
        }
        else // query all providers
        {
            // clang-format off
            qb
            .coreSegments().withName(objects::constants::CoreClassSegmentName)
            .providerSegments().all()
            .entities().all()
            .snapshots().latest(); // TODO beforeTime(timestamp);
            // clang-format on
        }
 
        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);
    }
 
    std::optional<robot_state::description::RobotDescription>
    Reader::queryDescription(const std::string& name,
                             const armem::Time& timestamp,
                             const std::optional<std::string>& providerName) const
    {
        ARMARX_TRACE;
 
        // Query all entities from provider.
        armem::client::query::Builder qb;
 
        if (providerName.has_value()) // query single provider
        {
            ARMARX_CHECK_NOT_EMPTY(providerName.value());
 
            // clang-format off
            qb
            .coreSegments().withName(objects::constants::CoreClassSegmentName)
            .providerSegments().withName(providerName.value())
            .entities().withName(name)
            .snapshots().beforeOrAtTime(timestamp);
            // clang-format on
        }
        else // query all providers
        {
            // clang-format off
            qb
            .coreSegments().withName(objects::constants::CoreClassSegmentName)
            .providerSegments().all()
            .entities().withName(name)
            .snapshots().beforeOrAtTime(timestamp);
            // clang-format on
        }
 
 
        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
        if (not qResult.success) /* c++20 [[unlikely]] */
        {
            ARMARX_WARNING << qResult.errorMessage;
            return std::nullopt;
        }
 
        return getRobotDescription(qResult.memory);
    }
 
    std::optional<robot_state::RobotState>
    Reader::queryState(const std::string& instanceName,
                       const armem::Time& timestamp,
                       const std::optional<std::string>& providerName) const
    {
        ARMARX_TRACE;
 
        // Query all entities from provider.
        armem::client::query::Builder qb;
 
        if (not providerName.has_value())
        {
            // clang-format off
            qb
            .coreSegments().withName(objects::constants::CoreInstanceSegmentName)
            .providerSegments().all() // no specific provider
            .entities().withName(instanceName)
            // .snapshots().beforeOrAtTime(timestamp);
            .snapshots().latest();
            // clang-format on
        }
        else
        {
            ARMARX_CHECK_NOT_EMPTY(providerName.value());
            ARMARX_VERBOSE << "Provider is `" << providerName.value() << "`.";
 
            // clang-format off
            qb
            .coreSegments().withName(objects::constants::CoreInstanceSegmentName)
            .providerSegments().withName(providerName.value()) // agent
            .entities().withName(instanceName)
            // .snapshots().beforeOrAtTime(timestamp);
            .snapshots().latest();
            // clang-format on
        }
 
 
        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 getArticulatedObjectState(qResult.memory);
    }
 
    std::optional<robot_state::RobotState>
    convertToRobotState(const armem::wm::EntityInstance& instance)
    {
        ARMARX_TRACE;
 
        armarx::armem::arondto::ObjectInstance aronObjectInstance;
        try
        {
            aronObjectInstance.fromAron(instance.data());
        }
        catch (...)
        {
            ARMARX_WARNING << "Conversion to ObjectInstance failed!";
            return std::nullopt;
        }
 
        objpose::ObjectPose objectPose;
        objpose::fromAron(aronObjectInstance.pose, objectPose);
 
        robot_state::RobotState robotState{.timestamp = objectPose.timestamp,
                                           .globalPose =
                                               Eigen::Isometry3f(objectPose.objectPoseGlobal),
                                           .jointMap = objectPose.objectJointValues,
                                           .proprioception = std::nullopt};
 
        return robotState;
    }
 
    std::optional<robot_state::RobotState>
    Reader::getArticulatedObjectState(const armarx::armem::wm::Memory& memory) const
    {
        ARMARX_TRACE;
 
        // clang-format off
        const armem::wm::CoreSegment& coreSegment = memory
                .getCoreSegment(objects::constants::CoreInstanceSegmentName);
        // clang-format on
 
        try
        {
            const wm::EntityInstance& instance = coreSegment.getLatestInstance();
            return convertToRobotState(instance);
        }
        catch (...)
        {
            ARMARX_FATAL << "Failed to obtain robot state";
            return std::nullopt;
        }
    }
 
    std::optional<robot_state::description::RobotDescription>
    Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
    {
        ARMARX_TRACE;
 
        // clang-format off
        const armem::wm::CoreSegment& coreSegment = memory.getCoreSegment(objects::constants::CoreClassSegmentName);
        // clang-format on
 
        std::optional<wm::EntityInstance> instance;
        coreSegment.forEachInstance([&instance](const wm::EntityInstance& i) { instance = i; });
 
        if (instance.has_value())
        {
            return convertRobotDescription(instance.value());
        }
 
        ARMARX_DEBUG << "No robot description";
        return std::nullopt;
    }
 
    std::vector<robot_state::description::RobotDescription>
    Reader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
    {
        ARMARX_TRACE;
 
        const armem::wm::CoreSegment& coreSegment =
            memory.getCoreSegment(objects::constants::CoreClassSegmentName);
 
        std::vector<robot_state::description::RobotDescription> descriptions;
        coreSegment.forEachEntity(
            [&descriptions](const wm::Entity& entity)
            {
                if (not entity.empty())
                {
                    const auto robotDescription =
                        convertRobotDescription(entity.getFirstSnapshot().getInstance(0));
                    if (robotDescription)
                    {
                        descriptions.push_back(*robotDescription);
                    }
                }
            });
 
        return descriptions;
    }
 
} // namespace armarx::armem::articulated_object