ObjectMemoryEditor.cpp

Go to the documentation of this file.

#include "ObjectMemoryEditor.h"
 
#include <ArmarXCore/core/time/Metronome.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
 
#include "Editor.h"
 
namespace armarx
{
    armarx::PropertyDefinitionsPtr ObjectMemoryEditor::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
 
        defs->optional(this->objectScaling, "Editor.ObjectScaling",
                       "Scaling factor that is applied to all intractable objects.");
 
        defs->optional(this->confidenceThreshold, "Editor.ConfidenceThreshold",
                       "Only objects with a confidence greater than this value are shown.");
 
        return defs;
    }
 
    std::string ObjectMemoryEditor::getDefaultName() const
    {
        return "InteractiveMemoryEditor";
    }
 
    void ObjectMemoryEditor::onInitComponent()
    {
        {
            providerInfo.objectType = objpose::ObjectType::KnownObject;
 
            for (const auto& dataset : objectFinder.getDatasets())
            {
                std::vector<ObjectInfo> objects = objectFinder.findAllObjectsOfDataset(dataset);
 
                for (const auto& obj: objects)
                {
                    providerInfo.supportedObjects.push_back(armarx::toIce(obj.id()));
                }
            }
        }
    }
 
    void ObjectMemoryEditor::onConnectComponent()
    {
        setDebugObserverBatchModeEnabled(true);
 
        objectVizTask = new SimpleRunningTask<>([this]()
                                                {
                                                    this->run();
                                                });
        objectVizTask->start();
    }
 
    void ObjectMemoryEditor::onDisconnectComponent()
    {
    }
 
    void ObjectMemoryEditor::onExitComponent()
    {
    }
 
 
    void ObjectMemoryEditor::run()
    {
        objpose::ObjectPoseClient client = getClient();
 
        Editor::Properties properties =
        {
            .providerName = getName(),
            .objectScaling = objectScaling,
            .confidenceThreshold = confidenceThreshold,
            .availableObjects = providerInfo.supportedObjects,
        };
 
        Editor editor(arviz, properties,
                      [this](objpose::ProvidedObjectPoseSeq &poses)
                      {
                          objectPoseTopic->reportObjectPoses(getName(), objpose::toIce(poses));
                      },
                      [client]() -> objpose::ObjectPoseSeq
                      {
                          return client.fetchObjectPoses();
                      });
 
        Metronome metronome(Frequency::Hertz(20));
        while (objectVizTask and not objectVizTask->isStopped())
        {
            editor.step();
 
            metronome.waitForNextTick();
        }
    }
 
    objpose::ProviderInfo ObjectMemoryEditor::getProviderInfo(const Ice::Current & /*unused*/)
    {
        return providerInfo;
    }
 
    objpose::provider::RequestObjectsOutput ObjectMemoryEditor::requestObjects(
            const objpose::provider::RequestObjectsInput &input, const Ice::Current & /*unused*/)
    {
        objpose::provider::RequestObjectsOutput output;
 
        for (const auto &id: input.objectIDs)
        {
            output.results[id].success = false;
        }
 
        return output;
    }
} // namespace armarx