From d16da19252cb72627b462dda216dbbe81d3467a0 Mon Sep 17 00:00:00 2001
From: phesch <ulila@student.kit.edu>
Date: Fri, 6 May 2022 22:31:38 +0200
Subject: [PATCH] Add ObjectPose pred. to MemoryInterface
---
.../server/ObjectMemory/ObjectMemory.cpp | 93 +++++++++++++++++++
.../armem/server/ObjectMemory/ObjectMemory.h | 18 +++-
.../armem_objects/server/instance/Segment.cpp | 8 ++
.../server/instance/SegmentAdapter.cpp | 1 +
4 files changed, 117 insertions(+), 3 deletions(-)
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index 4bd80b117..b3cb8043a 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/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;
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index 82f39db59..f078b436c 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/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;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 8239d5ce0..819eb729f 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/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);
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index 39e5d51ab..433b2d248 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/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 };
}
--
GitLab