LookAtHumanHand.cpp

Go to the documentation of this file.

#include "LookAtHumanHand.h"
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <VisionX/libraries/armem_human/aron_conversions.h>
 
namespace armarx::view_selection::skills
{
    LookAtHumanHand::LookAtHumanHand(const Services& srv) : Base(DefaultSkillDescription())
    {
        srv_.emplace(srv);
    }
 
    ::armarx::skills::Skill::InitResult
    LookAtHumanHand::init(const Base::SpecializedInitInput& in)
    {
        // initialize variables; store which human and which hand to follow
 
        fromAron(in.parameters.humanTrackingId, humanTrackingId);
        // TODO, if implemented when using a different hand data type:
        // fromAron(in.parameters.hand, hand);
        if (in.parameters.hand == armarx::view_selection::skills::arondto::Hand::LEFT)
        {
            hand = armarx::armem::robot_state::RobotReader::Hand::Left;
        }
        else if (in.parameters.hand == armarx::view_selection::skills::arondto::Hand::RIGHT)
        {
            hand = armarx::armem::robot_state::RobotReader::Hand::Right;
        }
        else
        {
            ARMARX_WARNING << "Could not parse which hand is requested to look at.";
        }
 
        // TODO: or receive/pass the virtual robot differently?
        handoverTargetProvider = std::make_unique<target_provider::handover::RobotReceiver>(
            humanTrackingId, srv_->robot);
        // TODO: always look at hands, despite just if the human is reaching out?
        handoverTargetProvider->setPhase(target_provider::handover::Phase::PreHandover);
 
        return ::armarx::skills::Skill::InitResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    ::armarx::skills::Skill::MainResult
    LookAtHumanHand::main(const Base::SpecializedMainInput& in)
    {
        // repeatedly update the robot's gaze target to follow the requested human
        // (method is blocking, as intended for skills)
 
        // setup metronome to control execution frequency
        const auto metronomeTargetPeriod =
            armarx::Duration::MilliSeconds(properties.updatePeriodMs);
        armarx::Metronome metronome(metronomeTargetPeriod);
 
        while (!shouldSkillTerminate())
        {
            // update the view target based on the current pose of the human
            update();
            metronome.waitForNextTick();
        }
 
        return ::armarx::skills::Skill::MainResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    void
    LookAtHumanHand::update()
    {
        std::optional<armem::human::HumanPose> latestPoseOfRequestedHuman =
            getLatestPoseOfRequestedHuman();
 
        if (latestPoseOfRequestedHuman.has_value())
        {
            // updateTargetsAfterHandover generates targets to look at the head
            // TODO: how to specify to look at a certain hand? (otherwise: remove parameter)
            std::vector<gaze_targets::GazeTarget> gazeTargets =
                handoverTargetProvider->updateTargetsAfterHandover(
                    latestPoseOfRequestedHuman.value());
            for (const auto& gazeTarget : gazeTargets)
            {
                ARMARX_INFO << "Committing gaze target " << gazeTarget;
            }
            srv_->viewSelectionClient.commitGazeTargets(gazeTargets);
        }
        else
        {
            ARMARX_VERBOSE << "Did not find any pose for the requested human tracking ID";
        }
    }
 
    std::optional<armem::human::HumanPose>
    LookAtHumanHand::getLatestPoseOfRequestedHuman()
    {
        // get the current pose of the human
        armarx::armem::client::QueryBuilder builder;
        builder.coreSegments()
            .withName(srv_->humanPoseMemoryID.coreSegmentName)
            .providerSegments()
            .all()
            .entities()
            .all()
            .snapshots()
            .latest();
        armem::client::QueryResult result =
            srv_->memoryNameSystem.getReader(srv_->humanPoseMemoryID)
                .query(builder.buildQueryInput());
 
        std::optional<armem::human::HumanPose> latestPoseOfRequestedHuman = std::nullopt;
        if (result.success)
        {
            result.memory.forEachInstance(
                [this, &latestPoseOfRequestedHuman](const armem::wm::EntityInstance& instance)
                {
                    armem::human::HumanPose humanPose;
                    armarx::human::arondto::HumanPose dto =
                        armarx::human::arondto::HumanPose::FromAron(instance.data());
                    fromAron(dto, humanPose);
                    if (humanPose.humanTrackingId == humanTrackingId &&
                        (!latestPoseOfRequestedHuman.has_value() ||
                         humanPose.timestamp > latestPoseOfRequestedHuman->timestamp))
                    {
                        latestPoseOfRequestedHuman = humanPose;
                    }
                });
        }
        else
        {
            ARMARX_WARNING << "Failed to query the human memory: " << result.errorMessage;
        }
        return latestPoseOfRequestedHuman;
    }
 
    void
    LookAtHumanHand::onStopRequested()
    {
    }
 
 
} // namespace armarx::view_selection::skills