diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index a827a7ea4b06898d3b41ff4692c112181e4f636e..e6db5bd3fef21d208203164bdb7e93b9e10996db 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -237,8 +237,15 @@ namespace armarx::armem::server::obj
}
armem::prediction::data::PredictionResultSeq
- ObjectMemory::predict(const armem::prediction::data::PredictionRequestSeq& requests)
+ ObjectMemory::predict(const armem::prediction::data::PredictionRequestSeq& iceRequests)
{
+ auto requests = armarx::fromIce<std::vector<armem::PredictionRequest>>(iceRequests);
+
+ std::vector<armem::PredictionResult> results = workingMemory().predict(requests);
+
+ auto iceResults = armarx::toIce<armem::prediction::data::PredictionResultSeq>(results);
+ return iceResults;
+
std::vector<armem::prediction::data::PredictionResult> results;
for (const auto& request : requests)
{
diff --git a/source/RobotAPI/libraries/armem/server/CMakeLists.txt b/source/RobotAPI/libraries/armem/server/CMakeLists.txt
index 34b02a4da78015ccdb32398a3f1de7f42c0311e3..b99f22f4c39d245e15e5a243ddc0b630be78d5d5 100644
--- a/source/RobotAPI/libraries/armem/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/server/CMakeLists.txt
@@ -170,6 +170,7 @@ set(LIB_HEADERS
wm/memory_definitions.h
wm/ice_conversions.h
wm/detail/MaxHistorySize.h
+ wm/detail/Prediction.h
plugins.h
plugins/Plugin.h
diff --git a/source/RobotAPI/libraries/armem/server/wm/detail/Prediction.h b/source/RobotAPI/libraries/armem/server/wm/detail/Prediction.h
new file mode 100644
index 0000000000000000000000000000000000000000..22d44938dbdc81c1e2f3a97f57387592616fe62d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/wm/detail/Prediction.h
@@ -0,0 +1,34 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::armem::core::base::detail
+ * @author phesch ( phesch at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/Prediction.h>
+
+
+namespace armarx::armem::server::wm::detail
+{
+
+
+
+} // namespace armarx::armem::base::detail
diff --git a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
index 35da4691d69dcb4a0290d84f19098608b7d3aab6..a734501b1608e63b794d72032e48a40145b917db 100644
--- a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
+++ b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
@@ -3,6 +3,8 @@
#include "error.h"
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <RobotAPI/libraries/armem/core/container_maps.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <map>
@@ -108,4 +110,108 @@ namespace armarx::armem::server::wm
return result;
}
+ std::vector<PredictionResult> Memory::predict(const std::vector<PredictionRequest>& requests)
+ {
+ const std::map<MemoryID, std::vector<PredictionEngine>> engines = getAllPredictionEngines();
+
+ std::vector<PredictionResult> results;
+ for (const PredictionRequest& request : requests)
+ {
+ PredictionResult& result = results.emplace_back();
+ result.snapshotID = request.snapshotID;
+
+ /*
+ * Subproblem: Find entry in engines that ...
+ * - contains the snapshot ID,
+ * - supports the requested einge
+ * - and is most specific.
+ */
+
+ auto it = armem::findMostSpecificEntryContainingIDIf(
+ engines, request.snapshotID,
+ [&request](const std::vector<PredictionEngine>& supported)
+ {
+ return std::find(supported.begin(), supported.end(), request.predictionSettings.predictionEngineID) != supported.end();
+ });
+ if (it != engines.end())
+ {
+ const MemoryID& responsibleID = it->first;
+ const std::vector<PredictionEngine>& supportedEngines = it->second;
+
+ // TODO: Get container and let it do the prediction.
+
+ if (armem::contains(id().withCoreSegmentName("Instance"), request.snapshotID)
+ and not request.snapshotID.hasGap()
+ and request.snapshotID.hasTimestamp())
+ {
+ objpose::ObjectPosePredictionRequest objPoseRequest;
+ toIce(objPoseRequest.timeWindow, Duration::SecondsDouble(predictionTimeWindow));
+ objPoseRequest.objectID = toIce(ObjectID(request.snapshotID.entityName));
+ objPoseRequest.settings = request.settings;
+ toIce(objPoseRequest.timestamp, request.snapshotID.timestamp);
+
+ objpose::ObjectPosePredictionResult objPoseResult =
+ predictObjectPoses({objPoseRequest}).at(0);
+ result.success = objPoseResult.success;
+ result.errorMessage = objPoseResult.errorMessage;
+
+ if (objPoseResult.success)
+ {
+ armem::client::QueryBuilder builder;
+ builder.latestEntitySnapshot(request.snapshotID);
+ auto queryResult = armarx::fromIce<armem::client::QueryResult>(
+ query(builder.buildQueryInputIce()));
+ std::string instanceError =
+ "Could not find instance '" + request.snapshotID.str() + "' in memory";
+ if (!queryResult.success)
+ {
+ result.success = false;
+ result.errorMessage << instanceError << ":\n" << queryResult.errorMessage;
+ }
+ else
+ {
+ if (not request.snapshotID.hasInstanceIndex())
+ {
+ request.snapshotID.instanceIndex = 0;
+ }
+ auto* aronInstance = queryResult.memory.findLatestInstance(
+ request.snapshotID, request.snapshotID.instanceIndex);
+ if (aronInstance == nullptr)
+ {
+ result.success = false;
+ result.errorMessage << instanceError << ": No latest instance found.";
+ }
+ 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 "
+ << request.snapshotID
+ << ". Have you given an instance index if requesting"
+ << " an object pose prediction?";
+ }
+ }
+ else
+ {
+ result.success = false;
+ result.errorMessage << "No predictions are supported for snapshot ID "
+ << request.snapshotID << ".";
+ result.prediction = nullptr;
+ }
+ }
+
+ return results;
+ }
+
}
diff --git a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
index e6889b7f8e0a6b40f16d28ad2bcc05f68250b1a5..bf06628e0fe0eab20813e1d2a454e78c0cfe457b 100644
--- a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
+++ b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
@@ -1,6 +1,7 @@
#pragma once
#include "detail/MaxHistorySize.h"
+#include "detail/Prediction.h"
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/core/base/EntityInstanceBase.h>
@@ -146,6 +147,11 @@ namespace armarx::armem::server::wm
*/
Base::UpdateResult updateLocking(const EntityUpdate& update);
+
+ std::vector<PredictionResult>
+ predict(const std::vector<PredictionRequest>& requests);
+
+
};
}