Store, visualize and provide transformations

source/RobotAPI/components/InteractiveMemoryEditor/InteractionObserver.cpp

@@ -2,12 +2,20 @@
 
 #include <utility>
 
-armarx::InteractionObserver::Observation &armarx::InteractionObserver::Observation::onContextMenu(size_t index, std::function<void()> action)
+armarx::InteractionObserver::Observation &
+armarx::InteractionObserver::Observation::onContextMenu(size_t index, std::function<void()> action)
 {
     contextMenuActions[index] = std::move(action);
     return *this;
 }
 
+armarx::InteractionObserver::Observation &
+armarx::InteractionObserver::Observation::onTransformEnd(std::function<void(Eigen::Matrix4f const&)> action)
+{
+    transformEndAction = std::make_optional(std::move(action));
+    return *this;
+}
+
 void armarx::InteractionObserver::Observation::process(viz::InteractionFeedback const& interaction)
 {
     if (interaction.type() == viz::InteractionFeedbackType::ContextMenuChosen)
@@ -20,6 +28,14 @@ void armarx::InteractionObserver::Observation::process(viz::InteractionFeedback
             action();
         }
     }
+
+    if (interaction.type() == viz::InteractionFeedbackType::Transform && interaction.isTransformEnd())
+    {
+        if (transformEndAction.has_value())
+        {
+            transformEndAction.value()(interaction.transformation());
+        }
+    }
 }
 
 void armarx::InteractionObserver::clearObservedLayer(armarx::viz::Layer & layer)

source/RobotAPI/components/InteractiveMemoryEditor/InteractionObserver.h

@@ -11,11 +11,13 @@ namespace armarx
         {
         public:
             Observation& onContextMenu(size_t index, std::function<void()> action);
+            Observation& onTransformEnd(std::function<void(Eigen::Matrix4f const&)> action);
 
             void process(viz::InteractionFeedback const& interaction);
 
         private:
             std::map<size_t, std::function<void()>> contextMenuActions;
+            std::optional<std::function<void(Eigen::Matrix4f const&)>> transformEndAction;
         };
 
         template <typename ElementT>

source/RobotAPI/components/InteractiveMemoryEditor/InteractiveMemoryEditor.cpp

@@ -4,6 +4,8 @@
 
 #include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
 #include <RobotAPI/components/InteractiveMemoryEditor/InteractionObserver.h>
+#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
 
 namespace armarx
 {
@@ -21,6 +23,14 @@ namespace armarx
 
     void InteractiveMemoryEditor::onInitComponent()
     {
+        {
+            providerInfo.objectType = objpose::ObjectType::KnownObject;
+            std::vector<ObjectInfo> objects = objectFinder.findAllObjectsOfDataset("KIT");
+            for (const auto& obj : objects)
+            {
+                providerInfo.supportedObjects.push_back(armarx::toIce(obj.id()));
+            }
+        }
     }
 
     void InteractiveMemoryEditor::onConnectComponent()
@@ -28,11 +38,12 @@ namespace armarx
         setDebugObserverBatchModeEnabled(true);
 
         isPushRequired = false;
+        isVizRequired = false;
         isPullRequired = true;
 
         objectVizTask = new SimpleRunningTask<>([this]()
                                                 {
-                                                    this->objectVizTaskRun();
+                                                    this->run();
                                                 });
         objectVizTask->start();
     }
@@ -46,7 +57,7 @@ namespace armarx
     }
 
 
