Scheduler.cpp

Go to the documentation of this file.

#include "Scheduler.h"
 
#include <map>
#include <mutex>
#include <optional>
#include <string>
#include <vector>
 
#include <algorithm>
 
#include <SimoxUtility/color/Color.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <RobotAPI/libraries/armem/core/MemoryID.h>
 
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <armarx/view_selection/gaze_targets/AttentionType.h>
#include <armarx/view_selection/gaze_targets/GazeTarget.h>
#include <armarx/view_selection/gaze_targets/TargetPriority.h>
#include <armarx/view_selection/gaze_targets/TargetStatus.h>
 
namespace armarx::view_selection::gaze_scheduler
{
 
    Scheduler::Scheduler(const InjectedServices& srv, const Params& params) :
        srv(srv), params(params), reachedMonitor(params.reached)
    {
        init();
    }
 
    void
    Scheduler::init()
    {
        robot_ = srv.virtualRobotReader->getRobotWaiting(
            params.robotName,
            armarx::DateTime::Invalid(),
            VirtualRobot::RobotIO::RobotDescription::eStructure);
        ARMARX_CHECK_NOT_NULL(robot_) << params.robotName;
 
        auto const infinite = armarx::Duration::MilliSeconds(-1);
        bool constexpr keepInQueue = true;
 
        _idleGazeTarget.name = "idle";
        _idleGazeTarget.position = FramedPosition(
            params.defaultTarget, robot_->getRootNode()->getName(), robot_->getName());
        _idleGazeTarget.priority =
            gaze_targets::TargetPriority(gaze_targets::AttentionType::RandomEvent, 0.0);
        _idleGazeTarget.duration = infinite;
        _idleGazeTarget.keepInQueue = keepInQueue;
        _idleGazeTarget.creationTimestamp = DateTime::Now();
        _idleGazeTarget.activationTimestamp = DateTime::Now();
 
        srv.controllerHandler->createController();
        resetRequestedTargets();
 
        Clock::WaitFor(Duration::MilliSeconds(500));
        srv.controllerHandler->activateController();
 
        if (task and task->isRunning())
        {
            task->stop();
        }
 
        task = new armarx::SimplePeriodicTask<>([&]() { scheduleNextTarget(); }, 100);
        task->start();
 
        if (visualizationTask and visualizationTask->isRunning())
        {
            visualizationTask->stop();
        }
 
        visualizationTask =
            new armarx::SimplePeriodicTask<>([&]() { visualizeActiveTarget(); }, 100);
        visualizationTask->start();
    }
 
    void
    Scheduler::resetPriorityQueue()
    {
        _clearGazeTargets.store(true);
    }
 
