MemoryStatusHandlerBase.cpp

Go to the documentation of this file.

#include "MemoryStatusHandlerBase.h"
 
#include <algorithm>
#include <mutex>
#include <vector>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
 
#include <armarx/view_selection/gaze_targets/GazeTargetStatus.h>
#include <armarx/view_selection/gaze_targets/aron/GazeTargetStatus.aron.generated.h>
#include <armarx/view_selection/gaze_targets/aron_conversions.h>
#include <armarx/view_selection/memory/constants.h>
 
namespace armarx::view_selection::client
{
 
    void
    MemoryStatusHandlerBase::handleMemory(const armem::wm::Memory& memory)
    {
        const std::scoped_lock lock(statusHandlingMutex);
 
        const armem::wm::CoreSegment* coreSegment =
            memory.findCoreSegment(memory::constants::GazeTargetStatusCoreSegmentName);
 
        if (coreSegment == nullptr)
        {
            return;
        }
 
        std::vector<armem::wm::EntityInstance> instances;
        coreSegment->forEachInstance([&](const armem::wm::EntityInstance& instance)
                                     { instances.push_back(instance); });
 
        // Transitions must be handled in the order they happened, or a client could see `Reached`
        // after the `Released` that followed it.
        std::sort(instances.begin(),
                  instances.end(),
                  [](const armem::wm::EntityInstance& lhs, const armem::wm::EntityInstance& rhs)
                  {
                      ARMARX_CHECK(lhs.id().hasTimestamp());
                      ARMARX_CHECK(rhs.id().hasTimestamp());
 
                      return lhs.id().timestamp < rhs.id().timestamp;
                  });
 
        for (const auto& instance : instances)
        {
            gaze_targets::GazeTargetStatus status;
            fromAron(gaze_targets::arondto::GazeTargetStatus::FromAron(instance.data()), status);
 
            handleStatus(status);
        }
    }
 
} // namespace armarx::view_selection::client