diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
index dc7309cf18c368d85994cc06a2944b856d9832f4..1e8f298029912c1eab459cd5d285ba0a8e144ff5 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
@@ -26,10 +26,11 @@
#include <SimoxUtility/algorithm/string.h>
+#include <RobotAPI/libraries/armem/client/Prediction.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
-#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/armem/core/ice_conversions_templates.h>
+#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.aron.generated.h>
@@ -162,7 +163,7 @@ namespace armarx
ExecuteActionOutputSeq output;
for (const auto& [id, path] : input)
{
- armem::MemoryID memoryID = armem::fromIce<armem::MemoryID>(id);
+ auto memoryID = fromIce<armem::MemoryID>(id);
if (path == ActionPath{"hi"})
{
ARMARX_INFO << "Hello, " << memoryID.str() << "!";
@@ -193,7 +194,8 @@ namespace armarx
update.instancesData = { instance->data() };
armem::Commit newCommit;
- this->commit(armem::toIce<armem::data::Commit>(newCommit));
+ newCommit.add(update);
+ this->commit(armarx::toIce<armem::data::Commit>(newCommit));
tab.rebuild = true;
ARMARX_INFO << "Duplicated " << memoryID;
@@ -209,6 +211,73 @@ namespace armarx
return output;
}
+ // PREDICTING
+
+ armem::prediction::data::PredictionResultSeq
+ ExampleMemory::predict(const armem::prediction::data::PredictionRequestSeq& requests)
+ {
+ armem::prediction::data::PredictionResultSeq result;
+ for (auto request : requests)
+ {
+ result.push_back(predictSingle(request));
+ }
+ return result;
+ }
+
+ armem::prediction::data::PredictionResult
+ ExampleMemory::predictSingle(const armem::prediction::data::PredictionRequest& request)
+ {
+ armem::client::PredictionResult result;
+
+ std::string engine = request.settings.predictionEngineID;
+ if (engine.empty() || engine == "Latest")
+ {
+ auto boID = fromIce<armem::MemoryID>(request.snapshotID);
+ armem::client::QueryBuilder builder;
+ builder.latestEntitySnapshot(boID);
+ auto queryResult =
+ query(armarx::toIce<armem::query::data::Input>(builder.buildQueryInput()));
+ if (queryResult.success)
+ {
+ auto readMemory = fromIce<armem::wm::Memory>(queryResult.memory);
+ auto* latest = readMemory.findLatestSnapshot(boID);
+ if (latest != nullptr)
+ {
+ auto instance = latest->hasInstance(boID)
+ ? latest->getInstance(boID)
+ : latest->getInstance(latest->getInstanceIndices().at(0));
+ result.success = true;
+ result.snapshotID = boID;
+ result.prediction = instance.data();
+ }
+ else
+ {
+ result.success = false;
+ result.errorMessage <<
+ "Could not find entity referenced by MemoryID " << boID;
+ }
+ }
+ else
+ {
+ result.success = false;
+ result.errorMessage << "Could not find entity referenced by MemoryID " << boID;
+ }
+ }
+ else
+ {
+ result.success = false;
+ result.errorMessage << "This memory does not support the prediction engine \"" << engine << "\"";
+ }
+
+ return result.toIce();
+ }
+
+ armem::prediction::data::PredictionEngineSeq
+ ExampleMemory::getAvailableEngines()
+ {
+ return {{"Latest"}};
+ }
+
// REMOTE GUI
void ExampleMemory::createRemoteGuiTab()
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
index 62ea177f496296082cac90aecc6e9e6f35d450a8..de89914ba8ed44517ce40639849afa826d022ef1 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
@@ -71,6 +71,13 @@ namespace armarx
armem::actions::GetActionsOutputSeq getActions(const armem::actions::GetActionsInputSeq& input) override;
armem::actions::ExecuteActionOutputSeq executeActions(const armem::actions::ExecuteActionInputSeq& input) override;
+ // PredictingMemoryInterface interface
+ public:
+ armem::prediction::data::PredictionResultSeq
+ predict(const armem::prediction::data::PredictionRequestSeq& requests) override;
+
+ armem::prediction::data::PredictionEngineSeq getAvailableEngines() override;
+
protected:
@@ -84,6 +91,9 @@ namespace armarx
private:
+ armem::prediction::data::PredictionResult
+ predictSingle(const armem::prediction::data::PredictionRequest& request);
+
armarx::DebugObserverInterfacePrx debugObserver;
struct Properties
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index 144dacf08d6f3f2fdbb33aab1e73d1d67052b1b1..1973976129415be09d640896b3bd2ec68736e60e 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -147,7 +147,7 @@ void KinematicUnitWidgetController::onInitComponent()
void KinematicUnitWidgetController::onConnectComponent()
{
- ARMARX_INFO << "kinematic unit gui :: onConnectComponent()";
+ // ARMARX_INFO << "Kinematic Unit Gui :: onConnectComponent()";
reportedJointAngles.clear();
reportedJointVelocities.clear();
reportedJointControlModes.clear();
@@ -156,10 +156,11 @@ void KinematicUnitWidgetController::onConnectComponent()
reportedJointStatuses.clear();
reportedJointTemperatures.clear();
-
jointAnglesUpdateFrequency = new filters::MedianFilter(100);
kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
topicName = kinematicUnitInterfacePrx->getReportTopicName();
+ usingTopic(topicName);
+
lastJointAngleUpdateTimestamp = TimeUtil::GetTime().toSecondsDouble();
robotVisu->removeAllChildren();
@@ -168,10 +169,9 @@ void KinematicUnitWidgetController::onConnectComponent()
std::string rfile;
Ice::StringSeq includePaths;
- // get robot filename
+ // Get robot filename
try
{
-
Ice::StringSeq packages = kinematicUnitInterfacePrx->getArmarXPackages();
packages.push_back(Application::GetProjectName());
ARMARX_VERBOSE << "ArmarX packages " << packages;
@@ -200,7 +200,7 @@ void KinematicUnitWidgetController::onConnectComponent()
}
catch (...)
{
- ARMARX_ERROR << "Unable to retrieve robot filename";
+ ARMARX_ERROR << "Unable to retrieve robot filename.";
}
try
@@ -208,6 +208,10 @@ void KinematicUnitWidgetController::onConnectComponent()
ARMARX_INFO << "Loading robot from file " << rfile;
robot = loadRobotFile(rfile);
}
+ catch (const std::exception& e)
+ {
+ ARMARX_ERROR << "Failed to init robot: " << e.what();
+ }
catch (...)
{
ARMARX_ERROR << "Failed to init robot";
@@ -216,35 +220,34 @@ void KinematicUnitWidgetController::onConnectComponent()
if (!robot || !robot->hasRobotNodeSet(robotNodeSetName))
{
getObjectScheduler()->terminate();
-
if (getWidget()->parentWidget())
{
getWidget()->parentWidget()->close();
}
-
- std::cout << "returning" << std::endl;
return;
}
- // check robot name and disable setZero Button if necessary
+ // Check robot name and disable setZero Button if necessary
if (not simox::alg::starts_with(robot->getName(), "Armar3"))
{
- ARMARX_IMPORTANT << "Disable the SetZero button because the robot name is " << robot->getName();
+ ARMARX_IMPORTANT << "Disable the SetZero button because the robot name is '" << robot->getName() << "'.";
ui.pushButtonKinematicUnitPos1->setDisabled(true);
}
kinematicUnitFile = rfile;
robotNodeSet = robot->getRobotNodeSet(robotNodeSetName);
-
- initGUIComboBox(robotNodeSet); // init the pull down menu (QT: ComboBox)
- initGUIJointListTable(robotNodeSet);
-
kinematicUnitVisualization = getCoinVisualization(robot);
kinematicUnitNode = kinematicUnitVisualization->getCoinVisualization();
robotVisu->addChild(kinematicUnitNode);
- // init control mode map
+ // Fetch the current joint angles.
+ synchronizeRobotJointAngles();
+
+ initGUIComboBox(robotNodeSet); // init the pull down menu (QT: ComboBox)
+ initGUIJointListTable(robotNodeSet);
+
+ // Init control mode map
try
{
reportedJointControlModes = kinematicUnitInterfacePrx->getControlModes();
@@ -255,7 +258,6 @@ void KinematicUnitWidgetController::onConnectComponent()
initializeUi();
- usingTopic(topicName);
QMetaObject::invokeMethod(this, "resetSlider");
enableMainWidgetAsync(true);
updateTimerId = startTimer(1000); // slow timer that only ensures that skipped updates are shown at all
@@ -355,13 +357,11 @@ void KinematicUnitWidgetController::saveSettings(QSettings* settings)
settings->setValue("enableValueValidator", enableValueValidator);
settings->setValue("viewerEnabled", viewerEnabled);
assert(historyTime % 1000 == 0);
- settings->setValue("historyTime", (int)(historyTime / 1000));
+ settings->setValue("historyTime", static_cast<int>(historyTime / 1000));
settings->setValue("currentValueMax", currentValueMax);
}
-
-
void KinematicUnitWidgetController::showVisuLayers(bool show)
{
if (debugDrawer)
@@ -410,7 +410,9 @@ void KinematicUnitWidgetController::updateGuiElements()
// modelUpdateCB();
}
-void KinematicUnitWidgetController::updateKinematicUnitListInDialog() {}
+void KinematicUnitWidgetController::updateKinematicUnitListInDialog()
+{
+}
void KinematicUnitWidgetController::modelUpdateCB()
{
@@ -418,7 +420,6 @@ void KinematicUnitWidgetController::modelUpdateCB()
SoNode* KinematicUnitWidgetController::getScene()
{
-
if (viewerEnabled)
{
ARMARX_INFO << "Returning scene ";
@@ -500,7 +501,7 @@ void KinematicUnitWidgetController::kinematicUnitZeroPosition()
{
}
- //set slider to 0 if position control mode is used
+ // Set slider to 0 if position control mode is used.
if (selectedControlMode == ePositionControl)
{
ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
@@ -615,9 +616,9 @@ void KinematicUnitWidgetController::setControlModePosition()
if (currentNode)
{
- QString unit = currentNode->isRotationalJoint() ?
- (ui.checkBoxUseDegree->isChecked() ? "deg" : "rad") :
- "mm";
+ QString unit = currentNode->isRotationalJoint()
+ ? (ui.checkBoxUseDegree->isChecked() ? "deg" : "rad")
+ : "mm";
ui.labelUnit->setText(unit);
const bool isDeg = ui.checkBoxUseDegree->isChecked();
const bool isRot = currentNode->isRotationalJoint();
@@ -638,8 +639,18 @@ void KinematicUnitWidgetController::setControlModePosition()
return;
}
+ {
+ // currentNode->getJointValue() can we wrong after we re-connected to the robot unit.
+ // E.g., it can be 0 although the torso joint was at -365 before the unit disconnected.
+ // Therefore, we first have to fetch the actual joint values and use that one.
+ // However, this should actually not be necessary, as the robot model should be updated
+ // via the topics.
+ synchronizeRobotJointAngles();
+ }
+
float pos = currentNode->getJointValue() * conversionFactor;
- ARMARX_INFO << "setting position control for current Node Name: " << currentNode->getName() << " with current value: " << pos;
+ ARMARX_INFO << "Setting position control for current node "
+ << "(name '" << currentNode->getName() << "' with current value " << pos << ")";
// Setting the slider position to pos will set the position to the slider tick closest to pos
// This will initially send a position target with a small delta to the joint.
@@ -695,10 +706,7 @@ void KinematicUnitWidgetController::setControlModeVelocity()
ui.horizontalSliderKinematicUnitPos->setMinimum(lo);
ui.horizontalSliderKinematicUnitPos->blockSignals(false);
resetSlider();
-
-
}
-
}
void KinematicUnitWidgetController::setControlModeTorque()
@@ -736,10 +744,7 @@ void KinematicUnitWidgetController::setControlModeTorque()
ui.horizontalSliderKinematicUnitPos->blockSignals(false);
resetSlider();
-
}
-
-
}
VirtualRobot::RobotPtr KinematicUnitWidgetController::loadRobotFile(std::string fileName)
@@ -1720,3 +1725,9 @@ void KinematicUnitWidgetController::on_pushButtonFromJson_clicked()
ARMARX_ERROR << "failed to switch mode or set angles";
}
}
+
+void KinematicUnitWidgetController::synchronizeRobotJointAngles()
+{
+ const NameValueMap currentJointAngles = kinematicUnitInterfacePrx->getJointAngles();
+ robot->setJointValues(currentJointAngles);
+}
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
index 8287176d247aebc0fbebac9b5a6b91d20df22c25..d0a98a0a1a57efb6958b103b81732e022afbedf4 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
@@ -231,7 +231,6 @@ namespace armarx
// ice proxies
KinematicUnitInterfacePrx kinematicUnitInterfacePrx;// send commands to kinematic unit
- KinematicUnitListenerPrx kinematicUnitListenerPrx; // receive reports from kinematic unit
bool verbose;
@@ -269,9 +268,13 @@ namespace armarx
void highlightCriticalValues();
protected slots:
+
void showVisuLayers(bool show);
void copyToClipboard();
void on_pushButtonFromJson_clicked();
+
+ void synchronizeRobotJointAngles();
+
private:
std::recursive_mutex mutexNodeSet;
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index be1193d665db683b844d28d60662e8e3088eec70..41a52f9f8a0a2d981583ae1a5c4e85a07f0ff750 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -110,6 +110,7 @@ set(SLICE_FILES
armem/io.ice
armem/commit.ice
armem/memory.ice
+ armem/prediction.ice
armem/query.ice
armem/client.ice
@@ -120,7 +121,7 @@ set(SLICE_FILES
armem/server/StoringMemoryInterface.ice
armem/server/LoadingMemoryInterface.ice
armem/server/MemoryInterface.ice
- armem/server/MemoryPredictorInterface.ice
+ armem/server/PredictingMemoryInterface.ice
armem/server/ReadingMemoryInterface.ice
armem/server/WritingMemoryInterface.ice
diff --git a/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice b/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice
index 2d6b220a2050001960fcc4b564d54b3526f4ed44..73d27e0788c89f7d31b0c4f21c546ff276420fd6 100644
--- a/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice
+++ b/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice
@@ -1,6 +1,7 @@
#pragma once
#include <RobotAPI/interface/armem/server/ActionsInterface.ice>
+#include <RobotAPI/interface/armem/server/PredictingMemoryInterface.ice>
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.ice>
#include <RobotAPI/interface/armem/server/WritingMemoryInterface.ice>
@@ -16,13 +17,14 @@ module armarx
/**
* A memory server can implement the reading and/or writing
* memory interface. They should be handled individually.
- * Additionally, it can implement an actions interface,
- * which is currently the case for all reading and writing memories.
+ * Additionally, it can implement prediction or actions interfaces,
+ * which is currently the case for all working memories.
*/
struct MemoryServerInterfaces
{
server::ReadingMemoryInterface* reading;
server::WritingMemoryInterface* writing;
+ server::PredictingMemoryInterface* prediction;
server::actions::ActionsInterface* actions;
};
dictionary<string, MemoryServerInterfaces> MemoryServerInterfacesMap;
diff --git a/source/RobotAPI/interface/armem/prediction.ice b/source/RobotAPI/interface/armem/prediction.ice
new file mode 100644
index 0000000000000000000000000000000000000000..f42535928e434be36702a949f9929246a8f08f7b
--- /dev/null
+++ b/source/RobotAPI/interface/armem/prediction.ice
@@ -0,0 +1,50 @@
+#pragma once
+
+#include <RobotAPI/interface/armem/memory.ice>
+#include <RobotAPI/interface/aron.ice>
+
+module armarx
+{
+ module armem
+ {
+ module prediction
+ {
+ module data
+ {
+ // Holds properties of a prediction engine identified by its engineID.
+ struct PredictionEngine
+ {
+ string engineID;
+ }
+ sequence<PredictionEngine> PredictionEngineSeq;
+
+ // Settings to use for a given prediction (e.g. which engine to use)
+ struct PredictionSettings
+ {
+ string predictionEngineID = "";
+ };
+
+ // A single request for a prediction from a memory.
+ // The timestamp to request the prediction for is in the MemoryID.
+ struct PredictionRequest
+ {
+ armem::data::MemoryID snapshotID;
+ PredictionSettings settings;
+ }
+ sequence<PredictionRequest> PredictionRequestSeq;
+
+ // The result of a single prediction.
+ // If successful, it contains the predicted data.
+ struct PredictionResult
+ {
+ bool success = false;
+ string errorMessage;
+
+ armem::data::MemoryID snapshotID;
+ aron::data::dto::Dict prediction;
+ }
+ sequence<PredictionResult> PredictionResultSeq;
+ };
+ };
+ };
+};
diff --git a/source/RobotAPI/interface/armem/server.ice b/source/RobotAPI/interface/armem/server.ice
index 4d9b0bce81301a18163bbe831c814577d8ab4f3b..5c7a32c4734ede6124ba8c4e028ce3dcd98bb556 100644
--- a/source/RobotAPI/interface/armem/server.ice
+++ b/source/RobotAPI/interface/armem/server.ice
@@ -3,3 +3,4 @@
#include <RobotAPI/interface/armem/server/MemoryInterface.ice>
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.ice>
#include <RobotAPI/interface/armem/server/WritingMemoryInterface.ice>
+#include <RobotAPI/interface/armem/server/PredictingMemoryInterface.ice>
diff --git a/source/RobotAPI/interface/armem/server/MemoryInterface.ice b/source/RobotAPI/interface/armem/server/MemoryInterface.ice
index 05b2ea8c54207f6bcfa7a2d708f8fd7e59df416b..2c6c1e14c5e2ff31ee61006cae3b9ebd6788c534 100644
--- a/source/RobotAPI/interface/armem/server/MemoryInterface.ice
+++ b/source/RobotAPI/interface/armem/server/MemoryInterface.ice
@@ -5,6 +5,8 @@
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.ice>
#include <RobotAPI/interface/armem/server/WritingMemoryInterface.ice>
+#include <RobotAPI/interface/armem/server/PredictingMemoryInterface.ice>
+
#include <RobotAPI/interface/armem/server/ActionsInterface.ice>
#include <RobotAPI/interface/armem/client/MemoryListenerInterface.ice>
@@ -27,6 +29,7 @@ module armarx
interface MemoryInterface extends
WorkingMemoryInterface,
LongTermMemoryInterface,
+ PredictingMemoryInterface,
actions::ActionsInterface,
client::MemoryListenerInterface
{
diff --git a/source/RobotAPI/interface/armem/server/MemoryPredictorInterface.ice b/source/RobotAPI/interface/armem/server/MemoryPredictorInterface.ice
deleted file mode 100644
index 42920804f6d80d9e79151600864dc50e3b0c7aa3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/interface/armem/server/MemoryPredictorInterface.ice
+++ /dev/null
@@ -1,16 +0,0 @@
-#pragma once
-
-#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.ice>
-#include <RobotAPI/interface/armem/server/WritingMemoryInterface.ice>
-
-module armarx {
- module armem {
- module server {
- interface MemoryPredictorInterface
- {
- // TODO
- void predict();
- }
- };
- };
-};
diff --git a/source/RobotAPI/interface/armem/server/PredictingMemoryInterface.ice b/source/RobotAPI/interface/armem/server/PredictingMemoryInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..5b8b668ab72a3c4aef15720e450813891b140124
--- /dev/null
+++ b/source/RobotAPI/interface/armem/server/PredictingMemoryInterface.ice
@@ -0,0 +1,27 @@
+#pragma once
+
+#include <RobotAPI/interface/armem/prediction.ice>
+#include <RobotAPI/interface/armem/query.ice>
+
+module armarx
+{
+ module armem
+ {
+ module server
+ {
+ interface PredictingMemoryInterface
+ {
+ /* Request multiple predictions from the memory.
+ * The timestamp to requst a prediction for is encoded in the MemoryIDs.
+ * The results are all packed into one QueryResult.
+ */
+ prediction::data::PredictionResultSeq
+ predict(prediction::data::PredictionRequestSeq requests);
+
+ /* Get the list of engines the memory offers for use in predictions.
+ */
+ prediction::data::PredictionEngineSeq getAvailableEngines();
+ }
+ };
+ };
+};
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index 6b105d8f67ed0ba87bfd5ae17a2fcf54fea79b70..237d90fa5c71d2ad987102a443c6438691079e9f 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -51,6 +51,7 @@ set(LIB_FILES
client/MemoryNameSystem.cpp
+ client/Prediction.cpp
client/Reader.cpp
client/Writer.cpp
@@ -125,6 +126,7 @@ set(LIB_HEADERS
client.h
client/forward_declarations.h
client/MemoryNameSystem.h
+ client/Prediction.h
client/Reader.h
client/Writer.h
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
index 813981444e2f62afc2a7226952d451422ccd2daf..1559cd1cd6e29dc17397c48f20848759620a39d9 100644
--- a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
@@ -68,6 +68,8 @@ namespace armarx::armem::client
};
foo(it->second.reading, server.reading);
foo(it->second.writing, server.writing);
+ foo(it->second.prediction, server.prediction);
+ foo(it->second.actions, server.actions);
}
}
// Remove all entries which are not there anymore.
@@ -171,19 +173,22 @@ namespace armarx::armem::client
Reader MemoryNameSystem::getReader(const MemoryID& memoryID)
{
- return Reader(resolveServer(memoryID).reading);
+ auto server = resolveServer(memoryID);
+ return Reader(server.reading, server.prediction);
}
Reader MemoryNameSystem::useReader(const MemoryID& memoryID)
{
- return Reader(useServer(memoryID).reading);
+ auto server = useServer(memoryID);
+ return Reader(server.reading, server.prediction);
}
Reader MemoryNameSystem::useReader(const MemoryID& memoryID, ManagedIceObject& component)
{
- return Reader(useServer(memoryID, component).reading);
+ auto server = useServer(memoryID, component);
+ return Reader(server.reading, server.prediction);
}
diff --git a/source/RobotAPI/libraries/armem/client/Prediction.cpp b/source/RobotAPI/libraries/armem/client/Prediction.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f58649a457fc8117309ceaf5426a0a84540f35f5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/Prediction.cpp
@@ -0,0 +1,135 @@
+/*
+ * 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::client
+ * @author phesch ( ulila at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "Prediction.h"
+
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
+
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+
+namespace armarx::armem::client
+{
+ PredictionEngine
+ PredictionEngine::fromIce(const armem::prediction::data::PredictionEngine& ice)
+ {
+ return armarx::fromIce<PredictionEngine>(ice);
+ }
+
+ armem::prediction::data::PredictionEngine
+ PredictionEngine::toIce() const
+ {
+ return armarx::toIce<armem::prediction::data::PredictionEngine>(*this);
+ }
+
+ PredictionSettings
+ PredictionSettings::fromIce(const armem::prediction::data::PredictionSettings& ice)
+ {
+ return armarx::fromIce<PredictionSettings>(ice);
+ }
+
+ armem::prediction::data::PredictionSettings
+ PredictionSettings::toIce() const
+ {
+ return armarx::toIce<armem::prediction::data::PredictionSettings>(*this);
+ }
+
+ PredictionRequest
+ PredictionRequest::fromIce(const armem::prediction::data::PredictionRequest& ice)
+ {
+ return armarx::fromIce<PredictionRequest>(ice);
+ }
+
+ armem::prediction::data::PredictionRequest
+ PredictionRequest::toIce() const
+ {
+ return armarx::toIce<armem::prediction::data::PredictionRequest>(*this);
+ }
+
+ PredictionResult
+ PredictionResult::fromIce(const armem::prediction::data::PredictionResult& ice)
+ {
+ return armarx::fromIce<PredictionResult>(ice);
+ }
+
+ armem::prediction::data::PredictionResult
+ PredictionResult::toIce() const
+ {
+ return armarx::toIce<armem::prediction::data::PredictionResult>(*this);
+ }
+
+ void
+ toIce(armem::prediction::data::PredictionEngine& ice, const PredictionEngine& engine)
+ {
+ ice.engineID = engine.engineID;
+ }
+
+ void
+ fromIce(const armem::prediction::data::PredictionEngine& ice, PredictionEngine& engine)
+ {
+ engine.engineID = ice.engineID;
+ }
+
+ void
+ toIce(armem::prediction::data::PredictionSettings& ice, const PredictionSettings& settings)
+ {
+ ice.predictionEngineID = settings.predictionEngineID;
+ }
+
+ void
+ fromIce(const armem::prediction::data::PredictionSettings& ice, PredictionSettings& settings)
+ {
+ settings.predictionEngineID = ice.predictionEngineID;
+ }
+
+ void
+ toIce(armem::prediction::data::PredictionRequest& ice, const PredictionRequest& request)
+ {
+ toIce(ice.snapshotID, request.snapshotID);
+ toIce(ice.settings, request.predictionSettings);
+ }
+
+ void
+ fromIce(const armem::prediction::data::PredictionRequest& ice, PredictionRequest& request)
+ {
+ fromIce(ice.snapshotID, request.snapshotID);
+ fromIce(ice.settings, request.predictionSettings);
+ }
+
+ void
+ toIce(armem::prediction::data::PredictionResult& ice, const PredictionResult& result)
+ {
+ ice.success = result.success;
+ ice.errorMessage = result.errorMessage.str();
+ toIce(ice.snapshotID, result.snapshotID);
+ ice.prediction = result.prediction->toAronDictDTO();
+ }
+
+ void
+ fromIce(const armem::prediction::data::PredictionResult& ice, PredictionResult& result)
+ {
+ result.success = ice.success;
+ result.errorMessage.str(ice.errorMessage);
+ fromIce(ice.snapshotID, result.snapshotID);
+ result.prediction = aron::data::Dict::FromAronDictDTO(ice.prediction);
+ }
+
+} // namespace armarx::armem::client
diff --git a/source/RobotAPI/libraries/armem/client/Prediction.h b/source/RobotAPI/libraries/armem/client/Prediction.h
new file mode 100644
index 0000000000000000000000000000000000000000..a1538758519228b93e4c6f95e131eed6c7fe9bad
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/Prediction.h
@@ -0,0 +1,86 @@
+/*
+ * 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::client
+ * @author phesch ( ulila 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/interface/armem/prediction.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx::armem::client
+{
+
+ struct PredictionEngine
+ {
+ std::string engineID;
+
+ static PredictionEngine fromIce(const armem::prediction::data::PredictionEngine& ice);
+ armem::prediction::data::PredictionEngine toIce() const;
+ };
+
+ struct PredictionSettings
+ {
+ std::string predictionEngineID;
+
+ static PredictionSettings fromIce(const armem::prediction::data::PredictionSettings& ice);
+ armem::prediction::data::PredictionSettings toIce() const;
+ };
+
+ struct PredictionRequest
+ {
+ armem::MemoryID snapshotID;
+ PredictionSettings predictionSettings;
+
+ static PredictionRequest fromIce(const armem::prediction::data::PredictionRequest& ice);
+ armem::prediction::data::PredictionRequest toIce() const;
+ };
+
+ struct PredictionResult
+ {
+ bool success;
+ std::stringstream errorMessage;
+
+ armem::MemoryID snapshotID;
+ aron::data::DictPtr prediction;
+
+ static PredictionResult fromIce(const armem::prediction::data::PredictionResult& ice);
+ armem::prediction::data::PredictionResult toIce() const;
+ };
+
+ void toIce(armem::prediction::data::PredictionEngine& ice,
+ const PredictionEngine& engine);
+ void fromIce(const armem::prediction::data::PredictionEngine& ice,
+ PredictionEngine& engine);
+
+ void toIce(armem::prediction::data::PredictionSettings& ice,
+ const PredictionSettings& settings);
+ void fromIce(const armem::prediction::data::PredictionSettings& ice,
+ PredictionSettings& settings);
+
+ void toIce(armem::prediction::data::PredictionRequest& ice, const PredictionRequest& request);
+ void fromIce(const armem::prediction::data::PredictionRequest& ice,
+ PredictionRequest& request);
+
+ void toIce(armem::prediction::data::PredictionResult& ice, const PredictionResult& result);
+ void fromIce(const armem::prediction::data::PredictionResult& ice, PredictionResult& result);
+
+} // namespace armarx::armem::client
diff --git a/source/RobotAPI/libraries/armem/client/Reader.cpp b/source/RobotAPI/libraries/armem/client/Reader.cpp
index 5bf6dcd0308cfad16c6401bed4c2ae940a972930..52be34eb42c8f5c54f2138731690a5f43728d6f8 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem/client/Reader.cpp
@@ -3,11 +3,11 @@
#include <sstream>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <RobotAPI/libraries/armem/aron/MemoryLink.aron.generated.h>
#include <RobotAPI/libraries/armem/core/MemoryID_operators.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/aron/common/util/object_finders.h>
@@ -20,7 +20,9 @@
namespace armarx::armem::client
{
- Reader::Reader(server::ReadingMemoryInterfacePrx memory) : memoryPrx(memory)
+ Reader::Reader(server::ReadingMemoryInterfacePrx readingMemory,
+ server::PredictingMemoryInterfacePrx predictingMemory) :
+ readingPrx(readingMemory), predictionPrx(predictingMemory)
{
}
@@ -35,30 +37,22 @@ namespace armarx::armem::client
armem::query::data::Result
Reader::query(const armem::query::data::Input& input) const
{
- auto makeErrorMsg = [](auto e)
- {
- std::stringstream ss;
- ss << "Memory query failed.\nReason: " << e.what();
- return ss.str();
- };
-
armem::query::data::Result result;
- ARMARX_CHECK_NOT_NULL(memoryPrx);
- try
- {
- result = memoryPrx->query(input);
- }
- catch (const Ice::ConnectionRefusedException& e)
+ if (!readingPrx)
{
- result.errorMessage = makeErrorMsg(e);
+ throw error::ProxyNotSet("ReadingMemoryInterfacePrx",
+ "Reading interface proxy must be set to perform a query.");
}
- catch (const Ice::ConnectionLostException& e)
+
+ try
{
- result.errorMessage = makeErrorMsg(e);
+ result = readingPrx->query(input);
}
- catch (const Ice::NotRegisteredException& e)
+ catch (const Ice::LocalException& e)
{
- result.errorMessage = makeErrorMsg(e);
+ std::stringstream sstream;
+ sstream << "Memory query failed.\nReason: " << e.what();
+ result.errorMessage = sstream.str();
}
return result;
}
@@ -97,35 +91,27 @@ namespace armarx::armem::client
armem::client::MemoryNameSystem& mns,
int recursionDepth) const
{
- auto makeErrorMsg = [](auto error)
+ if (!readingPrx)
{
- std::stringstream sstream;
- sstream << "Memory query failed.\nReason: " << error.what();
- return sstream.str();
- };
+ throw error::ProxyNotSet("ReadingMemoryInterfacePrx",
+ "Reading interface proxy must be set to perform a query.");
+ }
armem::query::data::Result result;
- ARMARX_CHECK_NOT_NULL(memoryPrx);
try
{
- result = memoryPrx->query(input);
+ result = readingPrx->query(input);
QueryResult bObj = QueryResult::fromIce(result);
bObj.memory.forEachInstance(
[&bObj, &mns, recursionDepth](armem::wm::EntityInstance& instance)
{ resolveMemoryLinks(instance, bObj.memory, mns, recursionDepth); });
result = bObj.toIce();
}
- catch (const Ice::ConnectionRefusedException& e)
- {
- result.errorMessage = makeErrorMsg(e);
- }
- catch (const Ice::ConnectionLostException& e)
- {
- result.errorMessage = makeErrorMsg(e);
- }
- catch (const Ice::NotRegisteredException& e)
+ catch (const Ice::LocalException& e)
{
- result.errorMessage = makeErrorMsg(e);
+ std::stringstream sstream;
+ sstream << "Memory query failed.\nReason: " << e.what();
+ result.errorMessage = sstream.str();
}
catch (const exceptions::local::ExpressionException& e)
{
@@ -424,7 +410,7 @@ namespace armarx::armem::client
data::StoreResult
Reader::readAndStore(const data::StoreInput& input) const
{
- server::StoringMemoryInterfacePrx storingMemoryPrx = server::StoringMemoryInterfacePrx::checkedCast(memoryPrx);
+ server::StoringMemoryInterfacePrx storingMemoryPrx = server::StoringMemoryInterfacePrx::checkedCast(readingPrx);
if (storingMemoryPrx)
{
return storingMemoryPrx->store(input);
@@ -438,8 +424,88 @@ namespace armarx::armem::client
void
- Reader::setReadingMemory(server::ReadingMemoryInterfacePrx memory)
+ Reader::setReadingMemory(server::ReadingMemoryInterfacePrx readingMemory)
+ {
+ this->readingPrx = readingMemory;
+ }
+
+ void
+ Reader::setPredictingMemory(server::PredictingMemoryInterfacePrx predictingMemory)
+ {
+ this->predictionPrx = predictingMemory;
+ }
+
+ std::vector<PredictionResult>
+ Reader::predict(const std::vector<PredictionRequest>& requests) const
+ {
+ if (!predictionPrx)
+ {
+ throw error::ProxyNotSet(
+ "PredictingMemoryInterfacePrx",
+ "Prediction interface proxy must be set to request a prediction.");
+ }
+
+ armem::prediction::data::PredictionRequestSeq iceRequests;
+ for (const auto& request : requests)
+ {
+ iceRequests.push_back(request.toIce());
+ }
+
+ armem::prediction::data::PredictionResultSeq results;
+ try
+ {
+ results = predictionPrx->predict(iceRequests);
+ }
+ catch (const Ice::LocalException& e)
+ {
+ armem::prediction::data::PredictionResult failure;
+ failure.success = false;
+ std::stringstream sstream;
+ sstream << "Prediction request failed. Reason: " << e.what();
+ failure.errorMessage = sstream.str();
+
+ for (size_t i = 0; i < requests.size(); ++i)
+ {
+ results.push_back(failure);
+ }
+ }
+
+ std::vector<PredictionResult> boResults;
+ for (const auto& result : results)
+ {
+ boResults.push_back(PredictionResult::fromIce(result));
+ }
+
+ return boResults;
+ }
+
+ std::vector<PredictionEngine>
+ Reader::getAvailablePredictionEngines() const
{
- this->memoryPrx = memory;
+ if (!predictionPrx)
+ {
+ throw error::ProxyNotSet(
+ "PredictingMemoryInterfacePrx",
+ "Prediction interface proxy must be set to request a prediction.");
+ }
+
+ std::vector<armem::prediction::data::PredictionEngine> engines;
+ try
+ {
+ engines = predictionPrx->getAvailableEngines();
+ }
+ catch (const Ice::LocalException& e)
+ {
+ // Just leave engines empty in this case.
+ }
+
+ std::vector<PredictionEngine> boEngines;
+ boEngines.reserve(engines.size());
+ for (const auto& engine : engines)
+ {
+ boEngines.push_back(armarx::fromIce<PredictionEngine>(engine));
+ }
+
+ return boEngines;
}
-}
+} // namespace armarx::armem::client
diff --git a/source/RobotAPI/libraries/armem/client/Reader.h b/source/RobotAPI/libraries/armem/client/Reader.h
index 1b957fde328f8c09b1086dc7fa2f0011cecd694b..beb0e53f5ff25078cb881d4477c19c5cd291fe1a 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.h
+++ b/source/RobotAPI/libraries/armem/client/Reader.h
@@ -12,6 +12,7 @@
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include "Query.h"
+#include "Prediction.h"
namespace armarx::armem::client
@@ -29,9 +30,11 @@ namespace armarx::armem::client
* @brief Construct a memory reader.
* @param memory The memory proxy.
*/
- Reader(server::ReadingMemoryInterfacePrx memory = nullptr);
+ Reader(server::ReadingMemoryInterfacePrx readingMemory = nullptr,
+ server::PredictingMemoryInterfacePrx predictingMemory = nullptr);
- void setReadingMemory(server::ReadingMemoryInterfacePrx memory);
+ void setReadingMemory(server::ReadingMemoryInterfacePrx readingMemory);
+ void setPredictingMemory(server::PredictingMemoryInterfacePrx predictingMemory);
/// Perform a query.
QueryResult query(const QueryInput& input) const;
@@ -162,10 +165,31 @@ namespace armarx::armem::client
//data::StoreResult readAndStore(data::StoreInputSeq& input);
data::StoreResult readAndStore(const data::StoreInput& input) const;
+ /**
+ * @brief Get a prediction for the state of multiple entity instances in the future.
+ *
+ * The timestamp to predict the entities' state at is given via the snapshot
+ * given in the MemoryID.
+ *
+ * The `predictionPrx` must not be null when calling this function.
+ *
+ * @param requests a list of requests for entity instance predictions
+ * @return the vector of prediction results
+ */
+ std::vector<PredictionResult> predict(const std::vector<PredictionRequest>& requests) const;
+
+ /**
+ * @brief Get the list of prediction engines supported by the memory.
+ *
+ * The `predictionPrx` must not be null when calling this function.
+ *
+ * @return the vector of supported prediction engines
+ */
+ std::vector<PredictionEngine> getAvailablePredictionEngines() const;
inline operator bool() const
{
- return bool(memoryPrx);
+ return bool(readingPrx);
}
private:
@@ -176,7 +200,8 @@ namespace armarx::armem::client
public:
- server::ReadingMemoryInterfacePrx memoryPrx;
+ server::ReadingMemoryInterfacePrx readingPrx;
+ server::PredictingMemoryInterfacePrx predictionPrx;
};
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
index ac8be17b51fd8f67d90de93c77513d15b6ff8320..87b687447b7277cbd3861d7c29e3df566e3e26b3 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
@@ -220,4 +220,21 @@ namespace armarx::armem::error
return ss.str();
}
+ ProxyNotSet::ProxyNotSet(const std::string& proxyName, const std::string& message) :
+ ArMemError(makeMsg(proxyName, message))
+ {
+ }
+
+ std::string
+ ProxyNotSet::makeMsg(const std::string& proxyName, const std::string& message)
+ {
+ std::stringstream sstream;
+ sstream << "Proxy \"" << proxyName << "\" not set.";
+ if (!message.empty())
+ {
+ sstream << "\n" << message;
+ }
+ return sstream.str();
+ }
+
} // namespace armarx::armem::error
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.h b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
index 5678ee1a9a8e452309f2127fa41f519858ee233c..6f5be02a243e9e953d56b67f5f1726be214b2745 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.h
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
@@ -203,5 +203,15 @@ namespace armarx::armem::error
};
-}
+ /**
+ * @brief Indicates that a proxy required for an operation wasn't usable.
+ */
+ class ProxyNotSet : public ArMemError
+ {
+ public:
+ ProxyNotSet(const std::string& proxyName, const std::string& message = "");
+
+ static std::string makeMsg(const std::string& proxyName, const std::string& message = "");
+ };
+} // namespace armarx::armem::error
diff --git a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
index 566cf7e0bcbdba70e8e16ca5c527120628c89f32..3247c4cc45133132a1346fa42b46166c183a60ad 100644
--- a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
+++ b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
@@ -59,7 +59,7 @@ namespace armarx::armem::mns
int row = 0;
grid.add(Label("Memory Name"), {row, 0})
.add(Label("Component Name"), {row, 1})
- .add(Label("R/W/A"), {row, 2})
+ .add(Label("R/W/P/A"), {row, 2})
.add(Label("Registration Time"), {row, 3})
;
row++;
@@ -79,6 +79,11 @@ namespace armarx::armem::mns
componentName = info.server.writing->ice_getIdentity().name;
mode += "W";
}
+ if (info.server.prediction)
+ {
+ componentName = info.server.prediction->ice_getIdentity().name;
+ mode += "P";
+ }
if (info.server.actions)
{
componentName = info.server.actions->ice_getIdentity().name;
diff --git a/source/RobotAPI/libraries/armem/mns/Registry.cpp b/source/RobotAPI/libraries/armem/mns/Registry.cpp
index ad3a8e54a3a292f8dfc8dd72ac72b2a7b5564041..79edf5d06193cd61f185044138edd0a19b1a989f 100644
--- a/source/RobotAPI/libraries/armem/mns/Registry.cpp
+++ b/source/RobotAPI/libraries/armem/mns/Registry.cpp
@@ -160,10 +160,14 @@ namespace armarx::armem::mns
}
+ server::PredictingMemoryInterfacePrx getPredictionInterface(const dto::MemoryServerInterfaces& server)
+ {
+ return server.prediction;
+ }
+
+
server::actions::ActionsInterfacePrx getActionsInterface(const dto::MemoryServerInterfaces& server)
{
return server.actions;
}
-
}
-
diff --git a/source/RobotAPI/libraries/armem/mns/Registry.h b/source/RobotAPI/libraries/armem/mns/Registry.h
index 7fcd1c04b95b5b0fe938ae4fdc383ada5bd147d5..0148d6c529fb57321117f21ad800c51d0d3cc549 100644
--- a/source/RobotAPI/libraries/armem/mns/Registry.h
+++ b/source/RobotAPI/libraries/armem/mns/Registry.h
@@ -4,6 +4,7 @@
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/interface/armem/server/ActionsInterface.h>
+#include <RobotAPI/interface/armem/server/PredictingMemoryInterface.h>
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
#include <RobotAPI/interface/armem/server/WritingMemoryInterface.h>
@@ -69,5 +70,6 @@ namespace armarx::armem::mns
server::ReadingMemoryInterfacePrx getReadingInterface(const dto::MemoryServerInterfaces& server);
server::WritingMemoryInterfacePrx getWritingInterface(const dto::MemoryServerInterfaces& server);
+ server::PredictingMemoryInterfacePrx getPredictionInterface(const dto::MemoryServerInterfaces& server);
server::actions::ActionsInterfacePrx getActionsInterface(const dto::MemoryServerInterfaces& server);
}
diff --git a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
index 4356aed6f4591eb5d33baaa86b0f9d4388f0a628..298983e478fbc03f5ec6187e9394da128e2a5171 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
@@ -78,6 +78,7 @@ namespace armarx::armem::server::plugins
mns::dto::MemoryServerInterfaces server;
server.reading = ReadingMemoryInterfacePrx::uncheckedCast(parent.getProxy());
server.writing = WritingMemoryInterfacePrx::uncheckedCast(parent.getProxy());
+ server.prediction = PredictingMemoryInterfacePrx::uncheckedCast(parent.getProxy());
server.actions = actions::ActionsInterfacePrx::uncheckedCast(parent.getProxy());
mns::dto::RegisterServerResult result;
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
index 41e09c13be1cbc68f0281023596be9c275f16dc5..115e38311ae09d9d5e1362bf362b1207a76173a8 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
@@ -1,6 +1,7 @@
#include "ReadWritePluginUser.h"
#include "Plugin.h"
+#include <RobotAPI/libraries/armem/client/Prediction.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
@@ -117,4 +118,39 @@ namespace armarx::armem::server::plugins
return {};
}
+ // PREDICTIONS
+ armem::prediction::data::PredictionResultSeq
+ ReadWritePluginUser::predict(const armem::prediction::data::PredictionRequestSeq& requests)
+ {
+ armem::prediction::data::PredictionResultSeq result;
+ for (auto request : requests)
+ {
+ armem::client::PredictionResult singleResult;
+ singleResult.success = false;
+ singleResult.errorMessage << "This memory does not implement predictions.";
+ singleResult.prediction = nullptr;
+ result.push_back(singleResult.toIce());
+ }
+ return result;
+ }
+
+ armem::prediction::data::PredictionEngineSeq
+ ReadWritePluginUser::getAvailableEngines()
+ {
+ return {};
+ }
+
+ armem::prediction::data::PredictionResultSeq
+ ReadWritePluginUser::predict(const armem::prediction::data::PredictionRequestSeq& requests,
+ const ::Ice::Current& /*unused*/)
+ {
+ return predict(requests);
+ }
+
+ armem::prediction::data::PredictionEngineSeq
+ ReadWritePluginUser::getAvailableEngines(const ::Ice::Current& /*unused*/)
+ {
+ return getAvailableEngines();
+ }
+
} // namespace armarx::armem::server::plugins
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h
index 83e761281bf064a111a90efe15a9148689daa2ff..1e0b4bc82568382ead82cbf7b337529db4f46251 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h
@@ -55,6 +55,13 @@ namespace armarx::armem::server::plugins
virtual armem::actions::GetActionsOutputSeq getActions(const armem::actions::GetActionsInputSeq& inputs, const ::Ice::Current&) override;
virtual armem::actions::ExecuteActionOutputSeq executeActions(const armem::actions::ExecuteActionInputSeq& inputs, const ::Ice::Current&) override;
+ // PredictingInterface interface
+ virtual armem::prediction::data::PredictionResultSeq predict(const armem::prediction::data::PredictionRequestSeq& requests);
+ virtual armem::prediction::data::PredictionEngineSeq getAvailableEngines();
+
+ virtual armem::prediction::data::PredictionResultSeq predict(const armem::prediction::data::PredictionRequestSeq& requests, const ::Ice::Current&) override;
+ virtual armem::prediction::data::PredictionEngineSeq getAvailableEngines(const ::Ice::Current&) override;
+
public:
Plugin& memoryServerPlugin();
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index beebabd2977e8f7e46e976892ee4da3dc18f2bad..0509f1dc7261542e675c06ddc55d8a89d75361ec 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -205,7 +205,7 @@ namespace armarx::armem::robot_state
.timestamp = timestamp, .globalPose = *globalPose, .jointMap = *jointMap};
}
- std::optional<robot::RobotState::Pose>
+ std::optional<::armarx::armem::robot_state::Transform>
RobotReader::queryOdometryPose(const robot::RobotDescription& description,
const armem::Time& timestamp) const
{
@@ -226,7 +226,7 @@ namespace armarx::armem::robot_state
return std::nullopt;
}
- return result.transform.transform;
+ return result.transform;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index e72b5fc64744a7ad3481990705568e92a8c56208..247deb3b20c2613ab2d3c4d14ce076b76f076790 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -108,7 +108,7 @@ namespace armarx::armem::robot_state
* @param timestamp
* @return std::optional<robot::RobotState::Pose>
*/
- std::optional<robot::RobotState::Pose>
+ std::optional<::armarx::armem::robot_state::Transform>
queryOdometryPose(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
index a595d5502c7139e51d9e940d71f24925ab39f199..02d98d377a0feb89157160baa8c4205b951a1970 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -6,6 +6,9 @@
#include <ArmarXCore/core/exceptions/LocalException.h>
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/time/DateTime.h"
+#include "ArmarXCore/core/time/Duration.h"
+#include "RobotAPI/libraries/armem/core/forward_declarations.h"
#include <RobotAPI/libraries/core/FramedPose.h>
@@ -41,6 +44,14 @@ namespace armarx::armem::common::robot_state::localization
const std::vector<Eigen::Affine3f> transforms = _obtainTransforms(
localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
+
+ const armem::Time sanitizedTimestamp = _obtainTimestamp(localizationCoreSegment, query.header.timestamp);
+ auto header = query.header;
+
+ ARMARX_INFO << header.timestamp << "vs" << sanitizedTimestamp;
+
+ header.timestamp = sanitizedTimestamp;
+
if (transforms.empty())
{
ARMARX_WARNING << deactivateSpam(1) << "No transform available.";
@@ -58,7 +69,7 @@ namespace armarx::armem::common::robot_state::localization
ARMARX_DEBUG << "Found valid transform";
- return {.transform = {.header = query.header, .transform = transform},
+ return {.transform = {.header = header, .transform = transform},
.status = TransformResult::Status::Success};
}
@@ -187,6 +198,30 @@ namespace armarx::armem::common::robot_state::localization
return chain;
}
+ template <class ...Args>
+ armarx::core::time::DateTime
+ TransformHelper::_obtainTimestamp(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment, const armem::Time& timestamp)
+ {
+
+ // first we check which the newest timestamp is
+ int64_t timeSinceEpochUs = 0;
+
+ localizationCoreSegment.forEachEntity([&timeSinceEpochUs, ×tamp](const auto& entity){
+ auto snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
+ const armem::wm::EntityInstance& item = snapshot->getInstance(0);
+ const auto tf = _convertEntityToTransform(item);
+
+ const auto& dataTs = tf.header.timestamp;
+
+ timeSinceEpochUs = std::max(timeSinceEpochUs, dataTs.toMicroSecondsSinceEpoch());
+ });
+
+ // then we ensure that the timestamp is not more recent than the query timestamp
+ timeSinceEpochUs = std::min(timeSinceEpochUs, timestamp.toMicroSecondsSinceEpoch());
+
+ return armarx::core::time::DateTime(armarx::core::time::Duration::MicroSeconds(timeSinceEpochUs));
+ }
+
template <class ...Args>
std::vector<Eigen::Affine3f>
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
index 9de1fbd994f5a17c00987ce70d31525db7d9d1dd..6e2611f7a178ba64ab3e25082b5e74a6b968dc23 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
@@ -111,6 +111,10 @@ namespace armarx::armem::common::robot_state::localization
const armem::base::ProviderSegmentBase<Args...>& agentProviderSegment,
const armem::Time& timestamp);
+ template <class ...Args>
+ static
+ armarx::core::time::DateTime
+ _obtainTimestamp(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment, const armem::Time& timestamp);
static
Eigen::Affine3f