GazeTargetReachedMonitor.h

Go to the documentation of this file.

#pragma once
 
#include <cstdint>
#include <mutex>
 
#include <VirtualRobot/MathTools.h>
 
#include <armarx/view_selection/gaze_controller/gaze_residual.h>
 
namespace armarx::view_selection::gaze_scheduler
{
 
    /**
     * @brief Thresholds deciding when the gaze counts as fixed on the active target.
     *
     * These live on the scheduler rather than in the controller config so they can be retuned
     * without redeploying the RobotUnit, and so they mean the same thing for every controller
     * variant (see gaze_controller::computeGazeResidual).
     */
    struct GazeReachedConfig
    {
        /// Angular gaze error below which the target counts as fixated [rad].
        float angularTh = VirtualRobot::MathTools::deg2rad(2.F);
 
        /// Lateral miss distance below which the target counts as fixated [mm].
        float lateralTh = 30.F;
 
        /// Consecutive controller reports required before a decision is taken.  At the controller's
        /// 10 Hz report rate, 5 is half a second of steady gaze.
        int filterCount = 5;
    };
 
    /**
     * @brief Debounces the low-level controller's residual into a "reached" / "unreachable"
     * decision for one control target.
     *
     * Modelled on navigation's server::GoalReachedMonitor.  Reports about any other target are
     * dropped, so a residual still in flight for a superseded target can never be misattributed.
     *
     * Thread-safe: reports arrive on the topic listener thread, the verdict is read by the
     * scheduler's periodic task.
     */
    class GazeTargetReachedMonitor
    {
    public:
        explicit GazeTargetReachedMonitor(const GazeReachedConfig& config);
 
        /// Begin monitoring another control target, dropping all state about the previous one.
        void reset(std::int64_t targetId);
 
        /// Feed one controller report.  Reports about a different target are ignored.
        void update(std::int64_t targetId,
                    const gaze_controller::GazeResidual& residual,
                    bool targetReachable);
 
        /// Whether the gaze has been on target for `filterCount` consecutive reports.
        bool reached() const;
 
        /// Whether the controller could not reach the target for `filterCount` consecutive reports.
        bool unreachable() const;
 
        /// Residual of the most recently accepted report.
        gaze_controller::GazeResidual residual() const;
 
    private:
        const GazeReachedConfig config;
 
        mutable std::mutex mutex;
 
        std::int64_t activeTargetId = -1;
 
        int reachedCount = 0;
 
        int unreachableCount = 0;
 
        gaze_controller::GazeResidual latestResidual;
    };
 
} // namespace armarx::view_selection::gaze_scheduler