LookForObjects.cpp

Go to the documentation of this file.

#include "LookForObjects.h"
 
#include <Eigen/Core>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/Metronome.h>
 
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
#include <RobotAPI/libraries/aron/core/aron_conversions.h>
 
#include <armarx/view_selection/skills/skill_ids.h>
 
#include "ScanLocation.h"
 
namespace armarx::view_selection::skills
{
    LookForObjects::LookForObjects(const Services& srv) : Base(DefaultSkillDescription())
    {
        srv_.emplace(srv);
    }
 
    ::armarx::skills::Skill::InitResult
    LookForObjects::init(const Base::SpecializedInitInput& in)
    {
        return ::armarx::skills::Skill::InitResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    ::armarx::skills::Skill::MainResult
    LookForObjects::main(const Base::SpecializedMainInput& in)
    {
        const auto tsStart = armarx::Clock::Now();
 
        std::set<armarx::ObjectID> requiredObjectClassIDs;
        for (const auto& objectID : in.parameters.requiredObjectClassIDs)
        {
            requiredObjectClassIDs.insert(armarx::aron::fromAron<armarx::ObjectID>(objectID));
        }
 
        const size_t numberOfRequiredObjectClassIDs = requiredObjectClassIDs.size();
 
        const std::vector<Eigen::Vector3f> gazePositions =
            ScanLocation::MakeGazeTargetPositions(in.parameters.scanLocation);
 
        const Duration duration =
            Duration::SecondsDouble(in.parameters.scanLocation.durationPerViewSeconds);
 
        throwIfSkillShouldTerminate();
 
        Result result;
 
        DateTime nextTargetDueTime = DateTime::Now();
        size_t nextTargetIndex = 0;
 
        Metronome metronome(Frequency::HertzDouble(5));
        while (true)
        {
            // Check object positions.
            {
                const objpose::ObjectPoseSeq objectPoses =
                    srv_->objectPoseClient.fetchObjectPoses();
 
                for (const objpose::ObjectPose& objectPose : objectPoses)
                {
                    // check if objectPose is too old
                    if (objectPose.timestamp < tsStart)
                    {
                        continue;
                    }
 
                    ARMARX_INFO << deactivateSpam(10., objectPose.objectID.str())
                                << "Detected object " << objectPose.objectID.str();
 
                    const ObjectID objectPoseClassID = objectPose.objectID.getClassID();
                    if (requiredObjectClassIDs.erase(objectPoseClassID) > 0)
                    {
                        toAron(result.foundObjectInstanceIDs.emplace_back(), objectPose.objectID);
                    }
                }
 
                if (requiredObjectClassIDs.empty())
                {
                    // All required object classes found - we are done here.
                    ARMARX_INFO << "All required objects found";
                    break;
                }
            }
 
            // Control head.
            const DateTime now = DateTime::Now();
            if (now > nextTargetDueTime)
            {
                if (nextTargetIndex >= gazePositions.size())
                {
                    // "Timeout" - viewed all positions.
                    break;
                }
 
                // Move to next target.
                const Eigen::Vector3f& position = gazePositions.at(nextTargetIndex);
 
                ARMARX_INFO << "Looking to position #" << (nextTargetIndex + 1) << "/"
                            << gazePositions.size() << " (" << position.transpose() << ")";
 
                const gaze_targets::GazeTarget gazeTarget =
                    ScanLocation::MakeGazeTarget(position, in.parameters.scanLocation);
 
                srv_->viewSelectionClient.commitGazeTarget(gazeTarget);
                this->latestGazeTarget_ = gazeTarget;
 
                ++nextTargetIndex;
                nextTargetDueTime = now + duration;
            }
            metronome.waitForNextTick();
            throwIfSkillShouldTerminate();
        }
 
        // Hotfix: Wait some time before proceeding to allow the localization to settle.
        const Duration waitDuration = Duration::SecondsDouble(2.0);
        {
            const DateTime start = DateTime::Now();
 
            Metronome metronome(Frequency::HertzDouble(5.0));
            while ((DateTime::Now() - start) < waitDuration)
            {
                metronome.waitForNextTick();
                throwIfSkillShouldTerminate();
            }
        }
 
        throwIfSkillShouldTerminate();
 
        result.allRequiredObjectClassesFound =
            result.foundObjectInstanceIDs.size() == numberOfRequiredObjectClassIDs;
 
        return MakeSucceededResult(result.toAron());
    }
 
    void
    LookForObjects::onStopRequested()
    {
        if (latestGazeTarget_.has_value())
        {
            gaze_targets::GazeTarget deletedTarget = latestGazeTarget_.value();
            deletedTarget.priority =
                gaze_targets::TargetPriority(gaze_targets::AttentionType::TaskDriven, 0.0);
            deletedTarget.keepInQueue = false;
            deletedTarget.duration = Duration::Seconds(0);
            deletedTarget.status = gaze_targets::TargetStatus::Aborted;
            srv_->viewSelectionClient.commitGazeTarget(deletedTarget);
        }
    }
 
    ::armarx::skills::SkillDescription
    LookForObjects::DefaultSkillDescription()
    {
        Params defaults;
        toAron(defaults.requiredObjectClassIDs.emplace_back(),
               armarx::ObjectID("Kitchen", "bio-milk"));
 
        defaults.scanLocation = arondto::ScanLocationParams::FromAron(
            ScanLocation::DefaultSkillDescription().rootProfileDefaults);
 
        return armarx::skills::SkillDescription{
            .skillId = skill_ids::LookForObjects,
            .description = "",
            .rootProfileDefaults = defaults.toAron(),
            .timeout = armarx::Duration::Minutes(10),
            .parametersType = Params::ToAronType(),
            .resultType = Result::ToAronType(),
        };
    }
 
} // namespace armarx::view_selection::skills