LookAtHumanFace.cpp

Go to the documentation of this file.

#include "LookAtHumanFace.h"
 
#include <memory>
#include <optional>
#include <vector>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/src/Core/Matrix.h>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/Metronome.h>
 
#include <RobotAPI/libraries/armem/client/Query.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/aron/core/aron_conversions.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/SkillStatusUpdate.h>
 
#include <armarx/view_selection/gaze_targets/GazeTarget.h>
#include <armarx/view_selection/gaze_targets/aron/AttentionType.aron.generated.h>
#include <armarx/view_selection/gaze_targets/aron_conversions.h>
#include <armarx/view_selection/skills/constants.h>
#include <armarx/view_selection/target_provider/handover/RobotReceiver.h>
#include <armarx/view_selection/target_provider/handover/types.h>
 
#include <VisionX/libraries/armem_human/aron/HumanPose.aron.generated.h>
#include <VisionX/libraries/armem_human/aron_conversions.h>
#include <VisionX/libraries/armem_human/types.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)
 
        const auto tsStart = armarx::Clock::Now();
 
        // setup metronome to control execution frequency
        const auto metronomeTargetPeriod =
            armarx::Duration::MilliSecondsDouble(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_VERBOSE << "Before update";
            update();
            ARMARX_VERBOSE << "After update";
 
            metronome.waitForNextTick();
 
            if ((armarx::Clock::Now() - tsStart) > in.parameters.timeout)
            {
                ARMARX_INFO << "Timeout (skill parameter " << in.parameters.timeout << " ) reached";
                break;
            }
        }
 
        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());
 
            
 
            auto in = getParameters();
            for (auto& gazeTarget : gazeTargets)
            {
                gazeTarget.priority.priority = in.priority;
                if (in.zOffset.has_value())
                {
                    gazeTarget.position.z += in.zOffset.value();
                    // Eigen::Vector3f pos = gazeTarget.position.toEigen();
                    // pos.z() += getParameters().zOffset.value();
                    // gazeTarget.position = FramedPosition(pos, gazeTarget.position.frame, gazeTarget.position.agent);
                }
                fromAron(getParameters().attentionType, gazeTarget.priority.attentionType);
 
                ARMARX_VERBOSE << "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;
    }
 
    armarx::skills::SkillDescription
    LookAtHumanFace::DefaultSkillDescription()
    {
        using armarx::view_selection::gaze_targets::arondto::AttentionType;
 
        ParamType defaultParameters;
        defaultParameters.priority = 1; //lowest possible priority
        defaultParameters.attentionType = AttentionType::TaskDriven;
        defaultParameters.timeout = armarx::Duration::Seconds(60);
        defaultParameters.zOffset = std::nullopt;
 
        return armarx::skills::SkillDescription{
            .skillId = {.skillName = constants::skill_names::LookAtHumanFace},
            .description = "Look at the face of a human specified by their tracking ID.",
            .rootProfileDefaults = defaultParameters.toAron(),
            .timeout = armarx::core::time::Duration::Seconds(60),
            .parametersType = Params::ToAronType()};
    }
} // namespace armarx::view_selection::skills