    void
    Scheduler::scheduleNextTarget()
    {
        bool constexpr debugQueue = false;
 
        auto debug = [&](std::string const& step)
        {
            if (not debugQueue)
            {
                return;
            }
 
            int i = 0;
            ARMARX_INFO << "Gaze target queue contents (" << step << "): ";
            for (auto const& targetInQueue : requestedTargets)
            {
                ARMARX_INFO << "    #" << i++ << " " << targetInQueue;
            }
        };
 
        if (requestedTargets.size() == 0)
        {
            ARMARX_WARNING << "No gaze targets in queue. At least the idle gaze target should be "
                              "in the queue.";
            return;
        }
 
        // Get new gaze targets and copy them into this thread.
        std::vector<gaze_targets::GazeTarget> newGazeTargets = [&]
        {
            std::scoped_lock<std::mutex> targetLock(_newGazeTargetsMutex);
            std::vector<gaze_targets::GazeTarget> copy = _newGazeTargets;
            _newGazeTargets.clear();
            return copy;
        }();
 
        // Get the reference timestamp for this scheduling step.
        DateTime const scheduleStep = Clock::Now();
 
        debug("before filter");
 
        // First, filter all requested targets that have expired. Consider all expired (except for
        // idle gaze target) if clearing all gaze targets was requested.
        if (_clearGazeTargets.load())
        {
            resetRequestedTargets();
            _clearGazeTargets.store(false);
        }
        else
        {
            RequestedTargets newRequestedTargets;
 
            for (auto const& targetInQueue : requestedTargets)
            {
                if (not targetInQueue.isExpired(scheduleStep))
                {
                    newRequestedTargets.emplace(targetInQueue);
                }
                else
                {
                    ARMARX_INFO << "Gaze target " << targetInQueue.name << " expired.";
                    leaveActiveSlot(targetInQueue, "duration elapsed", false);
                }
            }
 
            requestedTargets = newRequestedTargets;
        }
 
        debug("after filter, before insertion");
 
        // Second, add new gaze targets, replace task level targets or existing ones with the same
        // name if applicable.
        for (auto const& target : newGazeTargets)
        {
            RequestedTargets newRequestedTargets;
            newRequestedTargets.emplace(target);
 
            for (auto const& targetInQueue : requestedTargets)
            {
                if (targetInQueue.name == target.name)
                {
                    // Do not keep targets with the same name in the queue, since they should be
                    // overwritten with the new one.
                }
                else if (target.priority.attentionType ==
                             gaze_targets::AttentionType::TaskDriven and
                         targetInQueue.priority.attentionType ==
                             gaze_targets::AttentionType::TaskDriven and
                         not targetInQueue.keepInQueue)
                {
                    // New target is task driven, and this target is too and should not be kept in
                    // the queue.
                }
                else
                {
                    // Keep in queue.
                    newRequestedTargets.emplace(targetInQueue);
                }
            }
 
            requestedTargets = newRequestedTargets;
        }
 
        debug("after insertion, before selection");
 
        // Third, use the first gaze target as next gaze target.  It should be the first in the
        // queue.
        std::optional<gaze_targets::GazeTarget> const maybeNextTarget =
            [&]() -> std::optional<gaze_targets::GazeTarget>
        {
            if (requestedTargets.empty())
            {
                return std::nullopt;
            }
 
            return *requestedTargets.begin();
        }();
 
        if (not maybeNextTarget.has_value())
        {
            ARMARX_WARNING << "Could not find any gaze targets after filtering. At least the idle "
                              "gaze target was expected. Trying to recover by resetting gaze "
                              "targets queue ...";
            _clearGazeTargets.store(true); // Try to reset gaze targets list to recover.
            return;
        }
 
        auto const currentTarget = currentTargetBuffer.getUpToDateReadBuffer();
        gaze_targets::GazeTarget const nextTarget = maybeNextTarget.value();
 
        if (not currentTarget.has_value() or currentTarget->isExpired() or
            nextTarget != currentTarget)
        {
            if (currentTarget.has_value() and currentTarget->name != nextTarget.name)
            {
                const bool staysQueued =
                    currentTarget->keepInQueue and
                    std::any_of(requestedTargets.begin(),
                                requestedTargets.end(),
                                [&](const gaze_targets::GazeTarget& queued)
                                { return queued.name == currentTarget->name; });
 
                leaveActiveSlot(*currentTarget, "preempted by " + nextTarget.name, staysQueued);
            }
 
            submitControlTarget(nextTarget);
        }
    }
 
    void
    Scheduler::submitControlTarget(gaze_targets::GazeTarget const& target)
    {
        ARMARX_INFO << "Scheduling " << target << " now.";
 
        ++currentTargetId;
        reachedMonitor.reset(currentTargetId);
        reachedEmitted = false;
        unreachableEmitted = false;
 
        currentTargetBuffer.getWriteBuffer() = target;
        currentTargetBuffer.commitWrite();
        srv.controllerHandler->updateControllerTarget(target, currentTargetId);
 
        if (const std::optional<armem::MemoryID> gazeTargetID = gazeTargetIDOf(target.name))
        {
            srv.statusPublisher->scheduled(*gazeTargetID);
        }
    }
 
