SetCustomGazeTarget.cpp

Go to the documentation of this file.

#include "SetCustomGazeTarget.h"
 
#include <memory>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <armarx/view_selection/gaze_targets/GazeTarget.h>
 
namespace armarx::view_selection::skills
{
 
 
    SetCustomGazeTarget::SetCustomGazeTarget(const Context& services) :
        Base(DefaultSkillDescription()), srv_(services)
    {
    }
 
    void
    SetCustomGazeTarget::connect(const Context& srv)
    {
        srv_.emplace(srv);
    }
 
    ::armarx::skills::Skill::InitResult
    SetCustomGazeTarget::init(const Base::SpecializedInitInput& in)
    {
        gazeTarget.emplace(); // populate optional
        std::string frame = in.parameters.frame;
        std::string agent = in.parameters.agent;
        FramedPosition targetPos{
            Eigen::Vector3f{in.parameters.x, in.parameters.y, in.parameters.z}, frame, agent};
        gaze_targets::TargetPriority priority{gaze_targets::AttentionType::TaskDriven,
                                              in.parameters.priority};
        armarx::core::time::Duration duration =
            armarx::core::time::Duration::MilliSeconds(in.parameters.duration_ms);
        gaze_targets::GazeTarget target{
            "SetCustomGazeTarget", targetPos, priority, duration, false};
        gazeTarget.emplace(target);
 
        //fromAron(in.params.target, gazeTarget.value());
 
        return ::armarx::skills::Skill::InitResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    ::armarx::skills::Skill::MainResult
    SetCustomGazeTarget::main(const Base::SpecializedMainInput& in)
    {
        ARMARX_CHECK(gazeTarget.has_value());
 
        ARMARX_INFO << "Committing gaze target " << gazeTarget.value();
        srv_->viewSelectionClient.commitGazeTarget(gazeTarget.value());
 
        return ::armarx::skills::Skill::MainResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    void
    SetCustomGazeTarget::onStopRequested()
    {
        if (gazeTarget.has_value())
        {
            srv_->viewSelectionClient.commitGazeTarget(gazeTarget.value());
        }
    }
 
    armarx::skills::SkillDescription
    SetCustomGazeTarget::DefaultSkillDescription()
    {
        ParamType defaultParameters;
        defaultParameters.frame = "root";
        defaultParameters.agent = "Armar7";
        defaultParameters.priority = 1;
        defaultParameters.duration_ms = 1000;
        defaultParameters.x = 0;
        defaultParameters.y = 1500;
        defaultParameters.z = 1500;
        return armarx::skills::SkillDescription{
            .skillId =
                armarx::skills::SkillID{.skillName = constants::skill_names::SetCustomGazeTarget},
            .description = "Set a gaze target by coordinate, agent and frame. Supported frames "
                           "include the Global frame and the robot root frames.",
            .rootProfileDefaults = defaultParameters.toAron(),
            .timeout = armarx::core::time::Duration::MilliSeconds(500),
            .parametersType = Params::ToAronType(),
        };
    }
 
 
} // namespace armarx::view_selection::skills