ScanLocationsForObject.cpp

Go to the documentation of this file.

#include "ScanLocationsForObject.h"
 
#include <chrono>
 
#include <ArmarXCore/core/time/Metronome.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/skills/provider/SpecializedSkillProxy.h>
 
#include <armarx/view_selection/skills/aron/LookAtArticulatedObjectFrame.aron.generated.h>
#include <armarx/view_selection/skills/constants.h>
#include <armarx/view_selection/skills/skill_ids.h>
 
namespace armarx::view_selection::skills
{
 
    ::armarx::skills::SkillDescription
    ScanLocationsForObject::GetSkillDescription()
    {
        ParamType defaults;
 
        defaults.objectToSearch.dataset = "Kitchen-interior";
        defaults.objectToSearch.className = "fridge";
        defaults.objectToSearch.instanceName = "0";
 
        defaults.locationsToScan = {"FridgeDoorShelf_node"};
 
        defaults.durationPerLocationSeconds = 10;
 
        defaults.objectToSearchFor.dataset = "Kitchen-interior";
        defaults.objectToSearchFor.className = "bio-milk";
        defaults.objectToSearchFor.instanceName = "0";
 
        defaults.matchInstanceName = false;
 
        return ::armarx::skills::SkillDescription{
            .skillId = skill_ids::ScanLocationsForObject,
            .description = "Scan locations of an articulated object looking for another object.",
            .rootProfileDefaults = defaults.toAron(),
            .timeout = ::armarx::Duration::Minutes(10),
            .parametersType = ParamType::ToAronType(),
        };
    }
 
    ScanLocationsForObject::Properties::SubSkillIds::SubSkillIds() :
        lookAtArticulatedObjectFrame{
            .providerId =
                ::armarx::skills::ProviderID{
                    .providerName =
                        armarx::view_selection::skills::constants::ViewSelectionSkillProviderName},
            .skillName =
                armarx::view_selection::skills::constants::skill_names::LookAtArticulatedObjectFrame
 
        }
    {
    }
 
    ScanLocationsForObject::ScanLocationsForObject(const Services& services,
                                                   const Properties& properties) :
        ::armarx::skills::SimpleSpecializedSkill<ParamType>(GetSkillDescription()),
        services(services),
        properties(properties)
    {
    }
 
    ::armarx::skills::Skill::MainResult
    ScanLocationsForObject::main(const SpecializedMainInput& in)
    {
        armarx::DateTime skillStartTime = armarx::DateTime::Now();
 
        armarx::ObjectID objectToSearchFor;
        armarx::fromAron(in.parameters.objectToSearchFor, objectToSearchFor);
 
        ARMARX_INFO << "Scanning " << in.parameters.objectToSearch.dataset << "/"
                    << in.parameters.objectToSearch.className << "/"
                    << in.parameters.objectToSearch.instanceName << ". Looking for "
                    << objectToSearchFor;
        for (const auto& location : in.parameters.locationsToScan)
        {
            throwIfSkillShouldTerminate();
            ARMARX_VERBOSE << "Facing " << location;
 
            using Parameters =
                armarx::view_selection::skills::arondto::LookAtArticulatedObjectFrameParams;
 
            armarx::skills::SpecializedSkillProxy<Parameters> subSkill(
                manager, properties.subSkillIDs.lookAtArticulatedObjectFrame);
            Parameters subSkillParams = subSkill.getRootProfileParameters();
 
            subSkillParams.object = in.parameters.objectToSearch;
            subSkillParams.node = location; // node to face for searching
            subSkillParams.objectPoseProvider = std::nullopt; // "any"
            subSkillParams.durationSeconds = in.parameters.durationPerLocationSeconds;
            subSkillParams.priority = 100;
 
            const auto viewSelectionResult = callSubskill(subSkill, subSkillParams.toAron());
            if (not viewSelectionResult.has_value() or not viewSelectionResult->hasBeenSucceeded())
            {
                ARMARX_WARNING << "View selection failed";
                return MakeFailedResult();
            }
 
            armarx::DateTime timeout =
                armarx::DateTime::Now() +
                armarx::Duration::Seconds(in.parameters.durationPerLocationSeconds);
 
            // wait until the timeout has expired and periodically check whether the object has been found already.
            Metronome metronome(Frequency::Hertz(5));
            while (armarx::DateTime::Now() < timeout)
            {
                throwIfSkillShouldTerminate();
 
 
                const auto object = [&]() -> std::optional<objpose::ObjectPose>
                {
                    if (in.parameters.matchInstanceName)
                    {
                        // match object including instance name
                        return services.objectReader.queryLatestObjectInstance(objectToSearchFor);
                    }
 
                    // get all objects of the desired class
                    for (const auto& [_, obj] :
                         services.objectReader.queryLatestObjectInstances(objectToSearchFor))
                    {
                        // we count an object as found, if it was seen after the skill was started
                        if (obj.timestamp > skillStartTime)
                        {
                            return obj;
                        }
                    }
                    return std::nullopt;
                }();
 
                if (object.has_value() && object->timestamp > skillStartTime)
                {
                    // object has been found
                    ARMARX_INFO << "Object " << object->objectID << " found at "
                                << object->timestamp;
                    return MakeSucceededResult();
                }
                metronome.waitForNextTick();
            }
        }
 
        ARMARX_WARNING << "Object not found in any location";
        return MakeFailedResult();
    }
 
 
} // namespace armarx::view_selection::skills