status_reporting.h

Go to the documentation of this file.

#pragma once
 
#include <atomic>
#include <cstdint>
#include <mutex>
#include <string>
 
#include <ArmarXCore/core/time/DateTime.h>
 
#include <armarx/view_selection/gaze_controller/ice/GazeControllerStatus.h>
#include <armarx/view_selection/gaze_controller/gaze_residual.h>
 
namespace armarx::view_selection::gaze_controller
{
 
    /**
     * @brief Collects the gaze residual on the control side and reports it on a topic.
     *
     * Shared by both controller variants, which differ only in where they can compute the residual:
     * `atan2` does it in rtRun, `gaze_ik` in its non-RT IK task.  Both hand it over through atomics.
     *
     * The controller deliberately does not decide whether the gaze counts as fixed on the target --
     * it only says how far off it is.  The gaze scheduler applies the thresholds, since it is the
     * part that knows which target is active and who asked for it.  That is also why nothing here
     * touches the memory: this runs inside the RobotUnit.
     */
    class StatusReporter
    {
    public:
        /// Called from the control side, at whatever rate it computes the residual.
        void update(std::int64_t targetId, const GazeResidual& residual, bool targetReachable);
 
        /// Called from updateConfig(), so a report can name the target it is about.
        void setTarget(std::int64_t targetId, const std::string& targetName);
 
        /**
         * @brief Publishes the current residual, rate limited.  Call from onPublish().
         *
         * @param topic  May be null before the controller is connected; then nothing is sent.
         */
        void report(const GazeControllerStatusListenerPrx& topic);
 
    private:
        std::atomic<std::int64_t> targetId = -1;
 
        std::atomic<float> angularError = 0.F;
 
        std::atomic<float> lateralError = 0.F;
 
        std::atomic<bool> targetReachable = true;
 
        std::mutex targetNameMutex;
 
        std::int64_t targetNameId = -1;
 
        std::string targetName;
 
        armarx::DateTime lastReported = armarx::DateTime::Invalid();
    };
 
} // namespace armarx::view_selection::gaze_controller