MemorySubscriber.cpp

Go to the documentation of this file.

#include <algorithm>
#include <mutex>
#include <string>
#include <type_traits>
#include <vector>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Query.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedClass.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
 
#include <armarx/navigation/client/services/MemorySubscriber.h>
#include <armarx/navigation/core/aron/Events.aron.generated.h>
#include <armarx/navigation/core/aron_conversions.h>
#include <armarx/navigation/core/events.h>
 
namespace armarx::navigation::client
{
 
    MemorySubscriber::MemorySubscriber(const std::string& callerId,
                                       armem::client::MemoryNameSystem& mns) :
        callerId(callerId), memoryNameSystem(mns), lastMemoryPoll(armem::Time::Now())
    {
        const std::string memoryName = "Navigation";
 
        ARMARX_TRACE;
        memoryReader = memoryNameSystem.getReader(armem::MemoryID().withMemoryName(memoryName));
 
        ARMARX_INFO << "MemorySubscriber: will handle all events for caller id " << QUOTED(callerId)
                    << " newer than " << lastMemoryPoll << ".";
 
        // subscription api
        armem::MemoryID subscriptionID;
        subscriptionID.memoryName = memoryName;
        subscriptionID.coreSegmentName = "Events";
        subscriptionID.providerSegmentName = callerId;
 
        ARMARX_TRACE;
        subscriptionHandle =
            memoryNameSystem.subscribe(subscriptionID, this, &MemorySubscriber::onEntityUpdate);
    }
 
    void
    MemorySubscriber::onEntityUpdate(const std::vector<armem::MemoryID>& snapshotIDs)
    {
        ARMARX_VERBOSE << "Received " << snapshotIDs.size() << " events from memory";
        const armem::client::QueryResult qResult = memoryReader.queryMemoryIDs(snapshotIDs);
 
        if (not qResult.success) /* c++20 [[unlikely]] */
        {
            ARMARX_WARNING << deactivateSpam(0.1F) << "Memory lookup failed.";
            return;
        }
 
        ARMARX_TRACE;
        handleEvents(qResult.memory);
    }
 
    template <typename AronEventT, typename EventT>
    auto
    fromAron(const armem::wm::EntityInstance& entity, EventT& bo)
    {
        static_assert(std::is_base_of<armarx::aron::codegenerator::cpp::AronGeneratedClass,
                                      AronEventT>::value);
 
        // see events::Writer::storeImpl
        const auto dataDict =
            aron::data::Dict::DynamicCastAndCheck(entity.data()->getElement("data"));
        ARMARX_CHECK_NOT_NULL(dataDict);
        const auto dto = AronEventT::FromAron(dataDict);
 
        core::fromAron(dto, bo);
        return bo;
    }
 
    void
    MemorySubscriber::handleEvent(const armem::wm::EntityInstance& memoryEntity)
    {
        ARMARX_TRACE;
 
        // only one event will be handled at a time
        std::lock_guard g{eventHandlingMtx};
 
        ARMARX_CHECK(memoryEntity.id().hasEntityName());
        const auto& eventName = memoryEntity.id().entityName;
 
        ARMARX_VERBOSE << "Handling event " << QUOTED(eventName);
 
        // factory
        if (eventName == core::event_names::GlobalPlanningFailed)
        {
            core::GlobalPlanningFailedEvent evt;
            fromAron<core::arondto::GlobalPlanningFailedEvent>(memoryEntity, evt);
 
            globalPlanningFailed(evt);
        }
        else if (eventName == core::event_names::LocalPlanningFailed)
        {
            core::LocalPlanningFailedEvent evt;
            fromAron<core::arondto::LocalPlanningFailedEvent>(memoryEntity, evt);
 
            localPlanningFailed(evt);
        }
        else if (eventName == core::event_names::MovementStarted)
        {
            core::MovementStartedEvent evt;
            fromAron<core::arondto::MovementStartedEvent>(memoryEntity, evt);
 
            movementStarted(evt);
        }
        else if (eventName == core::event_names::GoalReached)
        {
            core::GoalReachedEvent evt;
            fromAron<core::arondto::GoalReachedEvent>(memoryEntity, evt);
 
            goalReached(evt);
        }
        else if (eventName == core::event_names::WaypointReached)
        {
            core::WaypointReachedEvent evt;
            fromAron<core::arondto::WaypointReachedEvent>(memoryEntity, evt);
 
            waypointReached(evt);
        }
        else if (eventName == core::event_names::SafetyThrottlingTriggered)
        {
            core::SafetyThrottlingTriggeredEvent evt;
            fromAron<core::arondto::SafetyThrottlingTriggeredEvent>(memoryEntity, evt);
 
            safetyThrottlingTriggered(evt);
        }
        else if (eventName == core::event_names::SafetyStopTriggered)
        {
            core::SafetyStopTriggeredEvent evt;
            fromAron<core::arondto::SafetyStopTriggeredEvent>(memoryEntity, evt);
 
            safetyStopTriggered(evt);
        }
        else if (eventName == core::event_names::UserAbortTriggered)
        {
            core::UserAbortTriggeredEvent evt;
            fromAron<core::arondto::UserAbortTriggeredEvent>(memoryEntity, evt);
 
            userAbortTriggered(evt);
        }
        else if (eventName == core::event_names::InternalError)
        {
            core::InternalErrorEvent evt;
            fromAron<core::arondto::InternalErrorEvent>(memoryEntity, evt);
 
            internalError(evt);
        }
        else if (eventName == core::event_names::GlobalTrajectoryUpdated)
        {
            core::GlobalTrajectoryUpdatedEvent evt;
            fromAron<core::arondto::GlobalTrajectoryUpdatedEvent>(memoryEntity, evt);
 
            globalTrajectoryUpdated(evt);
        }
        else if (eventName == core::event_names::LocalTrajectoryUpdated)
        {
            core::LocalTrajectoryUpdatedEvent evt;
            fromAron<core::arondto::LocalTrajectoryUpdatedEvent>(memoryEntity, evt);
 
            localTrajectoryUpdated(evt);
        }
        else
        {
            ARMARX_WARNING << "Unknown event " << QUOTED(eventName);
        }
    }
 
    MemorySubscriber::~MemorySubscriber() = default;
 
    void
    MemorySubscriber::handleEvents(const armem::wm::Memory& memory)
    {
        ARMARX_TRACE;
 
        // clang-format off
        const armem::wm::CoreSegment& coreSegment = memory
                .getCoreSegment("Events");
        // clang-format on
 
        std::vector<armem::wm::EntityInstance> events;
 
        coreSegment.forEachInstance([&](const armem::wm::EntityInstance& instance)
                                    { events.push_back(instance); });
 
        const auto sortByTimestampAsc = [](const armem::wm::EntityInstance& a,
                                           const armem::wm::EntityInstance& b) -> bool
        {
            ARMARX_CHECK(a.id().hasTimestamp());
            ARMARX_CHECK(b.id().hasTimestamp());
 
            return a.id().timestamp < b.id().timestamp;
        };
 
        std::sort(events.begin(), events.end(), sortByTimestampAsc);
 
        std::for_each(events.begin(), events.end(), [&](const auto& event) { handleEvent(event); });
    }
 
 
} // namespace armarx::navigation::client