ScanLocation.cpp

Go to the documentation of this file.

#include "ScanLocation.h"
 
#include <Eigen/Core>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Duration.h>
 
#include <RobotAPI/libraries/aron/core/aron_conversions.h>
 
#include <armarx/view_selection/skills/skill_ids.h>
 
namespace armarx::view_selection::skills
{
 
    ScanLocation::ScanLocation(const Services& srv) : Base(DefaultSkillDescription()), srv_(srv)
    {
    }
 
    gaze_targets::GazeTarget
    ScanLocation::MakeGazeTarget(const Eigen::Vector3f& position,
                                 const arondto::ScanLocationParams& parameters)
    {
        const std::string frame = parameters.frame;
        const std::string agent = parameters.agent;
 
        const FramedPosition targetPos{
            position,
            frame,
            agent,
        };
        const gaze_targets::TargetPriority priority{
            gaze_targets::AttentionType::TaskDriven,
            parameters.priority,
        };
 
        // we schedule targets with the given duration but to avoid that none of our targets is active, we add a small dt here
        const auto timeEps = Duration::Seconds(5);
 
        const Duration duration =
            Duration::SecondsDouble(parameters.durationPerViewSeconds) + timeEps;
        const gaze_targets::GazeTarget target{parameters.gazeTargetName,
                                              targetPos,
                                              priority,
                                              duration,
                                              false};
 
        return target;
    }
 
    std::vector<Eigen::Vector3f>
    ScanLocation::MakeGazeTargetPositions(const arondto::ScanLocationParams& parameters)
    {
        const float x = parameters.xDelta;
        const float y = parameters.y;
        const float z = parameters.z;
 
        const std::vector<Eigen::Vector3f> gazePositions{
            {-x, y, z},
            {0, y, z},
            {x, y, z},
        };
 
        return gazePositions;
    }
 
    ::armarx::skills::Skill::InitResult
    ScanLocation::init(const Base::SpecializedInitInput& in)
    {
        return ::armarx::skills::Skill::InitResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded,
        };
    }
 
    ::armarx::skills::Skill::MainResult
    ScanLocation::main(const Base::SpecializedMainInput& in)
    {
        const std::vector<Eigen::Vector3f> gazePositions = MakeGazeTargetPositions(in.parameters);
        const std::vector<std::string> directionNames{"left", "to the front", "to the right"};
 
        const Duration duration = Duration::SecondsDouble(in.parameters.durationPerViewSeconds);
 
        for (size_t i = 0; i < gazePositions.size(); ++i)
        {
            ARMARX_INFO << "Looking " << directionNames.at(i);
            srv_.viewSelectionClient.commitGazeTarget(
                MakeGazeTarget(gazePositions.at(i), in.parameters));
            Clock::WaitFor(duration);
        }
 
        if (in.parameters.keepCentralGazeTargetAfterScanning)
        {
            const float y = in.parameters.y;
            const float z = in.parameters.z;
 
            const Eigen::Vector3f gazePosition{0, y, z};
 
            auto const infinite = armarx::Duration::MilliSeconds(-1);
 
            auto gazeTarget = MakeGazeTarget(gazePosition, in.parameters);
            gazeTarget.duration = infinite;
 
            ARMARX_INFO << "Spawning final gaze target at the center";
            srv_.viewSelectionClient.commitGazeTarget(gazeTarget);
        }
 
        return ::armarx::skills::Skill::MainResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    void
    ScanLocation::onStopRequested()
    {
        ARMARX_IMPORTANT << "Abort requested.";
    }
 
    armarx::skills::SkillDescription
    ScanLocation::DefaultSkillDescription()
    {
        Params defaults;
        defaults.agent = "Armar7";
        defaults.frame = "root";
        defaults.priority = 10;
        defaults.gazeTargetName = "ScanLocationTarget";
        defaults.keepCentralGazeTargetAfterScanning = false;
 
        defaults.durationPerViewSeconds = 4; // [s]
        defaults.xDelta = 200; // [mm]
        defaults.y = 500; // [mm]
        defaults.z = 800; // [mm]
 
        return armarx::skills::SkillDescription{
            .skillId = skill_ids::ScanLocation,
            .description = "",
            .rootProfileDefaults = defaults.toAron(),
            .timeout = armarx::core::time::Duration::Seconds(20),
            .parametersType = Params::ToAronType(),
        };
    }
 
} // namespace armarx::view_selection::skills