    std::optional<armem::MemoryID>
    Scheduler::gazeTargetIDOf(const std::string& targetName) const
    {
        const auto it = gazeTargetIDs.find(targetName);
 
        if (it == gazeTargetIDs.end())
        {
            return std::nullopt;
        }
 
        return it->second;
    }
 
    void
    Scheduler::leaveActiveSlot(const gaze_targets::GazeTarget& target,
                               const std::string& reason,
                               const bool staysQueued)
    {
        const std::optional<armem::MemoryID> gazeTargetID = gazeTargetIDOf(target.name);
 
        if (not gazeTargetID.has_value())
        {
            return;
        }
 
        if (staysQueued)
        {
            srv.statusPublisher->preempted(*gazeTargetID, reason);
            return;
        }
 
        // Terminal: whether the requester got what it asked for is decided by whether the gaze ever
        // settled on this target, not by how it ended.
        if (srv.statusPublisher->wasReached(*gazeTargetID))
        {
            srv.statusPublisher->released(*gazeTargetID, reason);
        }
        else
        {
            srv.statusPublisher->aborted(*gazeTargetID, reason);
        }
 
        gazeTargetIDs.erase(target.name);
    }
 
    void
    Scheduler::handleControllerStatus(const gaze_controller::GazeControllerStatus& status)
    {
        const gaze_controller::GazeResidual residual{.angularError = status.angularError,
                                                     .lateralError = status.lateralError};
 
        reachedMonitor.update(status.targetId, residual, status.targetReachable);
 
        const auto currentTarget = currentTargetBuffer.getUpToDateReadBuffer();
 
        if (not currentTarget.has_value())
        {
            return;
        }
 
        const std::optional<armem::MemoryID> gazeTargetID = gazeTargetIDOf(currentTarget->name);
 
        if (not gazeTargetID.has_value())
        {
            return;
        }
 
        if (not reachedEmitted and reachedMonitor.reached())
        {
            reachedEmitted = true;
 
            // ARMARX_INFO on purpose: this is the first of the three hops the combined log shows
            // for a transition (detect here, commit in the publisher, receive in the client).
            ARMARX_INFO << "Gaze target " << QUOTED(currentTarget->name)
                        << " reached (angular error " << reachedMonitor.residual().angularError
                        << " rad, lateral error " << reachedMonitor.residual().lateralError
                        << " mm); controller reported at " << status.timestamp.timeSinceEpoch
                        << ", handled at " << armarx::Clock::Now() << ".";
 
            srv.statusPublisher->reached(*gazeTargetID, reachedMonitor.residual());
        }
 
        if (not unreachableEmitted and not reachedEmitted and reachedMonitor.unreachable())
        {
            unreachableEmitted = true;
 
            ARMARX_INFO << "Gaze target " << QUOTED(currentTarget->name)
                        << " cannot be reached by the controller; aborting it.";
 
            srv.statusPublisher->aborted(*gazeTargetID,
                                         "unreachable: the controller cannot look there");
            gazeTargetIDs.erase(currentTarget->name);
        }
    }
 
    void
    Scheduler::resetRequestedTargets()
    {
        ARMARX_INFO << "Resetting gaze targets queue, old requests are discarded.";
 
        for (const auto& targetInQueue : requestedTargets)
        {
            leaveActiveSlot(targetInQueue, "queue reset", false);
        }
 
        requestedTargets.clear();
 
        ARMARX_INFO << "Adding idle target `" << params.defaultTarget.transpose() << "`.";
 
        currentTargetBuffer.getWriteBuffer() = std::nullopt;
        currentTargetBuffer.commitWrite();
 
        requestedTargets.emplace(_idleGazeTarget);
    }
 
