Writer.cpp

Go to the documentation of this file.

#include "Writer.h"
 
#include <memory>
#include <mutex>
#include <string>
#include <type_traits>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/armem/core/Commit.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/aron/core/data/variant/primitive/String.h>
 
#include <armarx/navigation/core/aron/Events.aron.generated.h>
#include <armarx/navigation/core/aron_conversions.h>
#include <armarx/navigation/core/events.h>
#include <armarx/navigation/memory/constants.h>
 
namespace armarx::navigation::memory::client::events
{
 
    std::string
    Writer::propertyPrefix() const
    {
        return "mem.nav.events.";
    }
 
    Writer::Properties
    Writer::defaultProperties() const
    {
        return Properties{
            .memoryName = memory::constants::NavigationMemoryName,
            .coreSegmentName = memory::constants::EventsCoreSegmentName,
            .providerName = "" // clientId
        };
    }
 
    bool
    Writer::store(const armem::Commit& commit)
    {
        std::lock_guard g{writeMutex};
        armem::CommitResult updateResult = memoryWriter().commit(commit);
 
        ARMARX_DEBUG << updateResult;
 
        if (not updateResult.allSuccess())
        {
            ARMARX_WARNING << "Failed to commit to memory: " << updateResult.allErrorMessages();
        }
        return updateResult.allSuccess();
    }
 
    bool
    Writer::store(const armem::EntityUpdate& commit)
    {
        std::lock_guard g{writeMutex};
        const armem::EntityUpdateResult updateResult = memoryWriter().commit(commit);
 
        ARMARX_DEBUG << updateResult;
 
        if (not updateResult.success)
        {
            ARMARX_WARNING << "Failed to commit to memory: " << updateResult.errorMessage;
        }
        return updateResult.success;
    }
 
    bool
    Writer::store(const core::GlobalTrajectoryUpdatedEvent& event, const std::string& clientID)
    {
        return storeImpl<core::arondto::GlobalTrajectoryUpdatedEvent>(
            event, core::event_names::GlobalTrajectoryUpdated, clientID);
    }
 
    bool
    Writer::store(const core::LocalTrajectoryUpdatedEvent& event, const std::string& clientID)
    {
        return storeImpl<core::arondto::LocalTrajectoryUpdatedEvent>(
            event, core::event_names::LocalTrajectoryUpdated, clientID);
    }
 
    bool
    Writer::store(const core::GoalReachedEvent& event, const std::string& clientID)
    {
        return storeImpl<core::arondto::GoalReachedEvent>(
            event, core::event_names::GoalReached, clientID);
    }
 
    bool
    Writer::store(const core::WaypointReachedEvent& event, const std::string& clientID)
    {
        return storeImpl<core::arondto::WaypointReachedEvent>(
            event, core::event_names::WaypointReached, clientID);
    }
 
    bool
    Writer::store(const core::InternalErrorEvent& event, const std::string& clientID)
    {
        return storeImpl<core::arondto::InternalErrorEvent>(
            event, core::event_names::InternalError, clientID);
    }
 
    bool
    Writer::store(const core::GlobalPlanningFailedEvent& event, const std::string& clientID)
    {
        return storeImpl<core::arondto::GlobalPlanningFailedEvent>(
            event, core::event_names::GlobalPlanningFailed, clientID);
    }
 
    bool
    Writer::store(const core::LocalPlanningFailedEvent& event, const std::string& clientID)
    {
        return storeImpl<core::arondto::LocalPlanningFailedEvent>(
            event, core::event_names::LocalPlanningFailed, clientID);
    }
 
    bool
    Writer::store(const core::MovementStartedEvent& event, const std::string& clientID)
    {
        return storeImpl<core::arondto::MovementStartedEvent>(
            event, core::event_names::MovementStarted, clientID);
    }
 
    bool
    Writer::store(const core::UserAbortTriggeredEvent& event, const std::string& clientID)
    {
        return storeImpl<core::arondto::UserAbortTriggeredEvent>(
            event, core::event_names::UserAbortTriggered, clientID);
    }
 
    bool
    Writer::store(const core::SafetyThrottlingTriggeredEvent& event, const std::string& clientID)
    {
        return storeImpl<core::arondto::SafetyThrottlingTriggeredEvent>(
            event, core::event_names::SafetyThrottlingTriggered, clientID);
    }
 
    bool
    Writer::store(const core::SafetyStopTriggeredEvent& event, const std::string& clientID)
    {
        return storeImpl<core::arondto::SafetyStopTriggeredEvent>(
            event, core::event_names::SafetyStopTriggered, clientID);
    }
 
    template <typename AronEventT, typename EventT>
    bool
    Writer::storeImpl(const EventT& event,
                      const std::string& eventName,
                      const std::string& clientID)
    {
        static_assert(std::is_base_of<core::Event, EventT>::value, "Only events supported.");
 
        ARMARX_VERBOSE << "Storing event `" << eventName << "` in memory.";
 
        const auto& timestamp = event.timestamp;
 
        armem::EntityUpdate update;
        update.entityID = armem::MemoryID()
                              .withMemoryName(properties().memoryName)
                              .withCoreSegmentName(properties().coreSegmentName)
                              .withProviderSegmentName(clientID)
                              .withEntityName(eventName)
                              .withTimestamp(timestamp);
        update.referencedTime = timestamp;
 
        const auto dto = core::toAron<AronEventT>(event);
 
        aron::data::DictPtr element(new aron::data::Dict);
        element->setElementCopy("event", std::make_shared<aron::data::String>(eventName));
        element->setElementCopy("data", dto.toAron());
 
        update.instancesData = {element};
 
        return store(update);
    }
} // namespace armarx::navigation::memory::client::events