RobotWriter.cpp

Go to the documentation of this file.

#include "RobotWriter.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 "RobotAPI/libraries/armem/core/forward_declarations.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/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/RobotState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/TransformHeader.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
{
    RobotWriter::RobotWriter(const RobotWriter& r) :
        properties(r.properties), propertyPrefix(r.propertyPrefix), memoryWriter(r.memoryWriter)
    {
    }
 
    RobotWriter::~RobotWriter() = default;
 
    void
    RobotWriter::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
    {
    }
 
    void
    RobotWriter::connect(armem::client::MemoryNameSystem& memoryNameSystem)
    {
        // Wait for the memory to become available and add it as dependency.
        ARMARX_IMPORTANT << "RobotWriter: Waiting for memory '" << constants::memoryName << "' ...";
        try
        {
            memoryWriter = memoryNameSystem.useWriter(constants::memoryName);
            ARMARX_IMPORTANT << "RobotWriter: Connected to memory '" << constants::memoryName
                             << "'";
        }
        catch (const armem::error::CouldNotResolveMemoryServer& e)
        {
            ARMARX_ERROR << e.what();
            return;
        }
    }
 
    bool
    RobotWriter::storeDescription(const description::RobotDescription& description,
                                  const armem::Time& timestamp)
    {
        const auto validTimestamp = timestamp.isInvalid() ? armarx::Clock::Now() : timestamp;
 
        const auto providerId = armem::MemoryID(
            constants::memoryName, constants::descriptionCoreSegment, description.name);
        const auto entityID =
            providerId.withEntityName(constants::descriptionEntityName).withTimestamp(timestamp);
 
        armem::EntityUpdate update;
        update.entityID = entityID;
 
        arondto::RobotDescription aronDescription;
        toAron(aronDescription, description);
 
        update.instancesData = {aronDescription.toAron()};
        update.referencedTime = timestamp;
 
        ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
        armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
 
        ARMARX_DEBUG << updateResult;
 
        if (not updateResult.success)
        {
            ARMARX_ERROR << updateResult.errorMessage;
            return false;
        }
 
        return true;
    }
 
    bool
    RobotWriter::storeLocalization(const Eigen::Matrix4f& globalRootPose,
                                   const std::string& robotName,
                                   const std::string& robotRootNodeName,
                                   const armem::Time& timestamp)
    {
 
        const auto providerId =
            armem::MemoryID(constants::memoryName, constants::localizationCoreSegment, robotName);
        const auto entityID = providerId.withEntityName(GlobalFrame + "," + robotRootNodeName)
                                  .withTimestamp(timestamp);
 
        armem::EntityUpdate update;
        update.entityID = entityID;
 
        arondto::Transform aronTransform;
 
        // populate aronTransform
        {
            aronTransform.header.agent = robotName;
            aronTransform.header.frame = robotRootNodeName;
            aronTransform.header.parentFrame = GlobalFrame;
            aronTransform.header.timestamp = timestamp;
 
            aronTransform.transform = globalRootPose;
        }
 
        update.instancesData = {aronTransform.toAron()};
        update.referencedTime = timestamp;
 
        ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
        armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
 
        ARMARX_DEBUG << updateResult;
 
        if (not updateResult.success)
        {
            ARMARX_ERROR << updateResult.errorMessage;
            return false;
        }
 
        return true;
    }
 
    bool
    RobotWriter::storeProprioception(const std::map<std::string, float>& jointMap,
                                     const std::string& robotTypeName,
                                     const std::string& robotName,
                                     const armem::Time& timestamp)
    {
 
        const auto providerId = armem::MemoryID(
            constants::memoryName, constants::proprioceptionCoreSegment, robotTypeName);
        const auto entityID = providerId.withEntityName(robotName).withTimestamp(timestamp);
 
        armem::EntityUpdate update;
        update.entityID = entityID;
 
        arondto::Proprioception aronProprioception;
        aronProprioception.joints.position = jointMap;
 
        update.instancesData = {aronProprioception.toAron()};
        update.referencedTime = timestamp;
 
        ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
        armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
 
        ARMARX_DEBUG << updateResult;
 
        if (not updateResult.success)
        {
            ARMARX_ERROR << updateResult.errorMessage;
            return false;
        }
 
        return true;
    }
 
    bool
    RobotWriter::storeState(const RobotState& state,
                            const std::string& robotTypeName,
                            const std::string& robotName,
                            const std::string& robotRootNodeName)
    {
        const bool successLoc = storeLocalization(
            state.globalPose.matrix(), robotName, robotRootNodeName, state.timestamp);
        const bool successProp =
            storeProprioception(state.jointMap, robotTypeName, robotName, state.timestamp);
 
        return successLoc and successProp;
    }
 
 
} // namespace armarx::armem::robot_state