-    void InteractiveMemoryEditor::objectVizTaskRun()
+    void InteractiveMemoryEditor::run()
     {
         CycleUtil cycle(50);
 
@@ -56,35 +67,75 @@ namespace armarx
 
         InteractionObserver observer;
 
-        while (objectVizTask && !objectVizTask->isStopped())
+        objpose::ObjectPoseSeq requestedPoses;
+
+        while (objectVizTask and not objectVizTask->isStopped())
         {
             viz::StagedCommit stage = arviz.stage();
 
             if (isPullRequired)
             {
-                observer.clearObservedLayer(memoryLayer);
+                requestedPoses = client.fetchObjectPoses();
+
+                isPullRequired = false;
+                isVizRequired = true;
+            }
 
-                const objpose::ObjectPoseSeq objectPoses = client.fetchObjectPoses();
+            if (isVizRequired)
+            {
+                observer.clearObservedLayer(memoryLayer);
 
-                for (const objpose::ObjectPose &objectPose: objectPoses)
+                for (objpose::ObjectPose & objectPose: requestedPoses)
                 {
                     observer.addObserved(
-                            memoryLayer,
-                            viz::Object(objectPose.objectID.str())
-                                    .pose(objectPose.objectPoseGlobal)
-                                    .fileByObjectFinder(objectPose.objectID)
-                                    .alpha(objectPose.confidence)
-                                    .enable(viz::interaction()
-                                                    .selection()
-                                                    .transform()
-                                                    .hideDuringTransform()
-                                                    .contextMenu({"Push", "Pull"})))
+                                    memoryLayer,
+                                    viz::Object(objectPose.objectID.str())
+                                            .pose(objectPose.objectPoseGlobal)
+                                            .fileByObjectFinder(objectPose.objectID)
+                                            .alpha(objectPose.confidence)
+                                            .enable(viz::interaction()
+                                                            .selection()
+                                                            .transform()
+                                                            .hideDuringTransform()
+                                                            .contextMenu({"Push", "Pull"})))
                             .onContextMenu(0, [this] { this->isPushRequired = true; })
-                            .onContextMenu(1, [this] { this->isPullRequired = true; });
+                            .onContextMenu(1, [this] { this->isPullRequired = true; })
+                            .onTransformEnd([&](const Eigen::Matrix4f& transform)
+                                {
+                                    objectPose.objectPoseGlobal = transform;
+                                    isVizRequired = true;
+                                });
                 }
 
                 stage.add(memoryLayer);
-                isPullRequired = false;
+
+                isVizRequired = false;
+            }
+
+            if (isPushRequired)
+            {
+                armarx::objpose::ProvidedObjectPoseSeq providingPoses;
+
+                for (const objpose::ObjectPose & requested : requestedPoses)
+                {
+                    armarx::objpose::ProvidedObjectPose& providing = providingPoses.emplace_back();
+                    providing.providerName = getName();
+                    providing.objectType = objpose::ObjectType::KnownObject;
+
+                    providing.objectID = requested.objectID;
+
+                    providing.objectPose = requested.objectPoseGlobal;
+                    providing.objectPoseFrame = armarx::GlobalFrame;
+
+                    providing.objectPoseGaussian = requested.objectPoseGlobalGaussian;
+
+                    providing.confidence = requested.confidence;
+                    providing.timestamp = DateTime::Now();
+                }
+
+                objectPoseTopic->reportObjectPoses(getName(), objpose::toIce(providingPoses));
+
+                isPushRequired = false;
             }
 
             observer.requestInteractions(stage);
@@ -96,4 +147,22 @@ namespace armarx
             cycle.waitForCycleDuration();
         }
     }
+
+    objpose::ProviderInfo InteractiveMemoryEditor::getProviderInfo(const Ice::Current&)
+    {
+        return providerInfo;
+    }
+
+    objpose::provider::RequestObjectsOutput InteractiveMemoryEditor::requestObjects(
+            const objpose::provider::RequestObjectsInput& input, const Ice::Current&)
+    {
+        objpose::provider::RequestObjectsOutput output;
+
+        for (const auto& id : input.objectIDs)
+        {
+            output.results[id].success = false;
+        }
+
+        return output;
+    }
 } // namespace armarx

source/RobotAPI/components/InteractiveMemoryEditor/InteractiveMemoryEditor.h

@@ -3,10 +3,12 @@
 #include <ArmarXCore/core/Component.h>
 #include <ArmarXCore/core/services/tasks/TaskUtil.h>
 #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
 
 #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
 
 #include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h>
 
 namespace armarx
 {
@@ -14,31 +16,34 @@ namespace armarx
             virtual public armarx::Component,
             virtual public armarx::DebugObserverComponentPluginUser,
             virtual public armarx::ArVizComponentPluginUser,
-            virtual public armarx::ObjectPoseClientPluginUser
+            virtual public armarx::ObjectPoseClientPluginUser,
+            virtual public armarx::ObjectPoseProviderPluginUser
     {
     public:
         std::string getDefaultName() const override;
 
-
     protected:
         armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
-
         void onInitComponent() override;
-
         void onConnectComponent() override;
-
         void onDisconnectComponent() override;
-
         void onExitComponent() override;
 
+    public:
+        objpose::ProviderInfo getProviderInfo(const Ice::Current& = Ice::emptyCurrent) override;
+        objpose::provider::RequestObjectsOutput requestObjects(const objpose::provider::RequestObjectsInput& input, const Ice::Current&) override;
 
     private:
-        void objectVizTaskRun();
+        void run();
+
+        armarx::ObjectFinder objectFinder;
+        objpose::ProviderInfo providerInfo;
 
         armarx::SimpleRunningTask<>::pointer_type objectVizTask;
 
         bool isPushRequired;
         bool isPullRequired;
+        bool isVizRequired;
 
         static constexpr const char* MEMORY_LAYER_NAME = "Memory";
     };