LookAtObject.cpp

Go to the documentation of this file.

#include "LookAtObject.h"
 
#include <Eigen/Core>
 
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/Metronome.h>
 
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
#include <RobotAPI/libraries/aron/core/aron_conversions.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/skills/provider/SpecializedSkillProxy.h>
 
#include <armarx/view_selection/skills/aron/LookAt.aron.generated.h>
#include <armarx/view_selection/skills/constants.h>
#include <armarx/view_selection/skills/skill_ids.h>
 
namespace armarx::view_selection::skills
{
    LookAtObject::LookAtObject(const Services& srv) : Base(DefaultSkillDescription()), srv_(srv)
    {
    }
 
    ::armarx::skills::Skill::InitResult
    LookAtObject::init(const Base::SpecializedInitInput& in)
    {
        return ::armarx::skills::Skill::InitResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    ::armarx::skills::Skill::MainResult
    LookAtObject::main(const Base::SpecializedMainInput& in)
    {
        ARMARX_INFO << "Querying object";
 
        ObjectID objectID;
        fromAron(in.parameters.object, objectID);
 
        const auto result = srv_.objectInstanceReader->queryLatestObjectInstance(objectID);
 
        if (not result)
        {
            ARMARX_INFO << "Memory query did not provide any observation for " << objectID;
            return MakeFailedResult();
        }
 
        const armarx::objpose::ObjectPose objectObservation = result.value();
 
        const Eigen::Vector3f globalPosition = objectObservation.objectPoseGlobal.block<3, 1>(0, 3);
 
        // generate view target
        const armarx::view_selection::gaze_targets::arondto::GazeTarget gazeTarget = [&]()
        {
            armarx::view_selection::gaze_targets::arondto::GazeTarget gazeTarget;
            gazeTarget.name = "LookAtObject_" + objectID.str();
 
            gazeTarget.position.header.agent = ""; // not needed
            gazeTarget.position.header.frame = armarx::GlobalFrame; // not needed
 
            gazeTarget.position.position = globalPosition;
 
            gazeTarget.duration =
                armarx::core::time::Duration::SecondsDouble(in.parameters.durationSeconds);
 
            gazeTarget.priority.attentionType =
                gaze_targets::arondto::AttentionType::ImplEnum::TaskDriven;
            gazeTarget.priority.priority = in.parameters.priority;
 
            gazeTarget.creationTimestamp = Clock::Now();
            gazeTarget.keepInQueue = false;
 
            return gazeTarget;
        }();
 
        // execute subskill
        {
            using Parameters = armarx::view_selection::skills::arondto::LookAtParams;
 
            armarx::skills::SpecializedSkillProxy<Parameters> subSkill(
                manager,
                ::armarx::skills::SkillID{
                    .providerId =
                        ::armarx::skills::ProviderID{.providerName =
                                                         armarx::view_selection::skills::constants::
                                                             ViewSelectionSkillProviderName},
                    .skillName = armarx::view_selection::skills::constants::skill_names::LookAt});
            Parameters subSkillParams = subSkill.getRootProfileParameters();
 
            subSkillParams.target = gazeTarget;
 
            auto update = callSubskill(subSkill, subSkillParams.toAron());
 
            if (update.has_value() and not update->hasBeenSucceeded())
            {
                return MakeFailedResult();
            }
        }
 
        return MakeSucceededResult();
    }
 
    void
    LookAtObject::onStopRequested()
    {
    }
 
    ::armarx::skills::SkillDescription
    LookAtObject::DefaultSkillDescription()
    {
        Params defaults;
        defaults.priority = 100;
        //defaults.maxObservationAgeSeconds = 15 * 60; // quater of an hour
        defaults.durationSeconds = 1.5;
 
        return armarx::skills::SkillDescription{
            .skillId = skill_ids::LookAtObject,
            .description = "Look at the location where an instance of the object class has most "
                           "recently been seen.",
            .rootProfileDefaults = defaults.toAron(),
            .timeout = armarx::Duration::Minutes(5),
            .parametersType = Params::ToAronType(),
            .resultType = Result::ToAronType(),
        };
    }
 
} // namespace armarx::view_selection::skills