GazeTargetReachedMonitor.cpp

Go to the documentation of this file.

#include "GazeTargetReachedMonitor.h"
 
#include <mutex>
 
namespace armarx::view_selection::gaze_scheduler
{
 
    GazeTargetReachedMonitor::GazeTargetReachedMonitor(const GazeReachedConfig& config) :
        config(config)
    {
    }
 
    void
    GazeTargetReachedMonitor::reset(const std::int64_t targetId)
    {
        const std::scoped_lock lock(mutex);
 
        activeTargetId = targetId;
        reachedCount = 0;
        unreachableCount = 0;
        latestResidual = gaze_controller::GazeResidual{};
    }
 
    void
    GazeTargetReachedMonitor::update(const std::int64_t targetId,
                                     const gaze_controller::GazeResidual& residual,
                                     const bool targetReachable)
    {
        const std::scoped_lock lock(mutex);
 
        // Stale report about a target we already moved on from.
        if (targetId != activeTargetId)
        {
            return;
        }
 
        latestResidual = residual;
 
        const bool onTarget =
            residual.angularError <= config.angularTh and residual.lateralError <= config.lateralTh;
 
        reachedCount = onTarget ? reachedCount + 1 : 0;
        unreachableCount = targetReachable ? 0 : unreachableCount + 1;
    }
 
    bool
    GazeTargetReachedMonitor::reached() const
    {
        const std::scoped_lock lock(mutex);
 
        return reachedCount >= config.filterCount;
    }
 
    bool
    GazeTargetReachedMonitor::unreachable() const
    {
        const std::scoped_lock lock(mutex);
 
        return unreachableCount >= config.filterCount;
    }
 
    gaze_controller::GazeResidual
    GazeTargetReachedMonitor::residual() const
    {
        const std::scoped_lock lock(mutex);
 
        return latestResidual;
    }
 
} // namespace armarx::view_selection::gaze_scheduler