status_reporting.cpp

Go to the documentation of this file.

#include "status_reporting.h"
 
#include <mutex>
#include <string>
 
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/ice_conversions.h>
 
#include <armarx/view_selection/gaze_controller/constants.h>
 
namespace armarx::view_selection::gaze_controller
{
 
    void
    StatusReporter::update(const std::int64_t targetId,
                           const GazeResidual& residual,
                           const bool targetReachable)
    {
        this->angularError.store(residual.angularError);
        this->lateralError.store(residual.lateralError);
        this->targetReachable.store(targetReachable);
 
        // Written last: the scheduler drops reports whose id it does not recognize, so an id must
        // never be paired with a residual older than itself.
        this->targetId.store(targetId);
    }
 
    void
    StatusReporter::setTarget(const std::int64_t targetId, const std::string& targetName)
    {
        const std::scoped_lock lock(targetNameMutex);
 
        this->targetNameId = targetId;
        this->targetName = targetName;
    }
 
    void
    StatusReporter::report(const GazeControllerStatusListenerPrx& topic)
    {
        // Ice proxy handles do not compare against nullptr; they convert to bool.
        if (not topic)
        {
            return;
        }
 
        const armarx::DateTime now = armarx::Clock::Now();
 
        // onPublish runs at the RobotUnit's publish rate, far above what the scheduler needs to
        // debounce a decision.
        if (lastReported.isValid() and
            now - lastReported < constants::GazeControllerStatusPublishInterval)
        {
            return;
        }
 
        lastReported = now;
 
        GazeControllerStatus status;
        status.targetId = targetId.load();
        status.angularError = angularError.load();
        status.lateralError = lateralError.load();
        status.targetReachable = targetReachable.load();
        armarx::core::time::toIce(status.timestamp, now);
 
        {
            const std::scoped_lock lock(targetNameMutex);
 
            // Only if the name belongs to the target the residual was measured against.
            status.targetName = targetNameId == status.targetId ? targetName : std::string();
        }
 
        topic->reportGazeControllerStatus(status);
    }
 
} // namespace armarx::view_selection::gaze_controller