SetCustomGazeTarget.cpp

Go to the documentation of this file.

#include "SetCustomGazeTarget.h"
 
#include <memory>
#include <string>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Duration.h>
 
#include <RobotAPI/libraries/core/FramedPose.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/AttentionType.h>
#include <armarx/view_selection/gaze_targets/GazeTarget.h>
#include <armarx/view_selection/gaze_targets/TargetPriority.h>
#include <armarx/view_selection/skills/constants.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{
            in.parameters.gazeTargetName, 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());
 
        if (in.parameters.waitUntilFinished)
        {
            ARMARX_IMPORTANT << "NYI! Waiting for head to reach target (waitUntilFinished) "
                                "parameter is not yet implemented.";
        }
 
        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.gazeTargetName = "SetCustomGazeTarget";
        defaultParameters.priority = 1;
        defaultParameters.duration_ms = 1000;
        defaultParameters.x = 0;
        defaultParameters.y = 1500;
        defaultParameters.z = 1500;
        // waitUntilFinished must be "false" for legacy reasons.  The parameter has been
        // retrospectively introduced and this makes it backwards-compatible.
        defaultParameters.waitUntilFinished = false;
        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. Please note that "
                           "the waitUntilFinished parameter is not yet implemented.",
            .rootProfileDefaults = defaultParameters.toAron(),
            .timeout = armarx::core::time::Duration::MilliSeconds(5000),
            .parametersType = Params::ToAronType(),
        };
    }
 
 
} // namespace armarx::view_selection::skills