    void
    Scheduler::visualizeActiveTarget()
    {
        if (not(params.visualizeTaskDrivenGazeTarget or params.visualizeStimulusDrivenTarget or
                params.visualizeRandomEventTarget))
        {
            return;
        }
 
        std::string const activeTargetLayerName = "active_target";
 
        auto const currentTarget = currentTargetBuffer.getUpToDateReadBuffer();
 
        ARMARX_CHECK_NOT_NULL(srv.arviz);
 
        bool visualize = currentTarget.has_value();
 
        if (currentTarget.has_value())
        {
            switch (currentTarget->priority.attentionType)
            {
                case gaze_targets::AttentionType::TaskDriven:
                    visualize = params.visualizeTaskDrivenGazeTarget;
                    break;
                case gaze_targets::AttentionType::StimulusDriven:
                    visualize = params.visualizeStimulusDrivenTarget;
                    break;
                case gaze_targets::AttentionType::RandomEvent:
                    visualize = params.visualizeRandomEventTarget;
                    break;
            }
        }
 
        // Do not visualize if current gaze target has no value or if current gaze target is an
        // random event but only stimulus- and task-driven gaze targets should be visualized.
        if (not visualize)
        {
            srv.arviz->commitDeleteLayer(activeTargetLayerName);
            return;
        }
 
        if (not srv.virtualRobotReader->synchronizeRobot(*robot_, armarx::Clock::Now()))
        {
            ARMARX_VERBOSE << "Failed to synchronize robot. Cannot visualize targets.";
            return;
        }
 
        Eigen::Vector3f const globalPosition = currentTarget->position.toGlobalEigen(robot_);
 
        std::map<gaze_targets::AttentionType, simox::Color> attentionTypeColor{
            {gaze_targets::AttentionType::RandomEvent,
             simox::Color::gray(128, params.targetVizAlpha)},
            {gaze_targets::AttentionType::StimulusDriven,
             simox::Color::orange(255, params.targetVizAlpha)},
            {gaze_targets::AttentionType::TaskDriven,
             simox::Color::red(255, params.targetVizAlpha)}};
 
        auto const color = attentionTypeColor.at(currentTarget->priority.attentionType);
        Eigen::Vector3f const from =
            robot_->getRobotNode(params.gazeOriginFrameName)->getGlobalPosition();
        Eigen::Vector3f const direction_vec = (globalPosition - from).normalized();
        Eigen::Vector3f const to = (globalPosition - from).norm() > 750
                                       ? Eigen::Vector3f(from + (direction_vec * 700))
                                       : Eigen::Vector3f(globalPosition - (direction_vec * 50));
 
        auto l = srv.arviz->layer(activeTargetLayerName);
        l.add(armarx::viz::Sphere("target").position(globalPosition).radius(25).color(color));
        l.add(armarx::viz::Arrow("gaze_direction")
                  .fromTo(from + (direction_vec * 50), to)
                  .width(10)
                  .color(color));
        srv.arviz->commit(l);
    }
 
    void
    Scheduler::submitToQueue(gaze_targets::GazeTarget target, armem::MemoryID gazeTargetID)
    {
        {
            std::scoped_lock<std::mutex> targetLock(_newGazeTargetsMutex);
            target.activationTimestamp = armarx::Clock::Now(); // TODO: This is a hack for now.
            _newGazeTargets.push_back(target);
        }
 
        if (not gazeTargetID.hasEntityName())
        {
            // The idle target: nobody requested it, so there is nobody to report to.
            return;
        }
 
        gazeTargetIDs[target.name] = gazeTargetID;
 
        // Tells a waiting client the scheduler has seen its request -- which distinguishes "queued
        // behind something" from "the scheduler is not running".
        srv.statusPublisher->requested(gazeTargetID, target.creationTimestamp);
    }
 
    Scheduler::~Scheduler()
    {
 
        if (task->isRunning())
        {
            ARMARX_INFO << "Stopping task";
            task->stop();
        }
 
        if (visualizationTask->isRunning())
        {
            ARMARX_INFO << "Stopping visualization task";
            visualizationTask->stop();
        }
    }
} // namespace armarx::view_selection::gaze_scheduler