Add ObjectPose pred. to MemoryInterface

source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp

@@ -22,7 +22,14 @@
 
 #include "ObjectMemory.h"
 
+#include <ArmarXCore/core/time/ice_conversions.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/armem/client/Prediction.h>
+#include <RobotAPI/libraries/armem/client/query.h>
 #include <RobotAPI/libraries/armem/core/ice_conversions_templates.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
 
 
 namespace armarx::armem::server::obj
@@ -54,6 +61,9 @@ namespace armarx::armem::server::obj
         // defs->component(kinematicUnitObserver);  // Optional dependency.
         defs->defineOptionalProperty<std::string>("cmp.KinematicUnitObserverName", "KinematicUnitObserver",
                 "Name of the kinematic unit observer.");
+        defs->optional(predictionLookbackSeconds, "prediction.LookbackSeconds",
+                       "Lookback to use for predictions when requested via the PredictingMemoryInterface"
+                       " (in seconds).");
 
         return defs;
     }
@@ -215,6 +225,89 @@ namespace armarx::armem::server::obj
         return outputs;
     }
 
+    armem::prediction::data::PredictionResultSeq
+    ObjectMemory::predict(const armem::prediction::data::PredictionRequestSeq& requests)
+    {
+        std::vector<armem::prediction::data::PredictionResult> results;
+        for (const auto& request : requests)
+        {
+            auto boRequest = armarx::fromIce<armem::client::PredictionRequest>(request);
+            armem::client::PredictionResult result;
+            result.snapshotID = boRequest.snapshotID;
+            if (armem::contains(workingMemory().id().withCoreSegmentName("Instance"),
+                                boRequest.snapshotID) &&
+                !boRequest.snapshotID.hasGap() && boRequest.snapshotID.hasInstanceIndex())
+            {
+                objpose::ObjectPosePredictionRequest objPoseRequest;
+                objPoseRequest.lookback = armarx::toIce<armarx::core::time::dto::Duration>(
+                    Duration::SecondsDouble(predictionLookbackSeconds));
+                objPoseRequest.objectID = armarx::toIce<armarx::data::ObjectID>(
+                    ObjectID(boRequest.snapshotID.entityName + "/" +
+                             boRequest.snapshotID.instanceIndexStr()));
+                objPoseRequest.settings = request.settings;
+                objPoseRequest.timestamp = armarx::toIce<armarx::core::time::dto::DateTime>(
+                    boRequest.snapshotID.timestamp);
+                objpose::ObjectPosePredictionResult objPoseResult =
+                    predictObjectPoses({objPoseRequest}).at(0);
+                result.success = objPoseResult.success;
+                result.errorMessage.str(objPoseResult.errorMessage);
+
+                if (objPoseResult.success)
+                {
+
+                    armem::client::QueryBuilder builder;
+                    builder.singleEntitySnapshot(boRequest.snapshotID);
+                    auto queryResult = armarx::fromIce<armem::client::QueryResult>(
+                        query(builder.buildQueryInputIce()));
+                    std::string instanceError =
+                        "Could not find instance '" + boRequest.snapshotID.str() + "' in memory";
+                    if (!queryResult.success)
+                    {
+                        result.success = false;
+                        result.errorMessage << instanceError << ":\n" << queryResult.errorMessage;
+                    }
+                    else
+                    {
+                        auto* aronInstance = queryResult.memory.findInstance(boRequest.snapshotID);
+                        if (aronInstance == nullptr)
+                        {
+                            result.success = false;
+                            result.errorMessage << instanceError;
+                        }
+                        else
+                        {
+                            auto instance =
+                                armem::arondto::ObjectInstance::FromAron(aronInstance->data());
+                            objpose::toAron(
+                                instance.pose,
+                                armarx::fromIce<objpose::ObjectPose>(objPoseResult.prediction));
+                            result.prediction = instance.toAron();
+                        }
+                    }
+                }
+            }
+            else
+            {
+                result.success = false;
+                result.errorMessage << "No predictions are supported for MemoryID "
+                                    << boRequest.snapshotID
+                                    << ". Have you given an instance index if requesting"
+                                    << " an object pose prediction?";
+            }
+            results.push_back(result.toIce());
+        }
+
+        return results;
+    }
+
+    armem::prediction::data::EngineSupportMap ObjectMemory::getAvailableEngines()
+    {
+        // TODO(phesch): Replace with generic code in Memory implementation
+        return {{armarx::toIce<armem::data::MemoryID>(
+                     workingMemory().id().withCoreSegmentName("Instance")),
+                 {{"Linear Position Regression"}}}};
+    }
+
     void ObjectMemory::createRemoteGuiTab()
     {
         using namespace armarx::RemoteGui::Client;

source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h

@@ -96,11 +96,21 @@ namespace armarx::armem::server::obj
         // Without this, ObjectMemory draws the original
         // methods from ObjectMemoryInterface and complains
         // that an overload is being hidden.
-        using ReadWritePluginUser::getActions;
         using ReadWritePluginUser::executeActions;
+        using ReadWritePluginUser::getActions;
+        using ReadWritePluginUser::getAvailableEngines;
+        using ReadWritePluginUser::predict;
 
-        armem::actions::GetActionsOutputSeq getActions(const armem::actions::GetActionsInputSeq& inputs) override;
-        armem::actions::ExecuteActionOutputSeq executeActions(const armem::actions::ExecuteActionInputSeq& inputs) override;
+        // Actions
+        armem::actions::GetActionsOutputSeq
+        getActions(const armem::actions::GetActionsInputSeq& inputs) override;
+        armem::actions::ExecuteActionOutputSeq
+        executeActions(const armem::actions::ExecuteActionInputSeq& inputs) override;
+
+        // Predictions
+        armem::prediction::data::PredictionResultSeq
+        predict(const armem::prediction::data::PredictionRequestSeq& requests) override;
+        armem::prediction::data::EngineSupportMap getAvailableEngines() override;
 
         // Remote GUI
         void createRemoteGuiTab();
@@ -113,6 +123,8 @@ namespace armarx::armem::server::obj
         RobotStateComponentInterfacePrx robotStateComponent;
         KinematicUnitObserverInterfacePrx kinematicUnitObserver;
 
+        double predictionLookbackSeconds = 2;
+
         clazz::Segment classSegment;
 
         attachments::Segment attachmentSegment;

source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp

@@ -579,6 +579,14 @@ namespace armarx::armem::server::obj::instance
         objpose::ObjectPosePredictionResult result;
         result.success = false;
 
+        if (!request.settings.predictionEngineID.empty() &&
+            request.settings.predictionEngineID != "Linear Position Regression")
+        {
+            result.errorMessage = "Prediction engine '" + request.settings.predictionEngineID +
+                                  "' not available for object pose prediction.";
+            return result;
+        }
+
         const ObjectID objID = armarx::fromIce<ObjectID>(request.objectID);
 
         const Duration lookbackDuration = armarx::fromIce<Duration>(request.lookback);

source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp

@@ -450,6 +450,7 @@ namespace armarx::armem::server::obj::instance
     SegmentAdapter::getAvailableObjectPoseEngines(const Ice::Current&)
     {
         armem::prediction::data::PredictionEngine engine;
+        // TODO(phesch): Make this a constant somewhere
         engine.engineID = "Linear Position Regression";
         return { engine };
     }