GazeTargetWaiter.cpp

Go to the documentation of this file.

#include "GazeTargetWaiter.h"
 
#include <chrono>
#include <mutex>
#include <string>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <armarx/view_selection/client/services/StatusHandlerInterface.h>
#include <armarx/view_selection/gaze_targets/TargetStatus.h>
 
namespace armarx::view_selection::client
{
 
    namespace
    {
        /// How often the abort predicate is re-checked while blocked.
        constexpr auto abortPollInterval = std::chrono::milliseconds(50);
    } // namespace
 
    GazeTargetWaiter::Result::operator bool() const
    {
        return status.wasReached();
    }
 
    GazeTargetWaiter::GazeTargetWaiter(StatusHandlerInterface& handler)
    {
        handler.onAny([this](const gaze_targets::GazeTargetStatus& status)
                      { handleStatus(status); });
    }
 
    void
    GazeTargetWaiter::expect(const std::string& targetName)
    {
        const std::scoped_lock lock(mutex);
 
        // A fresh expectation: anything left from an earlier run of the same target is stale.
        expectations[targetName] = Expectation{};
    }
 
    void
    GazeTargetWaiter::bind(const std::string& targetName, const armem::MemoryID& gazeTargetID)
    {
        const std::scoped_lock lock(mutex);
 
        const auto it = expectations.find(targetName);
 
        if (it == expectations.end())
        {
            return;
        }
 
        it->second.gazeTargetID = gazeTargetID.getEntitySnapshotID();
 
        // An update may already have arrived under the name alone.  If it was about a different
        // request, it is not ours after all.
        if (it->second.status.gazeTargetID.hasEntityName() and
            it->second.status.gazeTargetID.getEntitySnapshotID() != *it->second.gazeTargetID)
        {
            it->second.status = gaze_targets::GazeTargetStatus{};
            it->second.settled = false;
        }
    }
 
    void
    GazeTargetWaiter::forget(const std::string& targetName)
    {
        const std::scoped_lock lock(mutex);
 
        expectations.erase(targetName);
    }
 
    void
    GazeTargetWaiter::handleStatus(const gaze_targets::GazeTargetStatus& status)
    {
        {
            const std::scoped_lock lock(mutex);
 
            const auto it = expectations.find(status.gazeTargetID.entityName);
 
            if (it == expectations.end())
            {
                return;
            }
 
            // Once bound, only the exact request counts.  This is what keeps two targets that are
            // in flight under the same name apart.
            if (it->second.gazeTargetID.has_value() and
                status.gazeTargetID.getEntitySnapshotID() != *it->second.gazeTargetID)
            {
                return;
            }
 
            it->second.status = status;
            it->second.settled = status.wasReached() or status.isTerminal();
        }
 
        condition.notify_all();
    }
 
    GazeTargetWaiter::Result
    GazeTargetWaiter::wait(const std::string& targetName,
                           const armarx::Duration& timeout,
                           const std::function<bool()>& shouldAbort)
    {
        // A non-positive timeout means "no deadline", as it does for a gaze target's own duration.
        const armarx::DateTime deadline =
            timeout.isPositive() ? armarx::Clock::Now() + timeout : armarx::DateTime::Invalid();
        const armarx::DateTime start = armarx::Clock::Now();
 
        if (deadline.isValid())
        {
            ARMARX_INFO << "Waiting until gaze target " << QUOTED(targetName)
                        << " is reached (timeout " << timeout.toMilliSecondsDouble() << " ms).";
        }
        else
        {
            ARMARX_INFO << "Waiting until gaze target " << QUOTED(targetName)
                        << " is reached (no timeout).";
        }
 
        Result result;
 
        {
            std::unique_lock lock(mutex);
 
            const auto it = expectations.find(targetName);
 
            if (it == expectations.end())
            {
                ARMARX_WARNING << "No expectation armed for gaze target " << QUOTED(targetName)
                               << ". Did you forget to call expect() before committing it?";
                return Result{.status = {}, .message = "not armed"};
            }
 
            while (not it->second.settled)
            {
                if (shouldAbort and shouldAbort())
                {
                    result.message = "aborted by caller";
                    break;
                }
 
                if (deadline.isValid() and armarx::Clock::Now() >= deadline)
                {
                    result.message = "wait timeout expired";
                    break;
                }
 
                condition.wait_for(lock, abortPollInterval);
            }
 
            result.status = it->second.status;
            expectations.erase(it);
        }
 
        const armarx::Duration elapsed = armarx::Clock::Now() - start;
 
        ARMARX_INFO << "Finished waiting for gaze target " << QUOTED(targetName) << " after "
                    << elapsed.toMilliSecondsDouble() << " ms: "
                    << QUOTED(gaze_targets::TargetStatusNames.to_name(result.status.status))
                    << (result.message.empty() ? std::string() : " (" + result.message + ")")
                    << ", reached: " << (result ? "yes" : "no") << ".";
 
        return result;
    }
 
} // namespace armarx::view_selection::client