MemoryPolling.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/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/util/StringHelpers.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/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/selectors.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/MemoryPolling.h>
#include <armarx/navigation/core/aron/Events.aron.generated.h>
#include <armarx/navigation/core/aron_conversions.h>
#include <armarx/navigation/core/events.h>
 
#include <range/v3/algorithm/for_each.hpp>
#include <range/v3/algorithm/sort.hpp>
 
namespace armarx::navigation::client
{
 
    MemoryPolling::MemoryPolling(const std::string& callerId,
                                 armem::client::MemoryNameSystem& mns) :
        callerId(callerId), memoryNameSystem(mns), lastMemoryEventReceived(armem::Time::Now())
    {
        const std::string memoryName = "Navigation";
 
        ARMARX_TRACE;
        memoryReader = memoryNameSystem.getReader(armem::MemoryID().withMemoryName(memoryName));
 
        ARMARX_INFO << "MemoryPolling: will handle all events newer than "
                    << lastMemoryEventReceived << ".";
 
        // subscription api
        // armem::MemoryID subscriptionID;
        // subscriptionID.memoryName = memoryName;
        // subscriptionID.coreSegmentName = "Events";
        // subscriptionID.providerSegmentName = callerId;
 
        ARMARX_TRACE;
        // memoryNameSystem.subscribe(subscriptionID, this, &MemoryPolling::onEntityUpdate);
        task = new PeriodicTask<MemoryPolling>(this, &MemoryPolling::runPollMemoryEvents, 100);
        task->start();
    }
 
    void
    MemoryPolling::onEntityUpdate(const std::vector<armem::MemoryID>& snapshotIDs)
    {
        ARMARX_INFO << "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
    MemoryPolling::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_IMPORTANT << "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::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 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
        {
            ARMARX_WARNING << "Unknown event " << QUOTED(eventName);
        }
    }
 
    MemoryPolling::~MemoryPolling()
    {
        ARMARX_INFO << "Stopping event polling";
        task->stop();
        ARMARX_INFO << "done.";
    }
 
    void
    MemoryPolling::runPollMemoryEvents()
    {
        ARMARX_TRACE;
 
        const armem::Time now = armem::Time::Now();
 
        armem::client::query::Builder qb;
        auto& q = qb.coreSegments().withName("Events");
 
        armem::client::query::ProviderSegmentSelector& qq =
            [&]() -> armem::client::query::ProviderSegmentSelector&
        {
            if (callerId.empty())
            {
                return q.providerSegments().all();
            }
            return q.providerSegments().withName(callerId);
        }();
 
 
        qq.entities().all().snapshots().timeRange(lastMemoryEventReceived, now);
 
        // ARMARX_DEBUG << "Polling memory events in interval "
        //              << "[" << lastMemoryPoll << ", " << now << "]";
 
        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 << deactivateSpam(0.1F) << "Memory lookup failed.";
            return;
        }
 
        handleEvents(qResult.memory);
    }
 
    void
    MemoryPolling::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;
        };
 
        ranges::sort(events, sortByTimestampAsc);
 
        if (not events.empty())
        {
            ARMARX_INFO << "Updating memory timestamp to " << events.back().id().timestamp;
            // we add 1 µs to ensure that the events are only handled once
            lastMemoryEventReceived =
                events.back().id().timestamp + armarx::Duration::MicroSeconds(1);
        }
 
        ranges::for_each(events, [&](const auto& event) { handleEvent(event); });
    }
 
 
} // namespace armarx::navigation::client