Scheduler.cpp

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     VS_CODE_USER_NAME ( VS_CODE_USER_EMAIL )
 * @date       2023
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "Scheduler.h"
 
#include <ArmarXCore/core/logging/Logging.h>
 
namespace armarx::view_selection::gaze_scheduler
{
 
    void
    Scheduler::init()
    {
        robot_ = srv.virtualRobotReader->getRobotWaiting(
            params.robotName,
            armarx::DateTime::Invalid(),
            VirtualRobot::RobotIO::RobotDescription::eStructure);
        ARMARX_CHECK_NOT_NULL(robot_) << params.robotName;
 
        srv.controllerHandler->createController();
        submitIdleTarget();
 
        Clock::WaitFor(Duration::MilliSeconds(500));
 
        srv.controllerHandler->activateController();
 
        resetPriorityQueue();
 
        task = new armarx::SimplePeriodicTask<>([&]() { runPeriodically(); }, 100);
        task->start();
    }
 
    Scheduler::Scheduler(const InjectedServices& srv, const Params& params) :
        srv(srv), params(params)
    {
        init();
    }
 
    void
    Scheduler::resetPriorityQueue()
    {
        ARMARX_VERBOSE << "Resetting PriorityQueue, old requests are discarded";
        {
            std::scoped_lock<std::mutex> targetLock(targetMutex);
            requestedTargets.clear();
        }
 
        ARMARX_VERBOSE << "Removing current target";
        currentTargetBuffer.getWriteBuffer() = std::nullopt;
        currentTargetBuffer.commitWrite();
 
        ARMARX_INFO << "Adding idle target.";
        submitIdleTarget();
    }
 
    void
    Scheduler::checkTargetDeadline()
    {
        bool deadlineViolation = false;
        {
            std::scoped_lock<std::mutex> targetLock(targetMutex);
            if (currentDeadline.isValid() && currentDeadline <= armarx::Clock::Now())
            {
                deadlineViolation = true;
                ARMARX_VERBOSE << "Deadline reached, deactivating target.";
                currentDeadline = DateTime::Invalid();
 
                currentTargetBuffer.getWriteBuffer() = std::nullopt;
                currentTargetBuffer.commitWrite();
            }
        }
        if (deadlineViolation)
        {
            scheduleNextTarget();
        }
    }
 
    void
    Scheduler::submitIdleTarget()
    {
        ARMARX_INFO << "Default target is " << params.defaultTarget;
 
        const FramedPosition viewCenter(
            params.defaultTarget, robot_->getRootNode()->getName(), robot_->getName());
 
        const auto infinite = armarx::Duration::MilliSeconds(-1);
        const gaze_targets::TargetPriority priority(gaze_targets::AttentionType::RandomEvent, 0.0);
 
        // this target has lowest priority and will never be removed from the queue
        {
            std::scoped_lock<std::mutex> targetLock(targetMutex);
            requestedTargets.emplace(
                gaze_targets::GazeTarget("idle", viewCenter, priority, infinite, true));
        }
 
        scheduleNextTarget();
    }
 
    void
    Scheduler::visualizeActiveTarget()
    {
        if (not params.visuEnabled)
        {
            return;
        }
 
        const std::string activeTargetLayerName = "active_target";
 
        const auto currentTarget = currentTargetBuffer.getUpToDateReadBuffer();
 
        if (not currentTarget.has_value()) // disable visu if target is not active
        {
            ARMARX_CHECK_NOT_NULL(srv.arviz);
            srv.arviz->commitDeleteLayer(activeTargetLayerName);
            return;
        }
 
        if (not srv.virtualRobotReader->synchronizeRobot(*robot_, armarx::Clock::Now()))
        {
            ARMARX_VERBOSE << "Failed to synchronize robot. Cannot visualize targets.";
            return;
        }
 
        const Eigen::Vector3f 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::blue(255, params.targetVizAlpha)},
            {gaze_targets::AttentionType::TaskDriven,
             simox::Color::red(255, params.targetVizAlpha)}};
 
        // TODO include priority into coloring
 
        auto l = srv.arviz->layer(activeTargetLayerName);
        l.add(armarx::viz::Sphere("target")
                  .position(globalPosition)
                  .radius(100)
                  .color(attentionTypeColor.at(currentTarget->priority.attentionType)));
        srv.arviz->commit(l);
    }
 
    void
    Scheduler::runPeriodically()
    {
        checkTargetDeadline();
        visualizeActiveTarget();
    }
 
    void
    Scheduler::submitToQueue(const gaze_targets::GazeTarget& target)
    {
        {
            std::scoped_lock<std::mutex> targetLock(targetMutex);
 
            // remove target if found in queue
            // (otherwise, two targets with the same name but different priorities will be in the queue)
            for (auto it = requestedTargets.cbegin(); it != requestedTargets.cend(); it++)
            {
                if (it->name == target.name)
                {
                    ARMARX_VERBOSE << "Updating targetRequest: " << target.name;
                    requestedTargets.erase(it);
                    break;
                }
            }
            // multiset automatically ensures order
            requestedTargets.emplace(target);
        }
    }
 
    void
    Scheduler::scheduleNextTarget()
    {
        std::scoped_lock<std::mutex> targetLock(targetMutex);
        if (requestedTargets.empty())
        {
            ARMARX_VERBOSE << "No requests, nothing to do.";
            return;
        }
 
        // reference to targetRequest with highest priority
        // this will be only used in one of the if-else cases so that they do not
        // influence each other
        auto nextTarget = requestedTargets.cbegin();
        ARMARX_VERBOSE << "Next target is " << nextTarget->name;
 
        ARMARX_DEBUG << "Targets in queue";
        for (const auto& target : requestedTargets)
        {
            ARMARX_DEBUG << target;
        }
 
        const auto currentTarget = currentTargetBuffer.getUpToDateReadBuffer();
 
        if (not currentTarget.has_value())
        {
            ARMARX_VERBOSE << "No currently active Targets, submitting new target: " << *nextTarget;
 
            submitControlTarget(*nextTarget);
 
            // keep or erase target after submission based on keepInQueue
            if (nextTarget->keepInQueue)
            {
                ARMARX_VERBOSE << "The target is kept in the queue.";
            }
            else
            {
                requestedTargets.erase(nextTarget);
                ARMARX_VERBOSE << "The target has been removed from the queue.";
            }
        }
        else
        {
            if (nextTarget->priority >= currentTarget->priority)
            {
                ARMARX_VERBOSE << "requested Target priority > current Target priority ("
                               << nextTarget->priority << " > " << currentTarget->priority << ")";
                // replaced target needs to be stopped properly, then next target will be
                // scheduled automatically
                submitControlTarget(*nextTarget);
            }
            else
            {
                ARMARX_VERBOSE << "current Target has higher priority (" << nextTarget->priority
                               << " <= " << currentTarget->priority
                               << ") waiting until it is reached";
            }
        }
    }
 
    void
    Scheduler::submitControlTarget(const gaze_targets::GazeTarget& target)
    {
        if (target.duration.toMicroSeconds() < 0)
        {
            // infinite duration
            currentDeadline = armarx::DateTime::Invalid();
        }
        else
        {
            currentDeadline = armarx::Clock::Now() + target.duration;
        }
 
        currentTargetBuffer.getWriteBuffer() = target;
        currentTargetBuffer.commitWrite();
 
        srv.controllerHandler->updateControllerTarget(target);
    }
 
 
} // namespace armarx::view_selection::gaze_scheduler