ResetGazeTargets.cpp

Go to the documentation of this file.

#include "ResetGazeTargets.h"
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/Duration.h>
 
#include <RobotAPI/libraries/skills/core/Skill.h>
#include <RobotAPI/libraries/skills/core/SkillDescription.h>
#include <RobotAPI/libraries/skills/core/SkillID.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
 
#include <armarx/view_selection/gaze_targets/GazeTarget.h>
#include <armarx/view_selection/skills/constants.h>
#include <armarx/view_selection/skills/skill_ids.h>
 
namespace armarx::view_selection::skills
{
 
    ResetGazeTargets::ResetGazeTargets(const Context& services) :
        Base(DefaultSkillDescription()), srv_(services)
    {
        ARMARX_CHECK_NOT_NULL(srv_->viewSelectionClient);
    }
 
    // ::armarx::skills::Skill::InitResult
    // ResetGazeTargets::init(const Base::InitInput& in)
    // {
    //     return ::armarx::skills::Skill::InitResult{
    //         .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    // }
 
    // ::armarx::skills::Skill::MainResult
    // ResetGazeTargets::main(const Base::MainInput& in)
    // {
    //     ARMARX_INFO << "Resetting all gaze targets";
    //     ARMARX_CHECK_NOT_NULL(srv_->viewSelectionClient);
    //     srv_->viewSelectionClient->resetPriorityQueue();
 
    //     return ::armarx::skills::Skill::MainResult{
    //         .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    // }
 
    ::armarx::skills::Skill::InitResult
    ResetGazeTargets::init(const Base::SpecializedInitInput& /*in*/)
    {
        return ::armarx::skills::Skill::InitResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    ::armarx::skills::Skill::MainResult
    ResetGazeTargets::main(const Base::SpecializedMainInput& /*in*/)
    {
        ARMARX_INFO << "Look ahead first, this fixes a bug.";
        // For example, if you:
        // 1) look right (robot will look right)
        // 2) reset gaze targets (robot will look ahead)
        // 3) look down
        // then the robot will not look down as expected without this fix, but instead will look
        // down/right since the last state is still fetched from memory. Calling look ahead before
        // resetting sets the center position again as last location for relative head movement
        // commands.
        callSubskill(skill_ids::LookAhead);
 
        armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(500));
 
        ARMARX_INFO << "Resetting all gaze targets";
        ARMARX_CHECK_NOT_NULL(srv_->viewSelectionClient);
        srv_->viewSelectionClient->resetPriorityQueue();
 
        return ::armarx::skills::Skill::MainResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    void
    ResetGazeTargets::onStopRequested()
    {
        ;
    }
 
    ::armarx::skills::SkillDescription
    ResetGazeTargets::DefaultSkillDescription()
    {
        Params defaults;
 
        return armarx::skills::SkillDescription{
            .skillId = skill_ids::ResetGazeTargets,
            .description = "",
            .rootProfileDefaults = defaults.toAron(),
            .timeout = armarx::core::time::Duration::Seconds(10),
            .parametersType = Params::ToAronType(),
        };
    }
 
} // namespace armarx::view_selection::skills