LookDirection.cpp

Go to the documentation of this file.

#include "LookDirection.h"
 
#include <cstdlib>
#include <string>
#include <utility>
 
#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 <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.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/aron/LookDirection.aron.generated.h>
#include <armarx/view_selection/skills/commit_and_wait.h>
#include <armarx/view_selection/skills/constants.h>
#include <armarx/view_selection/skills/skill_ids.h>
 
namespace armarx::view_selection::skills
{
 
    LookDirection::LookDirection(const Context& services,
                                 direction::state::StateModFn* stateModFn,
                                 armarx::skills::SkillDescription skillDescription) :
        Base(skillDescription), srv_(services), stateModFn(stateModFn)
    {
    }
 
    void
    LookDirection::connect(const Context& srv)
    {
        srv_.emplace(srv);
    }
 
    ::armarx::skills::Skill::InitResult
    LookDirection::init(const Base::SpecializedInitInput& in)
    {
        ARMARX_INFO << "LookDirection:init";
        gazeTarget.emplace(); // populate optional
        const std::string frame = in.parameters.frame;
        const std::string agent = in.parameters.agent;
 
        // Read current direction of this skill from memory
        const auto previousTarget = srv_->viewSelectionClient.readGazeTarget("LookDirection");
 
        direction::state::Type state = direction::state::Center;
        if (previousTarget.has_value())
        {
            const Eigen::Vector3f directionRead = previousTarget->position.toEigen();
            state = direction::state::from(directionRead);
        }
 
        // Update state
        state = stateModFn(state);
 
        const Eigen::Vector3f targetPosition = direction::state::generateTargetPosition(state);
 
        const FramedPosition targetPos{targetPosition, frame, agent};
        const gaze_targets::TargetPriority priority{gaze_targets::AttentionType::TaskDriven,
                                                    in.parameters.priority};
        const armarx::core::time::Duration duration = armarx::core::time::Duration::Seconds(-1);
        const gaze_targets::GazeTarget target{
            "LookDirection", targetPos, priority, duration, false};
        gazeTarget.emplace(target);
 
        return ::armarx::skills::Skill::InitResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    ::armarx::skills::Skill::MainResult
    LookDirection::main(const Base::SpecializedMainInput& in)
    {
        ARMARX_CHECK(gazeTarget.has_value());
 
        return commitAndMaybeWait(srv_->viewSelectionClient,
                                  gazeTarget.value(),
                                  in.parameters.waitUntilReached,
                                  in.parameters.waitTimeout_ms,
                                  [this]() { return shouldSkillTerminate(); });
    }
 
    void
    LookDirection::onStopRequested()
    {
        if (gazeTarget.has_value())
        {
            srv_->viewSelectionClient.commitGazeTarget(gazeTarget.value());
        }
    }
 
    armarx::skills::SkillDescription
    LookDirection::generateDefaultSkillDescription(std::string name, std::string directionText)
    {
        ParamType defaultParameters;
        defaultParameters.frame = "root";
        defaultParameters.agent = "Armar7";
        defaultParameters.priority = 1;
        return armarx::skills::SkillDescription{
            .skillId = armarx::skills::SkillID{.skillName = std::move(name)},
            .description = "Let the robot look " + directionText +
                           ". Multiple directions can be combined but stacking the same direction "
                           "does not move the head further.",
            .rootProfileDefaults = defaultParameters.toAron(),
            .timeout = armarx::core::time::Duration::MilliSeconds(5000),
            .parametersType = Params::ToAronType(),
        };
    }
 
    LookUp::LookUp(const Context& services) :
        /* very strange problem here: The SpecializedSkill base class is constructed before
         * LookDirection. This is a problem, because usually the default constructor would be
         * called, which is deleted. Therefore, we need to call it twice with the correct parameter.
         */
        armarx::skills::SimpleSpecializedSkill<
            arondto::LookDirectionParams>::SimpleSpecializedSkill(DefaultSkillDescription()),
        LookDirection(services, direction::state::up, DefaultSkillDescription())
    {
    }
 
    armarx::skills::SkillDescription
    LookUp::DefaultSkillDescription()
    {
        return generateDefaultSkillDescription(constants::skill_names::LookUp, "up");
    }
 
    LookDown::LookDown(const Context& services) :
        /* very strange problem here: The SpecializedSkill base class is constructed before
         * LookDirection. This is a problem, because usually the default constructor would be
         * called, which is deleted. Therefore, we need to call it twice with the correct parameter.
         */
        armarx::skills::SimpleSpecializedSkill<
            arondto::LookDirectionParams>::SimpleSpecializedSkill(DefaultSkillDescription()),
        LookDirection(services, direction::state::down, DefaultSkillDescription())
    {
    }
 
    LookDownstraight::LookDownstraight(const Context& services) :
        /* very strange problem here: The SpecializedSkill base class is constructed before
         * LookDirection. This is a problem, because usually the default constructor would be
         * called, which is deleted. Therefore, we need to call it twice with the correct parameter.
         */
        armarx::skills::SimpleSpecializedSkill<
            arondto::LookDirectionParams>::SimpleSpecializedSkill(DefaultSkillDescription()),
        LookDirection(services, direction::state::downstraight, DefaultSkillDescription())
    {
    }
 
    armarx::skills::SkillDescription
    LookDown::DefaultSkillDescription()
    {
        return generateDefaultSkillDescription(constants::skill_names::LookDown, "down");
    }
 
    armarx::skills::SkillDescription
    LookDownstraight::DefaultSkillDescription()
    {
        return generateDefaultSkillDescription(constants::skill_names::LookDownstraight,
                                               "downstraight");
    }
 
    LookLeft::LookLeft(const Context& services) :
        /* very strange problem here: The SpecializedSkill base class is constructed before
         * LookDirection. This is a problem, because usually the default constructor would be
         * called, which is deleted. Therefore, we need to call it twice with the correct parameter.
         */
        armarx::skills::SimpleSpecializedSkill<
            arondto::LookDirectionParams>::SimpleSpecializedSkill(DefaultSkillDescription()),
        LookDirection(services, direction::state::left, DefaultSkillDescription())
    {
    }
 
    armarx::skills::SkillDescription
    LookLeft::DefaultSkillDescription()
    {
        return generateDefaultSkillDescription(constants::skill_names::LookLeft, "left");
    }
 
    LookRight::LookRight(const Context& services) :
        /* very strange problem here: The SpecializedSkill base class is constructed before
         * LookDirection. This is a problem, because usually the default constructor would be
         * called, which is deleted. Therefore, we need to call it twice with the correct parameter.
         */
        armarx::skills::SimpleSpecializedSkill<
            arondto::LookDirectionParams>::SimpleSpecializedSkill(DefaultSkillDescription()),
        LookDirection(services, direction::state::right, DefaultSkillDescription())
    {
    }
 
    armarx::skills::SkillDescription
    LookRight::DefaultSkillDescription()
    {
        return generateDefaultSkillDescription(constants::skill_names::LookRight, "right");
    }
 
    LookAhead::LookAhead(const Context& services) :
        /* very strange problem here: The SpecializedSkill base class is constructed before
         * LookDirection. This is a problem, because usually the default constructor would be
         * called, which is deleted. Therefore, we need to call it twice with the correct parameter.
         */
        armarx::skills::SimpleSpecializedSkill<
            arondto::LookDirectionParams>::SimpleSpecializedSkill(DefaultSkillDescription()),
        LookDirection(services, direction::state::center, DefaultSkillDescription())
    {
    }
 
    armarx::skills::SkillDescription
    LookAhead::DefaultSkillDescription()
    {
        return generateDefaultSkillDescription(constants::skill_names::LookAhead, "straight");
    }
 
    direction::state::Type
    direction::state::from(Eigen::Vector3f targetPosition)
    {
        state::Type state = state::Center;
 
        // Check for each direction separately and set State bit
        float epsilon = 10.0;
        if (std::abs(centerPosition.z() + DeltaUp.z() - targetPosition.z()) < epsilon)
            state |= state::Up;
        if (std::abs(centerPosition.z() + deltaDown.z() - targetPosition.z()) < epsilon)
            state |= state::Down;
        if (std::abs(centerPosition.x() + deltaLeft.x() - targetPosition.x()) < epsilon)
            state |= state::Left;
        if (std::abs(centerPosition.x() + deltaRight.x() - targetPosition.x()) < epsilon)
            state |= state::Right;
 
        return state;
    }
 
    Eigen::Vector3f
    direction::state::generateTargetPosition(Type state)
    {
        Eigen::Vector3f targetPosition = centerPosition;
 
        if ((state & state::Up) != 0)
            targetPosition += DeltaUp;
        if ((state & state::Down) != 0)
            targetPosition += deltaDown;
        if ((state & state::Left) != 0)
            targetPosition += deltaLeft;
        if ((state & state::Right) != 0)
            targetPosition += deltaRight;
        if ((state & state::Downstraight) != 0)
            targetPosition = downstraightPosition;
        // ARMARX_INFO << "setting position to " << targetPosition;
        // ARMARX_INFO << "State is " << static_cast<std::uint64_t>(state);
        return targetPosition;
    }
 
    direction::state::Type
    direction::state::center(Type previous)
    {
        (void)previous; // Unused.
        return state::Center;
    }
 
    direction::state::Type
    direction::state::up(Type previous)
    {
        return (previous & ~state::Down) | state::Up;
    }
 
    direction::state::Type
    direction::state::down(Type previous)
    {
        return (previous & ~state::Up) | state::Down;
    }
 
    direction::state::Type
    direction::state::downstraight(Type previous)
    {
        return (previous & ~state::Downstraight) | state::Downstraight;
    }
 
    direction::state::Type
    direction::state::left(Type previous)
    {
        return (previous & ~state::Right) | state::Left;
    }
 
    direction::state::Type
    direction::state::right(Type previous)
    {
        return (previous & ~state::Left) | state::Right;
    }
 
} // namespace armarx::view_selection::skills