MemoryStatusPublisher.cpp

Go to the documentation of this file.

#include "MemoryStatusPublisher.h"
 
#include <mutex>
#include <string>
#include <utility>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/Clock.h>
 
#include <armarx/view_selection/gaze_targets/TargetStatus.h>
 
namespace armarx::view_selection::gaze_scheduler
{
 
    MemoryStatusPublisher::MemoryStatusPublisher(
        std::experimental::observer_ptr<memory::client::status::Writer> writer) :
        writer(std::move(writer))
    {
        ARMARX_CHECK_NOT_NULL(this->writer);
    }
 
    void
    MemoryStatusPublisher::publish(
        const armem::MemoryID& gazeTargetID,
        const std::function<void(gaze_targets::GazeTargetStatus&)>& mutate)
    {
        gaze_targets::GazeTargetStatus published;
 
        {
            const std::scoped_lock lock(mutex);
 
            gaze_targets::GazeTargetStatus& lifecycle = lifecycles[gazeTargetID];
            lifecycle.gazeTargetID = gazeTargetID;
            mutate(lifecycle);
 
            published = lifecycle;
 
            if (lifecycle.isTerminal())
            {
                lifecycles.erase(gazeTargetID);
            }
        }
 
        writer->store(published);
    }
 
    void
    MemoryStatusPublisher::requested(const armem::MemoryID& gazeTargetID,
                                     const armarx::DateTime& creationTimestamp)
    {
        publish(gazeTargetID,
                [&](gaze_targets::GazeTargetStatus& lifecycle)
                {
                    lifecycle.status = gaze_targets::TargetStatus::Requested;
                    lifecycle.creationTimestamp = creationTimestamp;
                });
    }
 
    void
    MemoryStatusPublisher::scheduled(const armem::MemoryID& gazeTargetID)
    {
        publish(gazeTargetID,
                [&](gaze_targets::GazeTargetStatus& lifecycle)
                {
                    lifecycle.status = gaze_targets::TargetStatus::Scheduled;
                    lifecycle.activationTimestamp = armarx::Clock::Now();
                });
    }
 
    void
    MemoryStatusPublisher::reached(const armem::MemoryID& gazeTargetID,
                                   const gaze_controller::GazeResidual& residual)
    {
        publish(gazeTargetID,
                [&](gaze_targets::GazeTargetStatus& lifecycle)
                {
                    lifecycle.status = gaze_targets::TargetStatus::Reached;
                    lifecycle.reachedTimestamp = armarx::Clock::Now();
                    lifecycle.angularError = residual.angularError;
                    lifecycle.lateralError = residual.lateralError;
                });
    }
 
    void
    MemoryStatusPublisher::preempted(const armem::MemoryID& gazeTargetID, const std::string& reason)
    {
        publish(gazeTargetID,
                [&](gaze_targets::GazeTargetStatus& lifecycle)
                {
                    lifecycle.status = gaze_targets::TargetStatus::Preempted;
                    lifecycle.preemptedTimestamp = armarx::Clock::Now();
                    lifecycle.message = reason;
                });
    }
 
    void
    MemoryStatusPublisher::released(const armem::MemoryID& gazeTargetID, const std::string& reason)
    {
        publish(gazeTargetID,
                [&](gaze_targets::GazeTargetStatus& lifecycle)
                {
                    lifecycle.status = gaze_targets::TargetStatus::Released;
                    lifecycle.releasedTimestamp = armarx::Clock::Now();
                    lifecycle.message = reason;
                });
    }
 
    void
    MemoryStatusPublisher::aborted(const armem::MemoryID& gazeTargetID, const std::string& reason)
    {
        publish(gazeTargetID,
                [&](gaze_targets::GazeTargetStatus& lifecycle)
                {
                    lifecycle.status = gaze_targets::TargetStatus::Aborted;
                    lifecycle.abortedTimestamp = armarx::Clock::Now();
                    lifecycle.message = reason;
                });
    }
 
    bool
    MemoryStatusPublisher::wasReached(const armem::MemoryID& gazeTargetID) const
    {
        const std::scoped_lock lock(mutex);
 
        const auto it = lifecycles.find(gazeTargetID);
 
        return it != lifecycles.end() and it->second.wasReached();
    }
 
} // namespace armarx::view_selection::gaze_scheduler