Writer.cpp

Go to the documentation of this file.

#include "Writer.h"
 
#include <mutex>
#include <string>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/armem/core/Commit.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
 
#include <armarx/navigation/core/aron/Trajectory.aron.generated.h>
#include <armarx/navigation/core/aron_conversions.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/memory/constants.h>
#include <armarx/navigation/server/StackResult.h>
 
namespace armarx::navigation::memory::client::stack_result
{
    bool
    Writer::store(const server::StackResult& result, const std::string& clientID)
    {
        // single ice call for all updates
        armem::Commit commit;
 
        storePrepare(result.globalPlan, clientID, commit);
        storePrepare(result.localTrajectory, clientID, commit);
        // storePrepare(result.controlVelocity, clientID, commit);
        // FIXME storePrepare(result.safeVelocity, clientID, commit);
 
        std::lock_guard g{writeMutex};
        armem::CommitResult updateResult = memoryWriter().commit(commit);
 
        ARMARX_DEBUG << updateResult;
 
        if (not updateResult.allSuccess())
        {
            ARMARX_ERROR << updateResult.allErrorMessages();
        }
        return updateResult.allSuccess();
    }
 
    bool
    Writer::store(const armarx::navigation::global_planning::GlobalPlannerResult& result,
                  const std::string& clientID)
    {
        armem::Commit commit;
        storePrepare(result, clientID, commit);
        std::lock_guard g{writeMutex};
        armem::CommitResult updateResult = memoryWriter().commit(commit);
        return updateResult.allSuccess();
    }
 
    bool
    Writer::store(const armarx::navigation::local_planning::LocalPlannerResult& result,
                  const std::string& clientID)
    {
        armem::Commit commit;
        storePrepare(result, clientID, commit);
        std::lock_guard g{writeMutex};
        armem::CommitResult updateResult = memoryWriter().commit(commit);
        return updateResult.allSuccess();
    }
 
    bool
    Writer::storePrepare(const armarx::navigation::global_planning::GlobalPlannerResult& result,
                         const std::string& clientID,
                         armem::Commit& commit)
    {
        // FIXME timestamp should be part of GlobalPlannerResult
        armem::Time timestamp = armem::Time::Now();
 
        armem::EntityUpdate update;
        update.entityID = armem::MemoryID()
                              .withMemoryName(properties().memoryName)
                              .withCoreSegmentName(constants::GlobalPlannerResultCoreSegment)
                              .withProviderSegmentName(clientID)
                              .withEntityName("trajectory")
                              .withTimestamp(timestamp);
 
 
        core::arondto::GlobalTrajectory aronDto;
        // FIXME create Aron type
        core::toAron(aronDto, result.trajectory);
 
        update.referencedTime = timestamp;
        update.instancesData = {aronDto.toAron()};
 
        commit.add(update);
 
        return true;
    }
 
    bool
    Writer::storePrepare(const local_planning::LocalPlannerResult& result,
                         const std::string& clientID,
                         armem::Commit& commit)
    {
        // FIXME timestamp should be part of GlobalPlannerResult
        armem::Time timestamp = armem::Time::Now();
 
        armem::EntityUpdate update;
        update.entityID = armem::MemoryID()
                              .withMemoryName(properties().memoryName)
                              .withCoreSegmentName(constants::LocalPlannerResultCoreSegment)
                              .withProviderSegmentName(clientID)
                              .withEntityName("trajectory")
                              .withTimestamp(timestamp);
 
 
        core::arondto::LocalTrajectory aronDto;
        // FIXME create Aron type
        toAron(aronDto, result.trajectory);
 
        update.referencedTime = timestamp;
        update.instancesData = {aronDto.toAron()};
 
        commit.add(update);
 
        return true;
    }
 
    std::string
    Writer::propertyPrefix() const
    {
        return "mem.nav.stack_result.";
    }
 
    Writer::Properties
    Writer::defaultProperties() const
    {
        return Properties{
            .memoryName = "Navigation",
            .coreSegmentName = "", // here we use multiple
            .providerName = "" // must be set by user
        };
    }
 
} // namespace armarx::navigation::memory::client::stack_result