LookAtHumanFace.cpp

Go to the documentation of this file.

#include "LookAtHumanFace.h"
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <VisionX/libraries/armem_human/aron_conversions.h>
 
namespace armarx::view_selection::skills
{
    LookAtHumanFace::LookAtHumanFace(const Services& srv) : Base(DefaultSkillDescription())
    {
        srv_.emplace(srv);
    }
 
    ::armarx::skills::Skill::InitResult
    LookAtHumanFace::init(const Base::SpecializedInitInput& in)
    {
        // initialize variables; store which human to follow
 
        fromAron(in.parameters.humanTrackingId, humanTrackingId);
 
        handoverTargetProvider = std::make_unique<target_provider::handover::RobotReceiver>(
            humanTrackingId, srv_->robot);
        handoverTargetProvider->setPhase(target_provider::handover::Phase::PostHandover);
 
        return ::armarx::skills::Skill::InitResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    ::armarx::skills::Skill::MainResult
    LookAtHumanFace::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 (not shouldSkillTerminate() and not stop_requested_)
        {
            // update the view target based on the current pose of the human
            ARMARX_INFO << "Before update";
            update();
            ARMARX_INFO << "After update";
 
            metronome.waitForNextTick();
        }
 
        ARMARX_INFO << "Successfully finished skill";
 
        return ::armarx::skills::Skill::MainResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    void
    LookAtHumanFace::update()
    {
        std::optional<armem::human::HumanPose> latestPoseOfRequestedHuman =
            getLatestPoseOfRequestedHuman();
 
        if (latestPoseOfRequestedHuman.has_value())
        {
            // updateTargetsAfterHandover generates targets to look at the head
            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>
    LookAtHumanFace::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
    LookAtHumanFace::onStopRequested()
    {
        ARMARX_INFO << "Stop requested";
        stop_requested_ = true;
    }
 
 
} // namespace armarx::view_selection::skills