commit_and_wait.cpp

Go to the documentation of this file.

#include "commit_and_wait.h"
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
 
#include <armarx/view_selection/client/GazeTargetWaiter.h>
#include <armarx/view_selection/client/ViewSelection.h>
 
namespace armarx::view_selection::skills
{
 
    ::armarx::skills::Skill::MainResult
    commitAndMaybeWait(client::ViewSelection& viewSelection,
                       const gaze_targets::GazeTarget& target,
                       const bool waitUntilReached,
                       const std::int64_t waitTimeout_ms,
                       const std::function<bool()>& shouldAbort)
    {
        ARMARX_INFO << "Committing gaze target " << target;
 
        if (not waitUntilReached)
        {
            viewSelection.commitGazeTarget(target);
 
            return ::armarx::skills::Skill::MainResult{
                .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
        }
 
        const client::GazeTargetWaiter::Result result = viewSelection.commitGazeTargetAndWait(
            target, armarx::Duration::MilliSeconds(waitTimeout_ms), shouldAbort);
 
        if (result)
        {
            return ::armarx::skills::Skill::MainResult{
                .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
        }
 
        ARMARX_WARNING << "Gaze target " << QUOTED(target.name) << " was not reached: "
                       << (result.message.empty() ? result.status.message : result.message);
 
        // Do not leave a target we gave up on competing for the head.
        viewSelection.releaseGazeTarget(target);
 
        return ::armarx::skills::Skill::MainResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Failed};
    }
 
} // namespace armarx::view_selection::skills