SimpleStatusHandler.cpp

Go to the documentation of this file.

#include "SimpleStatusHandler.h"
 
#include <mutex>
#include <vector>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <armarx/view_selection/gaze_targets/TargetStatus.h>
 
namespace armarx::view_selection::client
{
 
    void
    SimpleStatusHandler::onRequested(const Callback& callback)
    {
        const std::scoped_lock lock(callbackMutex);
        subscriptions.requested.push_back(callback);
    }
 
    void
    SimpleStatusHandler::onScheduled(const Callback& callback)
    {
        const std::scoped_lock lock(callbackMutex);
        subscriptions.scheduled.push_back(callback);
    }
 
    void
    SimpleStatusHandler::onReached(const Callback& callback)
    {
        const std::scoped_lock lock(callbackMutex);
        subscriptions.reached.push_back(callback);
    }
 
    void
    SimpleStatusHandler::onPreempted(const Callback& callback)
    {
        const std::scoped_lock lock(callbackMutex);
        subscriptions.preempted.push_back(callback);
    }
 
    void
    SimpleStatusHandler::onReleased(const Callback& callback)
    {
        const std::scoped_lock lock(callbackMutex);
        subscriptions.released.push_back(callback);
    }
 
    void
    SimpleStatusHandler::onAborted(const Callback& callback)
    {
        const std::scoped_lock lock(callbackMutex);
        subscriptions.aborted.push_back(callback);
    }
 
    void
    SimpleStatusHandler::onAny(const Callback& callback)
    {
        const std::scoped_lock lock(callbackMutex);
        subscriptions.any.push_back(callback);
    }
 
    void
    SimpleStatusHandler::handleStatus(const gaze_targets::GazeTargetStatus& status)
    {
        // ARMARX_INFO (not DEBUG) on purpose, and here rather than in each skill: paired with the
        // publish-side log in the scheduler's status writer, the combined log shows the memory
        // round trip for every transition, for both delivery modes and every client.
        ARMARX_INFO << "Received gaze target status "
                    << QUOTED(gaze_targets::TargetStatusNames.to_name(status.status)) << " for "
                    << QUOTED(status.gazeTargetID.str()) << " at " << armarx::Clock::Now() << ".";
 
        const std::vector<Callback> callbacks = [&]()
        {
            const std::scoped_lock lock(callbackMutex);
 
            std::vector<Callback> matching = subscriptions.any;
 
            const std::vector<Callback>& specific = [&]() -> const std::vector<Callback>&
            {
                switch (status.status)
                {
                    case gaze_targets::TargetStatus::Requested:
                        return subscriptions.requested;
                    case gaze_targets::TargetStatus::Scheduled:
                        return subscriptions.scheduled;
                    case gaze_targets::TargetStatus::Reached:
                        return subscriptions.reached;
                    case gaze_targets::TargetStatus::Preempted:
                        return subscriptions.preempted;
                    case gaze_targets::TargetStatus::Released:
                        return subscriptions.released;
                    case gaze_targets::TargetStatus::Aborted:
                        return subscriptions.aborted;
                }
 
                return subscriptions.any;
            }();
 
            matching.insert(matching.end(), specific.begin(), specific.end());
 
            return matching;
        }();
 
        // Called without the lock: a callback may well register another one, or block.
        for (const auto& callback : callbacks)
        {
            callback(status);
        }
    }
 
} // namespace armarx::view_selection::client