GazeTarget.cpp

Go to the documentation of this file.

#include "GazeTarget.h"
 
#include <iostream>
#include <optional>
#include <string>
 
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <armarx/view_selection/gaze_targets/TargetPriority.h>
#include <armarx/view_selection/gaze_targets/TargetStatus.h>
 
namespace armarx::view_selection::gaze_targets
{
    GazeTarget::GazeTarget() = default;
 
    GazeTarget::GazeTarget(std::string name,
                           const FramedPosition& position,
                           const TargetPriority& priority,
                           armarx::core::time::Duration duration,
                           const bool keepInQueue) :
        name(name),
        position(position),
        priority(priority),
        duration(duration),
        keepInQueue(keepInQueue)
    {
        creationTimestamp = Clock::Now();
 
        activationTimestamp = armarx::DateTime::Invalid();
        reachedTimestamp = armarx::DateTime::Invalid();
        releasedTimestamp = armarx::DateTime::Invalid();
        abortedTimestamp = armarx::DateTime::Invalid();
    }
 
    bool
    GazeTarget::isActive() const
    {
        return activationTimestamp.isValid();
    }
 
    bool
    GazeTarget::isReached() const
    {
        return reachedTimestamp.isValid();
    }
 
    bool
    GazeTarget::isReleased() const
    {
        return releasedTimestamp.isValid();
    }
 
    bool
    GazeTarget::isAborted() const
    {
        return abortedTimestamp.isValid();
    }
 
    bool
    GazeTarget::isExpired(std::optional<DateTime> reference) const
    {
        // TODO: Expiry time point should actually be the target reached time point + duration, but
        // since target reached cannot be assessed at this point, we use the activation as
        // surrogate.
        DateTime reached = activationTimestamp;
 
        return reached.isValid() and duration.isPositive() and
               reached + duration < reference.value_or(Clock::Now());
    }
 
    ::std::ostream&
    operator<<(::std::ostream& os, const GazeTarget& target)
    {
        return os << "<GazeTarget name=" << target.name << ", creation=" << target.creationTimestamp
                  << ", priority=" << target.priority << ", duration=" << target.duration
                  << ", status=" << TargetStatusNames.to_name(target.status)
                  << ", position=" << target.position.toEigen().transpose() << " (frame "
                  << target.position.agent << "/" << target.position.frame << ")>";
    }
} // namespace armarx::view_selection::gaze_targets