diff --git a/data/RobotAPI/robotStateMemoryConfig.txt b/data/RobotAPI/robotStateMemoryConfig.txt
index bc928c16990df50ca55c96550f320efac999c332..e6e6498e4ce03f08c6b930cb3b73c678a578f34c 100644
--- a/data/RobotAPI/robotStateMemoryConfig.txt
+++ b/data/RobotAPI/robotStateMemoryConfig.txt
@@ -1,5 +1,7 @@
// Configuration set for the Humanoid Robot ARMAR-6
-// Left Hand unit
+
+// Reft Hand
+
sens.Index L 1 Joint.acceleration => HandL
sens.Index L 1 Joint.gravityTorque => HandL
sens.Index L 1 Joint.inertiaTorque => HandL
@@ -103,7 +105,9 @@ sens.Thumb L 2 Joint.position => HandL
sens.Thumb L 2 Joint.torque => HandL
sens.Thumb L 2 Joint.velocity => HandL
-// Right Hand unit
+
+// Right Hand
+
sens.Index R 1 Joint.acceleration => HandR
sens.Index R 1 Joint.gravityTorque => HandR
sens.Index R 1 Joint.inertiaTorque => HandR
@@ -185,7 +189,165 @@ sens.Thumb R 2 Joint.position => HandR
sens.Thumb R 2 Joint.torque => HandR
sens.Thumb R 2 Joint.velocity => HandR
+sens.Pinky R 1 Joint.acceleration => HandR
+sens.Pinky R 1 Joint.gravityTorque => HandR
+sens.Pinky R 1 Joint.inertiaTorque => HandR
+sens.Pinky R 1 Joint.inverseDynamicsTorque => HandR
+sens.Pinky R 1 Joint.position => HandR
+sens.Pinky R 1 Joint.torque => HandR
+sens.Pinky R 1 Joint.velocity => HandR
+sens.Pinky R 2 Joint.acceleration => HandR
+sens.Pinky R 2 Joint.gravityTorque => HandR
+sens.Pinky R 2 Joint.inertiaTorque => HandR
+sens.Pinky R 2 Joint.inverseDynamicsTorque => HandR
+sens.Pinky R 2 Joint.position => HandR
+sens.Pinky R 2 Joint.torque => HandR
+sens.Pinky R 2 Joint.velocity => HandR
+sens.Pinky R 3 Joint.acceleration => HandR
+sens.Pinky R 3 Joint.gravityTorque => HandR
+sens.Pinky R 3 Joint.inertiaTorque => HandR
+sens.Pinky R 3 Joint.inverseDynamicsTorque => HandR
+sens.Pinky R 3 Joint.position => HandR
+sens.Pinky R 3 Joint.torque => HandR
+sens.Pinky R 3 Joint.velocity => HandR
+
+
+// Arm L
+
+sens.ArmL1_Cla1.acceleration => ArmL
+sens.ArmL1_Cla1.gravityTorque => ArmL
+sens.ArmL1_Cla1.inertiaTorque => ArmL
+sens.ArmL1_Cla1.inverseDynamicsTorque => ArmL
+sens.ArmL1_Cla1.position => ArmL
+sens.ArmL1_Cla1.torque => ArmL
+sens.ArmL1_Cla1.velocity => ArmL
+
+sens.ArmL2_Sho1.acceleration => ArmL
+sens.ArmL2_Sho1.gravityTorque => ArmL
+sens.ArmL2_Sho1.inertiaTorque => ArmL
+sens.ArmL2_Sho1.inverseDynamicsTorque => ArmL
+sens.ArmL2_Sho1.position => ArmL
+sens.ArmL2_Sho1.torque => ArmL
+sens.ArmL2_Sho1.velocity => ArmL
+
+sens.ArmL3_Sho2.acceleration => ArmL
+sens.ArmL3_Sho2.gravityTorque => ArmL
+sens.ArmL3_Sho2.inertiaTorque => ArmL
+sens.ArmL3_Sho2.inverseDynamicsTorque => ArmL
+sens.ArmL3_Sho2.position => ArmL
+sens.ArmL3_Sho2.torque => ArmL
+sens.ArmL3_Sho2.velocity => ArmL
+
+sens.ArmL4_Sho3.acceleration => ArmL
+sens.ArmL4_Sho3.gravityTorque => ArmL
+sens.ArmL4_Sho3.inertiaTorque => ArmL
+sens.ArmL4_Sho3.inverseDynamicsTorque => ArmL
+sens.ArmL4_Sho3.position => ArmL
+sens.ArmL4_Sho3.torque => ArmL
+sens.ArmL4_Sho3.velocity => ArmL
+
+sens.ArmL5_Elb1.acceleration => ArmL
+sens.ArmL5_Elb1.gravityTorque => ArmL
+sens.ArmL5_Elb1.inertiaTorque => ArmL
+sens.ArmL5_Elb1.inverseDynamicsTorque => ArmL
+sens.ArmL5_Elb1.position => ArmL
+sens.ArmL5_Elb1.torque => ArmL
+sens.ArmL5_Elb1.velocity => ArmL
+
+sens.ArmL6_Elb2.acceleration => ArmL
+sens.ArmL6_Elb2.gravityTorque => ArmL
+sens.ArmL6_Elb2.inertiaTorque => ArmL
+sens.ArmL6_Elb2.inverseDynamicsTorque => ArmL
+sens.ArmL6_Elb2.position => ArmL
+sens.ArmL6_Elb2.torque => ArmL
+sens.ArmL6_Elb2.velocity => ArmL
+
+sens.ArmL7_Wri1.acceleration => ArmL
+sens.ArmL7_Wri1.gravityTorque => ArmL
+sens.ArmL7_Wri1.inertiaTorque => ArmL
+sens.ArmL7_Wri1.inverseDynamicsTorque => ArmL
+sens.ArmL7_Wri1.position => ArmL
+sens.ArmL7_Wri1.torque => ArmL
+sens.ArmL7_Wri1.velocity => ArmL
+
+sens.ArmL8_Wri2.acceleration => ArmL
+sens.ArmL8_Wri2.gravityTorque => ArmL
+sens.ArmL8_Wri2.inertiaTorque => ArmL
+sens.ArmL8_Wri2.inverseDynamicsTorque => ArmL
+sens.ArmL8_Wri2.position => ArmL
+sens.ArmL8_Wri2.torque => ArmL
+sens.ArmL8_Wri2.velocity => ArmL
+
+
+// Arm R
+
+sens.ArmR1_Cla1.acceleration => ArmR
+sens.ArmR1_Cla1.gravityTorque => ArmR
+sens.ArmR1_Cla1.inertiaTorque => ArmR
+sens.ArmR1_Cla1.inverseDynamicsTorque => ArmR
+sens.ArmR1_Cla1.position => ArmR
+sens.ArmR1_Cla1.torque => ArmR
+sens.ArmR1_Cla1.velocity => ArmR
+
+sens.ArmR2_Sho1.acceleration => ArmR
+sens.ArmR2_Sho1.gravityTorque => ArmR
+sens.ArmR2_Sho1.inertiaTorque => ArmR
+sens.ArmR2_Sho1.inverseDynamicsTorque => ArmR
+sens.ArmR2_Sho1.position => ArmR
+sens.ArmR2_Sho1.torque => ArmR
+sens.ArmR2_Sho1.velocity => ArmR
+
+sens.ArmR3_Sho2.acceleration => ArmR
+sens.ArmR3_Sho2.gravityTorque => ArmR
+sens.ArmR3_Sho2.inertiaTorque => ArmR
+sens.ArmR3_Sho2.inverseDynamicsTorque => ArmR
+sens.ArmR3_Sho2.position => ArmR
+sens.ArmR3_Sho2.torque => ArmR
+sens.ArmR3_Sho2.velocity => ArmR
+
+sens.ArmR4_Sho3.acceleration => ArmR
+sens.ArmR4_Sho3.gravityTorque => ArmR
+sens.ArmR4_Sho3.inertiaTorque => ArmR
+sens.ArmR4_Sho3.inverseDynamicsTorque => ArmR
+sens.ArmR4_Sho3.position => ArmR
+sens.ArmR4_Sho3.torque => ArmR
+sens.ArmR4_Sho3.velocity => ArmR
+
+sens.ArmR5_Elb1.acceleration => ArmR
+sens.ArmR5_Elb1.gravityTorque => ArmR
+sens.ArmR5_Elb1.inertiaTorque => ArmR
+sens.ArmR5_Elb1.inverseDynamicsTorque => ArmR
+sens.ArmR5_Elb1.position => ArmR
+sens.ArmR5_Elb1.torque => ArmR
+sens.ArmR5_Elb1.velocity => ArmR
+
+sens.ArmR6_Elb2.acceleration => ArmR
+sens.ArmR6_Elb2.gravityTorque => ArmR
+sens.ArmR6_Elb2.inertiaTorque => ArmR
+sens.ArmR6_Elb2.inverseDynamicsTorque => ArmR
+sens.ArmR6_Elb2.position => ArmR
+sens.ArmR6_Elb2.torque => ArmR
+sens.ArmR6_Elb2.velocity => ArmR
+
+sens.ArmR7_Wri1.acceleration => ArmR
+sens.ArmR7_Wri1.gravityTorque => ArmR
+sens.ArmR7_Wri1.inertiaTorque => ArmR
+sens.ArmR7_Wri1.inverseDynamicsTorque => ArmR
+sens.ArmR7_Wri1.position => ArmR
+sens.ArmR7_Wri1.torque => ArmR
+sens.ArmR7_Wri1.velocity => ArmR
+
+sens.ArmR8_Wri2.acceleration => ArmR
+sens.ArmR8_Wri2.gravityTorque => ArmR
+sens.ArmR8_Wri2.inertiaTorque => ArmR
+sens.ArmR8_Wri2.inverseDynamicsTorque => ArmR
+sens.ArmR8_Wri2.position => ArmR
+sens.ArmR8_Wri2.torque => ArmR
+sens.ArmR8_Wri2.velocity => ArmR
+
+
// Neck
+
sens.Neck_1_Yaw.acceleration => Neck
sens.Neck_1_Yaw.gravityTorque => Neck
sens.Neck_1_Yaw.inertiaTorque => Neck
@@ -193,10 +355,70 @@ sens.Neck_1_Yaw.inverseDynamicsTorque => Neck
sens.Neck_1_Yaw.position => Neck
sens.Neck_1_Yaw.torque => Neck
sens.Neck_1_Yaw.velocity => Neck
+
sens.Neck_2_Pitch.acceleration => Neck
sens.Neck_2_Pitch.gravityTorque => Neck
sens.Neck_2_Pitch.inertiaTorque => Neck
-sens.Neck_2_Pitch.inverseDynamicsTorque
+sens.Neck_2_Pitch.inverseDynamicsTorque => Neck
sens.Neck_2_Pitch.position => Neck
sens.Neck_2_Pitch.torque => Neck
sens.Neck_2_Pitch.velocity => Neck
+
+
+// Torso
+
+sens.TorsoJoint.acceleration => Torso
+sens.TorsoJoint.gravityTorque => Torso
+sens.TorsoJoint.inertiaTorque => Torso
+sens.TorsoJoint.inverseDynamicsTorque => Torso
+sens.TorsoJoint.position => Torso
+sens.TorsoJoint.torque => Torso
+sens.TorsoJoint.velocity => Torso
+
+
+// Platform
+
+sens.Platform.accelerationX => Platform
+sens.Platform.accelerationY => Platform
+sens.Platform.accelerationRotation => Platform
+sens.Platform.absolutePositionX => Platform
+sens.Platform.absolutePositionY => Platform
+sens.Platform.absolutePositionRotation => Platform
+sens.Platform.relativePositionX => Platform
+sens.Platform.relativePositionY => Platform
+sens.Platform.relativePositionRotation => Platform
+sens.Platform.velocityX => Platform
+sens.Platform.velocityY => Platform
+sens.Platform.velocityRotation => Platform
+
+
+// FT L
+
+sens.FT L.fx => FT L
+sens.FT L.fy => FT L
+sens.FT L.fz => FT L
+sens.FT L.gravCompFx => FT L
+sens.FT L.gravCompFy => FT L
+sens.FT L.gravCompFz => FT L
+sens.FT L.gravCompTx => FT L
+sens.FT L.gravCompTy => FT L
+sens.FT L.gravCompTz => FT L
+sens.FT L.tx => FT L
+sens.FT L.ty => FT L
+sens.FT L.tz => FT L
+
+
+// FT R
+
+sens.FT R.fx => FT R
+sens.FT R.fy => FT R
+sens.FT R.fz => FT R
+sens.FT R.gravCompFx => FT R
+sens.FT R.gravCompFy => FT R
+sens.FT R.gravCompFz => FT R
+sens.FT R.gravCompTx => FT R
+sens.FT R.gravCompTy => FT R
+sens.FT R.gravCompTz => FT R
+sens.FT R.tx => FT R
+sens.FT R.ty => FT R
+sens.FT R.tz => FT R
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
index 5f99047a649e80be511efd37c1cfbc8ac438031a..778c2db5fd47742488efb3ee0f8ce6aaae4fe0e3 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
@@ -5,6 +5,7 @@
#include <Inventor/sensors/SoTimerSensor.h>
#include <Inventor/nodes/SoUnits.h>
+#include <QCoreApplication>
#include <thread>
@@ -88,11 +89,6 @@ namespace armarx::viz
&CoinVisualizerWrapper::onUpdateSuccess,
&CoinVisualizerWrapper::onUpdateFailure);
root = new SoSeparator;
-
- //timerSensor = new SoTimerSensor(updateVisualizationCB, this);
-
- //float cycleTimeMS = 33.0f;
- //timerSensor->setInterval(SbTime(cycleTimeMS / 1000.0f));
}
CoinVisualizer::~CoinVisualizer()
@@ -114,9 +110,6 @@ namespace armarx::viz
}
state = CoinVisualizerState::STARTING;
stateStorage = storage;
-
- //SoSensorManager* sensor_mgr = SoDB::getSensorManager();
- //sensor_mgr->insertTimerSensor(timerSensor);
}
void CoinVisualizer::stop()
@@ -126,15 +119,12 @@ namespace armarx::viz
return;
}
- SoSensorManager* sensor_mgr = SoDB::getSensorManager();
state = CoinVisualizerState::STOPPING;
while (state != CoinVisualizerState::STOPPED)
{
- sensor_mgr->processTimerQueue();
+ QCoreApplication::processEvents();
usleep(1000);
}
- sensor_mgr->removeTimerSensor(timerSensor);
-
}
CoinVisualizer_ApplyTiming CoinVisualizer::apply(data::LayerUpdate const& update)
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.h b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
index 49ad2a51bd6f70dfddac1785296b8abc07ba08c9..1494be07dc5615b787f2a50f12a48451b40e045a 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.h
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
@@ -229,7 +229,6 @@ namespace armarx::viz
std::mutex storageMutex;
viz::StorageInterfacePrx storage;
- SoTimerSensor* timerSensor = nullptr;
CoinLayerMap layers;
std::vector<std::type_index> elementVisualizersTypes;
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
index 487c9c6fc4c732fc1d4fe2f126724cb74c8dd810..72e5b8f33073f716e9d3a299ea26dda288dc6066 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
@@ -42,7 +42,7 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/armem_objects/types.h>
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
index f85a804e97337e1193ca1f38048beb103e65ce59..b4cab3fc71e6d6b1dd92e3128fd3a90a8c191109 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
@@ -14,7 +14,7 @@
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <RobotAPI/libraries/armem/client/ComponentPlugin.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h>
#include <RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h>
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
index 8ae42e00bd5385886696dd6ead35feb927665947..036d27d2bf49f7d5ac9f84e940bf7e1e88b033b1 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
@@ -34,7 +34,7 @@
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.aron.generated.h>
@@ -260,37 +260,24 @@ namespace armarx
if (qResult.success)
{
- armem::wm::Memory& memory = qResult.memory;
-
- ARMARX_CHECK_EQUAL(memory.coreSegments().size(), 1);
- armem::wm::CoreSegment& coreSeg = memory.coreSegments().begin()->second;
+ ARMARX_IMPORTANT << "Getting entity via ID";
- ARMARX_CHECK_EQUAL(coreSeg.providerSegments().size(), 1);
- armem::wm::ProviderSegment& provSeg = coreSeg.providerSegments().begin()->second;
+ armem::wm::Memory& memory = qResult.memory;
+ ARMARX_CHECK_GREATER_EQUAL(memory.size(), 1);
- ARMARX_CHECK_EQUAL(provSeg.entities().size(), 1);
- armem::wm::Entity& entity = provSeg.entities().begin()->second;
+ const armem::wm::Entity* entity = memory.findEntity(entityID);
+ ARMARX_CHECK_NOT_NULL(entity);
+ ARMARX_CHECK_GREATER_EQUAL(entity->size(), 1);
- ARMARX_CHECK_GREATER_EQUAL(entity.history().size(), 1);
- armem::wm::EntitySnapshot& snapshot = entity.history().begin()->second;
+ const armem::wm::EntitySnapshot& snapshot = entity->getLatestSnapshot();
+ ARMARX_CHECK_GREATER_EQUAL(snapshot.size(), 1);
ARMARX_INFO << "Result: "
- << "\n- memory: \t" << memory.name()
- << "\n- core segment: \t" << coreSeg.name()
- << "\n- prov segment: \t" << provSeg.name()
- << "\n- entity: \t" << entity.name()
+ << "\n- entity: \t" << entity->name()
<< "\n- snapshot: \t" << snapshot.time()
<< "\n- #instances: \t" << snapshot.size()
;
- ARMARX_IMPORTANT << "Getting entity via ID";
- const armem::wm::Entity& e = memory.getEntity(entityID);
- ARMARX_INFO << "Result: "
- << "\n- entity: \t" << e.name()
- << "\n- snapshot: \t" << e.getLatestSnapshot().time()
- << "\n- #instances: \t" << e.getLatestSnapshot().size()
- ;
-
// Show memory contents in remote gui.
tab.queryResult = std::move(memory);
tab.rebuild = true;
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
index 5680e316a6228021932afea5d73abd02fdcda6d8..ca28ab86bc1e755c49b93649fd77b26aac0a4250 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
@@ -34,7 +34,7 @@
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/client/ComponentPlugin.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
namespace armarx
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
index 6650e573785cce732f0f2bbe7c128aa34fc58626..8ab75efc78ddad8492598356df374266947fe515 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
@@ -23,7 +23,7 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/armem/core/Time.h>
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
index 4e1d01f5564e96c7e46ad2db33081835873dd7a6..33770ad5425f8824fca2f40ae45323861f29cc91 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
@@ -33,7 +33,7 @@
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/client/ComponentPlugin.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
index 3ed4b4612c5dd3f6e0afbd71630dba7a4753f460..e007e8f5e3ad47c57f9bb71c75e517d57af1addf 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
@@ -122,7 +122,7 @@ namespace armarx
using namespace armarx::RemoteGui::Client;
{
- std::scoped_lock lock(workingMemoryMutex);
+ // Core segments are locked by MemoryRemoteGui.
tab.memoryGroup = armem::server::MemoryRemoteGui().makeGroupBox(workingMemory);
}
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp
index c5ba35a13aa4ec127f64d0992021d29262d54407..56c9d08f49759de2f75c8085551ea63fb9263935 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp
@@ -41,7 +41,7 @@ BOOST_AUTO_TEST_CASE(test_ExampleData_type)
{
armarx::aron::typenavigator::ObjectNavigatorPtr type = ExampleData::toAronType();
- BOOST_CHECK_EQUAL(type->childrenSize(), 15);
+ BOOST_CHECK_EQUAL(type->childrenSize(), 16);
armem::wm::Memory memory;
armem::wm::CoreSegment& core = memory.addCoreSegment("ExampleData", type);
diff --git a/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp b/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp
index ef176a8ffe85af763135febb8ac3af45f317af4f..350810558ce9d2200e61bda865dd1633da15c128 100644
--- a/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp
+++ b/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp
@@ -42,7 +42,7 @@ namespace armarx
}
MotionMemory::MotionMemory() :
- mdbMotions(armem::server::ComponentPluginUser::iceMemory, armem::server::ComponentPluginUser::workingMemoryMutex)
+ mdbMotions(armem::server::ComponentPluginUser::iceMemory)
{
}
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index 91ea5b23400f568891326f6dd6c79b5e603d229c..c624379b89ae336aa561e364706f3780b7fbf0e9 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -59,22 +59,27 @@ namespace armarx::armem::server::obj
return defs;
}
+
ObjectMemory::ObjectMemory() :
server::ComponentPluginUser(),
- instance::SegmentAdapter(server::ComponentPluginUser::iceMemory,
- server::ComponentPluginUser::workingMemoryMutex),
- classSegment(server::ComponentPluginUser::iceMemory,
- server::ComponentPluginUser::workingMemoryMutex),
- attachmentSegment(server::ComponentPluginUser::iceMemory,
- server::ComponentPluginUser::workingMemoryMutex)
+ instance::SegmentAdapter(server::ComponentPluginUser::iceMemory),
+ classSegment(server::ComponentPluginUser::iceMemory),
+ attachmentSegment(server::ComponentPluginUser::iceMemory)
+ {
+ }
+
+
+ ObjectMemory::~ObjectMemory()
{
}
+
std::string ObjectMemory::getDefaultName() const
{
return "ObjectMemory";
}
+
void ObjectMemory::onInitComponent()
{
workingMemory.name() = defaultMemoryName;
@@ -114,6 +119,7 @@ namespace armarx::armem::server::obj
});
}
+
void ObjectMemory::onConnectComponent()
{
// onConnect can be called multiple times, but addRobot will fail if called more than once with the same ID
@@ -141,17 +147,17 @@ namespace armarx::armem::server::obj
ArVizComponentPluginUser::getArvizClient()
);
- attachmentSegment.connect(
- ArVizComponentPluginUser::getArvizClient()
- );
+ attachmentSegment.connect();
RemoteGui_startRunningTask();
}
+
void ObjectMemory::onDisconnectComponent()
{
}
+
void ObjectMemory::onExitComponent()
{
}
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index f29b2cef93efb2aa35a7983f4dce87d3db854b80..b90406be31ac76a40172d67076f931ebb1a6ba1a 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
@@ -38,8 +38,6 @@
#include <RobotAPI/libraries/armem_objects/server/class/Segment.h>
#include <RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h>
-#include <RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.h>
-#include <RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h>
#include <RobotAPI/libraries/armem_objects/server/attachments/Segment.h>
@@ -81,6 +79,7 @@ namespace armarx::armem::server::obj
public:
ObjectMemory();
+ virtual ~ObjectMemory();
/// @see armarx::ManagedIceObject::getDefaultName()
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
index 137453970c1a97f0f3bd9653c067e959db21b4a5..f49cb33c231a20857aa30458b4f7f5f14c05acdc 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
@@ -2,11 +2,14 @@ armarx_component_set_name("RobotStateMemory")
set(COMPONENT_LIBS
+ # ArmarXCore
ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
+ ArmarXCoreComponentPlugins
+ # ArmarXGUI
ArmarXGuiComponentPlugins
- RobotAPICore
- RobotAPIInterfaces
- RobotAPIComponentPlugins
+ # RobotAPI
+ RobotAPICore RobotAPIInterfaces
+ RobotAPIComponentPlugins
armem
armem_robot_state
)
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 111cced66ba9f678ba852d1005a10ded834d0946..4dde0d39cfc4d248188e9af772af470914c73290 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -23,46 +23,40 @@
#include "RobotStateMemory.h"
// STD
-#include <algorithm>
-#include <iostream>
-#include <fstream>
#include <memory>
-// Eigen
-#include <Eigen/Core>
-#include <Eigen/Geometry>
-
// Simox
#include <SimoxUtility/algorithm/string.h>
-#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
// ArmarX
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
+
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
+
namespace armarx::armem::server::robot_state
{
+
RobotStateMemory::RobotStateMemory()
- : descriptionSegment(server::ComponentPluginUser::iceMemory,
- server::ComponentPluginUser::workingMemoryMutex),
- proprioceptionSegment(server::ComponentPluginUser::iceMemory,
- server::ComponentPluginUser::workingMemoryMutex),
- localizationSegment(server::ComponentPluginUser::iceMemory,
- server::ComponentPluginUser::workingMemoryMutex),
+ : descriptionSegment(server::ComponentPluginUser::iceMemory),
+ proprioceptionSegment(server::ComponentPluginUser::iceMemory),
+ localizationSegment(server::ComponentPluginUser::iceMemory),
commonVisu(descriptionSegment, proprioceptionSegment, localizationSegment)
{
+ addPlugin(debugObserver);
+ ARMARX_CHECK_NOT_NULL(debugObserver);
+ addPlugin(robotUnit.plugin);
+ ARMARX_CHECK_NOT_NULL(robotUnit.plugin);
+ robotUnit.reader.setTag(getName());
+ robotUnit.writer.setTag(getName());
}
RobotStateMemory::~RobotStateMemory() = default;
@@ -75,21 +69,25 @@ namespace armarx::armem::server::robot_state
const std::string prefix = "mem.";
workingMemory.name() = "RobotState";
+ longtermMemory.name() = "RobotState";
defs->optional(workingMemory.name(), prefix + "MemoryName", "Name of this memory server.");
const std::string robotUnitPrefix{sensorValuePrefix};
- defs->optional(robotUnitSensorPrefix, robotUnitPrefix + "SensorValuePrefix", "Prefix of all sensor values");
- defs->optional(robotUnitMemoryBatchSize, robotUnitPrefix + "MemoryBatchSize", "The size of the entity snapshot to send to the memory. Min is 1");
- defs->optional(robotUnitPollFrequency, robotUnitPrefix + "UpdateFrequency", "The frequency in Hz to store values. All other values get discarded. Min is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY));
- defs->optional(robotUnitConfigPath, robotUnitPrefix + "ConfigPath", "Specify a configuration file to group the sensor values specifically.");
-
- descriptionSegment.defineProperties(defs);
- proprioceptionSegment.defineProperties(defs);
- localizationSegment.defineProperties(defs);
- commonVisu.defineProperties(defs);
-
- setRobotUnitAsOptionalDependency();
+ defs->optional(robotUnit.reader.properties.sensorPrefix, robotUnitPrefix + "SensorValuePrefix",
+ "Prefix of all sensor values.");
+ defs->optional(robotUnit.writer.properties.memoryBatchSize, robotUnitPrefix + "MemoryBatchSize",
+ "The size of the entity snapshot to send to the memory. Minimum is 1.")
+ .setMin(1);
+ defs->optional(robotUnit.pollFrequency, robotUnitPrefix + "UpdateFrequency",
+ "The frequency to store values in Hz. All other values get discarded. "
+ "Minimum is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY) + ".")
+ .setMin(1).setMax(ROBOT_UNIT_MAXIMUM_FREQUENCY);
+
+ descriptionSegment.defineProperties(defs, prefix + "desc.");
+ proprioceptionSegment.defineProperties(defs, prefix + "prop.");
+ localizationSegment.defineProperties(defs, prefix + "loc.");
+ commonVisu.defineProperties(defs, prefix + "visu.");
return defs;
}
@@ -103,17 +101,16 @@ namespace armarx::armem::server::robot_state
void RobotStateMemory::onInitComponent()
{
-
- descriptionSegment.init();
- proprioceptionSegment.init();
- localizationSegment.init();
+ descriptionSegment.onInit();
+ proprioceptionSegment.onInit();
+ localizationSegment.onInit();
commonVisu.init();
- robotUnitMemoryBatchSize = std::max((unsigned int) 1, robotUnitMemoryBatchSize);
- robotUnitPollFrequency = std::clamp(robotUnitPollFrequency, 1, ROBOT_UNIT_MAXIMUM_FREQUENCY);
+ robotUnit.pollFrequency = std::clamp(robotUnit.pollFrequency, 1, ROBOT_UNIT_MAXIMUM_FREQUENCY);
+ robotUnit.writer.properties.memoryBatchSize = std::max(static_cast<unsigned int>(1), robotUnit.writer.properties.memoryBatchSize);
- Ice::StringSeq includePaths;
- auto packages = armarx::Application::GetProjectDependencies();
+ std::vector<std::string> includePaths;
+ std::vector<std::string> packages = armarx::Application::GetProjectDependencies();
packages.push_back(Application::GetProjectName());
for (const std::string& projectName : packages)
@@ -124,44 +121,47 @@ namespace armarx::armem::server::robot_state
}
CMakePackageFinder project(projectName);
- auto pathsString = project.getIncludePaths();
- Ice::StringSeq projectIncludePaths = simox::alg::split(pathsString, ";,");
+ std::string pathsString = project.getIncludePaths();
+ std::vector<std::string> projectIncludePaths = simox::alg::split(pathsString, ";,");
includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
-
- ArmarXDataPath::getAbsolutePath(robotUnitConfigPath, robotUnitConfigPath, includePaths);
-
- // workingMemory.name() = workingMemoryName;
}
void RobotStateMemory::onConnectComponent()
{
+ ARMARX_CHECK_NOT_NULL(debugObserver->getDebugObserver());
- if (getRobotUnit())
+ if (robotUnit.plugin->getRobotUnit())
{
- waitUntilRobotUnitIsRunning();
+ robotUnit.plugin->waitUntilRobotUnitIsRunning();
}
+ RobotUnitInterfacePrx robotUnitPrx = robotUnit.plugin->getRobotUnit();
- ARMARX_CHECK_NOT_NULL(getRobotUnit()->getKinematicUnit());
-
- descriptionSegment.connect(getArvizClient(), getRobotUnit());
-
- proprioceptionSegment.connect(getArvizClient(), getRobotUnit());
- toIce(robotUnitProviderID, proprioceptionSegment.getRobotUnitProviderID());
+ descriptionSegment.onConnect(robotUnitPrx);
+ proprioceptionSegment.onConnect(robotUnitPrx);
+ localizationSegment.onConnect();
- localizationSegment.connect(getArvizClient());
+ commonVisu.connect(getArvizClient(), debugObserver->getDebugObserver());
- commonVisu.connect(
- getArvizClient()
- );
+ robotUnit.reader.connect(*robotUnit.plugin, *debugObserver,
+ proprioceptionSegment.getRobotUnitProviderID().providerSegmentName);
+ robotUnit.writer.connect(*debugObserver);
+ robotUnit.writer.properties.robotUnitProviderID = proprioceptionSegment.getRobotUnitProviderID();
- cfg.loggingNames.emplace_back(robotUnitSensorPrefix);
- handler = make_shared<RobotUnitDataStreamingReceiver>(this, getRobotUnit(), cfg);
- keys = handler->getDataDescription();
-
- robotUnitConversionTask = new PeriodicTask<RobotStateMemory>(this, &RobotStateMemory::convertRobotUnitStreamingDataToAron, (1000 / robotUnitPollFrequency));
- robotUnitStoringTask = new PeriodicTask<RobotStateMemory>(this, &RobotStateMemory::storeConvertedRobotUnitDataInMemory, (1000 / robotUnitPollFrequency));
+ robotUnit.reader.task = new SimpleRunningTask<>([this]()
+ {
+ robotUnit.reader.run(
+ robotUnit.pollFrequency, robotUnit.dataQueue, robotUnit.dataMutex
+ );
+ }, "Robot Unit Reader");
+ robotUnit.writer.task = new SimpleRunningTask<>([this]()
+ {
+ robotUnit.writer.run(
+ robotUnit.pollFrequency, robotUnit.dataQueue, robotUnit.dataMutex,
+ iceMemory, localizationSegment
+ );
+ }, "Robot State Writer");
startRobotUnitStream();
}
@@ -178,340 +178,45 @@ namespace armarx::armem::server::robot_state
stopRobotUnitStream();
}
+
/*************************************************************/
// RobotUnit Streaming functions
/*************************************************************/
- void RobotStateMemory::convertRobotUnitStreamingDataToAron()
- {
- auto& data = handler->getDataBuffer();
- if (data.size() == 0)
- {
- return;
- }
- auto& currentTimestep = data.back();
-
- ARMARX_DEBUG << "RobotUnitData: Generating new aron map for current timestep";
- auto start = std::chrono::high_resolution_clock::now();
-
- RobotUnitData convertedAndGroupedData;
- for (const auto& [nameEntry, dataEntry] : keys.entries)
- {
- std::string name = nameEntry;
- std::string jointOrWhateverName;
-
- std::string groupName = "";
- if (configSensorMapping.find(name) != configSensorMapping.end())
- {
- groupName = configSensorMapping.at(name);
- }
- else
- {
- const size_t first_dot_pos = name.find(".");
- const size_t second_dot_pos = name.find(".", first_dot_pos + 1); // find second occurence of "."
- if (second_dot_pos == std::string::npos)
- {
- ARMARX_WARNING << "Could not find a groupname for the sensor with name " << name << ". All sensors must be called sens.X.Y where X is the name of the group and Y is the actual sensor. Ignoring this sensor.";
- continue;
- }
- groupName = name.substr(0, second_dot_pos);
-
- jointOrWhateverName = name.substr(first_dot_pos + 1, second_dot_pos - first_dot_pos - 1);
-
- name = name.substr(second_dot_pos + 1); // remove the groupName, TODO check if +1 is valid
- }
-
- RobotUnitData::RobotUnitDataGroup e;
- if (auto it = convertedAndGroupedData.groups.find(groupName); it == convertedAndGroupedData.groups.end())
- {
- // generate new dict for the group
- auto dict = std::make_shared<aron::datanavigator::DictNavigator>();
- auto encGroupName = std::make_shared<aron::datanavigator::StringNavigator>(groupName);
- dict->addElement("EncoderGroupName", encGroupName);
-
- auto iterId = std::make_shared<aron::datanavigator::LongNavigator>(currentTimestep.iterationId);
- dict->addElement("IterationId", iterId);
-
- const auto nameId = std::make_shared<aron::datanavigator::StringNavigator>(jointOrWhateverName);
- dict->addElement("name", nameId);
-
- e.timestamp = currentTimestep.timestampUSec;
- e.name = groupName;
- e.data = dict;
- }
- else
- {
- // reuse existing entry
- e = it->second;
- }
-
- switch (dataEntry.type)
- {
- case RobotUnitDataStreaming::NodeTypeFloat:
- {
- float value = RobotUnitDataStreamingReceiver::GetAs<Ice::Float>(currentTimestep, dataEntry);
- auto aron = std::make_shared<aron::datanavigator::FloatNavigator>(value);
- e.data->addElement(name, aron);
- break;
- }
- case RobotUnitDataStreaming::NodeTypeBool:
- {
- bool value = RobotUnitDataStreamingReceiver::GetAs<bool>(currentTimestep, dataEntry);
- auto aron = std::make_shared<aron::datanavigator::BoolNavigator>(value);
- e.data->addElement(name, aron);
- break;
- }
- case RobotUnitDataStreaming::NodeTypeByte:
- {
- int value = RobotUnitDataStreamingReceiver::GetAs<Ice::Byte>(currentTimestep, dataEntry);
- auto aron = std::make_shared<aron::datanavigator::IntNavigator>(value);
- e.data->addElement(name, aron);
- break;
- }
- case RobotUnitDataStreaming::NodeTypeShort:
- {
- int value = RobotUnitDataStreamingReceiver::GetAs<Ice::Short>(currentTimestep, dataEntry);
- auto aron = std::make_shared<aron::datanavigator::IntNavigator>(value);
- e.data->addElement(name, aron);
- break;
- }
- case RobotUnitDataStreaming::NodeTypeInt:
- {
- int value = RobotUnitDataStreamingReceiver::GetAs<Ice::Int>(currentTimestep, dataEntry);
- auto aron = std::make_shared<aron::datanavigator::IntNavigator>(value);
- e.data->addElement(name, aron);
- break;
- }
- case RobotUnitDataStreaming::NodeTypeLong:
- {
- long value = RobotUnitDataStreamingReceiver::GetAs<Ice::Long>(currentTimestep, dataEntry);
- auto aron = std::make_shared<aron::datanavigator::LongNavigator>(value);
- e.data->addElement(name, aron);
- break;
- }
- case RobotUnitDataStreaming::NodeTypeDouble:
- {
- double value = RobotUnitDataStreamingReceiver::GetAs<Ice::Double>(currentTimestep, dataEntry);
- auto aron = std::make_shared<aron::datanavigator::DoubleNavigator>(value);
- e.data->addElement(name, aron);
- break;
- }
- default:
- throw LocalException("The enum type should not exist! Perhaps someone changed the RobotUnitDataStreaming::NodeType enum definition?");
- }
-
- convertedAndGroupedData.groups.insert({groupName, e});
- }
- {
- std::lock_guard g{robotUnitDataMutex};
- robotUnitDataQueue.push(convertedAndGroupedData);
- }
- auto stop = std::chrono::high_resolution_clock::now();
- ARMARX_DEBUG << "RobotUnitData: The total time needed to convert the data to aron is: " << std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
- }
-
- void RobotStateMemory::storeConvertedRobotUnitDataInMemory()
- {
- unsigned int size = 0;
- {
- std::lock_guard g{robotUnitDataMutex};
- size = robotUnitDataQueue.size(); // the size can only grow
- }
- if (size >= robotUnitMemoryBatchSize)
- {
- ARMARX_DEBUG << "RobotUnitData: Sending batch of " << robotUnitMemoryBatchSize << " timesteps to memory... The size of the queue is: " << size;
- auto start = std::chrono::high_resolution_clock::now();
-
- // send batch to memory
- armem::data::Commit proprioceptionCommit;
- armem::data::Commit odometryCommit;
-
- for (unsigned int i = 0; i < robotUnitMemoryBatchSize; ++i)
- {
- std::lock_guard g{robotUnitDataMutex};
- const auto& convertedAndGroupedData = robotUnitDataQueue.front();
- for (const auto& [encName, encTimestep] : convertedAndGroupedData.groups)
- {
- ARMARX_CHECK_EQUAL(encName, encTimestep.name);
-
- auto entityID = robotUnitProviderID;
- entityID.entityName = encName;
-
- const auto& timeUSec = encTimestep.timestamp;
- const auto& aron = encTimestep.data;
-
- armem::data::EntityUpdate& update = proprioceptionCommit.updates.emplace_back();
- update.entityID = entityID;
- update.timeCreatedMicroSeconds = timeUSec;
- update.instancesData = {aron->toAronDictPtr()};
- }
-
- // odometry pose -> localization segment
- auto it = convertedAndGroupedData.groups.find("sens.Platform");
- if (it != convertedAndGroupedData.groups.end())
- {
- ARMARX_DEBUG << "Found odometry data.";
-
- const RobotUnitData::RobotUnitDataGroup& timestep = it->second;
- const float relPosX = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionX"))->getValue();
- const float relPosY = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionY"))->getValue();
- const float relOrientation = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionRotation"))->getValue();
-
- Eigen::Affine3f odometryPose = Eigen::Affine3f::Identity();
- odometryPose.translation() << relPosX, relPosY, 0; // TODO set height
- odometryPose.linear() = simox::math::rpy_to_mat3f(0.F, 0.F, relOrientation);
-
- // const auto timestamp = armem::Time::microSeconds(it->second.timestamp);
- const auto timestamp = armem::Time::now();
-
- const ::armarx::armem::robot_state::Transform transform
- {
- .header =
- {
- .parentFrame = OdometryFrame,
- .frame = "root", // TODO: robot root node
- .agent = robotUnitProviderID.providerSegmentName,
- .timestamp = timestamp
- },
- .transform = odometryPose
- };
-
- localizationSegment.storeTransform(transform);
-
- }
- else
- {
- ARMARX_INFO << deactivateSpam(1000) << "No odometry data! If you are using a mobile platform this should not have happened";
- }
-
-
- robotUnitDataQueue.pop();
- }
-
- auto results = commit(proprioceptionCommit);
- auto stop = std::chrono::high_resolution_clock::now();
- ARMARX_DEBUG << "RobotUnitData: The total runtime of sending a batch to the memory is: " << std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
-
- for (const auto& result : results.results)
- {
- if (!result.success)
- {
- ARMARX_WARNING << "Could not add data to memory. Error message: " << result.errorMessage;
- }
- }
- }
- }
-
- void RobotStateMemory::stopRobotUnitStream()
- {
- std::lock_guard g{startStopMutex};
- robotUnitConversionTask->stop();
- robotUnitStoringTask->stop();
- }
void RobotStateMemory::startRobotUnitStream()
{
- std::lock_guard g{startStopMutex};
- if (robotUnitConversionTask->isRunning() || robotUnitStoringTask->isRunning())
+ std::lock_guard lock{startStopMutex};
+ if (robotUnit.reader.task->isRunning() || robotUnit.writer.task->isRunning())
{
- if (robotUnitConversionTask->isRunning() && robotUnitStoringTask->isRunning())
+ if (robotUnit.reader.task->isRunning() && robotUnit.writer.task->isRunning())
{
return;
}
ARMARX_WARNING << "Found inconsistency in running tasks. Restarting all!";
stopRobotUnitStream();
}
-
- std::stringstream ss;
- ss << "Getting sensor values for:" << std::endl;
- for (const auto& [name, dataEntry] : keys.entries)
{
- std::string type = "";
- switch (dataEntry.type)
+ std::stringstream ss;
+ ss << "Getting sensor values for:" << std::endl;
+ for (const auto& [name, dataEntry] : robotUnit.reader.description.entries)
{
- case RobotUnitDataStreaming::NodeTypeBool:
- {
- type = "Bool";
- break;
- }
- case RobotUnitDataStreaming::NodeTypeByte:
- {
- type = "Byte";
- break;
- }
- case RobotUnitDataStreaming::NodeTypeShort:
- {
- type = "Short";
- break;
- }
- case RobotUnitDataStreaming::NodeTypeInt:
- {
- type = "Int";
- break;
- }
- case RobotUnitDataStreaming::NodeTypeLong:
- {
- type = "Long";
- break;
- }
- case RobotUnitDataStreaming::NodeTypeFloat:
- {
- type = "Float";
- break;
- }
- case RobotUnitDataStreaming::NodeTypeDouble:
- {
- type = "Double";
- break;
- }
+ const std::string type = RobotUnitDataStreaming::DataEntryNames.to_name(dataEntry.type);
+ ss << "\t" << name << " (type: '" << type << "')" << std::endl;
}
- ss << "\t" << name << " (type: '" << type << "')" << std::endl;
+ ARMARX_INFO << ss.str();
}
- ARMARX_INFO << ss.str();
- // parse config
- std::filesystem::path configPath(robotUnitConfigPath);
- configSensorMapping.clear();
- if (std::filesystem::is_regular_file(configPath))
- {
- ARMARX_INFO << "Found a configuration file at: " << robotUnitConfigPath;
- // a simple self-made parser for the config file. Extend it if you need to
- std::ifstream in(configPath);
- std::string line;
- while (std::getline(in, line))
- {
- if (line.size() > 0)
- {
- if (simox::alg::starts_with(line, "//"))
- {
- continue;
- }
-
- std::vector<std::string> mappings = simox::alg::split(line, ",");
- for (const auto& mapping : mappings)
- {
- std::vector<std::string> split = simox::alg::split(mapping, "=>");
- if (split.size() < 2)
- {
- ARMARX_WARNING << "A mapping in the RobotUnitConfig file is not valid. Ignoring it: " << mapping;
- continue;
- }
- for (unsigned int i = 0; i < split.size() - 1; ++i)
- {
- ARMARX_DEBUG << "Setting sensor value '" << split[i] << "' to group '" << split[split.size() - 1] << "'.";
- configSensorMapping.insert({split[i], split[split.size() - 1]});
- }
- }
- }
- }
- }
- else
- {
- ARMARX_INFO << "The config path '" << robotUnitConfigPath << "' is not valid.";
- }
+ robotUnit.reader.task->start();
+ robotUnit.writer.task->start();
+ }
+
- robotUnitConversionTask->start();
- robotUnitStoringTask->start();
+ void RobotStateMemory::stopRobotUnitStream()
+ {
+ std::lock_guard lock{startStopMutex};
+ robotUnit.reader.task->stop();
+ robotUnit.writer.task->stop();
}
}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index e8174416e9b547f70af5e5c6b578242a0234743d..2cd2be9e3a5838f16b1a385e829416429b7869cd 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -24,28 +24,30 @@
// STD
#include <atomic>
-#include <thread>
+#include <optional>
#include <queue>
-// Simox
-#include <SimoxUtility/meta/enum/adapt_enum.h>
-
// ArmarX
#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-
-// BaseClass
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
-#include "RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h"
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
#include <RobotAPI/libraries/armem_robot_state/server/description/Segment.h>
-#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h>
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h>
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h>
+
+namespace armarx::plugins
+{
+ class DebugObserverComponentPlugin;
+ class RobotUnitComponentPlugin;
+}
namespace armarx::armem::server::robot_state
{
@@ -63,44 +65,36 @@ namespace armarx::armem::server::robot_state
class RobotStateMemory :
virtual public armarx::Component,
virtual public armem::server::ComponentPluginUser,
- virtual public RobotUnitComponentPluginUser,
virtual public armarx::ArVizComponentPluginUser
{
public:
+
RobotStateMemory();
+ virtual ~RobotStateMemory() override;
- virtual ~RobotStateMemory();
- /// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
+
protected:
- /// @see armarx::ManagedIceObject::onInitComponent()
- void onInitComponent() override;
- /// @see armarx::ManagedIceObject::onConnectComponent()
- void onConnectComponent() override;
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
- /// @see armarx::ManagedIceObject::onDisconnectComponent()
+ void onInitComponent() override;
+ void onConnectComponent() override;
void onDisconnectComponent() override;
-
- /// @see armarx::ManagedIceObject::onExitComponent()
void onExitComponent() override;
- /// @see PropertyUser::createPropertyDefinitions()
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
private:
- // RobotUnit Streaming
- void convertRobotUnitStreamingDataToAron();
- void storeConvertedRobotUnitDataInMemory();
void startRobotUnitStream();
void stopRobotUnitStream();
+
private:
- std::string workingMemoryName = "RobotStateMemory";
- bool addCoreSegmentOnUsage = false;
+
+ armarx::plugins::DebugObserverComponentPlugin* debugObserver = nullptr;
mutable std::recursive_mutex startStopMutex;
@@ -118,41 +112,27 @@ namespace armarx::armem::server::robot_state
// Joint visu for all segments => robot pose and configuration
Visu commonVisu;
- // RobotUnit stuff
- RobotUnitDataStreaming::DataStreamingDescription keys;
- std::vector<RobotUnitDataStreaming::DataEntry> keysList;
- RobotUnitDataStreaming::Config cfg;
- RobotUnitDataStreamingReceiverPtr handler;
- std::map<std::string, std::string> configSensorMapping;
- struct RobotUnitData
+ struct RobotUnit
{
- struct RobotUnitDataGroup
- {
- long timestamp;
- std::string name;
- aron::datanavigator::DictNavigatorPtr data;
- };
-
- std::map<std::string, RobotUnitDataGroup> groups;
+ int pollFrequency = 50;
+
+ armarx::plugins::RobotUnitComponentPlugin* plugin = nullptr;
+ proprioception::RobotUnitReader reader;
+ proprioception::RobotStateWriter writer;
+
+ // queue
+ std::queue<proprioception::RobotUnitData> dataQueue;
+ mutable std::mutex dataMutex;
};
+ RobotUnit robotUnit;
+
// params
static constexpr int ROBOT_UNIT_MAXIMUM_FREQUENCY = 100;
static constexpr const char* sensorValuePrefix = "RobotUnit.";
- int robotUnitPollFrequency = 50;
- std::string robotUnitSensorPrefix = "sens.*";
- unsigned int robotUnitMemoryBatchSize = 50;
- std::string robotUnitConfigPath = "NO CONFIG SET";
-
- // queue
- std::queue<RobotUnitData> robotUnitDataQueue;
- mutable std::mutex robotUnitDataMutex;
- // running tasks
- armarx::PeriodicTask<RobotStateMemory>::pointer_type robotUnitConversionTask;
- armarx::PeriodicTask<RobotStateMemory>::pointer_type robotUnitStoringTask;
};
} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
index c0a9fd2f33211d9a130f236b9350836b75b51466..6c86b6afa76fe043b4dea16affb609f7db049caf 100644
--- a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
+++ b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
@@ -29,7 +29,8 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/armem_skills/aron_conversions.h>
@@ -70,7 +71,7 @@ namespace armarx
workingMemory.name() = p.memoryName;
{
- armarx::armem::wm::CoreSegment& c = workingMemory.addCoreSegment(p.statechartCoreSegmentName, armarx::armem::arondto::Statechart::Transition::toAronType());
+ armarx::armem::server::wm::CoreSegment& c = workingMemory.addCoreSegment(p.statechartCoreSegmentName, armarx::armem::arondto::Statechart::Transition::toAronType());
c.addProviderSegment("Transitions", armarx::armem::arondto::Statechart::Transition::toAronType());
}
}
diff --git a/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt b/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
index 15153b3dca0bac00d1f8a6192191cead86227a05..f1e94f9b2478222809011ad2c150f2b97d0c9e72 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
+++ b/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
@@ -33,7 +33,7 @@ armarx_add_component(
# RobotAPI
RobotAPICore
## RobotAPIInterfaces
- ## RobotAPIComponentPlugins # For ArViz and other plugins.
+ RobotAPIComponentPlugins # For ArViz and other plugins.
# This project
## ${PROJECT_NAME}Interfaces # For ice interfaces from this package.
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
index 6c84b806ffc36dca5a6e981da5d19a2a67f329d2..04fa0b34bf676e53beae6ba6cdaf185598d72447 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
@@ -39,9 +39,16 @@ namespace armarx
switch (runState)
{
case RunState::scannerInit:
- initScanner();
- //read the scanner parameters for initialization
- result = scanner->loopOnce(scanData, scanTime, scanInfo, true);
+ if (initCnt < 5)
+ {
+ initCnt++;
+ initScanner();
+ }
+ else
+ {
+ ARMARX_WARNING << "Maximum number of reinitializations reached - going to idle state";
+ runState = RunState::scannerFinalize;
+ }
break;
case RunState::scannerRun:
if (result == sick_scan::ExitSuccess) // OK -> loop again
@@ -50,10 +57,10 @@ namespace armarx
if (scanTopic)
{
TimestampVariantPtr scanT(new TimestampVariant(scanTime));
- scanTopic->reportSensorValues(ip, frameName, scanData, scanT);
+ scanTopic->reportSensorValues(ip, scannerName, scanData, scanT);
scanData.clear();
//trigger heartbeat
- //heartbeat.heartbeat(channel);
+ scannerHeartbeat->heartbeat(scannerName);
}
else
{
@@ -66,7 +73,8 @@ namespace armarx
}
break;
case RunState::scannerFinalize:
- ARMARX_WARNING << "scanner offline";
+ ARMARX_WARNING << "Scanner offline - stopping task.";
+ this->task->stop();
break;
default:
ARMARX_WARNING << "Invalid run state in task loop";
@@ -77,27 +85,25 @@ namespace armarx
void SickLaserScanDevice::initScanner()
{
- this->isSensorInitialized = false;
ARMARX_INFO_S << "Start initialising scanner [Ip: " << this->ip
<< "] [Port: " << this->port << "]";
// attempt to connect/reconnect
- delete this->scanner; // disconnect scanner
- if (this->useTcp)
+ if (this->scanner)
{
- this->scanner = new SickScanAdapter(this->ip, this->port, this->timelimit, this->parser, 'A');
+ ARMARX_WARNING_S << "Scanner already initized.";
+ this->scanner.reset(); // disconnect scanner
}
- else
+ this->scanner = std::make_unique<SickScanAdapter>(this->ip, this->port, this->timelimit, this->parser.get(), 'A');
+ this->result = this->scanner->init();
+
+ if (this->result == sick_scan::ExitSuccess)
{
- ARMARX_ERROR_S << "TCP is not switched on. Probably hostname or port not set.\n";
- return;
+ //read the scanner parameters for initialization
+ this->result = scanner->loopOnce(scanData, scanTime, scanInfo, true);
}
- result = this->scanner->init();
-
-
- if (result == sick_scan::ExitSuccess) // OK -> loop again
+ if (this->result == sick_scan::ExitSuccess) // OK -> loop again
{
- this->isSensorInitialized = true;
- ARMARX_INFO_S << "Scanner initialized.";
+ ARMARX_INFO_S << "Scanner successfully initialized.";
this->runState = RunState::scannerRun; // after initialising switch to run state
}
else
@@ -115,34 +121,35 @@ namespace armarx
// Publish to a topic (passing the TopicListenerPrx).
// def->topic(myTopicListener);
- // Subscribe to a topic (passing the topic name).
- def->topic<LaserScannerUnitListenerPrx>(properties.topicName);
-
// Use (and depend on) another component (passing the ComponentInterfacePrx).
// def->component(myComponentProxy)
- def->optional(properties.topicName, "topicName", "Name of the topic");
+ def->topic(topic, properties.topicName, "TopicName", "Name of the laserscanner topic to report to.");
//Scanner parameters
def->optional(properties.devices, "devices", "List of Devices in format frame1,ip1,port1;frame2,ip2,port2");
+ def->optional(properties.scannerType, "scannerType", "Name of the LaserScanner");
def->optional(properties.timelimit, "timelimit", "timelimit for communication");
- def->required(properties.scannerType, "scannerType", "Name of the LaserScanner");
- def->optional(properties.angleOffset, "angleOffset", "offset to the scanning angle");
def->optional(properties.rangeMin, "rangeMin", "minimum Range of the Scanner");
def->optional(properties.rangeMax, "rangeMax", "maximum Range of the Scanner");
- def->optional(properties.timeIncrement, "timeIncrement", "timeIncrement??");
return def;
}
+ SickLaserUnit::SickLaserUnit()
+ {
+ //heartbeat = addPlugin<plugins::HeartbeatComponentPlugin>("SickLaserUnit");
+ addPlugin(heartbeat);
+ ARMARX_CHECK_NOT_NULL(heartbeat);
+ }
+
void SickLaserUnit::onInitComponent()
{
+ ARMARX_INFO << "On init";
// Topics and properties defined above are automagically registered.
// Keep debug observer data until calling `sendDebugObserverBatch()`.
// (Requies the armarx::DebugObserverComponentPluginUser.)
// setDebugObserverBatchModeEnabled(true);
- ARMARX_INFO_S << "initializing SickLaserUnit.";
-
std::vector<std::string> splitDeviceStrings = Split(properties.devices, ";");
scanDevices.clear();
scanDevices.reserve(splitDeviceStrings.size());
@@ -152,57 +159,54 @@ namespace armarx
if (deviceInfo.size() != 3)
{
ARMARX_WARNING << "Unexpected format for laser scanner device: " << deviceString
- << " (split size: " << deviceInfo.size() << ")";
+ << " (split size: " << deviceInfo.size() << ", expected: 3)";
continue;
}
SickLaserScanDevice& device = scanDevices.emplace_back();
device.scanTopic = topic;
- device.isSensorInitialized = false;
- device.frameName = deviceInfo[0];
+ device.scannerName = deviceInfo[0];
if (deviceInfo[1] != "")
{
- device.useTcp = true;
device.ip = deviceInfo[1];
}
+ else
+ {
+ ARMARX_FATAL << "TCP is not switched on. Probably hostname or port not set.";
+ return;
+
+ }
device.port = deviceInfo[2];
- device.angleOffset = properties.angleOffset;
device.timelimit = properties.timelimit;
- device.scannerType = properties.scannerType;
- device.rangeMin = properties.rangeMin;
- device.rangeMax = properties.rangeMax;
- device.timeIncrement = properties.timeIncrement;
+ //scanInfo
+ device.scanInfo.device = device.ip;
+ device.scanInfo.frame = device.scannerName;
//scanner Parameters
try
{
- device.parser = new sick_scan::SickGenericParser(device.scannerType);
+ device.parser = std::make_unique<sick_scan::SickGenericParser>(properties.scannerType);
device.parser->set_range_min(properties.rangeMin);
device.parser->set_range_max(properties.rangeMax);
- device.parser->set_time_increment(properties.timeIncrement);
+ device.parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
}
catch (std::exception const& e)
{
ARMARX_ERROR_S << "Could not create parser. Wrong Scanner name.";
return;
}
- device.parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
- device.colaDialectId = 'A';
+ device.scannerHeartbeat = heartbeat;
+ device.scannerHeartbeat->configureHeartbeatChannel(device.scannerName, armarx::RobotHealthHeartbeatArgs(100, 200, "No updates available"));
}
-
- //addPlugin(heartbeat);
- //configureHeartbeatChannel();
}
void SickLaserUnit::onConnectComponent()
{
- topic = getTopic<LaserScannerUnitListenerPrx>(properties.topicName);
- offeringTopic(properties.topicName);
-
for (SickLaserScanDevice& device : scanDevices)
{
device.scanTopic = topic;
//start the laser scanner
if (device.task)
{
+ ARMARX_WARNING << "this should not happen.";
device.task->stop();
device.task = nullptr;
}
@@ -217,23 +221,9 @@ namespace armarx
// The data can be viewed in the ObserverView and the LivePlotter.
// (Before starting any threads, we don't need to lock mutexes.)
{
- setDebugObserverDatafield("numBoxes", properties.numBoxes);
- setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
- sendDebugObserverBatch();
- }
- */
-
- /* (Requires the armarx::ArVizComponentPluginUser.)
- // Draw boxes in ArViz.
- // (Before starting any threads, we don't need to lock mutexes.)
- drawBoxes(properties, arviz);
- */
-
- /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
- // Setup the remote GUI.
- {
- createRemoteGuiTab();
- RemoteGui_startRunningTask();
+ setDebugObserverDatafield("numBoxes", properties.numBoxes);
+ setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
+ sendDebugObserverBatch();
}
*/
}
@@ -248,99 +238,16 @@ namespace armarx
device.task->stop();
device.task = nullptr;
}
- if (device.scanner)
- {
- delete device.scanner;
- }
- if (device.parser)
- {
- delete device.parser;
- }
}
}
- void SickLaserUnit::onExitComponent() {}
-
- std::string SickLaserUnit::getDefaultName() const
- {
- return "SickLaserUnit";
- }
-
- /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
- void SickLaserUnit::createRemoteGuiTab()
- {
- using namespace armarx::RemoteGui::Client;
-
- // Setup the widgets.
-
- tab.boxLayerName.setValue(properties.boxLayerName);
-
- tab.numBoxes.setValue(properties.numBoxes);
- tab.numBoxes.setRange(0, 100);
-
- tab.drawBoxes.setLabel("Draw Boxes");
-
- // Setup the layout.
-
- GridLayout grid;
- int row = 0;
- {
- grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
- ++row;
-
- grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
- ++row;
-
- grid.add(tab.drawBoxes, {row, 0}, {2, 1});
- ++row;
- }
-
- VBoxLayout root = {grid, VSpacer()};
- RemoteGui_createTab(getName(), root, &tab);
- }
-
-
- void SickLaserUnit::RemoteGui_update()
- {
- if (tab.boxLayerName.hasValueChanged() || tab.numBoxes.hasValueChanged())
- {
- std::scoped_lock lock(propertiesMutex);
- properties.boxLayerName = tab.boxLayerName.getValue();
- properties.numBoxes = tab.numBoxes.getValue();
-
- {
- setDebugObserverDatafield("numBoxes", properties.numBoxes);
- setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
- sendDebugObserverBatch();
- }
- }
- if (tab.drawBoxes.wasClicked())
+ void SickLaserUnit::onExitComponent()
{
- // Lock shared variables in methods running in seperate threads
- // and pass them to functions. This way, the called functions do
- // not need to think about locking.
- std::scoped_lock lock(propertiesMutex, arvizMutex);
- drawBoxes(properties, arviz);
}
- }
- */
-
- /* (Requires the armarx::ArVizComponentPluginUser.)
- void SickLaserUnit::drawBoxes(const SickLaserUnit::Properties& p, viz::Client&
- arviz)
- {
- // Draw something in ArViz (requires the armarx::ArVizComponentPluginUser.
- // See the ArVizExample in RobotAPI for more examples.
- viz::Layer layer = arviz.layer(p.boxLayerName);
- for (int i = 0; i < p.numBoxes; ++i)
+ std::string SickLaserUnit::getDefaultName() const
{
- layer.add(viz::Box("box_" + std::to_string(i))
- .position(Eigen::Vector3f(i * 100, 0, 0))
- .size(20).color(simox::Color::blue()));
- }
- arviz.commit(layer);
+ return "SickLaserUnit";
}
- */
} // namespace armarx
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
index 69d6385c130969a4d18a6d104f91765ec4d9a876..662260de57aea4188e45bc6052061a9d01d19598 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
@@ -27,7 +27,8 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
-// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+//#include <RobotAPI/interface/components/RobotHealthInterface.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h>
#include <vector>
@@ -54,32 +55,24 @@ namespace armarx
struct SickLaserScanDevice
{
- //scanner parameters
- std::string scannerType = "sick_tim_5xx";
- double angleOffset = 0.0;
- double rangeMin;
- double rangeMax;
- double timeIncrement;
+ std::string scannerName = "LaserScannerFront";
//communication parameters
std::string ip;
std::string port;
int timelimit = 5;
- bool useTcp = false;
- char colaDialectId = 'A';
//data and task pointers
IceUtil::Time scanTime;
LaserScan scanData;
LaserScannerInfo scanInfo;
+ int initCnt = 0;
+ int result = sick_scan::ExitError;
RunState runState = RunState::scannerFinalize;
RunningTask<SickLaserScanDevice>::pointer_type task;
- std::string frameName = "LaserScannerFront";
LaserScannerUnitListenerPrx scanTopic;
-
- sick_scan::SickScanConfig cfg;
- sick_scan::SickGenericParser* parser;
- SickScanAdapter* scanner;
- int result = sick_scan::ExitError;
- bool isSensorInitialized = false;
+ plugins::HeartbeatComponentPlugin* scannerHeartbeat;
+ //scanner pointers
+ std::unique_ptr<sick_scan::SickGenericParser> parser;
+ std::unique_ptr<SickScanAdapter> scanner;
void initScanner();
void run();
@@ -98,13 +91,14 @@ namespace armarx
class SickLaserUnit :
//virtual public armarx::LaserScannerUnitInterface,
virtual public armarx::Component
- // , virtual public armarx::DebugObserverComponentPluginUser
+ // , virtual public armarx::RobotHealthComponentUser
// , virtual public armarx::LightweightRemoteGuiComponentPluginUser
// , virtual public armarx::ArVizComponentPluginUser
{
public:
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
+ SickLaserUnit();
protected:
/// @see PropertyUser::createPropertyDefinitions()
@@ -134,39 +128,15 @@ namespace armarx
std::string topicName = "SICKLaserScanner";
//scanner parameters
std::string devices = "LaserScannerFront,192.168.8.133,2112";
- int timelimit = 5;
std::string scannerType = "sick_tim_5xx";
- double angleOffset = 0.0;
+ int timelimit = 5;
double rangeMin = 0.0;
double rangeMax = 10.0;
- double timeIncrement = 0.1;
};
Properties properties;
std::vector<SickLaserScanDevice> scanDevices;
LaserScannerUnitListenerPrx topic;
- //HeartbeatComponentPlugin heartbeat;
+ plugins::HeartbeatComponentPlugin* heartbeat;
- /* Use a mutex if you access variables from different threads
- * (e.g. ice functions and RemoteGui_update()).
- std::mutex propertiesMutex;
- */
-
- /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
- /// Tab shown in the Remote GUI.
- struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
- {
- armarx::RemoteGui::Client::LineEdit boxLayerName;
- armarx::RemoteGui::Client::IntSpinBox numBoxes;
-
- armarx::RemoteGui::Client::Button drawBoxes;
- };
- RemoteGuiTab tab;
- */
-
- /* (Requires the armarx::ArVizComponentPluginUser.)
- * When used from different threads, an ArViz client needs to be synchronized.
- /// Protects the arviz client inherited from the ArViz plugin.
- std::mutex arvizMutex;
- */
};
} // namespace armarx
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
index 1e9a011f3b044f6dbab583c5769d8c142fcf9a7c..715265cb484cabb48629d08b770ece7171a567d6 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
@@ -84,8 +84,8 @@
#define deg2rad_const (0.017453292519943295769236907684886f)
-std::vector<unsigned char> exampleData(65536);
-std::vector<unsigned char> receivedData(65536);
+//std::vector<unsigned char> exampleData(65536);
+//std::vector<unsigned char> receivedData(65536);
//static long receivedDataLen = 0;
static int getDiagnosticErrorCode()
@@ -106,12 +106,10 @@ namespace armarx
sick_scan::SickScanCommon(parser),
socket_(io_service_),
deadline_(io_service_),
- parser_ptr(parser),
hostname_(hostname),
port_(port),
timelimit_(timelimit)
{
-
if ((cola_dialect_id == 'a') || (cola_dialect_id == 'A'))
{
this->setProtocolType(CoLa_A);
@@ -121,7 +119,7 @@ namespace armarx
{
this->setProtocolType(CoLa_B);
}
- assert(this->getProtocolType() != CoLa_Unknown);
+ ARMARX_CHECK(this->getProtocolType() != CoLa_Unknown);
m_numberOfBytesInReceiveBuffer = 0;
m_alreadyReceivedBytes = 0;
@@ -137,7 +135,7 @@ namespace armarx
SickScanAdapter::~SickScanAdapter()
{
- // stop_scanner();
+ //stopScanData();
close_device();
}
@@ -147,7 +145,7 @@ namespace armarx
/*!
- \brief parsing datagram and publishing ros messages
+ \brief parsing datagram into ARMARX message
\return error code
*/
int SickScanAdapter::loopOnce(LaserScan& scanData, IceUtil::Time& scanTime, LaserScannerInfo& scanInfo, bool updateScannerInfo)
@@ -157,6 +155,7 @@ namespace armarx
int packetsInLoop = 0;
ros::Time recvTimeStamp = ros::Time::now(); // timestamp incoming package, will be overwritten by get_datagram
+ //use ASCII always
int result = get_datagram(recvTimeStamp, receiveBuffer, 65536, &actual_length, false, &packetsInLoop);
//ros::Duration dur = recvTimeStampPush - recvTimeStamp;
if (result != 0)
@@ -169,12 +168,10 @@ namespace armarx
return sick_scan::ExitSuccess;
} // return success to continue looping
- //TODO: convert ros::Time recvTimeStamp to IceUtil
+ //convert ros::Time recvTimeStamp to IceUtil
ros::Time correctedStamp = recvTimeStamp + ros::Duration(config_.time_offset);
uint64_t recvMsec = correctedStamp.toNSec() / 1000;
- scanTime = IceUtil::Time::milliSeconds(recvMsec);
- //scanTime = TimeUtil::GetTime();
- //msg.header.stamp = recvTimeStamp + ros::Duration(config_.time_offset);
+ scanTime = IceUtil::Time::microSeconds(recvMsec);
//datagrams are enclosed in <STX> (0x02), <ETX> (0x03) pairs
char* buffer_pos = (char*) receiveBuffer;
@@ -347,6 +344,7 @@ namespace armarx
// 23: Starting angle (FFF92230)
if (updateScannerInfo)
{
+ scanInfo.device = hostname_;
uint starting_angle = (uint) - 1;
sscanf(fields[23], "%x", &starting_angle);
scanInfo.minAngle = (starting_angle / 10000.0) / 180.0 * M_PI - M_PI / 2;
@@ -438,7 +436,6 @@ namespace armarx
ARMARX_ERROR_S << "Number of distance measurements does not match number of intensity values - Skipping";
return sick_scan::ExitError;
}
- //TODO: Write ScanSteps with intensity
scanData.reserve(distVal.size());
for (int i = 0; i < (int) distVal.size(); i++)
{
@@ -461,7 +458,6 @@ namespace armarx
void SickScanAdapter::disconnectFunction()
{
-
}
void SickScanAdapter::disconnectFunctionS(void* obj)
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
index 5b7e772f33217ed39e9c708275c6f7e5d10b2251..1707952d9583de18bd6de004f986e216e9c13781 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
@@ -72,10 +72,6 @@ namespace armarx
class SickScanAdapter : public sick_scan::SickScanCommon
{
public:
- int loopOnce(LaserScan& scanData, IceUtil::Time& scanTime, LaserScannerInfo& scanInfo, bool updateScannerInfo = false);
-
- int parseDatagram(char* datagram, size_t datagram_length, LaserScan& scanData, LaserScannerInfo& scanInfo, bool updateScannerInfo = false);
-
static void disconnectFunctionS(void* obj);
SickScanAdapter(const std::string& hostname, const std::string& port, int& timelimit, sick_scan::SickGenericParser* parser,
@@ -83,6 +79,10 @@ namespace armarx
virtual ~SickScanAdapter();
+ int loopOnce(LaserScan& scanData, IceUtil::Time& scanTime, LaserScannerInfo& scanInfo, bool updateScannerInfo = false);
+
+ int parseDatagram(char* datagram, size_t datagram_length, LaserScan& scanData, LaserScannerInfo& scanInfo, bool updateScannerInfo = false);
+
static void readCallbackFunctionS(void* obj, UINT8* buffer, UINT32& numOfBytes);
void readCallbackFunction(UINT8* buffer, UINT32& numOfBytes);
@@ -163,7 +163,6 @@ namespace armarx
boost::system::error_code ec_;
size_t bytes_transfered_;
- sick_scan::SickGenericParser* parser_ptr;
std::string hostname_;
std::string port_;
int timelimit_;
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
index 4f7372a5e178ed8ec895049ef094af354d297ee2..e210b778f25dd9ce8f83bfd299ff3ec4c75497e2 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
@@ -810,30 +810,34 @@ namespace armarx
void ArVizWidgetController::onReplayTimerTick()
{
- if (mode != ArVizWidgetMode::ReplayingTimed)
+ if (mode == ArVizWidgetMode::ReplayingTimed)
{
- return;
- }
- double replaySpeed = widget.replaySpeedSpinBox->value();
+ double replaySpeed = widget.replaySpeedSpinBox->value();
- replayCurrentTimestamp += 33000 * replaySpeed;
+ long currentRealTime = IceUtil::Time::now().toMicroSeconds();
+ long elapsedRealTime = currentRealTime - lastReplayRealTime;
- long revision = getRevisionForTimestamp(replayCurrentTimestamp);
- if (revision == -2)
- {
- if (widget.replayLoopbackCheckBox->checkState() == Qt::Checked)
+ replayCurrentTimestamp += elapsedRealTime * replaySpeed;
+
+ long revision = getRevisionForTimestamp(replayCurrentTimestamp);
+ if (revision == -2)
{
- replayCurrentTimestamp = currentRecording.firstTimestampInMicroSeconds;
+ if (widget.replayLoopbackCheckBox->checkState() == Qt::Checked)
+ {
+ replayCurrentTimestamp = currentRecording.firstTimestampInMicroSeconds;
+ }
+ else
+ {
+ revision = currentRecording.lastRevision;
+ }
}
- else
+ if (revision >= 0)
{
- revision = currentRecording.lastRevision;
+ widget.replayRevisionSlider->setValue(revision);
}
}
- if (revision >= 0)
- {
- widget.replayRevisionSlider->setValue(revision);
- }
+
+ lastReplayRealTime = IceUtil::Time::now().toMicroSeconds();
}
void ArVizWidgetController::changeMode(ArVizWidgetMode newMode)
@@ -917,10 +921,16 @@ namespace armarx
auto& entry = recordingBatchCache[batch.header.index];
entry.data = batch;
entry.lastUsed = IceUtil::Time::now();
+
+ limitRecordingBatchCacheSize();
+
+ recordingWaitingForBatchIndex = -1;
}
viz::data::RecordingBatch const& ArVizWidgetController::getRecordingBatch(long index)
{
+ ARMARX_TRACE;
+
IceUtil::Time now = IceUtil::Time::now();
std::unique_lock<std::mutex> lock(recordingBatchCacheMutex);
@@ -930,16 +940,23 @@ namespace armarx
{
// Start prefetching neighbouring batches
bool asyncPrefetchIsRunning = callbackResult && !callbackResult->isCompleted();
- if (!asyncPrefetchIsRunning)
+ if (!asyncPrefetchIsRunning && recordingWaitingForBatchIndex == -1)
{
if (index + 1 < long(currentRecording.batchHeaders.size())
&& recordingBatchCache.count(index + 1) == 0)
{
+ // ARMARX_WARNING << "after begin_getRecordingBatch: " << (index + 1)
+ // << " waiting for " << recordingWaitingForBatchIndex;
callbackResult = storage->begin_getRecordingBatch(currentRecording.id, index + 1, callback);
+ recordingWaitingForBatchIndex = index + 1;
+ ARMARX_INFO << "Now waiting for " << recordingWaitingForBatchIndex;
}
else if (index > 0 && recordingBatchCache.count(index - 1) == 0)
{
+ // ARMARX_WARNING << "before begin_getRecordingBatch: " << (index - 1)
+ // << " waiting for " << recordingWaitingForBatchIndex;
callbackResult = storage->begin_getRecordingBatch(currentRecording.id, index - 1, callback);
+ recordingWaitingForBatchIndex = index - 1;
}
}
@@ -949,6 +966,19 @@ namespace armarx
return entry.data;
}
+ // Maybe there has already been sent a asynchronous request to get
+ if (index == recordingWaitingForBatchIndex)
+ {
+ lock.unlock();
+ ARMARX_INFO << "Waiting to receive async batch: " << index;
+ // Wait until request completes
+ while (recordingWaitingForBatchIndex != -1)
+ {
+ QCoreApplication::processEvents();
+ }
+ return getRecordingBatch(index);
+ }
+
ARMARX_INFO << "Batch #" << index << " is not in the cache. Getting synchronously, blocking GUI...";
// Entry is not in the cache, we have to get it from ArVizStorage
@@ -956,6 +986,13 @@ namespace armarx
newEntry.lastUsed = now;
newEntry.data = storage->getRecordingBatch(currentRecording.id, index);
+ limitRecordingBatchCacheSize();
+
+ return newEntry.data;
+ }
+
+ void ArVizWidgetController::limitRecordingBatchCacheSize()
+ {
if (recordingBatchCache.size() > recordingBatchCacheMaxSize)
{
// Remove the entry with the oldest last used timestamp
@@ -972,8 +1009,6 @@ namespace armarx
recordingBatchCache.erase(oldestIter);
}
-
- return newEntry.data;
}
SoNode* ArVizWidgetController::getScene()
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
index 31383a63ea78c510ec781951ff56b5fdbcafee06..4a493dbd19a4c4cab6e05aa2a7ab5376936a3ebe 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
@@ -182,6 +182,7 @@ namespace armarx
QTimer* replayTimer;
long replayCurrentTimestamp = 0;
+ long lastReplayRealTime = 0;
std::string storageName;
armarx::viz::StorageInterfacePrx storage;
@@ -207,9 +208,11 @@ namespace armarx
};
viz::data::RecordingBatch const& getRecordingBatch(long index);
+ void limitRecordingBatchCacheSize();
std::size_t recordingBatchCacheMaxSize = 5;
std::mutex recordingBatchCacheMutex;
+ std::atomic<long> recordingWaitingForBatchIndex = -1;
std::map<long, TimestampedRecordingBatch> recordingBatchCache;
ArVizWidgetMode mode = ArVizWidgetMode::NotConnected;
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
index dffe7711debc728d596f5ecd7cd851df2527e120..adb7c77bd1e04491b7e16ecb1599119df0ee2ce6 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
@@ -201,7 +201,7 @@ namespace armarx
{
bool expand = item->childCount() == 0;
- TreeWidgetBuilder builder(objectPoses);
+ TreeWidgetBuilder<objpose::ObjectPose> builder;
builder.setNameFn([](const objpose::ObjectPose & pose)
{
return pose.objectID.str();
@@ -245,7 +245,7 @@ namespace armarx
return true;
});
- builder.updateTree(item, objectPoses);
+ builder.updateTreeWithContainer(item, objectPoses);
if (expand)
{
@@ -299,7 +299,7 @@ namespace armarx
{
(void) dataset;
- TreeWidgetBuilder builder(datasetData);
+ TreeWidgetBuilder<std::pair<std::string, std::string>> builder;
builder.setCompareFn([](const std::pair<std::string, std::string>& lhs, QTreeWidgetItem * item)
{
auto rhs = std::make_pair(item->text(0).toStdString(), item->text(1).toStdString());
@@ -324,7 +324,7 @@ namespace armarx
}
return true;
});
- builder.updateTree(datasetItem, datasetData);
+ builder.updateTreeWithContainer(datasetItem, datasetData);
return true;
});
diff --git a/source/RobotAPI/interface/armem/query.ice b/source/RobotAPI/interface/armem/query.ice
index 2a1586fece346e89cbe491cd21c026ddab603e0e..936294507f0b05627051fa6974309faa1db4d758 100644
--- a/source/RobotAPI/interface/armem/query.ice
+++ b/source/RobotAPI/interface/armem/query.ice
@@ -14,8 +14,7 @@ module armarx
enum QueryTarget
{
WM,
- LTM,
- Disk
+ LTM
};
sequence<QueryTarget> QueryTargets;
diff --git a/source/RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice b/source/RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice
index 3097f6c7596fd3fdf82aedd9d974712081cf503c..1dfe8b9f299d24684a0acdd3d6f53ed61063d397 100644
--- a/source/RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice
+++ b/source/RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice
@@ -81,7 +81,7 @@ module armarx
};
struct AttachObjectToRobotNodeOutput
{
- bool success;
+ bool success = false;
data::ObjectAttachmentInfo attachment;
};
@@ -99,7 +99,7 @@ module armarx
struct DetachObjectFromRobotNodeOutput
{
/// Whether the object was attached before.
- bool wasAttached;
+ bool wasAttached = false;
};
struct DetachAllObjectsFromRobotNodesInput
@@ -113,7 +113,7 @@ module armarx
struct DetachAllObjectsFromRobotNodesOutput
{
/// Number of objects that have been detached.
- int numDetached;
+ int numDetached = 0;
};
struct AgentFrames
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index b00a86caf56aec6a6e3f6c59e734ded8aea893b8..84614f0d5d98bb23fc63a14bc4c0a237189b2779 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -306,6 +306,7 @@ module armarx
double phaseDist1 = 10;
double posToOriRatio = 100;
+
Ice::FloatSeq Kpos;
Ice::FloatSeq Dpos;
Ice::FloatSeq Kori;
@@ -319,6 +320,13 @@ module armarx
float torqueLimit;
+
+ string forceSensorName;
+ float waitTimeForCalibration;
+ float forceFilter;
+ float forceDeadZone;
+ Eigen::Vector3f forceThreshold;
+ string forceFrameName = "ArmR8_Wri2";
};
interface NJointTaskSpaceImpedanceDMPControllerInterface extends NJointControllerInterface
@@ -328,6 +336,7 @@ module armarx
void setUseNullSpaceJointDMP(bool enable);
bool isFinished();
+ bool isDMPRunning();
void runDMP(Ice::DoubleSeq goals);
void runDMPWithTime(Ice::DoubleSeq goals, double timeDuration);
@@ -352,6 +361,9 @@ module armarx
void setNullspaceVelocityKd(Eigen::VectorXf jointValues);
void setNullspaceVelocityKp(Eigen::VectorXf jointValues);
+ void enableForceStop();
+ void disableForceStop();
+ void setForceThreshold(Eigen::Vector3f forceThreshold);
};
class NJointTaskSpaceAdaptiveDMPControllerConfig extends NJointControllerConfig
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
index a4ca9b08ad6d3b0937294c5a9050adb6680abfcc..594e05ed14002d7098c2ecaa41a5d627e3f9ea24 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
@@ -25,6 +25,7 @@ set(LIB_FILES
NaturalIKComponentPlugin.cpp
HandUnitComponentPlugin.cpp
FrameTrackingComponentPlugin.cpp
+ HeartbeatComponentPlugin.cpp
)
set(LIB_HEADERS
RobotStateComponentPlugin.h
@@ -40,6 +41,7 @@ set(LIB_HEADERS
NaturalIKComponentPlugin.h
HandUnitComponentPlugin.h
FrameTrackingComponentPlugin.h
+ HeartbeatComponentPlugin.h
)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..510ff8933b6e045ec96e1020e6914f2b064636f2
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
@@ -0,0 +1,98 @@
+#include "HeartbeatComponentPlugin.h"
+
+#include "ArmarXCore/core/Component.h"
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
+#include <RobotAPI/interface/components/RobotHealthInterface.h>
+
+namespace armarx::plugins
+{
+ void
+ HeartbeatComponentPlugin::configureHeartbeatChannel(const std::string& channel,
+ const RobotHealthHeartbeatArgs& args)
+ {
+ channelHeartbeatConfig.emplace(channel, args);
+ }
+
+ void
+ HeartbeatComponentPlugin::heartbeat()
+ {
+
+ if (robotHealthTopic)
+ {
+ robotHealthTopic->heartbeat(componentName, heartbeatArgs);
+ }
+ else
+ {
+ ARMARX_WARNING << "No robot health topic available!";
+ }
+ }
+
+ void
+ HeartbeatComponentPlugin::heartbeat(const std::string& channel)
+ {
+ const auto argsIt = channelHeartbeatConfig.find(channel);
+ ARMARX_CHECK(argsIt != channelHeartbeatConfig.end())
+ << "heartbeat() called for unknown channel '" << channel << "'."
+ << "You must register the config using configureHeartbeatChannel(channel) first!";
+
+ const auto& args = argsIt->second;
+
+ if (robotHealthTopic)
+ {
+ robotHealthTopic->heartbeat(componentName + "_" + channel, args);
+ }
+ else
+ {
+ ARMARX_WARNING << "No robot health topic available!";
+ }
+ }
+
+ void
+ HeartbeatComponentPlugin::preOnInitComponent()
+ {
+ if (topicName.empty())
+ {
+ parent<Component>().getProperty(topicName, makePropertyName(topicPropertyName));
+ }
+ parent<Component>().offeringTopic(topicName);
+ }
+
+ void
+ HeartbeatComponentPlugin::postOnInitComponent()
+ {
+ }
+
+ void
+ HeartbeatComponentPlugin::preOnConnectComponent()
+ {
+ robotHealthTopic = parent<Component>().getTopic<RobotHealthInterfacePrx>(topicName);
+ }
+
+ void
+ HeartbeatComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(makePropertyName(topicPropertyName)))
+ {
+ properties->defineOptionalProperty<std::string>(
+ makePropertyName(topicPropertyName),
+ "DebugObserver",
+ "Name of the topic the DebugObserver listens on");
+ }
+
+ if (not properties->hasDefinition(makePropertyName(maximumCycleTimeWarningMSPropertyName)))
+ {
+ properties->defineRequiredProperty<std::string>(
+ makePropertyName(maximumCycleTimeWarningMSPropertyName),
+ "TODO: maximumCycleTimeWarningMS");
+ }
+
+ if (not properties->hasDefinition(makePropertyName(maximumCycleTimeErrorMSPropertyName)))
+ {
+ properties->defineRequiredProperty<std::string>(
+ makePropertyName(maximumCycleTimeErrorMSPropertyName),
+ "TODO: maximumCycleTimeErrorMS");
+ }
+ }
+
+} // namespace armarx::plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..6a4a6c2bd692484c94bbd42bdfff399b0f5a0aa9
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h
@@ -0,0 +1,89 @@
+/**
+ * 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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/ComponentPlugin.h>
+
+#include <RobotAPI/interface/components/RobotHealthInterface.h>
+
+namespace armarx::plugins
+{
+
+ class HeartbeatComponentPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ /**
+ * @brief Configures a heartbeat subchannel.
+ *
+ * @param channel Identifier of the heartbeat channel
+ * @param args Configuration of this channel's heartbeat properties
+ */
+ void configureHeartbeatChannel(const std::string& channel,
+ const RobotHealthHeartbeatArgs& args);
+
+ /**
+ * @brief Sends out a heartbeat using the default config
+ *
+ */
+ void heartbeat();
+
+ /**
+ * @brief Sends out a heartbeat for a subchannel.
+ *
+ * Note: You must call configureHeartbeatChannel(...) first to register the channel config!
+ *
+ * @param channel Identifier of the heartbeat channel
+ */
+ void heartbeat(const std::string& channel);
+
+ protected:
+ void preOnInitComponent() override;
+ void postOnInitComponent() override;
+ void preOnConnectComponent() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ private:
+ //! heartbeat topic name (outgoing)
+ std::string topicName;
+
+ //! name of this component used as identifier for heartbeats
+ std::string componentName;
+
+ //
+ static constexpr auto topicPropertyName = "heartbeat.TopicName";
+ static constexpr auto maximumCycleTimeWarningMSPropertyName =
+ "heartbeat.maximumCycleTimeWarningMS";
+ static constexpr auto maximumCycleTimeErrorMSPropertyName =
+ "heartbeat.maximumCycleTimeErrorMS";
+
+ RobotHealthInterfacePrx robotHealthTopic;
+
+ //! default config used in heartbeat(), set via properties
+ RobotHealthHeartbeatArgs heartbeatArgs;
+
+ //! configs used in heartbeat(channel), set by user via configureHeartbeatChannel(...)
+ std::unordered_map<std::string, RobotHealthHeartbeatArgs> channelHeartbeatConfig;
+ };
+} // namespace armarx::plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
index 4e6e7966cc4c71e819da47cd9a7ee06ccd536775..6647cf38585cb868a3df29e9c328bbfe8ac3a677 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
@@ -116,6 +116,19 @@ namespace armarx::plugins
}
+ void RobotUnitComponentPlugin::waitUntilRobotUnitIsRunning(const std::function<bool ()>& termCond) const
+ {
+ ARMARX_INFO << "Waiting until robot unit is running ...";
+
+ while (not(termCond() or not isNullptr(getRobotUnit()) or getRobotUnit()->isRunning()))
+ {
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ }
+
+ ARMARX_INFO << "Robot unit is up and running.";
+ }
+
+
} // namespace armarx::plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
index 80357a96e9a3bb6c889d7b73435baef90a385b54..4371934b97182947afbaad3d60fd5295a717615a 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
@@ -34,6 +34,19 @@ namespace armarx
}
+ /**
+ * @brief Waits until the robot unit is running.
+ *
+ * Although the robot unit proxy might be initialized (\see getRobotUnit()), the robot unit might
+ * not be fully initialized.
+ *
+ * @param termCond Termination condition.
+ * If it evaluates to true, waitUntilRobotUnitIsRunning returns without waiting
+ * for the robot unit to become available.
+ */
+ void waitUntilRobotUnitIsRunning(const std::function<bool()>& termCond = [] {return false;}) const;
+
+
//controllers
public:
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
index 79a090adbae49e70b400ca3cd358b0599a11c9f1..e8a401f485b38a2c85f4a9a718f94d8a1d0a905a 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
@@ -472,6 +472,7 @@ namespace armarx
debugRT.getWriteBuffer().targetVel = targetVel;
debugRT.getWriteBuffer().adaptK = adaptK;
debugRT.getWriteBuffer().isPhaseStop = isPhaseStop;
+ debugRT.getWriteBuffer().currentTwist = currentTwist;
rt2CtrlData.getWriteBuffer().currentPose = currentPose;
rt2CtrlData.getWriteBuffer().currentTwist = currentTwist;
@@ -673,6 +674,11 @@ namespace armarx
datafields["targetVel_y"] = new Variant(targetVel(1));
datafields["targetVel_z"] = new Variant(targetVel(2));
+ Eigen::VectorXf currentVel = debugRT.getUpToDateReadBuffer().currentTwist;
+ datafields["currentVel_x"] = new Variant(currentVel(0));
+ datafields["currentVel_y"] = new Variant(currentVel(1));
+ datafields["currentVel_z"] = new Variant(currentVel(2));
+
Eigen::Vector3f adaptK = debugRT.getUpToDateReadBuffer().adaptK;
datafields["adaptK_x"] = new Variant(adaptK(0));
datafields["adaptK_y"] = new Variant(adaptK(1));
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
index ebb3b05a5241c338cbdfc73ebd30ef3226d7219d..477feb6900efcdaca0981deb92534cc5dfb0c172 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
@@ -99,6 +99,7 @@ namespace armarx
Eigen::Vector3f adaptK;
Eigen::VectorXf targetVel;
Eigen::Matrix4f currentPose;
+ Eigen::VectorXf currentTwist;
bool isPhaseStop;
float manidist;
};
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 1c573254359a77ed99e2365a48d9e1c71c67167e..0a08c5396283856d5e5ccc5e8ebfdbb9cb59500c 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -11,12 +11,14 @@ namespace armarx
const armarx::NJointControllerConfigPtr& config,
const VirtualRobot::RobotPtr&)
{
+ ARMARX_TRACE;
ARMARX_INFO << "creating impedance dmp controller";
cfg = NJointTaskSpaceImpedanceDMPControllerConfigPtr::dynamicCast(config);
+ ARMARX_CHECK_NOT_NULL(cfg);
useSynchronizedRtRobot();
rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
-
+ ARMARX_INFO << "1";
for (size_t i = 0; i < rns->getSize(); ++i)
{
std::string jointName = rns->getNode(i)->getName();
@@ -39,11 +41,20 @@ namespace armarx
velocitySensors.push_back(velocitySensor);
positionSensors.push_back(positionSensor);
};
-
+ const SensorValueBase* svlf = robotUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
+ forceSensor = svlf->asA<SensorValueForceTorque>();
+
+ ARMARX_TRACE;
+ forceOffset.setZero();
+ filteredForce.setZero();
+ filteredForceInRoot.setZero();
+ ARMARX_INFO << cfg->forceThreshold;
+ forceThreshold.reinitAllBuffers(cfg->forceThreshold);
tcp = rns->getTCP();
ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
ik->setDampedSvdLambda(0.0001);
+ ARMARX_TRACE;
numOfJoints = targets.size();
// set DMP
TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
@@ -80,6 +91,7 @@ namespace armarx
}
defaultNullSpaceJointValues.reinitAllBuffers(nullspaceValues);
+ ARMARX_TRACE;
Eigen::Vector3f kpos(cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2]);
Eigen::Vector3f dpos(cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2]);
Eigen::Vector3f kori(cfg->Kori[0], cfg->Kori[1], cfg->Kori[2]);
@@ -114,6 +126,7 @@ namespace armarx
controllerSensorData.reinitAllBuffers(initControllerSensorData);
firstRun = true;
+ useForceStop = false;
ARMARX_INFO << "Finished controller constructor ";
}
@@ -127,6 +140,7 @@ namespace armarx
void NJointTaskSpaceImpedanceDMPController::rtPreActivateController()
{
+ ARMARX_TRACE;
NJointTaskSpaceImpedanceDMPControllerControlData initData;
initData.targetPose = tcp->getPoseInRootFrame();
initData.targetVel.resize(6);
@@ -140,8 +154,6 @@ namespace armarx
void NJointTaskSpaceImpedanceDMPController::controllerRun()
{
-
-
if (!dmpCtrl)
{
return;
@@ -234,7 +246,72 @@ namespace armarx
void NJointTaskSpaceImpedanceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
{
+
+ Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
+
double deltaT = timeSinceLastIteration.toSecondsDouble();
+ Eigen::Matrix4f targetPose;
+ Eigen::VectorXf targetVel;
+ Eigen::VectorXf desiredNullSpaceJointValues;
+ if (firstRun)
+ {
+ firstRun = false;
+ targetPose = currentPose;
+ stopPose = currentPose;
+ targetVel.setZero(6);
+ desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
+ }
+ else
+ {
+ if (!started)
+ {
+ targetPose = stopPose;
+ targetVel.setZero(6);
+ desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
+ forceOffset = (1 - cfg->forceFilter) * forceOffset + cfg->forceFilter * forceSensor->force;
+ timeForCalibration = timeForCalibration + timeSinceLastIteration.toSecondsDouble();
+ }
+ else
+ {
+ targetPose = rtGetControlStruct().targetPose;
+ targetVel = rtGetControlStruct().targetVel;
+ desiredNullSpaceJointValues = rtGetControlStruct().desiredNullSpaceJointValues;
+
+ if (useForceStop)
+ {
+ /* handle force stop */
+ filteredForce = (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * (forceSensor->force - forceOffset);
+
+ for (size_t i = 0; i < 3; ++i)
+ {
+ if (fabs(filteredForce(i)) > cfg->forceDeadZone)
+ {
+ filteredForce(i) -= (filteredForce(i) / fabs(filteredForce(i))) * cfg->forceDeadZone;
+ }
+ else
+ {
+ filteredForce(i) = 0;
+ }
+ }
+ Eigen::Matrix4f forceFrameInRoot = rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame();
+ filteredForceInRoot = forceFrameInRoot.block<3, 3>(0, 0) * filteredForce;
+
+ for (size_t i = 0; i < 3; ++i)
+ {
+ if (fabs(filteredForceInRoot[i]) > forceThreshold.getUpToDateReadBuffer()[i])
+ {
+ stopPose = currentPose;
+ targetVel.setZero(6);
+ desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
+ started = false;
+ break;
+ }
+ }
+ }
+
+ }
+ }
+
Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
@@ -247,7 +324,6 @@ namespace armarx
qvel(i) = velocitySensors[i]->velocity;
}
- Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
Eigen::VectorXf currentTwist = jacobi * qvel;
controllerSensorData.getWriteBuffer().currentPose = currentPose;
@@ -261,24 +337,6 @@ namespace armarx
jacobi.block(0, 0, 3, numOfJoints) = 0.001 * jacobi.block(0, 0, 3, numOfJoints); // convert mm to m
-
- Eigen::Matrix4f targetPose;
- Eigen::VectorXf targetVel;
- Eigen::VectorXf desiredNullSpaceJointValues;
- if (firstRun)
- {
- firstRun = false;
- targetPose = currentPose;
- targetVel.setZero(6);
- desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
- }
- else
- {
- targetPose = rtGetControlStruct().targetPose;
- targetVel = rtGetControlStruct().targetVel;
- desiredNullSpaceJointValues = rtGetControlStruct().desiredNullSpaceJointValues;
- }
-
Eigen::Vector3f kpos = ctrlParams.getUpToDateReadBuffer().kpos;
Eigen::Vector3f dpos = ctrlParams.getUpToDateReadBuffer().dpos;
Eigen::Vector3f kori = ctrlParams.getUpToDateReadBuffer().kori;
@@ -315,9 +373,6 @@ namespace armarx
jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
-
-
-
// torque limit
ARMARX_CHECK_EXPRESSION(!targets.empty());
ARMARX_CHECK_LESS(targets.size(), 1000);
@@ -487,34 +542,24 @@ namespace armarx
void NJointTaskSpaceImpedanceDMPController::runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration,
const Ice::Current&)
{
- firstRun = true;
- while (firstRun)
- {
- usleep(100);
- }
-
- Eigen::Matrix4f pose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
- dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
-
dmpCtrl->canVal = timeDuration;
dmpCtrl->config.motionTimeDuration = timeDuration;
- finished = false;
-
- if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)
- {
- ARMARX_INFO << "Using Null Space Joint DMP";
- }
-
- started = true;
- stopped = false;
- // controllerTask->start();
+ runDMP(goals);
}
void NJointTaskSpaceImpedanceDMPController::runDMP(const Ice::DoubleSeq& goals, const Ice::Current&)
{
firstRun = true;
- while (firstRun)
+ timeForCalibration = 0;
+ started = false;
+
+ while (firstRun || timeForCalibration < cfg->waitTimeForCalibration)
+ {
+ usleep(100);
+ }
+
+ while (!interfaceData.updateReadBuffer())
{
usleep(100);
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index 9ded363e9e110bf803ada28b5523057b69b3bd4f..58fd840fb1121b4a159c185abb2e72fd7470f082 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -6,6 +6,7 @@
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
#include <VirtualRobot/IK/DifferentialIK.h>
#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
@@ -55,11 +56,16 @@ namespace armarx
return finished;
}
+ bool isDMPRunning(const Ice::Current&) override
+ {
+ return started;
+ }
+
void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
void learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&) override;
- void runDMP(const Ice::DoubleSeq& goals, const Ice::Current&) override;
+ void runDMP(const Ice::DoubleSeq& goals, const Ice::Current& iceCurrent = Ice::emptyCurrent) override;
void runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&) override;
Ice::Double getVirtualTime(const Ice::Current&) override
@@ -85,7 +91,20 @@ namespace armarx
void setUseNullSpaceJointDMP(bool enable, const Ice::Current&) override;
void setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues, const Ice::Current&) override;
+ void enableForceStop(const Ice::Current&) override
+ {
+ useForceStop = true;
+ }
+ void disableForceStop(const Ice::Current&) override
+ {
+ useForceStop = false;
+ }
+ void setForceThreshold(const Eigen::Vector3f& f, const Ice::Current& current) override
+ {
+ forceThreshold.getWriteBuffer() = f;
+ forceThreshold.commitWrite();
+ }
protected:
virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
@@ -180,6 +199,7 @@ namespace armarx
WriteBufferedTripleBuffer<CtrlParams> ctrlParams;
+
DMP::Vec<DMP::DMPState> currentJointState;
DMP::UMIDMPPtr nullSpaceJointDMPPtr;
@@ -230,6 +250,16 @@ namespace armarx
bool started = false;
bool stopped = false;
+ Eigen::Matrix4f stopPose;
+
+ Eigen::Vector3f filteredForce;
+ Eigen::Vector3f forceOffset;
+ Eigen::Vector3f filteredForceInRoot;
+ WriteBufferedTripleBuffer<Eigen::Vector3f> forceThreshold;
+ std::atomic<bool> useForceStop;
+ std::atomic<float> timeForCalibration;
+ const SensorValueForceTorque* forceSensor;
+
Eigen::Matrix4f oldPose;
// NJointController interface
protected:
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index eb53706cb8836fc40891372811738240b3522aa0..b08141a6ef60148532ce3f1097eafacb6ae384b9 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -25,38 +25,34 @@ set(LIB_FILES
core/Commit.cpp
core/MemoryID.cpp
core/MemoryID_operators.cpp
+ core/operations.cpp
core/SuccessHeader.cpp
core/Time.cpp
- core/ice_conversions.cpp
core/aron_conversions.cpp
+ core/ice_conversions.cpp
core/json_conversions.cpp
core/base/detail/MemoryItem.cpp
- core/base/detail/MaxHistorySize.cpp
core/base/detail/MemoryContainerBase.cpp
core/base/detail/EntityContainerBase.cpp
core/base/detail/AronTyped.cpp
+ core/base/detail/iteration_mixins.cpp
+ core/base/detail/negative_index_semantics.cpp
- core/base/CoreSegmentBase.cpp
- core/base/EntityBase.cpp
+ # core/base/CoreSegmentBase.cpp
+ # core/base/EntityBase.cpp
core/base/EntityInstanceBase.cpp
core/base/EntitySnapshotBase.cpp
- core/base/MemoryBase.cpp
- core/base/ProviderSegmentBase.cpp
-
- core/workingmemory/ice_conversions.cpp
- core/workingmemory/detail/CopyWithoutData.cpp
- core/workingmemory/CoreSegment.cpp
- core/workingmemory/Entity.cpp
- core/workingmemory/EntityInstance.cpp
- core/workingmemory/EntitySnapshot.cpp
- core/workingmemory/Memory.cpp
- core/workingmemory/ProviderSegment.cpp
- core/workingmemory/ice_conversions.cpp
- core/workingmemory/json_conversions.cpp
- core/workingmemory/entityInstance_conversions.cpp
- core/workingmemory/visitor/Visitor.cpp
- core/workingmemory/visitor/FunctionalVisitor.cpp
+ # core/base/MemoryBase.cpp
+ # core/base/ProviderSegmentBase.cpp
+ core/base/ice_conversions.cpp
+
+ core/wm/memory_definitions.cpp
+ core/wm/ice_conversions.cpp
+ core/wm/json_conversions.cpp
+ core/wm/aron_conversions.cpp
+ core/wm/visitor/Visitor.cpp
+ core/wm/visitor/FunctionalVisitor.cpp
core/longtermmemory/CoreSegment.cpp
core/longtermmemory/Entity.cpp
@@ -97,7 +93,13 @@ set(LIB_FILES
server/ComponentPlugin.cpp
server/MemoryRemoteGui.cpp
server/RemoteGuiAronDataVisitor.cpp
- server/Segment.cpp
+
+ server/wm/memory_definitions.cpp
+ server/wm/ice_conversions.cpp
+ server/wm/detail/MaxHistorySize.cpp
+
+ server/segment/Segment.cpp
+ server/segment/SpecializedSegment.cpp
server/query_proc/base/BaseQueryProcessorBase.cpp
server/query_proc/base/EntityQueryProcessorBase.cpp
@@ -105,23 +107,9 @@ set(LIB_FILES
server/query_proc/base/CoreSegmentQueryProcessorBase.cpp
server/query_proc/base/MemoryQueryProcessorBase.cpp
- server/query_proc/workingmemory/BaseQueryProcessor.cpp
- server/query_proc/workingmemory/EntityQueryProcessor.cpp
- server/query_proc/workingmemory/ProviderSegmentQueryProcessor.cpp
- server/query_proc/workingmemory/CoreSegmentQueryProcessor.cpp
- server/query_proc/workingmemory/MemoryQueryProcessor.cpp
-
- server/query_proc/longtermmemory/BaseQueryProcessor.cpp
- server/query_proc/longtermmemory/EntityQueryProcessor.cpp
- server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.cpp
- server/query_proc/longtermmemory/CoreSegmentQueryProcessor.cpp
- server/query_proc/longtermmemory/MemoryQueryProcessor.cpp
-
- server/query_proc/diskmemory/BaseQueryProcessor.cpp
- server/query_proc/diskmemory/EntityQueryProcessor.cpp
- server/query_proc/diskmemory/ProviderSegmentQueryProcessor.cpp
- server/query_proc/diskmemory/CoreSegmentQueryProcessor.cpp
- server/query_proc/diskmemory/MemoryQueryProcessor.cpp
+ server/query_proc/diskmemory.cpp
+ server/query_proc/ltm.cpp
+ server/query_proc/wm.cpp
mns/MemoryNameSystem.cpp
mns/ComponentPlugin.cpp
@@ -131,15 +119,19 @@ set(LIB_FILES
set(LIB_HEADERS
core.h
+ core/forward_declarations.h
+
core/Commit.h
core/DataMode.h
core/MemoryID.h
core/MemoryID_operators.h
+ core/operations.h
core/SuccessHeader.h
core/Time.h
core/aron_conversions.h
core/json_conversions.h
core/ice_conversions.h
+ core/ice_conversions_boost_templates.h
core/ice_conversions_templates.h
core/error.h
@@ -147,10 +139,11 @@ set(LIB_HEADERS
core/error/mns.h
core/base/detail/MemoryItem.h
- core/base/detail/MaxHistorySize.h
core/base/detail/MemoryContainerBase.h
core/base/detail/EntityContainerBase.h
core/base/detail/AronTyped.h
+ core/base/detail/iteration_mixins.h
+ core/base/detail/negative_index_semantics.h
core/base/CoreSegmentBase.h
core/base/EntityBase.h
@@ -158,20 +151,15 @@ set(LIB_HEADERS
core/base/EntitySnapshotBase.h
core/base/MemoryBase.h
core/base/ProviderSegmentBase.h
+ core/base/ice_conversions.h
- core/workingmemory/detail/CopyWithoutData.h
- core/workingmemory/CoreSegment.h
- core/workingmemory/Entity.h
- core/workingmemory/EntityInstance.h
- core/workingmemory/EntitySnapshot.h
- core/workingmemory/Memory.h
- core/workingmemory/ProviderSegment.h
- core/workingmemory/ice_conversions.h
- core/workingmemory/json_conversions.h
- core/workingmemory/entityInstance_conversions.h
- core/workingmemory/visitor.h
- core/workingmemory/visitor/Visitor.h
- core/workingmemory/visitor/FunctionalVisitor.h
+ core/wm.h
+ core/wm/memory_definitions.h
+ core/wm/aron_conversions.h
+ core/wm/ice_conversions.h
+ core/wm/json_conversions.h
+ core/wm/visitor/Visitor.h
+ core/wm/visitor/FunctionalVisitor.h
core/longtermmemory/CoreSegment.h
core/longtermmemory/Entity.h
@@ -214,43 +202,38 @@ set(LIB_HEADERS
client/util/SimpleWriterBase.h
server.h
+ server/forward_declarations.h
+
server/ComponentPlugin.h
server/MemoryToIceAdapter.h
server/MemoryRemoteGui.h
server/RemoteGuiAronDataVisitor.h
- server/Segment.h
+
+ server/wm/memory_definitions.h
+ server/wm/ice_conversions.h
+ server/wm/detail/MaxHistorySize.h
+
+ server/segment/Segment.h
+ server/segment/SpecializedSegment.h
server/query_proc.h
+
+ server/query_proc/base.h
server/query_proc/base/BaseQueryProcessorBase.h
server/query_proc/base/EntityQueryProcessorBase.h
server/query_proc/base/ProviderSegmentQueryProcessorBase.h
server/query_proc/base/CoreSegmentQueryProcessorBase.h
server/query_proc/base/MemoryQueryProcessorBase.h
- server/query_proc/workingmemory/BaseQueryProcessor.h
- server/query_proc/workingmemory/EntityQueryProcessor.h
- server/query_proc/workingmemory/ProviderSegmentQueryProcessor.h
- server/query_proc/workingmemory/CoreSegmentQueryProcessor.h
- server/query_proc/workingmemory/MemoryQueryProcessor.h
-
- server/query_proc/longtermmemory/BaseQueryProcessor.h
- server/query_proc/longtermmemory/EntityQueryProcessor.h
- server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.h
- server/query_proc/longtermmemory/CoreSegmentQueryProcessor.h
- server/query_proc/longtermmemory/MemoryQueryProcessor.h
-
- server/query_proc/diskmemory/BaseQueryProcessor.h
- server/query_proc/diskmemory/EntityQueryProcessor.h
- server/query_proc/diskmemory/ProviderSegmentQueryProcessor.h
- server/query_proc/diskmemory/CoreSegmentQueryProcessor.h
- server/query_proc/diskmemory/MemoryQueryProcessor.h
+ server/query_proc/diskmemory.h
+ server/query_proc/ltm.h
+ server/query_proc/wm.h
mns.h
mns/MemoryNameSystem.h
mns/ComponentPlugin.h
util/util.h
-
)
armarx_add_library(
diff --git a/source/RobotAPI/libraries/armem/client/ComponentPlugin.cpp b/source/RobotAPI/libraries/armem/client/ComponentPlugin.cpp
index a68e742cd24489694da91ef73ff557a6e8be1294..a88f374d6bddede5fd0b05eb28481d524f5f6b0a 100644
--- a/source/RobotAPI/libraries/armem/client/ComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/client/ComponentPlugin.cpp
@@ -31,7 +31,8 @@ armarx::armem::client::ComponentPluginUser::setMemoryServer(server::MemoryInterf
armarx::armem::data::WaitForMemoryResult
armarx::armem::client::ComponentPluginUser::useMemoryServer(const std::string& memoryName)
{
- armem::data::WaitForMemoryResult result = MemoryNameSystemComponentPluginUser::useMemoryServer(memoryName);
+ armem::data::WaitForMemoryResult result;
+ result.proxy = memoryNameSystem.useServer(MemoryID().withMemoryName(memoryName));
if (result.proxy)
{
setMemoryServer(result.proxy);
diff --git a/source/RobotAPI/libraries/armem/client/ComponentPlugin.h b/source/RobotAPI/libraries/armem/client/ComponentPlugin.h
index 3bdbde93bd046bcceb361d94183be5fd1c09db23..ae3f26b26557c11046d84cde8a07483e4ba9ad49 100644
--- a/source/RobotAPI/libraries/armem/client/ComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/ComponentPlugin.h
@@ -2,7 +2,7 @@
// ArmarX
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
// RobotAPI
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
diff --git a/source/RobotAPI/libraries/armem/client/Query.cpp b/source/RobotAPI/libraries/armem/client/Query.cpp
index 1777839f24e6f2522e7f0efb9670f79ff463231b..52000cf3d25f118ea80cf7dfc7c5ea5830cb36f8 100644
--- a/source/RobotAPI/libraries/armem/client/Query.cpp
+++ b/source/RobotAPI/libraries/armem/client/Query.cpp
@@ -1,6 +1,6 @@
#include "Query.h"
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
namespace armarx::armem::client
{
diff --git a/source/RobotAPI/libraries/armem/client/Query.h b/source/RobotAPI/libraries/armem/client/Query.h
index 916278a8fcde1679545ed39d59531f5772852c13..56b9caa79b06bf598836a8cb4df5ab492177604a 100644
--- a/source/RobotAPI/libraries/armem/client/Query.h
+++ b/source/RobotAPI/libraries/armem/client/Query.h
@@ -3,10 +3,10 @@
// RobotAPI
#include <RobotAPI/interface/armem/query.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/SuccessHeader.h>
#include <RobotAPI/libraries/armem/core/DataMode.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
namespace armarx::armem::client::query
@@ -60,6 +60,38 @@ namespace armarx::armem::client
}
}
+ void replaceQueryTarget(const armem::query::data::QueryTarget search, const armem::query::data::QueryTarget replace)
+ {
+ for (const auto& memoryQuery : memoryQueries)
+ {
+ if (auto it = std::find(memoryQuery->targets.begin(), memoryQuery->targets.end(), search); it != memoryQuery->targets.end())
+ {
+ memoryQuery->targets.push_back(replace);
+ }
+ for (const auto& coreSegmentQuery : memoryQuery->coreSegmentQueries)
+ {
+ if (auto it = std::find(coreSegmentQuery->targets.begin(), coreSegmentQuery->targets.end(), search); it != coreSegmentQuery->targets.end())
+ {
+ coreSegmentQuery->targets.push_back(replace);
+ }
+ for (const auto& providerSegmentQuery : coreSegmentQuery->providerSegmentQueries)
+ {
+ if (auto it = std::find(providerSegmentQuery->targets.begin(), providerSegmentQuery->targets.end(), search); it != providerSegmentQuery->targets.end())
+ {
+ providerSegmentQuery->targets.push_back(replace);
+ }
+ for (const auto& entityQuery : providerSegmentQuery->entityQueries)
+ {
+ if (auto it = std::find(entityQuery->targets.begin(), entityQuery->targets.end(), search); it != entityQuery->targets.end())
+ {
+ entityQuery->targets.push_back(replace);
+ }
+ }
+ }
+ }
+ }
+ }
+
static QueryInput fromIce(const armem::query::data::Input& ice);
armem::query::data::Input toIce() const;
};
diff --git a/source/RobotAPI/libraries/armem/client/Reader.cpp b/source/RobotAPI/libraries/armem/client/Reader.cpp
index a589512f13bcdaaa2536f597a54474eb72f96574..1f0e2ee51717e011de75f9fad4dd9c2c598ada88 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem/client/Reader.cpp
@@ -6,9 +6,8 @@
#include <RobotAPI/libraries/armem/core/MemoryID_operators.h>
#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem/util/util.h>
#include "query/Builder.h"
@@ -162,30 +161,25 @@ namespace armarx::armem::client
}
- struct FindLatestSnapshotVisitor : public wm::Visitor
- {
- std::optional<wm::EntitySnapshot> latest = std::nullopt;
-
- bool visitEnter(const wm::EntitySnapshot& snapshot) override;
- };
- bool FindLatestSnapshotVisitor::visitEnter(const wm::EntitySnapshot& snapshot)
- {
- if (not latest.has_value() or snapshot.time() < latest->time())
- {
- latest = snapshot;
- }
- return true;
- }
-
-
std::optional<wm::EntitySnapshot> Reader::getLatestSnapshotIn(const MemoryID& id, DataMode dataMode) const
{
client::QueryResult result = getLatestSnapshotsIn(id, dataMode);
if (result.success)
{
- FindLatestSnapshotVisitor visitor;
- visitor.applyTo(result.memory);
- return visitor.latest;
+ std::optional<wm::EntitySnapshot> latest = std::nullopt;
+ result.memory.forEachEntity([&latest](const wm::Entity & entity)
+ {
+ if (not entity.empty())
+ {
+ const wm::EntitySnapshot& snapshot = entity.getLatestSnapshot();
+ if (not latest.has_value() or latest->time() < snapshot.time())
+ {
+ latest = snapshot;
+ }
+ }
+ return true;
+ });
+ return latest;
}
else
{
diff --git a/source/RobotAPI/libraries/armem/client/Reader.h b/source/RobotAPI/libraries/armem/client/Reader.h
index 1d69491d2cf33e7bdf460634ef69bc26db4bb52b..e9ee1b69afef7d2befdaaf0169b0117a13edbad5 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.h
+++ b/source/RobotAPI/libraries/armem/client/Reader.h
@@ -8,8 +8,7 @@
// RobotAPI
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
#include <RobotAPI/interface/armem/server/StoringMemoryInterface.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h>
-// #include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include "Query.h"
diff --git a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp
index 3204c83cc094f3895e4ace10a4b91c9cdfeb1070..3ac9f7438408460c157f7484d313ef0d6a3808a6 100644
--- a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp
@@ -2,6 +2,7 @@
// ArmarX
+#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
// RobotAPI
@@ -47,11 +48,13 @@ namespace armarx::armem::client
armem::data::WaitForMemoryResult ReaderComponentPluginUser::useMemoryServer(const std::string& memoryName)
{
- armem::data::WaitForMemoryResult result = MemoryNameSystemComponentPluginUser::useMemoryServer(memoryName);
+ armem::data::WaitForMemoryResult result;
+ result.proxy = memoryNameSystem.useServer(memoryName);
if (result.proxy)
{
setReadingMemoryServer(result.proxy);
}
return result;
}
+
}
diff --git a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
index 0a2e37ba953e024858090cc64d6510dfebfd8a77..c0139e609e830e2c07517837f6cc3d48aff434db 100644
--- a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
@@ -5,7 +5,7 @@
#include <vector>
// ArmarX
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
// RobotAPI
#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
diff --git a/source/RobotAPI/libraries/armem/client/Writer.h b/source/RobotAPI/libraries/armem/client/Writer.h
index dd953fa368d9a46363677d3bc39bc71f8da2a9ce..92c7ce04a3d1ec9317f33404bafef2ac58c8c3cd 100644
--- a/source/RobotAPI/libraries/armem/client/Writer.h
+++ b/source/RobotAPI/libraries/armem/client/Writer.h
@@ -2,7 +2,7 @@
#include <RobotAPI/interface/armem/server/WritingMemoryInterface.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
namespace armarx::armem::client
diff --git a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp
index 093f29e6fa2178f19ae13983e21772f17400856d..d4b95ca3565e8552322e7e7cdfac131335ca06f3 100644
--- a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp
@@ -2,6 +2,7 @@
// ArmarX
+#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
// RobotAPI
@@ -43,9 +44,11 @@ namespace armarx::armem::client
memoryWriter.setWritingMemory(memory);
}
+
armem::data::WaitForMemoryResult WriterComponentPluginUser::useMemoryServer(const std::string& memoryName)
{
- armem::data::WaitForMemoryResult result = MemoryNameSystemComponentPluginUser::useMemoryServer(memoryName);
+ armem::data::WaitForMemoryResult result;
+ result.proxy = memoryNameSystem.useServer(MemoryID().withMemoryName(memoryName));
if (result.proxy)
{
setWritingMemoryServer(result.proxy);
diff --git a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
index 435881b48b27b9053dd5da5e87b794d3561f3b3c..3ae9a66bfab912237934d4bea6580a2e32a2c7fb 100644
--- a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
@@ -2,7 +2,7 @@
// ArmarX
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
// RobotAPI
#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
diff --git a/source/RobotAPI/libraries/armem/client/query/selectors.cpp b/source/RobotAPI/libraries/armem/client/query/selectors.cpp
index 98e79d06ab3642b25bee40e89daf728ab3dfee2b..ecd3f8c5f4a1b3ed0a7dde04c8dc1ac8a06b5265 100644
--- a/source/RobotAPI/libraries/armem/client/query/selectors.cpp
+++ b/source/RobotAPI/libraries/armem/client/query/selectors.cpp
@@ -1,7 +1,7 @@
#include "selectors.h"
#include "RobotAPI/libraries/armem/core/ice_conversions.h"
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
namespace dq = ::armarx::armem::query::data;
diff --git a/source/RobotAPI/libraries/armem/core.h b/source/RobotAPI/libraries/armem/core.h
index c8ddee31ce054decbc8fdc33edf7959502601da7..11abe789b8cd0aef6ab2d9017c96cb1237cc5040 100644
--- a/source/RobotAPI/libraries/armem/core.h
+++ b/source/RobotAPI/libraries/armem/core.h
@@ -6,13 +6,7 @@
#include "core/MemoryID.h"
#include "core/Time.h"
-#include "core/workingmemory/Memory.h"
-#include "core/workingmemory/CoreSegment.h"
-#include "core/workingmemory/ProviderSegment.h"
-#include "core/workingmemory/Entity.h"
-#include "core/workingmemory/EntitySnapshot.h"
-#include "core/workingmemory/EntityInstance.h"
-#include "core/workingmemory/ice_conversions.h"
+#include "core/wm.h"
namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/core/Commit.cpp b/source/RobotAPI/libraries/armem/core/Commit.cpp
index fed7ed660b450bb0865a018f9c8d7119dd96a7b4..fed012450b059f50159c659217694c160e8f636a 100644
--- a/source/RobotAPI/libraries/armem/core/Commit.cpp
+++ b/source/RobotAPI/libraries/armem/core/Commit.cpp
@@ -1,9 +1,11 @@
#include "Commit.h"
-#include <SimoxUtility/algorithm/apply.hpp>
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <SimoxUtility/algorithm/apply.hpp>
+
namespace armarx::armem
{
diff --git a/source/RobotAPI/libraries/armem/core/Commit.h b/source/RobotAPI/libraries/armem/core/Commit.h
index fc69a4b53dcaecc52d703aa42c00405f4162825f..685886a7f27b5d43b78cd0195693e32c3a58fffa 100644
--- a/source/RobotAPI/libraries/armem/core/Commit.h
+++ b/source/RobotAPI/libraries/armem/core/Commit.h
@@ -1,11 +1,12 @@
#pragma once
-#include <vector>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h>
-#include "../core/MemoryID.h"
-#include "../core/Time.h"
+#include <memory>
+#include <vector>
namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
index 6dc77c57625b0a8ebd33200885f06045eef49bbf..0de79cccd0af6a85dd396b8891e91c572ee24fb2 100644
--- a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
@@ -6,7 +6,7 @@
#include "ProviderSegmentBase.h"
#include "detail/AronTyped.h"
#include "detail/EntityContainerBase.h"
-#include "detail/MaxHistorySize.h"
+#include "detail/iteration_mixins.h"
namespace armarx::armem::base
@@ -18,8 +18,10 @@ namespace armarx::armem::base
template <class _ProviderSegmentT, class _Derived>
class CoreSegmentBase :
public detail::EntityContainerBase<_ProviderSegmentT, typename _ProviderSegmentT::EntityT, _Derived>,
- public detail::MaxHistorySize,
- public detail::AronTyped
+ public detail::AronTyped,
+ public detail::ForEachEntityInstanceMixin<_Derived>,
+ public detail::ForEachEntitySnapshotMixin<_Derived>,
+ public detail::ForEachEntityMixin<_Derived>
{
using Base = detail::EntityContainerBase<_ProviderSegmentT, typename _ProviderSegmentT::EntityT, _Derived>;
@@ -50,20 +52,21 @@ namespace armarx::armem::base
{}
};
+
public:
CoreSegmentBase()
{
}
- CoreSegmentBase(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
- CoreSegmentBase(MemoryID().withCoreSegmentName(name), aronType)
+ explicit CoreSegmentBase(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
+ CoreSegmentBase(name, MemoryID(), aronType)
{
}
- CoreSegmentBase(const std::string& name, const MemoryID& parentID, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
- CoreSegmentBase(parentID.withProviderSegmentName(name), aronType)
+ explicit CoreSegmentBase(const std::string& name, const MemoryID& parentID, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
+ CoreSegmentBase(parentID.withCoreSegmentName(name), aronType)
{
}
- CoreSegmentBase(const MemoryID& id, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
+ explicit CoreSegmentBase(const MemoryID& id, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
Base(id),
AronTyped(aronType)
{
@@ -75,6 +78,8 @@ namespace armarx::armem::base
CoreSegmentBase& operator=(CoreSegmentBase&& other) = default;
+ // READ ACCESS
+
inline const std::string& name() const
{
return this->id().coreSegmentName;
@@ -85,19 +90,14 @@ namespace armarx::armem::base
}
- inline const auto& providerSegments() const
- {
- return this->_container;
- }
- inline auto& providerSegments()
+ bool hasProviderSegment(const std::string& name) const
{
- return this->_container;
+ return this->_container.count(name) > 0;
}
-
- bool hasProviderSegment(const std::string& name) const
+ std::vector<std::string> getProviderSegmentNames() const
{
- return this->_container.count(name) > 0;
+ return simox::alg::get_keys(this->_container);
}
ProviderSegmentT& getProviderSegment(const std::string& name)
@@ -119,13 +119,13 @@ namespace armarx::armem::base
}
using Base::getEntity;
- const EntityT& getEntity(const MemoryID& id) const override
+ const EntityT& getEntity(const MemoryID& id) const
{
this->_checkContainerName(id.coreSegmentName, this->getKeyString());
return getProviderSegment(id.providerSegmentName).getEntity(id);
}
- const EntityT* findEntity(const MemoryID& id) const override
+ const EntityT* findEntity(const MemoryID& id) const
{
this->_checkContainerName(id.coreSegmentName, this->getKeyString());
if (id.hasProviderSegmentName())
@@ -145,6 +145,45 @@ namespace armarx::armem::base
}
}
+
+ // ITERATION
+
+ /**
+ * @param func Function like: bool process(ProviderSegmentT& provSeg)
+ */
+ template <class ProviderSegmentFunctionT>
+ bool forEachProviderSegment(ProviderSegmentFunctionT&& func)
+ {
+ return this->forEachChild(func);
+ }
+ /**
+ * @param func Function like: bool process(const ProviderSegmentT& provSeg)
+ */
+ template <class ProviderSegmentFunctionT>
+ bool forEachProviderSegment(ProviderSegmentFunctionT&& func) const
+ {
+ return this->forEachChild(func);
+ }
+
+ // forEachEntity() is provided by ForEachEntityMixin.
+ // forEachSnapshot() is provided by ForEachEntitySnapshotMixin.
+ // forEachInstance() is provided by ForEachEntityInstanceMixin.
+
+
+ [[deprecated("Direct container access is deprecated. Use forEach*() instead.")]]
+ inline const auto& providerSegments() const
+ {
+ return this->_container;
+ }
+ [[deprecated("Direct container access is deprecated. Use forEach*() instead.")]]
+ inline auto& providerSegments()
+ {
+ return this->_container;
+ }
+
+
+ // MODIFICATION
+
/**
* @brief Updates an entity's history.
*
@@ -154,53 +193,61 @@ namespace armarx::armem::base
{
this->_checkContainerName(update.entityID.coreSegmentName, this->name());
+ auto [inserted, provSeg] = addProviderSegmentIfMissing(update.entityID.providerSegmentName);
+
+
+ // Update entry.
+ UpdateResult ret(provSeg->update(update));
+ if (inserted)
+ {
+ ret.coreSegmentUpdateType = UpdateType::InsertedNew;
+ }
+ else
+ {
+ ret.coreSegmentUpdateType = UpdateType::UpdatedExisting;
+ }
+ return ret;
+ }
+
+ std::pair<bool, ProviderSegmentT*> addProviderSegmentIfMissing(const std::string& providerSegmentName)
+ {
ProviderSegmentT* provSeg;
- UpdateType updateType = UpdateType::InsertedNew;
- auto it = this->_container.find(update.entityID.providerSegmentName);
+ auto it = this->_container.find(providerSegmentName);
if (it == this->_container.end())
{
if (_addMissingProviderSegmentDuringUpdate)
{
// Insert into map.
- provSeg = &addProviderSegment(update.entityID.providerSegmentName);
- provSeg->setMaxHistorySize(_maxHistorySize);
- updateType = UpdateType::InsertedNew;
+ provSeg = &addProviderSegment(providerSegmentName);
+ return {true, provSeg};
}
else
{
- throw error::MissingEntry::create<EntitySnapshotT>(update.entityID.providerSegmentName, *this);
+ throw error::MissingEntry::create<ProviderSegmentT>(providerSegmentName, *this);
}
}
else
{
provSeg = &it->second;
- updateType = UpdateType::UpdatedExisting;
+ return {false, provSeg};
}
-
- // Update entry.
- UpdateResult ret(provSeg->update(update));
- ret.coreSegmentUpdateType = updateType;
- return ret;
}
void append(const _Derived& m)
{
- ARMARX_INFO << "CoreSegment: Merge name '" << m.name() << "' into '" << name() << "'";
+ // ARMARX_INFO << "CoreSegment: Merge name '" << m.name() << "' into '" << name() << "'";
- for (const auto& [k, s] : m.container())
+ m.forEachProviderSegment([this](const ProviderSegmentT & provSeg)
{
- if (const auto& it = this->_container.find(k); it != this->_container.end())
+ auto it = this->_container.find(provSeg.name());
+ if (it == this->_container.end())
{
- // segment already exists
- it->second.append(s);
+ it = this->_container.emplace(provSeg.name(), this->id().withProviderSegmentName(provSeg.name())).first;
}
- else
- {
- auto wms = this->_container.emplace(k, this->id().withProviderSegmentName(k));
- wms.first->second.append(s);
- }
- }
+ it->second.append(provSeg);
+ return true;
+ });
}
/**
@@ -212,7 +259,7 @@ namespace armarx::armem::base
ProviderSegmentT& addProviderSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr providerSegmentType = nullptr)
{
aron::typenavigator::ObjectNavigatorPtr type = providerSegmentType ? providerSegmentType : this->aronType();
- return addProviderSegment(ProviderSegmentT(name, type));
+ return addProviderSegment(ProviderSegmentT(name, this->id(), type));
}
/// Copy and insert a provider segment.
@@ -227,31 +274,18 @@ namespace armarx::armem::base
if (hasProviderSegment(providerSegment.name()))
{
throw armem::error::ContainerEntryAlreadyExists(
- providerSegment.getLevelName(), providerSegment.name(), getLevelName(), this->getKeyString());
+ ProviderSegmentT::getLevelName(), providerSegment.name(), getLevelName(), this->getKeyString());
}
auto it = this->_container.emplace(providerSegment.name(), std::move(providerSegment)).first;
it->second.id().setCoreSegmentID(this->id());
- it->second.setMaxHistorySize(_maxHistorySize);
return it->second;
}
- /**
- * @brief Sets the maximum history size of entities in this segment.
- * This affects all current entities as well as new ones.
- * @see Entity::setMaxHistorySize()
- */
- void setMaxHistorySize(long maxSize) override
- {
- MaxHistorySize::setMaxHistorySize(maxSize);
- for (auto& [name, seg] : this->_container)
- {
- seg.setMaxHistorySize(maxSize);
- }
- }
+ // MISC
- virtual bool equalsDeep(const CoreSegmentBase& other) const
+ bool equalsDeep(const DerivedT& other) const
{
//std::cout << "CoreSegment::equalsDeep" << std::endl;
if (this->size() != other.size())
@@ -272,33 +306,17 @@ namespace armarx::armem::base
return true;
}
- std::string getLevelName() const override
+ static std::string getLevelName()
{
return "core segment";
}
- std::string getKeyString() const override
+ std::string getKeyString() const
{
return this->name();
}
- protected:
-
- virtual void _copySelf(DerivedT& other) const override
- {
- Base::_copySelf(other);
- other.aronType() = _aronType;
- other._addMissingProviderSegmentDuringUpdate = _addMissingProviderSegmentDuringUpdate;
- }
- virtual void _copySelfEmpty(DerivedT& other) const override
- {
- Base::_copySelfEmpty(other);
- other.aronType() = _aronType;
- other._addMissingProviderSegmentDuringUpdate = _addMissingProviderSegmentDuringUpdate;
- }
-
-
private:
bool _addMissingProviderSegmentDuringUpdate = true;
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityBase.cpp b/source/RobotAPI/libraries/armem/core/base/EntityBase.cpp
index 0d2c67b63d566f7170bd9b31cfaf18785b85f87b..d3038a357bdad823f04e8d154a569a7eebaf90d5 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityBase.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/EntityBase.cpp
@@ -1 +1,2 @@
#include "EntityBase.h"
+
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityBase.h b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
index ab227c33ee321475f129d8f864b5cbf31e3f4845..05cbf780d1e95611d629d320581122b59c5f358c 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
@@ -5,14 +5,13 @@
#include <SimoxUtility/algorithm/get_map_keys_values.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "../../core/Time.h"
-#include "../../core/MemoryID.h"
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
#include "EntitySnapshotBase.h"
-#include "detail/MaxHistorySize.h"
#include "detail/MemoryContainerBase.h"
+#include "detail/iteration_mixins.h"
+#include "detail/negative_index_semantics.h"
namespace armarx::armem::base
@@ -40,7 +39,7 @@ namespace armarx::armem::base
template <class _EntitySnapshotT, class _Derived>
class EntityBase :
public detail::MemoryContainerBase<std::map<Time, _EntitySnapshotT>, _Derived>,
- public detail::MaxHistorySize
+ public detail::ForEachEntityInstanceMixin<_Derived>
{
using Base = detail::MemoryContainerBase<std::map<Time, _EntitySnapshotT>, _Derived>;
@@ -66,11 +65,11 @@ namespace armarx::armem::base
EntityBase()
{
}
- EntityBase(const std::string& name, const MemoryID& parentID = {}) :
+ explicit EntityBase(const std::string& name, const MemoryID& parentID = {}) :
EntityBase(parentID.withEntityName(name))
{
}
- EntityBase(const MemoryID& id) :
+ explicit EntityBase(const MemoryID& id) :
Base(id)
{
}
@@ -82,27 +81,7 @@ namespace armarx::armem::base
EntityBase& operator=(EntityBase&& other) = default;
- virtual bool equalsDeep(const EntityBase& other) const
- {
- //std::cout << "Entity::equalsDeep" << std::endl;
- if (this->size() != other.size())
- {
- return false;
- }
- for (const auto& [key, snapshot] : this->_container)
- {
- if (not other.hasSnapshot(key))
- {
- return false;
- }
- if (not snapshot.equalsDeep(other.getSnapshot(key)))
- {
- return false;
- }
- }
- return true;
- }
-
+ // READING
inline const std::string& name() const
{
@@ -113,26 +92,14 @@ namespace armarx::armem::base
return const_cast<std::string&>(const_cast<const EntityBase*>(this)->name());
}
-
- inline const std::map<Time, EntitySnapshotT>& history() const
- {
- return this->_container;
- }
- inline std::map<Time, EntitySnapshotT>& history()
- {
- return const_cast<std::map<Time, EntitySnapshotT>&>(const_cast<const EntityBase*>(this)->history());
- }
-
-
/**
* @brief Indicates whether a history entry for the given time exists.
*/
- virtual bool hasSnapshot(const Time& time)
+ bool hasSnapshot(const Time& time)
{
return const_cast<const EntityBase*>(this)->hasSnapshot(time);
}
-
- virtual bool hasSnapshot(const Time& time) const
+ bool hasSnapshot(const Time& time) const
{
return this->_container.count(time) > 0;
}
@@ -154,12 +121,7 @@ namespace armarx::armem::base
/**
* @brief Get all timestamps in the history.
*/
- virtual std::vector<Time> getTimestamps()
- {
- return const_cast<const EntityBase*>(this)->getTimestamps();
- }
-
- virtual std::vector<Time> getTimestamps() const
+ std::vector<Time> getTimestamps() const
{
return simox::alg::get_keys(this->_container);
}
@@ -172,12 +134,12 @@ namespace armarx::armem::base
*
* @throws `armem::error::MissingEntry` If there is no such entry.
*/
- virtual EntitySnapshotT& getSnapshot(const Time& time)
+ EntitySnapshotT& getSnapshot(const Time& time)
{
return const_cast<EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getSnapshot(time));
}
- virtual const EntitySnapshotT& getSnapshot(const Time& time) const
+ const EntitySnapshotT& getSnapshot(const Time& time) const
{
auto it = this->_container.find(time);
if (it != this->_container.end())
@@ -210,150 +172,234 @@ namespace armarx::armem::base
{
return const_cast<EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getLatestSnapshot());
}
-
const EntitySnapshotT& getLatestSnapshot() const
{
return getLatestItem().second;
}
/**
- * @brief Return the lastest snapshot before or at time.
+ * @brief Return the snapshot with the most recent timestamp.
+ * @return The latest snapshot.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ */
+ EntitySnapshotT& getFirstSnapshot()
+ {
+ return const_cast<EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getFirstSnapshot());
+ }
+ const EntitySnapshotT& getFirstSnapshot() const
+ {
+ return getFirstItem().second;
+ }
+
+
+ /**
+ * @brief Return the lastest snapshot before time.
* @param time The time.
* @return The latest snapshot.
* @throw `armem::error::EntityHistoryEmpty` If the history is empty.
* @throw `armem::error::MissingEntry` If no such snapshot
*/
- virtual const EntitySnapshotT& getLatestSnapshotBeforeOrAt(const Time& time) const
+ const EntitySnapshotT& getLatestSnapshotBefore(const Time& time) const
{
- // first element equal or greater
- typename std::map<Time, EntitySnapshotT>::const_iterator refIt = this->_container.upper_bound(time);
+ if (this->empty())
+ {
+ throw error::EntityHistoryEmpty(this->name());
+ }
- if (refIt == this->_container.begin())
+ // We want the rightmost element < time
+ // lower_bound() gives us the the leftmost >= time, which is probably the right neighbour
+ typename ContainerT::const_iterator greaterEq = this->_container.lower_bound(time);
+ // Special cases:
+ // refIt = begin() => no element < time
+ if (greaterEq == this->_container.begin())
{
- if (refIt->first == time)
- {
- return refIt->second;
- }
throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
}
- // last element less than
- typename std::map<Time, EntitySnapshotT>::const_iterator refItPrev = std::prev(refIt);
+ // end(): No element >= time, we can still have one < time (rbegin()) => std::prev(end())
+ // empty has been checked above
- // ... or we return the element before if possible
- if (refItPrev != this->_container.begin())
- {
- return refItPrev->second;
- }
+ // std::prev of end() is ok
+ typename ContainerT::const_iterator less = std::prev(greaterEq);
- throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ // we return the element before if possible
+ return less->second;
}
/**
- * @brief Return the lastest snapshot before time.
+ * @brief Return the lastest snapshot before or at time.
* @param time The time.
* @return The latest snapshot.
* @throw `armem::error::EntityHistoryEmpty` If the history is empty.
* @throw `armem::error::MissingEntry` If no such snapshot
*/
- virtual const EntitySnapshotT& getLatestSnapshotBefore(const Time& time) const
+ const EntitySnapshotT& getLatestSnapshotBeforeOrAt(const Time& time) const
+ {
+ return getLatestSnapshotBefore(time + Time::microSeconds(1));
+ }
+
+ /**
+ * @brief Return first snapshot after or at time.
+ * @param time The time.
+ * @return The first snapshot.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ * @throw `armem::error::MissingEntry` If no such snapshot
+ */
+ const EntitySnapshotT& getFirstSnapshotAfterOrAt(const Time& time) const
{
- // first element equal or greater
- typename std::map<Time, EntitySnapshotT>::const_iterator refIt = this->_container.upper_bound(time);
+ // We want the leftmost element >= time.
+ // That's lower bound.
+ typename ContainerT::const_iterator greaterEq = this->_container.lower_bound(time);
- if (refIt == this->_container.begin())
+ if (greaterEq == this->_container.end())
{
throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
}
+ return greaterEq->second;
+ }
- // last element less than
- typename std::map<Time, EntitySnapshotT>::const_iterator refItPrev = std::prev(refIt);
+ /**
+ * @brief Return first snapshot after time.
+ * @param time The time.
+ * @return The first snapshot.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ * @throw `armem::error::MissingEntry` If no such snapshot
+ */
+ const EntitySnapshotT& getFirstSnapshotAfter(const Time& time) const
+ {
+ return getFirstSnapshotAfter(time - Time::microSeconds(1));
+ }
- // we return the element before if possible
- if (refItPrev != this->_container.begin())
- {
- return refItPrev->second;
- }
- throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ // ITERATION
+
+ /**
+ * @param func Function like: bool process(EntitySnapshotT& snapshot)
+ */
+ template <class SnapshotFunctionT>
+ bool forEachSnapshot(SnapshotFunctionT&& func)
+ {
+ return this->forEachChild(func);
+ }
+ /**
+ * @param func Function like void process(const EntitySnapshotT& snapshot)
+ */
+ template <class SnapshotFunctionT>
+ bool forEachSnapshot(SnapshotFunctionT&& func) const
+ {
+ return this->forEachChild(func);
}
+ // forEachInstance() is provided by ForEachEntityInstanceMixin.
+
/**
- * @brief Return all snapshots before or at time.
+ * @brief Return all snapshots before (excluding) time.
* @param time The time.
* @return The latest snapshots.
- * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
- * @throw `armem::error::MissingEntry` If no such snapshots
*/
- virtual const std::vector<std::reference_wrapper<const EntitySnapshotT>> getSnapshotsBefore(const Time& time) const
+ template <class FunctionT>
+ void forEachSnapshotBefore(const Time& time, FunctionT&& func) const
{
- std::vector<std::reference_wrapper<const EntitySnapshotT>> ret;
- const EntitySnapshotT& latest = getLatestSnapshotBefore(time);
-
for (const auto& [timestamp, snapshot] : this->_container)
{
- ret.emplace_back(snapshot);
- if (timestamp == latest.id().timestamp)
+ if (timestamp >= time)
+ {
+ break;
+ }
+ if (not call(func, snapshot))
{
break;
}
}
- return ret;
}
/**
- * @brief Return first snapshot after or at time.
+ * @brief Return all snapshots before or at time.
* @param time The time.
- * @return The first snapshot.
- * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
- * @throw `armem::error::MissingEntry` If no such snapshot
+ * @return The latest snapshots.
*/
- virtual const EntitySnapshotT& getFirstSnapshotAfterOrAt(const Time& time) const
+ template <class FunctionT>
+ void forEachSnapshotBeforeOrAt(const Time& time, FunctionT&& func) const
{
- // first element equal or greater
- typename std::map<Time, EntitySnapshotT>::const_iterator refIt = this->_container.upper_bound(time);
-
- if (refIt == this->_container.end())
- {
- throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
- }
- return refIt->second;
+ getSnapshotsBefore(time + Time::microSeconds(1), func);
}
+
/**
- * @brief Return first snapshot after time.
- * @param time The time.
- * @return The first snapshot.
- * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
- * @throw `armem::error::MissingEntry` If no such snapshot
+ * @brief Return all snapshots between, including, min and max.
+ * @param min The lowest time to include.
+ * @param min The highest time to include.
+ * @return The snapshots in [min, max].
*/
- virtual const EntitySnapshotT& getFirstSnapshotAfter(const Time& time) const
+ template <class FunctionT>
+ void forEachSnapshotInTimeRange(const Time& min, const Time& max, FunctionT&& func) const
{
- // first element equal or greater
- typename std::map<Time, EntitySnapshotT>::const_iterator refIt = this->_container.upper_bound(time);
+ // Returns an iterator pointing to the first element that is not less than (i.e. greater or equal to) key.
+ auto begin = min.toMicroSeconds() > 0 ? this->_container.lower_bound(min) : this->_container.begin();
+ // Returns an iterator pointing to the first element that is *greater than* key.
+ auto end = max.toMicroSeconds() > 0 ? this->_container.upper_bound(max) : this->_container.end();
- if (refIt == this->_container.end())
+ for (auto it = begin; it != end && it != this->_container.end(); ++it)
{
- throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ if (not call(func, it->second))
+ {
+ break;
+ }
}
+ }
- if (refIt->first > time)
+ /**
+ * @brief Return all snapshots from first to last index.
+ *
+ * Negative index are counted from the end, e.g.
+ * last == -1 results in getting all queries until the end.
+ *
+ * @param first The first index to include.
+ * @param first The last index to include.
+ * @return The snapshots in [first, last].
+ */
+ template <class FunctionT>
+ void forEachSnapshotInIndexRange(long first, long last, FunctionT&& func) const
+ {
+ if (this->empty())
{
- return refIt->second;
+ return;
}
- // first element greater than
- typename std::map<Time, EntitySnapshotT>::const_iterator refItNext = std::next(refIt);
+ const size_t first_ = detail::negativeIndexSemantics(first, this->size());
+ const size_t last_ = detail::negativeIndexSemantics(last, this->size());
- if (refItNext != this->_container.end())
+ if (first_ <= last_)
{
- return refItNext->second;
+ auto it = this->_container.begin();
+ std::advance(it, first_);
+
+ size_t num = last_ - first_ + 1; // +1 to make last inclusive
+ for (size_t i = 0; i < num; ++i, ++it)
+ {
+ if (not call(func, it->second))
+ {
+ break;
+ }
+ }
}
+ }
- throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+
+ [[deprecated("Direct container access is deprecated. Use forEach*() instead.")]]
+ inline const ContainerT& history() const
+ {
+ return this->_container;
+ }
+ [[deprecated("Direct container access is deprecated. Use forEach*() instead.")]]
+ inline ContainerT& history()
+ {
+ return const_cast<ContainerT&>(const_cast<const EntityBase*>(this)->history());
}
+ // MODIFICATION
+
/**
* @brief Add the given update to this entity's history.
* @param update The update.
@@ -371,7 +417,7 @@ namespace armarx::armem::base
{
// Insert into history.
snapshot = &addSnapshot(update.timeCreated);
- ret.removedSnapshots = truncate();
+ // ret.removedSnapshots = this->truncate();
ret.entityUpdateType = UpdateType::InsertedNew;
}
else
@@ -380,30 +426,30 @@ namespace armarx::armem::base
ret.entityUpdateType = UpdateType::UpdatedExisting;
}
// Update entry.
- snapshot->update(update, this->id());
+ snapshot->update(update);
ret.id = snapshot->id();
return ret;
}
+
void append(const DerivedT& m)
{
- ARMARX_INFO << "Entity: Merge name '" << m.name() << "' into '" << name() << "'";
+ // ARMARX_INFO << "Entity: Merge name '" << m.name() << "' into '" << name() << "'";
- for (const auto& [k, s] : m.container())
+ m.forEachSnapshot([this](const EntitySnapshotT & snapshot)
{
- if (const auto& it = this->_container.find(k); it != this->_container.end())
- {
- // segment already exists
- // We assume that a snapshot does not change, so ignore
- }
- else
+ auto it = this->_container.find(snapshot.time());
+ if (it == this->_container.end())
{
- auto snapshot = s.copy();
- snapshot.id() = this->id().withTimestamp(k); // update id (e.g. memory name) if necessary
- this->_container.insert(std::make_pair(k, std::move(snapshot)));
+ EntitySnapshotT copy { snapshot };
+ copy.id() = this->id().withTimestamp(snapshot.time()); // update id (e.g. memory name) if necessary
+ this->_container.emplace(snapshot.time(), copy);
}
- }
+ // else: segment already exists
+ // We assume that a snapshot does not change, so ignore
+ return true;
+ });
}
/**
@@ -418,33 +464,46 @@ namespace armarx::armem::base
EntitySnapshotT& addSnapshot(EntitySnapshotT&& snapshot)
{
- auto it = this->_container.emplace(snapshot.time(), std::move(snapshot)).first;
+ auto it = this->_container.emplace_hint(this->_container.end(), snapshot.time(), std::move(snapshot));
it->second.id().setEntityID(this->id());
return it->second;
}
EntitySnapshotT& addSnapshot(const Time& timestamp)
{
- return addSnapshot(EntitySnapshotT(timestamp));
+ return addSnapshot(EntitySnapshotT(timestamp, this->id()));
}
- /**
- * @brief Sets the maximum history size.
- *
- * The current history is truncated if necessary.
- */
- void setMaxHistorySize(long maxSize) override
+
+ // MISC
+
+ bool equalsDeep(const DerivedT& other) const
{
- MaxHistorySize::setMaxHistorySize(maxSize);
- truncate();
+ //std::cout << "Entity::equalsDeep" << std::endl;
+ if (this->size() != other.size())
+ {
+ return false;
+ }
+ for (const auto& [key, snapshot] : this->_container)
+ {
+ if (not other.hasSnapshot(key))
+ {
+ return false;
+ }
+ if (not snapshot.equalsDeep(other.getSnapshot(key)))
+ {
+ return false;
+ }
+ }
+ return true;
}
- std::string getKeyString() const override
+ std::string getKeyString() const
{
return this->id().entityName;
}
- std::string getLevelName() const override
+ static std::string getLevelName()
{
return "entity";
}
@@ -452,36 +511,37 @@ namespace armarx::armem::base
protected:
- /// If maximum size is set, ensure `history`'s is not higher.
- std::vector<EntitySnapshotT> truncate()
+ /**
+ * @brief Return the snapshot with the most recent timestamp.
+ * @return The latest snapshot.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ */
+ const typename ContainerT::value_type&
+ getLatestItem() const
{
- std::vector<EntitySnapshotT> removedElements;
- if (_maxHistorySize >= 0)
+ if (this->_container.empty())
{
- while (this->_container.size() > size_t(_maxHistorySize))
- {
- removedElements.push_back(std::move(this->_container.begin()->second));
- this->_container.erase(this->_container.begin());
- }
- ARMARX_CHECK_LESS_EQUAL(this->_container.size(), _maxHistorySize);
+ throw armem::error::EntityHistoryEmpty(name(), "when getting the latest snapshot.");
}
- return removedElements;
+ return *this->_container.rbegin();
}
/**
- * @brief Return the snapshot with the most recent timestamp.
- * @return The latest snapshot.
+ * @brief Return the snapshot with the least recent timestamp.
+ * @return The first snapshot.
* @throw `armem::error::EntityHistoryEmpty` If the history is empty.
*/
- virtual const typename std::map<Time, EntitySnapshotT>::value_type& getLatestItem() const
+ const typename ContainerT::value_type&
+ getFirstItem() const
{
if (this->_container.empty())
{
- throw armem::error::EntityHistoryEmpty(name(), "when getting the latest snapshot.");
+ throw armem::error::EntityHistoryEmpty(name(), "when getting the first snapshot.");
}
- return *this->_container.rbegin();
+ return *this->_container.begin();
}
+
};
}
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.cpp b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.cpp
index a6cfc7103728d5a81fecefe4ade6c96efef2814e..1cc2e9544d00ce96d65658acaf853c670418bdce 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.cpp
@@ -1 +1,25 @@
#include "EntityInstanceBase.h"
+
+
+namespace armarx::armem::base
+{
+ bool EntityInstanceMetadata::operator==(const EntityInstanceMetadata& other) const
+ {
+ return timeCreated == other.timeCreated
+ && timeSent == other.timeSent
+ && timeArrived == other.timeArrived
+ && std::abs(confidence - other.confidence) < 1e-6f;
+ }
+}
+
+
+std::ostream& armarx::armem::base::operator<<(std::ostream& os, const EntityInstanceMetadata& d)
+{
+ os << "EntityInstanceMetadata: "
+ << "\n- t_create = \t" << armem::toStringMicroSeconds(d.timeCreated) << " us"
+ << "\n- t_sent = \t" << armem::toStringMicroSeconds(d.timeSent) << " us"
+ << "\n- t_arrived = \t" << armem::toStringMicroSeconds(d.timeArrived) << " us"
+ << "\n- confidence = \t" << d.confidence << " us"
+ ;
+ return os;
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
index 8dd474c667687e6235a96a191e577163ca194cba..fcb1869f791d9094fd793ff3af721e5fb942f111 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
@@ -1,17 +1,53 @@
#pragma once
-#include "../../core/Time.h"
+#include <RobotAPI/libraries/armem/core/Commit.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
-#include "../Commit.h"
#include "detail/MemoryItem.h"
namespace armarx::armem::base
{
+
+ /**
+ * @brief Default data of an entity instance (empty).
+ */
+ struct NoData
+ {
+ };
+
+
+ /**
+ * @brief Metadata of an entity instance.
+ */
+ struct EntityInstanceMetadata
+ {
+ /// Time when this value was created.
+ Time timeCreated;
+ /// Time when this value was sent to the memory.
+ Time timeSent;
+ /// Time when this value has arrived at the memory.
+ Time timeArrived;
+
+ /// An optional confidence, may be used for things like decay.
+ float confidence = 1.0;
+
+
+ bool operator==(const EntityInstanceMetadata& other) const;
+ inline bool operator!=(const EntityInstanceMetadata& other) const
+ {
+ return !(*this == other);
+ }
+ };
+
+ std::ostream& operator<<(std::ostream& os, const EntityInstanceMetadata& rhs);
+
+
+
/**
* @brief Data of a single entity instance.
*/
- template <class _DerivedT>
+ template <class _DataT = NoData, class _MetadataT = EntityInstanceMetadata>
class EntityInstanceBase :
public detail::MemoryItem
{
@@ -19,19 +55,18 @@ namespace armarx::armem::base
public:
- using DerivedT = _DerivedT;
+ using MetadataT = _MetadataT;
+ using DataT = _DataT;
- public:
-
EntityInstanceBase()
{
}
- EntityInstanceBase(int index, const MemoryID& parentID = {}) :
+ explicit EntityInstanceBase(int index, const MemoryID& parentID = {}) :
EntityInstanceBase(parentID.withInstanceIndex(index))
{
}
- EntityInstanceBase(const MemoryID& id) :
+ explicit EntityInstanceBase(const MemoryID& id) :
Base(id)
{
}
@@ -46,40 +81,47 @@ namespace armarx::armem::base
return id().instanceIndex;
}
- /**
- * @brief Fill `*this` with the update's values.
- * @param update The update.
- * @param index The instances index.
- */
- virtual void update(const EntityUpdate& update, int index) = 0;
- virtual bool equalsDeep(const DerivedT& other) const = 0;
+ EntityInstanceMetadata& metadata()
+ {
+ return _metadata;
+ }
+ const EntityInstanceMetadata& metadata() const
+ {
+ return _metadata;
+ }
- virtual DerivedT copy() const
+ const DataT& data() const
{
- DerivedT d;
- this->_copySelf(d);
- return d;
+ return _data;
}
+ DataT& data()
+ {
+ return _data;
+ }
- std::string getLevelName() const override
+
+ static std::string getLevelName()
{
return "entity instance";
}
- std::string getKeyString() const override
+ std::string getKeyString() const
{
return std::to_string(index());
}
+
protected:
- virtual void _copySelf(DerivedT& other) const
- {
- Base::_copySelf(other);
- }
+ /// The metadata.
+ MetadataT _metadata;
+
+ /// The data. May be nullptr.
+ DataT _data;
+
};
diff --git a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.cpp b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.cpp
index 5f7899f50d954f16b2d9c0a9569519529895eb20..72b4c48877c78dce70737fa58c58cd1b82b2d2e5 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.cpp
@@ -1 +1,11 @@
#include "EntitySnapshotBase.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+void armarx::armem::base::detail::throwIfNotEqual(const Time& ownTime, const Time& updateTime)
+{
+ ARMARX_CHECK_EQUAL(ownTime, updateTime)
+ << "Cannot update a snapshot to an update with another timestamp. \n"
+ << "Note: A snapshot's timestamp must not be changed after construction.";
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
index cf68286be26368cb4821fecbbe241e3ef33e0fbe..7653136447dbe968d3d53f878ba47619481594fc 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
@@ -1,15 +1,19 @@
#pragma once
-#include <memory>
#include <vector>
-#include "../MemoryID.h"
-#include "../Time.h"
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
#include "EntityInstanceBase.h"
#include "detail/MemoryContainerBase.h"
+#include "detail/iteration_mixins.h"
+namespace armarx::armem::base::detail
+{
+ void throwIfNotEqual(const Time& ownTime, const Time& updateTime);
+}
namespace armarx::armem::base
{
/**
@@ -34,11 +38,11 @@ namespace armarx::armem::base
EntitySnapshotBase()
{
}
- EntitySnapshotBase(Time time, const MemoryID& parentID = {}) :
+ explicit EntitySnapshotBase(Time time, const MemoryID& parentID = {}) :
EntitySnapshotBase(parentID.withTimestamp(time))
{
}
- EntitySnapshotBase(const MemoryID& id) :
+ explicit EntitySnapshotBase(const MemoryID& id) :
Base(id)
{
}
@@ -49,24 +53,7 @@ namespace armarx::armem::base
EntitySnapshotBase& operator=(EntitySnapshotBase&& other) = default;
- bool equalsDeep(const EntitySnapshotBase& other) const
- {
- //std::cout << "EntitySnapshot::equalsDeep" << std::endl;
- if (this->size() != other.size())
- {
- return false;
- }
- int i = 0;
- for (const auto& instance : this->_container)
- {
- if (not instance.equalsDeep(other.getInstance(i)))
- {
- return false;
- }
- i++;
- }
- return true;
- }
+ // READING
inline Time& time()
{
@@ -78,34 +65,6 @@ namespace armarx::armem::base
return this->id().timestamp;
}
-
- inline const std::vector<EntityInstanceT>& instances() const
- {
- return this->_container;
- }
- inline std::vector<EntityInstanceT>& instances()
- {
- return const_cast<std::vector<EntityInstanceT>&>(const_cast<const EntitySnapshotBase*>(this)->instances());
- }
-
-
- void update(const EntityUpdate& update, std::optional<MemoryID> parentID = std::nullopt)
- {
- if (parentID)
- {
- this->id() = *parentID;
- }
- time() = update.timeCreated;
-
- this->_container.clear();
- for (int i = 0; i < int(update.instancesData.size()); ++i)
- {
- EntityInstanceT& data = this->_container.emplace_back(i, this->id());
- data.update(update, i);
- }
- }
-
-
bool hasInstance(int index) const
{
size_t si = size_t(index);
@@ -122,7 +81,6 @@ namespace armarx::armem::base
{
return const_cast<EntityInstanceT&>(const_cast<const EntitySnapshotBase*>(this)->getInstance(index));
}
-
const EntityInstanceT& getInstance(int index) const
{
if (hasInstance(index))
@@ -135,6 +93,23 @@ namespace armarx::armem::base
}
}
+ EntityInstanceT* findInstance(int index)
+ {
+ return const_cast<EntityInstanceT*>(const_cast<const EntitySnapshotBase*>(this)->findInstance(index));
+ }
+ const EntityInstanceT* findInstance(int index) const
+ {
+ if (hasInstance(index))
+ {
+ return &this->_container[static_cast<size_t>(index)];
+ }
+ else
+ {
+ return nullptr;
+ }
+ }
+
+
/**
* @brief Get the given instance.
* @param index The instance's index.
@@ -157,6 +132,51 @@ namespace armarx::armem::base
}
+ // ITERATION
+
+ /**
+ * @param func Function like void process(EntityInstanceT& instance)>
+ */
+ template <class InstanceFunctionT>
+ bool forEachInstance(InstanceFunctionT&& func)
+ {
+ return this->forEachChild(func);
+ }
+ /**
+ * @param func Function like void process (const EntityInstanceT& instance)
+ */
+ template <class InstanceFunctionT>
+ bool forEachInstance(InstanceFunctionT&& func) const
+ {
+ return this->forEachChild(func);
+ }
+
+ [[deprecated("Direct container access is deprecated. Use forEach*() instead.")]]
+ inline const std::vector<EntityInstanceT>& instances() const
+ {
+ return this->_container;
+ }
+ [[deprecated("Direct container access is deprecated. Use forEach*() instead.")]]
+ inline std::vector<EntityInstanceT>& instances()
+ {
+ return const_cast<std::vector<EntityInstanceT>&>(const_cast<const EntitySnapshotBase*>(this)->instances());
+ }
+
+
+ // MODIFICATION
+
+ void update(const EntityUpdate& update)
+ {
+ detail::throwIfNotEqual(time(), update.timeCreated);
+
+ this->_container.clear();
+ for (int index = 0; index < int(update.instancesData.size()); ++index)
+ {
+ EntityInstanceT& data = this->_container.emplace_back(index, this->id());
+ data.update(update);
+ }
+ }
+
/**
* @brief Add a single instance with data.
* @param instance The instance.
@@ -181,27 +201,49 @@ namespace armarx::armem::base
"Cannot add an EntityInstance because its index is too big.");
}
- int new_index = this->_container.size();
- auto& it = this->_container.emplace_back(std::move(instance));
- it.index() = new_index;
- return it;
+ int index = static_cast<int>(this->_container.size());
+ EntityInstanceT& added = this->_container.emplace_back(std::move(instance));
+ added.id() = this->id().withInstanceIndex(index);
+ return added;
}
EntityInstanceT& addInstance()
{
- int new_index = this->_container.size();
- auto& it = this->_container.emplace_back(EntityInstanceT());;
- it.index() = new_index;
- it.id() = this->id().withInstanceIndex(new_index);
- return it;
+ int index = static_cast<int>(this->size());
+ EntityInstanceT& added = this->_container.emplace_back(EntityInstanceT());
+ added.id() = this->id().withInstanceIndex(index);
+ return added;
}
- std::string getKeyString() const override
+
+ // MISC
+
+ bool equalsDeep(const DerivedT& other) const
+ {
+ //std::cout << "EntitySnapshot::equalsDeep" << std::endl;
+ if (this->size() != other.size())
+ {
+ return false;
+ }
+ int i = 0;
+ for (const auto& instance : this->_container)
+ {
+ if (not instance.equalsDeep(other.getInstance(i)))
+ {
+ return false;
+ }
+ i++;
+ }
+ return true;
+ }
+
+
+ std::string getKeyString() const
{
return toDateTimeMilliSeconds(this->time());
}
- std::string getLevelName() const override
+ static std::string getLevelName()
{
return "entity snapshot";
}
diff --git a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
index d7c8b8cd203fc8b140cbed0b959110f0868a4b83..9865f6da1398ac78dda2c704eff1d621b00442f4 100644
--- a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
@@ -3,10 +3,9 @@
#include <map>
#include <string>
-#include <ArmarXCore/core/logging/Logging.h>
-
#include "CoreSegmentBase.h"
#include "detail/EntityContainerBase.h"
+#include "detail/iteration_mixins.h"
namespace armarx::armem::base
@@ -17,7 +16,11 @@ namespace armarx::armem::base
*/
template <class _CoreSegmentT, class _Derived>
class MemoryBase :
- public detail::EntityContainerBase<_CoreSegmentT, typename _CoreSegmentT::ProviderSegmentT::EntityT, _Derived>
+ public detail::EntityContainerBase<_CoreSegmentT, typename _CoreSegmentT::ProviderSegmentT::EntityT, _Derived>,
+ public detail::ForEachEntityInstanceMixin<_Derived>,
+ public detail::ForEachEntitySnapshotMixin<_Derived>,
+ public detail::ForEachEntityMixin<_Derived>,
+ public detail::ForEachProviderSegmentMixin<_Derived>
{
using Base = detail::EntityContainerBase<_CoreSegmentT, typename _CoreSegmentT::ProviderSegmentT::EntityT, _Derived>;
@@ -57,11 +60,11 @@ namespace armarx::armem::base
MemoryBase()
{
}
- MemoryBase(const std::string& name) :
+ explicit MemoryBase(const std::string& name) :
MemoryBase(MemoryID().withMemoryName(name))
{
}
- MemoryBase(const MemoryID& id) :
+ explicit MemoryBase(const MemoryID& id) :
Base(id)
{
}
@@ -72,6 +75,8 @@ namespace armarx::armem::base
MemoryBase& operator=(MemoryBase&& other) = default;
+ // READ ACCESS
+
inline const std::string& name() const
{
return this->id().memoryName;
@@ -82,19 +87,14 @@ namespace armarx::armem::base
}
- inline auto& coreSegments() const
- {
- return this->_container;
- }
- inline auto& coreSegments()
+ bool hasCoreSegment(const std::string& name) const
{
- return this->_container;
+ return this->_container.count(name) > 0;
}
-
- bool hasCoreSegment(const std::string& name) const
+ std::vector<std::string> getCoreSegmentNames() const
{
- return this->_container.count(name) > 0;
+ return simox::alg::get_keys(this->_container);
}
CoreSegmentT& getCoreSegment(const std::string& name)
@@ -141,13 +141,13 @@ namespace armarx::armem::base
using Base::getEntity;
- const EntityT& getEntity(const MemoryID& id) const override
+ const EntityT& getEntity(const MemoryID& id) const
{
this->_checkContainerName(id.memoryName, this->name());
return getCoreSegment(id.coreSegmentName).getEntity(id);
}
- const EntityT* findEntity(const MemoryID& id) const override
+ const EntityT* findEntity(const MemoryID& id) const
{
this->_checkContainerName(id.memoryName, this->name());
if (id.hasCoreSegmentName())
@@ -167,32 +167,66 @@ namespace armarx::armem::base
}
}
+
+ // ITERATION
+
+ /**
+ * @param func Function like: bool process(CoreSegmentT& coreSeg)
+ */
+ template <class CoreSegmentFunctionT>
+ bool forEachCoreSegment(CoreSegmentFunctionT&& func)
+ {
+ return this->forEachChild(func);
+ }
+ /**
+ * @param func Function like: bool process(const CoreSegmentT& coreSeg)
+ */
+ template <class CoreSegmentFunctionT>
+ bool forEachCoreSegment(CoreSegmentFunctionT&& func) const
+ {
+ return this->forEachChild(func);
+ }
+
+ // forEachProviderSegment() is provided by ForEachProviderSegmentMixin.
+ // forEachEntity() is provided by ForEachEntityMixin.
+ // forEachSnapshot() is provided by ForEachEntitySnapshotMixin.
+ // forEachInstance() is provided by ForEachEntityInstanceMixin.
+
+
+ [[deprecated("Direct container access is deprecated. Use forEach*() instead.")]]
+ inline auto& coreSegments() const
+ {
+ return this->_container;
+ }
+ [[deprecated("Direct container access is deprecated. Use forEach*() instead.")]]
+ inline auto& coreSegments()
+ {
+ return this->_container;
+ }
+
+
+ // MODIFICATION
+
/**
* @brief Add an empty core segment with the given name.
* @param name The core segment name.
* @param coreSegmentType The core segment type (optional).
* @return The added core segment.
*/
- CoreSegmentT& addCoreSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr coreSegmentType = nullptr)
+ CoreSegmentT& addCoreSegment(
+ const std::string& name, aron::typenavigator::ObjectNavigatorPtr coreSegmentType = nullptr)
{
- return addCoreSegment(CoreSegmentT(name, coreSegmentType));
+ return _addCoreSegment(name, name, this->id(), coreSegmentType);
}
/// Copy and insert a core segment.
CoreSegmentT& addCoreSegment(const CoreSegmentT& coreSegment)
{
- return addCoreSegment(CoreSegmentT(coreSegment));
+ return _addCoreSegment(coreSegment.name(), CoreSegmentT(coreSegment));
}
/// Move and insert a core segment.
CoreSegmentT& addCoreSegment(CoreSegmentT&& coreSegment)
{
- if (this->_container.count(coreSegment.name()) > 0)
- {
- throw armem::error::ContainerEntryAlreadyExists(coreSegment.getLevelName(), coreSegment.name(),
- this->getLevelName(), this->name());
- }
- auto it = this->_container.emplace(coreSegment.name(), std::move(coreSegment)).first;
- it->second.id().setMemoryID(this->id());
- return it->second;
+ return _addCoreSegment(coreSegment.name(), coreSegment);
}
/**
@@ -211,7 +245,7 @@ namespace armarx::armem::base
}
catch (const armem::error::ContainerEntryAlreadyExists& e)
{
- ARMARX_INFO << e.what() << "\nIgnoring multiple addition.";
+ // ARMARX_INFO << e.what() << "\nIgnoring multiple addition.";
}
}
return segments;
@@ -226,7 +260,7 @@ namespace armarx::armem::base
std::vector<UpdateResult> update(const Commit& commit)
{
std::vector<UpdateResult> results;
- for (const auto& update : commit.updates)
+ for (const EntityUpdate& update : commit.updates)
{
results.push_back(this->update(update));
}
@@ -242,43 +276,43 @@ namespace armarx::armem::base
{
this->_checkContainerName(update.entityID.memoryName, this->name());
- auto it = this->_container.find(update.entityID.coreSegmentName);
- if (it != this->_container.end())
+ auto [inserted, coreSeg] = _addCoreSegmentIfMissing(update.entityID.coreSegmentName);
+
+ // Update entry.
+ UpdateResult ret(coreSeg->update(update));
+ if (inserted)
{
- UpdateResult ret(it->second.update(update));
- ret.memoryUpdateType = UpdateType::UpdatedExisting;
- return ret;
+ ret.memoryUpdateType = UpdateType::InsertedNew;
}
else
{
- throw armem::error::MissingEntry::create<CoreSegmentT>(update.entityID.coreSegmentName, *this);
+ ret.memoryUpdateType = UpdateType::UpdatedExisting;
}
+ return ret;
}
+
/**
* @brief Merge another memory into this one. Append all data
* @param m The other memory
*/
void append(const _Derived& m)
{
- ARMARX_INFO << "Memory: Merge name '" << m.name() << "' into '" << name() << "'";
+ // ARMARX_INFO << "Memory: Merge name '" << m.name() << "' into '" << name() << "'";
- for (const auto& [k, s] : m.container())
+ m.forEachCoreSegment([this](const CoreSegmentT & coreSeg)
{
- if (const auto& it = this->_container.find(k); it != this->_container.end())
+ auto it = this->_container.find(coreSeg.name());
+ if (it == this->_container.end())
{
- // segment already exists
- it->second.append(s);
+ it = this->_container.emplace(coreSeg.name(), this->id().withCoreSegmentName(coreSeg.name())).first;
}
- else
- {
- auto wms = this->_container.emplace(k, this->id().withCoreSegmentName(k));
- wms.first->second.append(s);
- }
- }
+ it->second.append(coreSeg);
+ return true;
+ });
}
- virtual bool equalsDeep(const MemoryBase& other) const
+ bool equalsDeep(const MemoryBase& other) const
{
//std::cout << "Memory::equalsDeep" << std::endl;
if (this->size() != other.size())
@@ -299,14 +333,96 @@ namespace armarx::armem::base
return true;
}
- std::string getLevelName() const override
+ static std::string getLevelName()
{
return "memory";
}
- std::string getKeyString() const override
+ std::string getKeyString() const
{
return this->name();
}
+
+ std::string dump() const
+ {
+ std::stringstream ss;
+ ss << "Memory '" << this->name() << "': \n";
+ forEachCoreSegment([&ss](const CoreSegmentT & cseg)
+ {
+ ss << "+- Core segment '" << cseg.name() << "' (" << cseg.size() << "): \n";
+ cseg.forEachProviderSegment([&ss](const ProviderSegmentT & pseg)
+ {
+ ss << "| +- Provider segment '" << pseg.name() << "' (" << pseg.size() << "): \n";
+ pseg.forEachEntity([&ss](const EntityT & entity)
+ {
+ ss << "| | +- Entity '" << entity.name() << "' (" << entity.size() << "): \n";
+ return true;
+ });
+ return true;
+ });
+ return true;
+ });
+ return ss.str();
+ }
+
+
+ private:
+
+ /**
+ * This function allows to emplace a CoreSegment directly in the
+ * container from its constructor arguments, instead of constructing
+ * it outside and moving it.
+ * This is necessary if CoreSegmentT is not movable.
+ */
+ template <class ...Args>
+ CoreSegmentT& _addCoreSegment(const std::string& name, Args... args)
+ {
+ auto [it, inserted] = this->_container.try_emplace(name, args...);
+ if (not inserted)
+ {
+ throw armem::error::ContainerEntryAlreadyExists(
+ CoreSegmentT::getLevelName(), name, DerivedT::getLevelName(), this->name());
+ }
+ else
+ {
+ it->second.id().setMemoryID(this->id());
+ it->second.id().coreSegmentName = name;
+ return it->second;
+ }
+ }
+
+
+ std::pair<bool, CoreSegmentT*> _addCoreSegmentIfMissing(const std::string& coreSegmentName)
+ {
+ CoreSegmentT* coreSeg;
+
+ auto it = this->_container.find(coreSegmentName);
+ if (it == this->_container.end())
+ {
+ if (_addMissingCoreSegmentDuringUpdate)
+ {
+ // Insert into map.
+ coreSeg = &addCoreSegment(coreSegmentName);
+ return {true, coreSeg};
+ }
+ else
+ {
+ throw error::MissingEntry::create<CoreSegmentT>(coreSegmentName, *this);
+ }
+ }
+ else
+ {
+ coreSeg = &it->second;
+ return {false, coreSeg};
+ }
+ }
+
+
+ public:
+
+ // ToDo: Remove from base structure - this should be specific to the server versions.
+ // If necessary at all.
+ bool _addMissingCoreSegmentDuringUpdate = false;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
index 57ff3210244fa516551f4906a2ea07c2cc81f09d..e5163241738a0e8dbba5edbd35f8e5a568060746 100644
--- a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
@@ -6,7 +6,7 @@
#include "EntityBase.h"
#include "detail/AronTyped.h"
#include "detail/EntityContainerBase.h"
-#include "detail/MaxHistorySize.h"
+#include "detail/iteration_mixins.h"
namespace armarx::armem::base
@@ -18,8 +18,9 @@ namespace armarx::armem::base
template <class _EntityT, class _Derived>
class ProviderSegmentBase :
public detail::EntityContainerBase<_EntityT, _EntityT, _Derived>,
- public detail::MaxHistorySize,
- public detail::AronTyped
+ public detail::AronTyped,
+ public detail::ForEachEntityInstanceMixin<_Derived>,
+ public detail::ForEachEntitySnapshotMixin<_Derived>
{
using Base = detail::EntityContainerBase<_EntityT, _EntityT, _Derived>;
@@ -53,15 +54,15 @@ namespace armarx::armem::base
{
}
- ProviderSegmentBase(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
- ProviderSegmentBase(MemoryID().withProviderSegmentName(name), aronType)
+ explicit ProviderSegmentBase(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
+ ProviderSegmentBase(name, MemoryID(), aronType)
{
}
- ProviderSegmentBase(const std::string& name, const MemoryID parentID, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
+ explicit ProviderSegmentBase(const std::string& name, const MemoryID parentID, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
ProviderSegmentBase(parentID.withProviderSegmentName(name), aronType)
{
}
- ProviderSegmentBase(const MemoryID id, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
+ explicit ProviderSegmentBase(const MemoryID id, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
Base(id),
AronTyped(aronType)
{
@@ -73,6 +74,8 @@ namespace armarx::armem::base
ProviderSegmentBase& operator=(ProviderSegmentBase&& other) = default;
+ // READ ACCESS
+
inline const std::string& name() const
{
return this->id().providerSegmentName;
@@ -83,23 +86,18 @@ namespace armarx::armem::base
}
- inline const auto& entities() const
- {
- return this->_container;
- }
- inline auto& entities()
+ bool hasEntity(const std::string& name) const
{
- return this->_container;
+ return this->_container.count(name) > 0;
}
-
- bool hasEntity(const std::string& name) const
+ std::vector<std::string> getEntityNames() const
{
- return this->_container.count(name) > 0;
+ return simox::alg::get_keys(this->_container);
}
using Base::getEntity;
- const EntityT& getEntity(const MemoryID& id) const override
+ const EntityT& getEntity(const MemoryID& id) const
{
this->_checkContainerName(id.providerSegmentName, this->getKeyString());
return getEntity(id.entityName);
@@ -123,7 +121,7 @@ namespace armarx::armem::base
}
}
- const EntityT* findEntity(const MemoryID& id) const override
+ const EntityT* findEntity(const MemoryID& id) const
{
this->_checkContainerName(id.providerSegmentName, this->getKeyString());
auto it = this->_container.find(id.entityName);
@@ -137,6 +135,46 @@ namespace armarx::armem::base
}
}
+
+ // ITERATION
+
+ // All functors are taken as universal reference (F&&).
+
+ /**
+ * @param func Function like: bool process(EntityT& entity)
+ */
+ template <class EntityFunctionT>
+ bool forEachEntity(EntityFunctionT&& func)
+ {
+ return this->forEachChild(func);
+ }
+ /**
+ * @param func Function like: bool process(const EntityT& entity)
+ */
+ template <class EntityFunctionT>
+ bool forEachEntity(EntityFunctionT&& func) const
+ {
+ return this->forEachChild(func);
+ }
+
+ // forEachSnapshot() is provided by ForEachEntitySnapshotMixin.
+ // forEachInstance() is provided by ForEachEntityInstanceMixin.
+
+
+ [[deprecated("Direct container access is deprecated. Use forEach*() instead.")]]
+ inline const ContainerT& entities() const
+ {
+ return this->_container;
+ }
+ [[deprecated("Direct container access is deprecated. Use forEach*() instead.")]]
+ inline ContainerT& entities()
+ {
+ return this->_container;
+ }
+
+
+ // MODIFICATION
+
/**
* @brief Updates an entity's history.
*
@@ -154,7 +192,6 @@ namespace armarx::armem::base
{
// Add entity entry.
entity = &addEntity(update.entityID.entityName);
- entity->setMaxHistorySize(_maxHistorySize);
updateType = UpdateType::InsertedNew;
}
else
@@ -170,27 +207,24 @@ namespace armarx::armem::base
void append(const _Derived& m)
{
- ARMARX_INFO << "ProviderSegment: Merge name '" << m.name() << "' into '" << name() << "'";
+ // ARMARX_INFO << "ProviderSegment: Merge name '" << m.name() << "' into '" << name() << "'";
- for (const auto& [k, s] : m.container())
+ m.forEachEntity([this](const EntityT & entity)
{
- if (const auto& it = this->_container.find(k); it != this->_container.end())
+ auto it = this->_container.find(entity.name());
+ if (it == this->_container.end())
{
- // segment already exists
- it->second.append(s);
+ it = this->_container.emplace(entity.name(), this->id().withEntityName(entity.name())).first;
}
- else
- {
- auto wms = this->_container.emplace(k, this->id().withEntityName(k));
- wms.first->second.append(s);
- }
- }
+ it->second.append(entity);
+ return true;
+ });
}
/// Add an empty entity with the given name.
EntityT& addEntity(const std::string& name)
{
- return addEntity(EntityT(name));
+ return addEntity(EntityT(name, this->id()));
}
/// Copy and insert an entity.
EntityT& addEntity(const EntityT& entity)
@@ -206,22 +240,9 @@ namespace armarx::armem::base
}
- /**
- * @brief Sets the maximum history size of container in this segment.
- * This affects all current container as well as new ones.
- * @see Entity::setMaxHistorySize()
- */
- void setMaxHistorySize(long maxSize) override
- {
- MaxHistorySize::setMaxHistorySize(maxSize);
- for (auto& [name, entity] : this->_container)
- {
- entity.setMaxHistorySize(maxSize);
- }
- }
+ // MISC
-
- virtual bool equalsDeep(const ProviderSegmentBase& other) const
+ bool equalsDeep(const DerivedT& other) const
{
//std::cout << "ProviderSegment::equalsDeep" << std::endl;
if (this->size() != other.size())
@@ -243,30 +264,17 @@ namespace armarx::armem::base
return true;
}
- std::string getLevelName() const override
+
+ static std::string getLevelName()
{
return "provider segment";
}
- std::string getKeyString() const override
+ std::string getKeyString() const
{
return this->name();
}
-
- protected:
-
- virtual void _copySelf(DerivedT& other) const override
- {
- Base::_copySelf(other);
- other.aronType() = _aronType;
- }
- virtual void _copySelfEmpty(DerivedT& other) const override
- {
- Base::_copySelfEmpty(other);
- other.aronType() = _aronType;
- }
-
};
}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.cpp b/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.cpp
index ec7ca0402ad128dc6d342aa28279929c376df557..48147758528cb5ac46baa8a952272ee831ed1e01 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.cpp
@@ -1,5 +1,8 @@
#include "AronTyped.h"
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+
+
namespace armarx::armem::base::detail
{
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.h b/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.h
index fb0ad088fb4fb27ca39cf09f034ea7eb3b202cfe..16233556c83f8febb732824a7001c9b2e8d1e2ac 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.h
@@ -1,6 +1,6 @@
#pragma once
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h>
namespace armarx::armem::base::detail
@@ -13,7 +13,7 @@ namespace armarx::armem::base::detail
{
public:
- AronTyped(aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
+ explicit AronTyped(aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
bool hasAronType() const;
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h b/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h
index 95993d560c6435fd147ddfc7a2a7aa9267d94855..53cddfff656cdf0fb000323afc4aefcc18c2f50f 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h
@@ -1,12 +1,12 @@
#pragma once
-#include "../../Commit.h"
-#include "../../error/ArMemError.h"
+#include <RobotAPI/libraries/armem/core/Commit.h>
+#include <RobotAPI/libraries/armem/core/error/ArMemError.h>
#include "MemoryContainerBase.h"
-#include "../EntityBase.h"
-#include "../EntitySnapshotBase.h"
+#include <RobotAPI/libraries/armem/core/base/EntityBase.h>
+#include <RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h>
namespace armarx::armem::base::detail
@@ -34,8 +34,9 @@ namespace armarx::armem::base::detail
public:
- using Base::MemoryContainerBase;
- using Base::operator=;
+ // `using Base::MemoryContainerBase` breaks code completion of QtCreator.
+ using MemoryContainerBase<std::map<std::string, _ValueT>, _Derived>::MemoryContainerBase;
+ using MemoryContainerBase<std::map<std::string, _ValueT>, _Derived>::operator=;
/**
* @brief Retrieve an entity.
@@ -43,11 +44,11 @@ namespace armarx::armem::base::detail
* @return The entity.
* @throw An exception deriving from `armem::error::ArMemError` if the entity is missing.
*/
+ // const EntityT& getEntity(const MemoryID& id) const;
EntityT& getEntity(const MemoryID& id)
{
- return const_cast<EntityT&>(const_cast<const EntityContainerBase*>(this)->getEntity(id));
+ return const_cast<EntityT&>(const_cast<const EntityContainerBase*>(this)->_derived().getEntity(id));
}
- virtual const EntityT& getEntity(const MemoryID& id) const = 0;
/**
* @brief Find an entity.
@@ -61,7 +62,11 @@ namespace armarx::armem::base::detail
* @param id The entities ID.
* @return A pointer to the first matching entity or `nullptr` if none was found.
*/
- virtual const EntityT* findEntity(const MemoryID& id) const = 0;
+ // const EntityT* findEntity(const MemoryID& id) const;
+ EntityT* findEntity(const MemoryID& id)
+ {
+ return const_cast<EntityT*>(const_cast<const EntityContainerBase*>(this)->_derived().findEntity(id));
+ }
/**
@@ -75,12 +80,12 @@ namespace armarx::armem::base::detail
*/
EntitySnapshotT& getEntitySnapshot(const MemoryID& id)
{
- return const_cast<EntitySnapshotT&>(const_cast<const EntityContainerBase*>(this)->getEntitySnapshot(id));
+ return const_cast<EntitySnapshotT&>(const_cast<const EntityContainerBase*>(this)->_derived().getEntitySnapshot(id));
}
const EntitySnapshotT& getEntitySnapshot(const MemoryID& id) const
{
- const EntityT& entity = getEntity(id);
+ const EntityT& entity = _derived().getEntity(id);
if (id.hasTimestamp())
{
@@ -101,6 +106,19 @@ namespace armarx::armem::base::detail
{
return getEntitySnapshot(id).getInstance(id);
}
+
+
+ private:
+
+ DerivedT& _derived()
+ {
+ return static_cast<DerivedT&>(*this);
+ }
+ const DerivedT& _derived() const
+ {
+ return static_cast<const DerivedT&>(*this);
+ }
+
};
}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h b/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h
deleted file mode 100644
index 1fc07ed85e54716d24d2447fa17cb95b291f2766..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h
+++ /dev/null
@@ -1,35 +0,0 @@
-#pragma once
-
-
-
-namespace armarx::armem::base::detail
-{
- // TODO: Replace by ConstrainedHistorySize (not only max entries, e.g. delete oldest / delete least accessed / ...)
- class MaxHistorySize
- {
- public:
-
- virtual ~MaxHistorySize();
-
- /**
- * @brief Sets the maximum history size of entities in this segment.
- * This affects all current entities as well as new ones.
- * @see Entity::setMaxHistorySize()
- */
- virtual void setMaxHistorySize(long maxSize);
- virtual long getMaxHistorySize() const;
-
-
- protected:
-
- /**
- * @brief Maximum size of entity histories.
- *
- * If negative, the size of `history` is not limited.
- *
- * @see Entity::maxHstorySize
- */
- long _maxHistorySize = -1;
-
- };
-}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h b/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
index 1a6a216653d243da9613ec967363dc5d44b5c503..4d7cbec1877176f511e854eeefa60a9d6159aded 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
@@ -3,6 +3,7 @@
#include <RobotAPI/libraries/armem/core/error/ArMemError.h>
#include "MemoryItem.h"
+#include "iteration_mixins.h"
namespace armarx::armem::base::detail
@@ -18,16 +19,16 @@ namespace armarx::armem::base::detail
using Base = MemoryItem;
public:
+
using DerivedT = _DerivedT;
using ContainerT = _ContainerT;
public:
-
MemoryContainerBase()
{}
- MemoryContainerBase(const MemoryID& id) :
+ explicit MemoryContainerBase(const MemoryID& id) :
MemoryItem(id)
{
}
@@ -48,32 +49,56 @@ namespace armarx::armem::base::detail
{
return _container.size();
}
- bool hasData() const
- {
- return size() > 0;
- }
void clear()
{
return _container.clear();
}
+
+ // ITERATION
+
+ /**
+ * @param func Function like: bool process(ChildT& provSeg)
+ */
+ template <class ChildFunctionT>
+ bool forEachChild(ChildFunctionT&& func)
+ {
+ return base::detail::forEachChild(this->_container, func);
+ }
+ /**
+ * @param func Function like: bool process(const ChildT& provSeg)
+ */
+ template <class ChildFunctionT>
+ bool forEachChild(ChildFunctionT&& func) const
+ {
+ return base::detail::forEachChild(this->_container, func);
+ }
+
+
+ [[deprecated("Direct container access is deprecated. Use forEach*() instead.")]]
typename ContainerT::const_iterator begin() const
{
return _container.begin();
}
+ [[deprecated("Direct container access is deprecated. Use forEach*() instead.")]]
typename ContainerT::iterator begin()
{
return _container.begin();
}
+ [[deprecated("Direct container access is deprecated. Use forEach*() instead.")]]
typename ContainerT::const_iterator end() const
{
return _container.end();
}
+ [[deprecated("Direct container access is deprecated. Use forEach*() instead.")]]
typename ContainerT::iterator end()
{
return _container.end();
}
+
+ protected:
+
const ContainerT& container() const
{
return _container;
@@ -83,24 +108,6 @@ namespace armarx::armem::base::detail
return _container;
}
- // Copying
- DerivedT copy() const
- {
- DerivedT d;
- this->_copySelf(d);
- return d;
- }
-
- /// Make a copy not containing any elements.
- DerivedT copyEmpty() const
- {
- DerivedT d;
- this->_copySelfEmpty(d);
- return d;
- }
-
-
- protected:
/**
* @throw `armem::error::ContainerNameMismatch` if `gottenName` does not match `actualName`.
@@ -110,21 +117,11 @@ namespace armarx::armem::base::detail
{
if (!((emptyOk && gottenName.empty()) || gottenName == actualName))
{
- throw armem::error::ContainerNameMismatch(gottenName, this->getLevelName(), actualName);
+ throw armem::error::ContainerNameMismatch(
+ gottenName, DerivedT::getLevelName(), actualName);
}
}
- virtual void _copySelf(DerivedT& other) const
- {
- Base::_copySelf(other);
- other.container() = _container;
- }
-
- virtual void _copySelfEmpty(DerivedT& other) const
- {
- Base::_copySelf(other);
- }
-
protected:
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.cpp b/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.cpp
index a6a9f73e17443091503c09472bc3cee7259644ac..8f4a1920f0a1fcd669b9c3fc2193a617b4dba205 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.cpp
@@ -8,33 +8,15 @@ namespace armarx::armem::base::detail
{
}
+
MemoryItem::MemoryItem(const MemoryID& id) :
_id(id)
{
}
- /*
- MemoryItem::MemoryItem(const MemoryItem& other) :
- _id(other.id())
- {}
- */
-
MemoryItem::~MemoryItem()
{
}
- /*
- MemoryItem& MemoryItem::operator=(const MemoryItem& other)
- {
- other._copySelf(*this);
- return *this;
- }
- */
-
- void MemoryItem::_copySelf(MemoryItem& other) const
- {
- other.id() = id();
- }
-
}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.h b/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.h
index 45226e6327520b7bbddc198364c16c2b8229352c..6aea07121aa201fcbbf0f0b18f46ccc3ab4e2aac 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.h
@@ -2,7 +2,7 @@
#include <string>
-#include "../../MemoryID.h"
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
namespace armarx::armem::base::detail
@@ -16,18 +16,13 @@ namespace armarx::armem::base::detail
public:
MemoryItem();
- MemoryItem(const MemoryID& id);
+ explicit MemoryItem(const MemoryID& id);
MemoryItem(const MemoryItem& other) = default;
MemoryItem(MemoryItem&& other) = default;
MemoryItem& operator=(const MemoryItem& other) = default;
MemoryItem& operator=(MemoryItem&& other) = default;
- virtual ~MemoryItem();
-
-
- //MemoryItem& operator=(const MemoryItem& other);
-
inline MemoryID& id()
{
@@ -39,19 +34,11 @@ namespace armarx::armem::base::detail
}
- // Introspection
-
- /// Get a string version of `*this`' key.
- virtual std::string getKeyString() const = 0;
-
- /// Get a readable name of this level for messages, errors etc.
- virtual std::string getLevelName() const = 0;
-
-
protected:
- /// Copy `*this` to `other`.
- virtual void _copySelf(MemoryItem& other) const;
+ // Protected so we get a compile error if someone tries to destruct a MemoryItem
+ // instead of a derived type.
+ ~MemoryItem();
protected:
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.cpp b/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..459cd4d5c7bd694bcb289496352d7e2cad4966cd
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.cpp
@@ -0,0 +1,7 @@
+#include "iteration_mixins.h"
+
+
+namespace armarx::armem::base::detail
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h b/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h
new file mode 100644
index 0000000000000000000000000000000000000000..c923a857e1d016b06b313f7e860ab57ec438a9d4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h
@@ -0,0 +1,214 @@
+#pragma once
+
+#include <type_traits>
+
+
+namespace armarx::armem::base::detail
+{
+
+ // Helper functions to implement the forEach*() method at the current level.
+
+ // Handle functions with different return type.
+ template <class FunctionT, class ChildT>
+ bool call(FunctionT&& func, ChildT&& child)
+ {
+ if constexpr(std::is_same_v<decltype(func(child)), bool>)
+ {
+ if (!func(child))
+ {
+ return false;
+ }
+ return true;
+ }
+ else
+ {
+ func(child);
+ return true;
+ }
+ }
+
+
+ // Single-valued containers.
+ template <class ContainerT, class FunctionT>
+ bool forEachChildSingle(ContainerT& container, FunctionT&& func)
+ {
+ for (auto& child : container)
+ {
+ if (not call(func, child))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+
+ // Pair-valued containers.
+ template <class ContainerT, class FunctionT>
+ bool forEachChildPair(ContainerT& container, FunctionT&& func)
+ {
+ for (auto& [_, child] : container)
+ {
+ if (not call(func, child))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+
+ // see: https://en.cppreference.com/w/cpp/types/void_t
+
+ // primary template handles types that have no nested ::type member:
+ template< class, class = void >
+ struct has_mapped_type : std::false_type { };
+
+ // specialization recognizes types that do have a nested ::type member:
+ template< class T >
+ struct has_mapped_type<T, std::void_t<typename T::mapped_type>> : std::true_type { };
+
+
+ template <class ContainerT, class FunctionT>
+ bool forEachChild(ContainerT& container, FunctionT&& func)
+ {
+ if constexpr(has_mapped_type<ContainerT>::value)
+ {
+ return forEachChildPair(container, func);
+ }
+ else
+ {
+ return forEachChildSingle(container, func);
+ }
+ }
+
+
+ // We use auto instead of, e.g. DerivedT::EntitySnapshotT,
+ // as we cannot use the typedef before DerivedT was completely defined.
+
+ template <class DerivedT>
+ struct ForEachEntityInstanceMixin
+ {
+ // not: using EntitySnapshotT = typename DerivedT::EntitySnapshotT;
+
+ /**
+ * @param func Function like: bool process(EntityInstanceT& instance)>
+ */
+ template <class InstanceFunctionT>
+ bool forEachInstance(InstanceFunctionT&& func)
+ {
+ return static_cast<DerivedT*>(this)->forEachSnapshot(
+ [&func](auto & snapshot) -> bool
+ {
+ return snapshot.forEachInstance(func);
+ });
+ }
+
+ /**
+ * @param func Function like: bool process(const EntityInstanceT& instance)
+ */
+ template <class InstanceFunctionT>
+ bool forEachInstance(InstanceFunctionT&& func) const
+ {
+ return static_cast<const DerivedT*>(this)->forEachSnapshot(
+ [&func](const auto & snapshot) -> bool
+ {
+ return snapshot.forEachInstance(func);
+ });
+ }
+ };
+
+
+ template <class DerivedT>
+ struct ForEachEntitySnapshotMixin
+ {
+ /**
+ * @param func Function like: bool process(EntitySnapshotT& snapshot)>
+ */
+ template <class SnapshotFunctionT>
+ bool forEachSnapshot(SnapshotFunctionT&& func)
+ {
+ return static_cast<DerivedT*>(this)->forEachEntity(
+ [&func](auto & entity) -> bool
+ {
+ return entity.forEachSnapshot(func);
+ });
+ }
+
+ /**
+ * @param func Function like: bool process(const EntitySnapshotT& snapshot)
+ */
+ template <class SnapshotFunctionT>
+ bool forEachSnapshot(SnapshotFunctionT&& func) const
+ {
+ return static_cast<const DerivedT*>(this)->forEachEntity(
+ [&func](const auto & entity) -> bool
+ {
+ return entity.forEachSnapshot(func);
+ });
+ }
+ };
+
+
+ template <class DerivedT>
+ struct ForEachEntityMixin
+ {
+ /**
+ * @param func Function like: bool process(EntityT& entity)>
+ */
+ template <class FunctionT>
+ bool forEachEntity(FunctionT&& func)
+ {
+ return static_cast<DerivedT*>(this)->forEachProviderSegment(
+ [&func](auto & providerSegment) -> bool
+ {
+ return providerSegment.forEachEntity(func);
+ });
+ }
+
+ /**
+ * @param func Function like: bool process(const EntityT& entity)
+ */
+ template <class FunctionT>
+ bool forEachEntity(FunctionT&& func) const
+ {
+ return static_cast<const DerivedT*>(this)->forEachProviderSegment(
+ [&func](const auto & providerSegment) -> bool
+ {
+ return providerSegment.forEachEntity(func);
+ });
+ }
+ };
+
+
+ template <class DerivedT>
+ struct ForEachProviderSegmentMixin
+ {
+ /**
+ * @param func Function like: bool process(ProviderSegmentT& providerSegment)>
+ */
+ template <class FunctionT>
+ bool forEachProviderSegment(FunctionT&& func)
+ {
+ return static_cast<DerivedT*>(this)->forEachCoreSegment(
+ [&func](auto & coreSegment) -> bool
+ {
+ return coreSegment.forEachProviderSegment(func);
+ });
+ }
+
+ /**
+ * @param func Function like: bool process(const ProviderSegmentT& providerSegment)
+ */
+ template <class FunctionT>
+ bool forEachProviderSegment(FunctionT&& func) const
+ {
+ return static_cast<const DerivedT*>(this)->forEachCoreSegment(
+ [&func](const auto & coreSegment) -> bool
+ {
+ return coreSegment.forEachProviderSegment(func);
+ });
+ }
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.cpp b/source/RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a1875ff53545a3d2574cf9764a14d321946e5514
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.cpp
@@ -0,0 +1,17 @@
+#include "negative_index_semantics.h"
+
+#include <algorithm>
+
+
+size_t armarx::armem::base::detail::negativeIndexSemantics(long index, size_t size)
+{
+ const size_t max = size > 0 ? size - 1 : 0;
+ if (index >= 0)
+ {
+ return std::clamp<size_t>(static_cast<size_t>(index), 0, max);
+ }
+ else
+ {
+ return static_cast<size_t>(std::clamp<long>(static_cast<long>(size) + index, 0, static_cast<long>(max)));
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.h b/source/RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.h
new file mode 100644
index 0000000000000000000000000000000000000000..4efe169276c83ff12c13400e6de162a8b4db5d8d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.h
@@ -0,0 +1,11 @@
+#pragma once
+
+#include <stddef.h>
+
+
+namespace armarx::armem::base::detail
+{
+
+ size_t negativeIndexSemantics(long index, size_t size);
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/ice_conversions.cpp b/source/RobotAPI/libraries/armem/core/base/ice_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..48c084154c41b26b174504d2565fe57d3a49d55a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/ice_conversions.cpp
@@ -0,0 +1,78 @@
+#include "ice_conversions.h"
+
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions_templates.h>
+
+
+namespace armarx::armem::base
+{
+
+ void detail::toIceItem(data::detail::MemoryItem& ice, const armem::base::detail::MemoryItem& item)
+ {
+ toIce(ice.id, item.id());
+ }
+ void detail::fromIceItem(const data::detail::MemoryItem& ice, armem::base::detail::MemoryItem& item)
+ {
+ fromIce(ice.id, item.id());
+ }
+
+
+ void detail::toIce(aron::data::AronDictPtr& ice, const aron::datanavigator::DictNavigatorPtr& data)
+ {
+ ice = data ? data->toAronDictPtr() : nullptr;
+ }
+ void detail::fromIce(const aron::data::AronDictPtr& ice, aron::datanavigator::DictNavigatorPtr& data)
+ {
+ if (ice)
+ {
+ data = aron::datanavigator::DictNavigator::FromAronDictPtr(ice);
+ }
+ else
+ {
+ data = nullptr;
+ };
+ }
+
+ void detail::toIce(aron::type::AronTypePtr& ice, const aron::typenavigator::ObjectNavigatorPtr& bo)
+ {
+ ice = bo ? bo->toAronPtr() : nullptr;
+ }
+ void detail::fromIce(const aron::type::AronTypePtr& ice, aron::typenavigator::ObjectNavigatorPtr& bo)
+ {
+ bo = ice
+ ? aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(aron::typenavigator::Navigator::FromAronType(ice))
+ : nullptr;
+ }
+
+}
+namespace armarx::armem
+{
+ void base::toIce(data::EntityInstanceMetadata& ice, const EntityInstanceMetadata& metadata)
+ {
+ ice.confidence = metadata.confidence;
+ toIce(ice.timeArrivedMicroSeconds, metadata.timeArrived);
+ toIce(ice.timeCreatedMicroSeconds, metadata.timeCreated);
+ toIce(ice.timeSentMicroSeconds, metadata.timeSent);
+ }
+ void base::fromIce(const data::EntityInstanceMetadata& ice, EntityInstanceMetadata& metadata)
+ {
+ metadata.confidence = ice.confidence;
+ fromIce(ice.timeArrivedMicroSeconds, metadata.timeArrived);
+ fromIce(ice.timeCreatedMicroSeconds, metadata.timeCreated);
+ fromIce(ice.timeSentMicroSeconds, metadata.timeSent);
+ }
+
+
+ void base::toIce(data::EntityInstanceMetadataPtr& ice, const EntityInstanceMetadata& metadata)
+ {
+ armem::toIce<data::EntityInstanceMetadata>(ice, metadata);
+ }
+ void base::fromIce(const data::EntityInstanceMetadataPtr& ice, EntityInstanceMetadata& metadata)
+ {
+ armem::fromIce<data::EntityInstanceMetadata>(ice, metadata);
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/ice_conversions.h b/source/RobotAPI/libraries/armem/core/base/ice_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..646754cc4cbfc3d61c0ce0b1e19c333ee2d7f92b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/ice_conversions.h
@@ -0,0 +1,195 @@
+#pragma once
+
+#include "EntityInstanceBase.h"
+#include "EntitySnapshotBase.h"
+#include "EntityBase.h"
+#include "ProviderSegmentBase.h"
+#include "CoreSegmentBase.h"
+#include "MemoryBase.h"
+
+#include <RobotAPI/libraries/armem/core/ice_conversions_templates.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+
+#include <RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h>
+
+#include <RobotAPI/interface/armem/memory.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx::armem::base::detail
+{
+ void toIceItem(data::detail::MemoryItem& ice, const MemoryItem& item);
+ void fromIceItem(const data::detail::MemoryItem& ice, MemoryItem& item);
+
+ void toIce(aron::data::AronDictPtr& ice, const aron::datanavigator::DictNavigatorPtr& bo);
+ void fromIce(const aron::data::AronDictPtr& ice, aron::datanavigator::DictNavigatorPtr& bo);
+
+ void toIce(aron::type::AronTypePtr& ice, const aron::typenavigator::ObjectNavigatorPtr& bo);
+ void fromIce(const aron::type::AronTypePtr& ice, aron::typenavigator::ObjectNavigatorPtr& bo);
+}
+namespace armarx::armem::base
+{
+ void toIce(data::EntityInstanceMetadata& ice, const EntityInstanceMetadata& metadata);
+ void fromIce(const data::EntityInstanceMetadata& ice, EntityInstanceMetadata& metadata);
+
+ void toIce(data::EntityInstanceMetadataPtr& ice, const EntityInstanceMetadata& metadata);
+ void fromIce(const data::EntityInstanceMetadataPtr& ice, EntityInstanceMetadata& metadata);
+
+
+ template <class ...Args>
+ void toIce(data::EntityInstance& ice, const EntityInstanceBase<Args...>& data)
+ {
+ detail::toIceItem(ice, data);
+
+ // detail::toIce(std::ref(ice.data).get(), data.data());
+ detail::toIce(ice.data, data.data());
+ toIce(ice.metadata, data.metadata());
+ }
+ template <class ...Args>
+ void fromIce(const data::EntityInstance& ice, EntityInstanceBase<Args...>& data)
+ {
+ detail::fromIceItem(ice, data);
+
+ detail::fromIce(ice.data, data.data());
+ fromIce(ice.metadata, data.metadata());
+ }
+
+ template <class ...Args>
+ void toIce(data::EntitySnapshot& ice, const EntitySnapshotBase<Args...>& snapshot)
+ {
+ detail::toIceItem(ice, snapshot);
+
+ ice.instances.clear();
+ snapshot.forEachInstance([&ice](const auto & instance)
+ {
+ armem::toIce(ice.instances.emplace_back(), instance);
+ });
+ }
+ template <class ...Args>
+ void fromIce(const data::EntitySnapshot& ice, EntitySnapshotBase<Args...>& snapshot)
+ {
+ detail::fromIceItem(ice, snapshot);
+
+ snapshot.clear();
+ for (const data::EntityInstancePtr& iceInstance : ice.instances)
+ {
+ snapshot.addInstance(armem::fromIce<typename EntitySnapshotBase<Args...>::EntityInstanceT>(iceInstance));
+ }
+ }
+
+ template <class ...Args>
+ void toIce(data::Entity& ice, const EntityBase<Args...>& entity)
+ {
+ detail::toIceItem(ice, entity);
+
+ ice.history.clear();
+ entity.forEachSnapshot([&ice](const auto & snapshot)
+ {
+ armem::toIce(ice.history[armem::toIce<long>(snapshot.time())], snapshot);
+ });
+ }
+ template <class ...Args>
+ void fromIce(const data::Entity& ice, EntityBase<Args...>& entity)
+ {
+ detail::fromIceItem(ice, entity);
+
+ entity.clear();
+ for (const auto& [key, snapshot] : ice.history)
+ {
+ entity.addSnapshot(armem::fromIce<typename EntityBase<Args...>::EntitySnapshotT>(snapshot));
+ }
+ }
+
+
+ template <class ...Args>
+ void toIce(data::ProviderSegment& ice, const ProviderSegmentBase<Args...>& providerSegment)
+ {
+ detail::toIceItem(ice, providerSegment);
+
+ detail::toIce(ice.aronType, providerSegment.aronType());
+ ARMARX_CHECK(!providerSegment.aronType() || ice.aronType);
+
+ // toIce(ice.entities, providerSegment.entities());
+ ice.entities.clear();
+ providerSegment.forEachEntity([&ice](const auto & entity)
+ {
+ armem::toIce(ice.entities[entity.name()], entity);
+ });
+ }
+ template <class ...Args>
+ void fromIce(const data::ProviderSegment& ice, ProviderSegmentBase<Args...>& providerSegment)
+ {
+ detail::fromIceItem(ice, providerSegment);
+
+ detail::fromIce(ice.aronType, providerSegment.aronType());
+ ARMARX_CHECK(!providerSegment.aronType() || ice.aronType);
+
+ // fromIce(ice.entities, providerSegment.entities());
+ providerSegment.clear();
+ for (const auto& [key, entity] : ice.entities)
+ {
+ providerSegment.addEntity(
+ armem::fromIce<typename ProviderSegmentBase<Args...>::EntityT>(entity));
+ }
+ }
+
+ template <class ...Args>
+ void toIce(data::CoreSegment& ice, const CoreSegmentBase<Args...>& coreSegment)
+ {
+ detail::toIceItem(ice, coreSegment);
+
+ detail::toIce(ice.aronType, coreSegment.aronType());
+ ARMARX_CHECK(!coreSegment.aronType() || ice.aronType);
+
+ // toIce(ice.providerSegments, coreSegment.providerSegments());
+ ice.providerSegments.clear();
+ coreSegment.forEachProviderSegment([&ice](const auto & providerSegment)
+ {
+ armem::toIce(ice.providerSegments[providerSegment.name()], providerSegment);
+ return true;
+ });
+ }
+ template <class ...Args>
+ void fromIce(const data::CoreSegment& ice, CoreSegmentBase<Args...>& coreSegment)
+ {
+ detail::fromIceItem(ice, coreSegment);
+
+ detail::fromIce(ice.aronType, coreSegment.aronType());
+ ARMARX_CHECK(!coreSegment.aronType() || ice.aronType);
+
+ // fromIce(ice.providerSegments, coreSegment.providerSegments());
+ coreSegment.clear();
+ for (const auto& [key, providerSegment] : ice.providerSegments)
+ {
+ coreSegment.addProviderSegment(
+ armem::fromIce<typename CoreSegmentBase<Args...>::ProviderSegmentT>(providerSegment));
+ }
+ }
+
+ template <class ...Args>
+ void toIce(data::Memory& ice, const MemoryBase<Args...>& memory)
+ {
+ base::detail::toIceItem(ice, memory);
+
+ ice.coreSegments.clear();
+ memory.forEachCoreSegment([&ice](const auto & coreSegment)
+ {
+ armem::toIce(ice.coreSegments[coreSegment.name()], coreSegment);
+ return true;
+ });
+ }
+ template <class ...Args>
+ void fromIce(const data::Memory& ice, MemoryBase<Args...>& memory)
+ {
+ base::detail::fromIceItem(ice, memory);
+
+ memory.clear();
+ for (const auto& [key, coreSegment] : ice.coreSegments)
+ {
+ // We can avoid using CoreSegment's copy constructor this way:
+ armem::fromIce(coreSegment, memory.addCoreSegment(coreSegment->id.coreSegmentName));
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp
index 0f1bd0be881f477908464bcda5a4a858afb54488..204aa8f86e20d99df852abbdc79be3d1a07ef39f 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp
@@ -78,9 +78,9 @@ namespace armarx::armem::d_ltm
std::filesystem::create_directories(_fullPath());
TypeIO::writeAronType(_aronType, _fullPath());
- for (const auto& [k, s] : m)
+ m.forEachProviderSegment([this](const wm::ProviderSegment & s)
{
- if (const auto& it = _container.find(k); it != _container.end())
+ if (auto it = _container.find(s.name()); it != _container.end())
{
it->second.append(s);
}
@@ -88,18 +88,19 @@ namespace armarx::armem::d_ltm
{
try
{
- std::filesystem::create_directory(_fullPath() / k);
+ std::filesystem::create_directory(_fullPath() / s.name());
}
catch (...)
{
ARMARX_WARNING << GetHandledExceptionString();
- return;
+ return false;
}
- auto wms = _container.emplace(std::make_pair(k, id().withProviderSegmentName(k)));
+ auto wms = _container.emplace(s.name(), id().withProviderSegmentName(s.name()));
wms.first->second.path = path;
wms.first->second.append(s);
}
- }
+ return true;
+ });
}
}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.h b/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.h
index 76bb297e76d268ce8517449686ffae1cdb3ed2cd..2d8bd8f29e7027ea63b797d0a19f4b96e116898e 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.h
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.h
@@ -2,8 +2,8 @@
#include <filesystem>
-#include "../base/CoreSegmentBase.h"
-#include "../workingmemory/CoreSegment.h"
+#include <RobotAPI/libraries/armem/core/base/CoreSegmentBase.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include "ProviderSegment.h"
@@ -22,7 +22,6 @@ namespace armarx::armem::d_ltm
public:
using Base::CoreSegmentBase;
- using Base::operator=;
// Conversion
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/Entity.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/Entity.cpp
index 5d784a097de4ef68ec2df0bdc1adce94fa737809..d4d0ac233f1fac1e910869f56cc12541928798ce 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/Entity.cpp
@@ -1,5 +1,8 @@
#include "Entity.h"
+#include <ArmarXCore/core/logging/Logging.h>
+
+
namespace armarx::armem::d_ltm
{
std::filesystem::path Entity::_fullPath() const
@@ -63,20 +66,21 @@ namespace armarx::armem::d_ltm
void Entity::append(const wm::Entity& m)
{
std::filesystem::create_directories(_fullPath());
- for (const auto& [k, s] : m.container())
+ m.forEachSnapshot([this](const wm::EntitySnapshot & s)
{
- if (const auto& it = _container.find(k); it != _container.end())
+ if (auto it = _container.find(s.time()); it != _container.end())
{
// timestamp already exists
// We assume that a snapshot does not change, so ignore
}
else
{
- std::filesystem::create_directory(_fullPath() / std::to_string(k.toMicroSeconds()));
- auto wms = _container.emplace(std::make_pair(k, id().withTimestamp(k)));
+ std::filesystem::create_directory(_fullPath() / std::to_string(s.time().toMicroSeconds()));
+ auto wms = _container.emplace(std::make_pair(s.time(), id().withTimestamp(s.time())));
wms.first->second.path = path;
wms.first->second.setTo(s);
}
- }
+ return true;
+ });
}
}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/Entity.h b/source/RobotAPI/libraries/armem/core/diskmemory/Entity.h
index 35022275570c93378cbe0c767aad1776da594838..63011ceff58a0ec7ff4b77bf2afeaa0252bbcec0 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/Entity.h
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/Entity.h
@@ -2,8 +2,8 @@
#include <filesystem>
-#include "../base/EntityBase.h"
-#include "../workingmemory/Entity.h"
+#include <RobotAPI/libraries/armem/core/base/EntityBase.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include "EntitySnapshot.h"
@@ -38,7 +38,6 @@ namespace armarx::armem::d_ltm
public:
using Base::EntityBase;
- using Base::operator=;
// Conversion
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.cpp
index dd0458b83bbb8ed461ddeab50af42181fda843e4..56786756655453bb97952a206ff642b9f65174cb 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.cpp
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.cpp
@@ -1,22 +1,22 @@
#include "EntityInstance.h"
-#include <iostream>
-#include <fstream>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/wm/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/wm/json_conversions.h>
-#include "../../core/error.h"
-#include "ArmarXCore/core/exceptions/LocalException.h"
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <SimoxUtility/json/json.hpp>
+
+#include <iostream>
+#include <fstream>
+
+
namespace armarx::armem::d_ltm
{
- EntityInstance::EntityInstance(const EntityInstance& other) :
- Base(other),
- path(other.path)
- {
- }
-
bool EntityInstance::equalsDeep(const EntityInstance& other) const
{
return id() == other.id();
@@ -24,25 +24,11 @@ namespace armarx::armem::d_ltm
- void EntityInstance::update(const EntityUpdate& update, int index)
- {
- ARMARX_CHECK_FITS_SIZE(index, update.instancesData.size());
-
- this->index() = index;
- }
-
- EntityInstance EntityInstance::copy() const
+ void EntityInstance::update(const EntityUpdate& update)
{
- EntityInstance d;
- this->_copySelf(d);
- return d;
+ ARMARX_CHECK_FITS_SIZE(this->index(), update.instancesData.size());
}
- void EntityInstance::_copySelf(EntityInstance& other) const
- {
- EntityInstanceBase<EntityInstance>::_copySelf(other);
- other.path = path;
- }
std::filesystem::path EntityInstance::_fullPath() const
{
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.h b/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.h
index 89b96c011fcccb6bfc5471850185b0cea49f77b0..c56a944f264c08e1f26b35df1ecb622dff92c0e6 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.h
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.h
@@ -2,11 +2,10 @@
#include <filesystem>
-#include "../base/EntityInstanceBase.h"
-#include "../workingmemory/EntityInstance.h"
+#include <RobotAPI/libraries/armem/core/base/EntityInstanceBase.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include "../workingmemory/entityInstance_conversions.h"
-#include "../workingmemory/json_conversions.h"
+#include <RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h>
namespace armarx::armem::d_ltm
@@ -15,30 +14,24 @@ namespace armarx::armem::d_ltm
* @brief Data of a single entity instance.
*/
class EntityInstance :
- public base::EntityInstanceBase<EntityInstance>
+ public base::EntityInstanceBase<base::NoData, base::NoData>
{
- using Base = base::EntityInstanceBase<EntityInstance>;
+ using Base = base::EntityInstanceBase<base::NoData, base::NoData>;
public:
using Base::EntityInstanceBase;
- EntityInstance(const EntityInstance& other);
- EntityInstance(EntityInstance&& other) = default;
- EntityInstance& operator=(const EntityInstance& other) = default;
- EntityInstance& operator=(EntityInstance&& other) = default;
-
/**
* @brief Fill `*this` with the update's values.
* @param update The update.
* @param index The instances index.
*/
- virtual void update(const EntityUpdate& update, int index) override;
+ void update(const EntityUpdate& update);
- virtual bool equalsDeep(const EntityInstance& other) const override;
+ bool equalsDeep(const EntityInstance& other) const;
- virtual EntityInstance copy() const override;
// Conversion
wm::EntityInstance convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const;
@@ -47,8 +40,6 @@ namespace armarx::armem::d_ltm
void reload(const std::shared_ptr<std::filesystem::path>&);
void setTo(const wm::EntityInstance&);
- protected:
- virtual void _copySelf(EntityInstance& other) const override;
private:
std::filesystem::path _fullPath() const;
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.cpp
index ab5b56d2274f78c350818693c269af18f0a4a979..9354ab7d52e0dcbd058aec022892132b753b51ab 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.cpp
@@ -83,7 +83,7 @@ namespace armarx::armem::d_ltm
_container.clear();
int i = 0;
- for (const auto& s : m.instances())
+ m.forEachInstance([this, &i](wm::EntityInstance & s)
{
try
{
@@ -92,12 +92,14 @@ namespace armarx::armem::d_ltm
catch (...)
{
ARMARX_WARNING << GetHandledExceptionString();
- continue;;
+ return true;
}
auto& wms = _container.emplace_back(id().withInstanceIndex(i++));
wms.path = path;
wms.setTo(s);
- }
+
+ return true;
+ });
}
}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.h b/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.h
index 545a8c0d64641c65e7faf7aa26dea77ee9865e0c..9587e3be44435814d4709979a1fb394c67b953c7 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.h
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.h
@@ -2,11 +2,13 @@
#include <filesystem>
-#include "../base/EntitySnapshotBase.h"
-#include "../workingmemory/EntitySnapshot.h"
+#include <RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include "EntityInstance.h"
+#include <RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h>
+
namespace armarx::armem::d_ltm
{
@@ -22,7 +24,6 @@ namespace armarx::armem::d_ltm
public:
using Base::EntitySnapshotBase;
- using Base::operator=;
// Conversion
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp
index 0fb6fba92b2ebc116787bffba2d7ed5bc743227b..3fc60e0496aa21c441cf5e8a60443e43ef741a46 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp
@@ -65,9 +65,9 @@ namespace armarx::armem::d_ltm
void Memory::append(const wm::Memory& m)
{
std::filesystem::create_directories(_fullPath());
- for (const auto& [k, s] : m.container())
+ m.forEachCoreSegment([this](const wm::CoreSegment & s) -> bool
{
- if (const auto& it = _container.find(k); it != _container.end())
+ if (auto it = _container.find(s.name()); it != _container.end())
{
it->second.append(s);
}
@@ -75,18 +75,20 @@ namespace armarx::armem::d_ltm
{
try
{
- std::filesystem::create_directory(_fullPath() / k);
+ std::filesystem::create_directory(_fullPath() / s.name());
}
catch (...)
{
ARMARX_WARNING << GetHandledExceptionString();
- return;
+ return false;
}
- auto wms = _container.emplace(k, id().withCoreSegmentName(k));
+ auto wms = _container.emplace(s.name(), id().withCoreSegmentName(s.name()));
wms.first->second.path = path;
wms.first->second.append(s);
}
- }
+
+ return true;
+ });
}
}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/Memory.h b/source/RobotAPI/libraries/armem/core/diskmemory/Memory.h
index 7f7872f7a638cc31122f09b00a95755ede4d9606..aaa1e460a5bff94195ae6da7662a7e53b33861e4 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/Memory.h
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/Memory.h
@@ -2,8 +2,8 @@
#include <filesystem>
-#include "../base/MemoryBase.h"
-#include "../workingmemory/Memory.h"
+#include <RobotAPI/libraries/armem/core/base/MemoryBase.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include "CoreSegment.h"
@@ -22,7 +22,6 @@ namespace armarx::armem::d_ltm
public:
using Base::MemoryBase;
- using Base::operator=;
// Conversion
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp
index 620e819f6b1fc05d4aeadc285edb087f5ea0e119..08ed0c2b07edc156764f02dc88c5f76160f9d355 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp
@@ -95,9 +95,9 @@ namespace armarx::armem::d_ltm
TypeIO::writeAronType(_aronType, _fullPath());
- for (const auto& [k, s] : m.container())
+ m.forEachEntity([this](const wm::Entity & s)
{
- if (const auto& it = _container.find(k); it != _container.end())
+ if (auto it = _container.find(s.name()); it != _container.end())
{
it->second.append(s);
}
@@ -106,18 +106,20 @@ namespace armarx::armem::d_ltm
try
{
- std::filesystem::create_directory(_fullPath() / k);
+ std::filesystem::create_directory(_fullPath() / s.name());
}
catch (...)
{
ARMARX_WARNING << GetHandledExceptionString();
- return;
+ return false;
}
- auto wms = _container.emplace(k, id().withEntityName(k));
+ auto wms = _container.emplace(s.name(), id().withEntityName(s.name()));
wms.first->second.path = path;
wms.first->second.append(s);
}
- }
+
+ return true;
+ });
}
}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.h b/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.h
index 8ad96ea0630af0d5aba8b7270fe229c235c2a010..0e866b08cc5c50f623fd9b5676136834078c953a 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.h
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.h
@@ -2,8 +2,8 @@
#include <filesystem>
-#include "../base/ProviderSegmentBase.h"
-#include "../workingmemory/ProviderSegment.h"
+#include <RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include "Entity.h"
@@ -22,7 +22,6 @@ namespace armarx::armem::d_ltm
public:
using Base::ProviderSegmentBase;
- using Base::operator=;
// Conversion
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.h b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
index e6843d13be5bc07e71d90b00cf6a911ca717e524..219d489fad038da1098d724bddd9e2bc74b0a5d7 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.h
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
@@ -84,8 +84,8 @@ namespace armarx::armem::error
template <class MissingT, class ContainerT>
static MissingEntry create(const std::string& missingKey, const ContainerT& container)
{
- return MissingEntry(MissingT().getLevelName(), missingKey,
- container.getLevelName(), container.getKeyString(), container.size());
+ return MissingEntry(MissingT::getLevelName(), missingKey,
+ ContainerT::getLevelName(), container.getKeyString(), container.size());
}
diff --git a/source/RobotAPI/libraries/armem/core/forward_declarations.h b/source/RobotAPI/libraries/armem/core/forward_declarations.h
new file mode 100644
index 0000000000000000000000000000000000000000..e39274b7c332fe7ee4b0505c3aa7bce8c5c0a8ca
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/forward_declarations.h
@@ -0,0 +1,58 @@
+#pragma once
+
+
+namespace IceUtil
+{
+ class Time;
+}
+
+namespace armarx::armem
+{
+ using Time = IceUtil::Time;
+ using Duration = IceUtil::Time;
+
+ class MemoryID;
+ class Commit;
+ class EntityUpdate;
+ class CommitResult;
+ class EntityUpdateResult;
+}
+
+namespace armarx::armem::base
+{
+ struct NoData;
+ struct EntityInstanceMetadata;
+
+ template <class _DataT, class _MetadataT>
+ class EntityInstanceBase;
+ template <class _EntityInstanceT, class _Derived>
+ class EntitySnapshotBase;
+ template <class _EntitySnapshotT, class _Derived>
+ class EntityBase;
+ template <class _EntityT, class _Derived>
+ class ProviderSegmentBase;
+ template <class _ProviderSegmentT, class _Derived>
+ class CoreSegmentBase;
+ template <class _CoreSegmentT, class _Derived>
+ class MemoryBase;
+}
+
+namespace armarx::armem::wm
+{
+ class EntityInstance;
+ class EntitySnapshot;
+ class Entity;
+ class ProviderSegment;
+ class CoreSegment;
+ class Memory;
+}
+
+namespace armarx::armem::server::wm
+{
+ using EntityInstance = armem::wm::EntityInstance;
+ using EntitySnapshot = armem::wm::EntitySnapshot;
+ class Entity;
+ class ProviderSegment;
+ class CoreSegment;
+ class Memory;
+}
diff --git a/source/RobotAPI/libraries/armem/core/ice_conversions.cpp b/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
index d79acf1da2b86f94414205131173797e0354bc24..cca45a3b2cf04f26ec76d158e9cf1e711bbfecb2 100644
--- a/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
@@ -1,20 +1,20 @@
#include "ice_conversions.h"
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
-namespace armarx
+
+void IceUtil::toIce(long& ice, const IceUtil::Time& time)
{
+ ice = time.toMicroSeconds();
+}
- /*
- void armem::fromIce(long ice, Time& time)
- {
- time = Time::microSeconds(ice);
- }
+void IceUtil::fromIce(const long& ice, IceUtil::Time& time)
+{
+ time = Time::microSeconds(ice);
+}
- void armem::toIce(long& ice, const Time& time)
- {
- ice = time.toMicroSeconds();
- }
- */
+namespace armarx
+{
void armem::toIce(data::MemoryID& ice, const MemoryID& id)
{
@@ -144,14 +144,5 @@ namespace armarx
update.timeArrived = timeArrived;
}
-
- void armem::detail::toIceItem(data::detail::MemoryItem& ice, const armem::base::detail::MemoryItem& item)
- {
- toIce(ice.id, item.id());
- }
-
- void armem::detail::fromIceItem(const data::detail::MemoryItem& ice, armem::base::detail::MemoryItem& item)
- {
- fromIce(ice.id, item.id());
- }
}
+
diff --git a/source/RobotAPI/libraries/armem/core/ice_conversions.h b/source/RobotAPI/libraries/armem/core/ice_conversions.h
index 874684cac2a6a52b8742c36dcc2bdea8346e1586..e8f104fe04ebcb721b0ffbf4b70b18ada0af234c 100644
--- a/source/RobotAPI/libraries/armem/core/ice_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/ice_conversions.h
@@ -6,29 +6,21 @@
#include "ice_conversions_templates.h"
#include "Commit.h"
-#include "base/detail/MemoryItem.h"
+#include "MemoryID.h"
+#include "Time.h"
namespace IceUtil
{
// Define in original namespace to allow ADL.
- inline void toIce(long& ice, const Time& time)
- {
- ice = time.toMicroSeconds();
- }
- inline void fromIce(const long& ice, Time& time)
- {
- time = Time::microSeconds(ice);
- }
+ void toIce(long& ice, const Time& time);
+ void fromIce(const long& ice, Time& time);
}
namespace armarx::armem
{
- // void fromIce(long ice, Time& time);
- // void toIce(long& ice, const Time& time);
-
void fromIce(const data::MemoryID& ice, MemoryID& id);
void toIce(data::MemoryID& ice, const MemoryID& id);
@@ -51,12 +43,5 @@ namespace armarx::armem
void fromIce(const data::Commit& ice, Commit& commit, Time timeArrived);
void fromIce(const data::EntityUpdate& ice, EntityUpdate& update, Time timeArrived);
-
- namespace detail
- {
- void toIceItem(data::detail::MemoryItem& ice, const armem::base::detail::MemoryItem& item);
- void fromIceItem(const data::detail::MemoryItem& ice, armem::base::detail::MemoryItem& item);
- }
-
}
diff --git a/source/RobotAPI/libraries/armem/core/ice_conversions_boost_templates.h b/source/RobotAPI/libraries/armem/core/ice_conversions_boost_templates.h
new file mode 100644
index 0000000000000000000000000000000000000000..7b6681236bc3d0517d6fd61656a1dc598a8d6a36
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/ice_conversions_boost_templates.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include <map>
+
+#include <boost/container/flat_map.hpp>
+
+
+namespace armarx::armem
+{
+ // std::map <-> boost::container::flat_map
+
+ template <class IceKeyT, class IceValueT, class CppKeyT, class CppValueT>
+ void toIce(std::map<IceKeyT, IceValueT>& iceMap,
+ const boost::container::flat_map<CppKeyT, CppValueT>& cppMap)
+ {
+ iceMap.clear();
+ for (const auto& [key, value] : cppMap)
+ {
+ iceMap.emplace(toIce<IceKeyT>(key), toIce<IceValueT>(value));
+ }
+ }
+
+ template <class IceKeyT, class IceValueT, class CppKeyT, class CppValueT>
+ void fromIce(const std::map<IceKeyT, IceValueT>& iceMap,
+ boost::container::flat_map<CppKeyT, CppValueT>& cppMap)
+ {
+ cppMap.clear();
+ for (const auto& [key, value] : iceMap)
+ {
+ cppMap.emplace(fromIce<CppKeyT>(key), fromIce<CppValueT>(value));
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/ice_conversions_templates.h b/source/RobotAPI/libraries/armem/core/ice_conversions_templates.h
index e0c71faac37cd899d85d174c33196499a29df770..8060e22a0782e23c6bb989fe900fcce99c7ff7c6 100644
--- a/source/RobotAPI/libraries/armem/core/ice_conversions_templates.h
+++ b/source/RobotAPI/libraries/armem/core/ice_conversions_templates.h
@@ -1,9 +1,12 @@
#pragma once
-#include <memory>
-
#include "Commit.h"
+#include <Ice/Handle.h>
+
+#include <map>
+#include <memory>
+#include <vector>
namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
index a64a5982e143e2627718f5039d3559b2d3479348..1fc021d65fe460b4b91331196446ef8661337aa6 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
@@ -1,8 +1,10 @@
#include "CoreSegment.h"
+#include "error.h"
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "error.h"
+#include <SimoxUtility/json/json.hpp>
namespace armarx::armem::ltm
@@ -10,8 +12,6 @@ namespace armarx::armem::ltm
wm::CoreSegment CoreSegment::convert() const
{
- ARMARX_INFO << "CoreSegment: Converting with connection to: " << dbsettings.toString();
-
wm::CoreSegment m(id());
for (const auto& [_, s] : _container)
{
@@ -24,8 +24,6 @@ namespace armarx::armem::ltm
{
_container.clear();
- ARMARX_INFO << "CoreSegment: (Re)Establishing connection to: " << dbsettings.toString();
-
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
mongocxx::collection coll = db[id().str()];
@@ -37,7 +35,7 @@ namespace armarx::armem::ltm
nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(doc));
ARMARX_INFO << "CoreSegment: Found foreign key: " << json.at("foreign_key");
- MemoryID i((std::string) json.at("foreign_key"));
+ MemoryID i = MemoryID::fromString(json.at("foreign_key").get<std::string>());
if (i.coreSegmentName != id().coreSegmentName)
{
throw error::InvalidMemoryID(i, "A MemoryID in mongodb was invalid. Found the wrong coreSegment name. Expected " + id().coreSegmentName);
@@ -45,9 +43,9 @@ namespace armarx::armem::ltm
std::string k = i.providerSegmentName;
- if (const auto& it = _container.find(k); it != _container.end())
+ if (auto it = _container.find(k); it != _container.end())
{
- throw error::ArMemError("Somehow after clearing the container a key k = " + k + " was found. Do you have double entries in mongodb?");
+ throw error::ArMemError("Somehow after clearing the (core) container a key k = " + k + " was found. Do you have double entries in mongodb?");
}
else
{
@@ -56,6 +54,8 @@ namespace armarx::armem::ltm
wms.first->second.reload();
}
}
+
+ ARMARX_INFO << "After reload has core segment " << id().str() << " size: " << _container.size();
}
void CoreSegment::append(const wm::CoreSegment& m)
@@ -64,24 +64,23 @@ namespace armarx::armem::ltm
mongocxx::database db = client[dbsettings.database];
mongocxx::collection coll = db[id().str()];
- for (const auto& [k, s] : m.container())
+ m.forEachProviderSegment([this, &coll](const wm::ProviderSegment & provSeg)
{
- if (const auto& it = _container.find(k); it != _container.end())
+ auto it = _container.find(provSeg.name());
+ if (it == _container.end())
{
- it->second.append(s);
- }
- else
- {
- auto builder = bsoncxx::builder::stream::document{};
+ bsoncxx::builder::stream::document builder;
bsoncxx::document::value foreign_key = builder
- << "foreign_key" << s.id().withProviderSegmentName(k).str()
+ << "foreign_key" << provSeg.id().str()
<< bsoncxx::builder::stream::finalize;
coll.insert_one(foreign_key.view());
- auto wms = _container.emplace(k, id().withProviderSegmentName(k));
- wms.first->second.dbsettings = dbsettings;
- wms.first->second.append(s);
+ it = _container.emplace(provSeg.name(), id().withProviderSegmentName(provSeg.name())).first;
+ it->second.dbsettings = dbsettings;
}
- }
+ it->second.append(provSeg);
+
+ return true;
+ });
}
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h
index 3a7225058a265949de50e98a63d1d3c6ce5bf371..32f05ef2f773388b6006c573b7017064193d80c3 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h
@@ -1,10 +1,10 @@
#pragma once
-#include "../base/CoreSegmentBase.h"
-
#include "ProviderSegment.h"
+#include "mongodb/MongoDBConnectionManager.h"
-#include "../workingmemory/CoreSegment.h"
+#include <RobotAPI/libraries/armem/core/base/CoreSegmentBase.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
namespace armarx::armem::ltm
@@ -21,7 +21,6 @@ namespace armarx::armem::ltm
public:
using Base::CoreSegmentBase;
- using Base::operator=;
// Conversion
@@ -31,21 +30,10 @@ namespace armarx::armem::ltm
void reload();
void append(const wm::CoreSegment&);
- protected:
- virtual void _copySelf(CoreSegment& other) const override
- {
- Base::_copySelf(other);
- other.dbsettings = dbsettings;
- }
-
- virtual void _copySelfEmpty(CoreSegment& other) const override
- {
- Base::_copySelfEmpty(other);
- other.dbsettings = dbsettings;
- }
-
public:
+
MongoDBConnectionManager::MongoDBSettings dbsettings;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
index 153852eaf50f4d71573c1bafdc44b052c0e1317c..6604c26e935cb4619fc7439d8e9a1f318a14ff63 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
@@ -1,12 +1,15 @@
#include "Entity.h"
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <SimoxUtility/json/json.hpp>
+
+
namespace armarx::armem::ltm
{
wm::Entity Entity::convert() const
{
- ARMARX_INFO << "Entity: Converting with connection to: " << dbsettings.toString();
-
wm::Entity m(id());
for (const auto& [_, s] : _container)
{
@@ -19,8 +22,6 @@ namespace armarx::armem::ltm
{
_container.clear();
- ARMARX_INFO << "Entity: (Re)Establishing connection to: " << dbsettings.toString();
-
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
mongocxx::collection coll = db[id().str()];
@@ -29,19 +30,14 @@ namespace armarx::armem::ltm
int i = 0;
for (auto doc : cursor)
{
- if (i > MAX_HISTORY_SIZE)
- {
- // TODO: Add logic for most queried data?
- break;
- }
nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(doc));
auto k = armem::Time::microSeconds(json.at("timestamp"));
//ARMARX_INFO << "Entity: Found timestamp: " << std::to_string(k.toMicroSeconds());
- if (const auto& it = _container.find(k); it != _container.end())
+ if (auto it = _container.find(k); it != _container.end())
{
- throw error::ArMemError("Somehow after clearing the container a key k = " + std::to_string(k.toMicroSeconds()) + " was found. Do you have double entries in mongodb?");
+ throw error::ArMemError("Somehow after clearing the (entity) container a key k = " + std::to_string(k.toMicroSeconds()) + " was found. Do you have double entries in mongodb?");
}
else
{
@@ -51,6 +47,8 @@ namespace armarx::armem::ltm
}
++i;
}
+
+ ARMARX_INFO << "After reload has entity " << id().str() << " size: " << _container.size();
}
void Entity::append(const wm::Entity& m)
@@ -59,21 +57,22 @@ namespace armarx::armem::ltm
mongocxx::database db = client[dbsettings.database];
mongocxx::collection coll = db[id().str()];
- for (const auto& [k, s] : m.container())
+ m.forEachSnapshot([this](const wm::EntitySnapshot & snapshot)
{
- if (const auto& it = _container.find(k); it != _container.end())
+ if (auto it = _container.find(snapshot.time()); it != _container.end())
{
// timestamp already exists
// We assume that a snapshot does not change, so ignore
}
else
{
- auto wms = _container.emplace(std::make_pair(k, id().withTimestamp(k)));
+ auto wms = _container.emplace(std::make_pair(snapshot.time(), id().withTimestamp(snapshot.time())));
wms.first->second.dbsettings = dbsettings;
- wms.first->second.setTo(s, k);
+ wms.first->second.setTo(snapshot, snapshot.time());
//truncateHistoryToSize();
}
- }
+ return true;
+ });
}
bool Entity::hasSnapshot(const Time& time) const
@@ -88,25 +87,26 @@ namespace armarx::armem::ltm
mongocxx::database db = client[dbsettings.database];
mongocxx::collection coll = db[id().str()];
- bsoncxx::stdx::optional<bsoncxx::document::value> maybe_result = coll.find_one(document{} << "timestamp" << time.toMicroSeconds() << finalize);
+ bsoncxx::stdx::optional<bsoncxx::document::value> maybe_result =
+ coll.find_one(document{} << "timestamp" << time.toMicroSeconds() << finalize);
if (!maybe_result)
{
return false;
}
- auto json = nlohmann::json::parse(bsoncxx::to_json(*maybe_result));
- auto id = MemoryID::fromString(json["id"].get<std::string>());
- auto instances = json["instances"];
+ nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(*maybe_result));
+ MemoryID id = MemoryID::fromString(json["id"].get<std::string>());
+ nlohmann::json instances = json["instances"];
EntitySnapshot snapshot(id);
snapshot.dbsettings = dbsettings;
- for (unsigned int i = 0; i < instances.size(); ++i)
+ for (size_t i = 0; i < instances.size(); ++i)
{
- EntityInstance instance(id.withInstanceIndex(i));
+ EntityInstance instance(id.withInstanceIndex(static_cast<int>(i)));
snapshot.addInstance(instance);
}
- _container.insert({time, snapshot});
+ _container.emplace(time, snapshot);
//truncateHistoryToSize();
return true;
}
@@ -142,7 +142,7 @@ namespace armarx::armem::ltm
return Base::getSnapshot(time);
}
- const std::map<Time, EntitySnapshot>::value_type& Entity::getLatestItem() const
+ auto Entity::getLatestItem() const -> const ContainerT::value_type&
{
// Directly query mongodb (cache cant know whether it is the latest or not)
// TODO
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h
index a5bca08c7ee0b7aff2a79d2c45aa37fc4baec1c4..99bc3a736f0abe803ec101a81f4b1ac75b87d92e 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h
@@ -1,10 +1,10 @@
#pragma once
-#include "../base/EntityBase.h"
-
#include "EntitySnapshot.h"
+#include "mongodb/MongoDBConnectionManager.h"
-#include "../workingmemory/Entity.h"
+#include <RobotAPI/libraries/armem/core/base/EntityBase.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
namespace armarx::armem::ltm
@@ -36,13 +36,7 @@ namespace armarx::armem::ltm
public:
using Base::EntityBase;
- using Base::operator=;
- Entity()
- {
- // the history of snapshots is just a cache of frequently used elements
- setMaxHistorySize(MAX_HISTORY_SIZE);
- }
// Conversion
wm::Entity convert() const;
@@ -51,33 +45,19 @@ namespace armarx::armem::ltm
void reload();
void append(const wm::Entity&);
- // virtual overrides for LTM lookups
- virtual bool hasSnapshot(const Time& time) const override;
- virtual std::vector<Time> getTimestamps() const override;
- virtual const EntitySnapshot& getSnapshot(const Time& time) const override;
-
- protected:
- virtual void _copySelf(Entity& other) const override
- {
- Base::_copySelf(other);
- other.dbsettings = dbsettings;
- }
+ // overrides for LTM lookups
+ bool hasSnapshot(const Time& time) const;
+ std::vector<Time> getTimestamps() const;
+ const EntitySnapshot& getSnapshot(const Time& time) const;
- virtual void _copySelfEmpty(Entity& other) const override
- {
- Base::_copySelfEmpty(other);
- other.dbsettings = dbsettings;
- }
protected:
- // virtual overrides for LTM storage
- virtual const std::map<Time, EntitySnapshot>::value_type& getLatestItem() const override;
+ // overrides for LTM storage
+ const ContainerT::value_type& getLatestItem() const;
public:
MongoDBConnectionManager::MongoDBSettings dbsettings;
- private:
- const static constexpr int MAX_HISTORY_SIZE = 200;
};
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.cpp
index 5e44714904efe74f920b891b665e13497b749508..63fe18145c561e9efda0ba2a14cb07175b029d1c 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.cpp
@@ -1,30 +1,19 @@
#include "EntityInstance.h"
-#include "../../core/error.h"
-
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
namespace armarx::armem::ltm
{
- bool EntityInstanceMetadata::operator==(const EntityInstanceMetadata& other) const
- {
- return timeCreated == other.timeCreated
- && timeSent == other.timeSent
- && timeArrived == other.timeArrived
- && std::abs(confidence - other.confidence) < 1e-6f;
- }
-
bool EntityInstance::equalsDeep(const EntityInstance& other) const
{
return id() == other.id() && _metadata == other.metadata();
}
- void EntityInstance::update(const EntityUpdate& update, int index)
+ void EntityInstance::update(const EntityUpdate& update)
{
- ARMARX_CHECK_FITS_SIZE(index, update.instancesData.size());
-
- this->index() = index;
+ ARMARX_CHECK_FITS_SIZE(this->index(), update.instancesData.size());
this->_metadata.confidence = update.confidence;
this->_metadata.timeCreated = update.timeCreated;
@@ -32,16 +21,4 @@ namespace armarx::armem::ltm
this->_metadata.timeArrived = update.timeArrived;
}
- EntityInstance EntityInstance::copy() const
- {
- EntityInstance d;
- this->_copySelf(d);
- return d;
- }
-
- void EntityInstance::_copySelf(EntityInstance& other) const
- {
- Base::_copySelf(other);
- other._metadata = _metadata;
- }
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.h b/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.h
index e95ae277e83b2a927172ad1e663c42a700fef849..6046f6ac50902d6f7f770361915dbd30d5a15ec1 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.h
@@ -1,84 +1,35 @@
#pragma once
-#include "../base/EntityInstanceBase.h"
+#include <RobotAPI/libraries/armem/core/base/EntityInstanceBase.h>
-#include "../workingmemory/EntityInstance.h"
-#include "mongodb/MongoDBConnectionManager.h"
-
-#include "../workingmemory/entityInstance_conversions.h"
-#include "../workingmemory/json_conversions.h"
namespace armarx::armem::ltm
{
- /**
- * @brief Metadata of an entity instance.
- */
- struct EntityInstanceMetadata
- {
- /// Time when this value was created.
- Time timeCreated;
- /// Time when this value was sent to the memory.
- Time timeSent;
- /// Time when this value has arrived at the memory.
- Time timeArrived;
+ using EntityInstanceMetadata = base::EntityInstanceMetadata;
- /// An optional confidence, may be used for things like decay.
- float confidence = 1.0;
-
-
- bool operator==(const EntityInstanceMetadata& other) const;
- inline bool operator!=(const EntityInstanceMetadata& other) const
- {
- return !(*this == other);
- }
- };
/**
* @brief Data of a single entity instance.
*/
class EntityInstance :
- public base::EntityInstanceBase<EntityInstance>
+ public base::EntityInstanceBase<base::NoData, EntityInstanceMetadata>
{
- using Base = base::EntityInstanceBase<EntityInstance>;
+ using Base = base::EntityInstanceBase<base::NoData, EntityInstanceMetadata>;
public:
using Base::EntityInstanceBase;
- EntityInstance(const EntityInstance& other) = default;
- EntityInstance(EntityInstance&& other) = default;
- EntityInstance& operator=(const EntityInstance& other) = default;
- EntityInstance& operator=(EntityInstance&& other) = default;
-
-
- EntityInstanceMetadata& metadata()
- {
- return _metadata;
- }
- inline const EntityInstanceMetadata& metadata() const
- {
- return _metadata;
- }
-
/**
* @brief Fill `*this` with the update's values.
* @param update The update.
* @param index The instances index.
*/
- virtual void update(const EntityUpdate& update, int index) override;
-
- virtual bool equalsDeep(const EntityInstance& other) const override;
-
- virtual EntityInstance copy() const override;
-
- protected:
- virtual void _copySelf(EntityInstance& other) const override;
+ void update(const EntityUpdate& update);
+ bool equalsDeep(const EntityInstance& other) const;
- private:
- /// The metadata.
- EntityInstanceMetadata _metadata;
};
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
index 47a680731f9794c99433a114dd94a9ad22f5cbea..8cbe688125a7571a3e8f0a19031b25722bb79bbf 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
@@ -2,8 +2,11 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "../workingmemory/entityInstance_conversions.h"
-#include "../workingmemory/json_conversions.h"
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+
+#include <RobotAPI/libraries/armem/core/wm/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/wm/json_conversions.h>
+
#include "error.h"
@@ -13,14 +16,11 @@ namespace armarx::armem::ltm
wm::EntitySnapshot EntitySnapshot::convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const
{
- ARMARX_INFO << "EntitySnapshot: Converting with connection to: " << dbsettings.toString();
-
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
mongocxx::collection coll = db[id().getEntityID().str()];
auto res = coll.find_one(document{} << "id" << id().getEntitySnapshotID().str() << finalize);
-
if (!res)
{
throw error::ArMemError("Could not load an instance from the memory '" + id().getEntityID().str() + "'. Tried to access: " + id().getEntitySnapshotID().str());
@@ -52,8 +52,6 @@ namespace armarx::armem::ltm
{
_container.clear();
- ARMARX_INFO << "EntitySnapshot: (Re)Establishing connection to: " << dbsettings.toString();
-
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
mongocxx::collection coll = db[id().getEntityID().str()];
@@ -68,10 +66,8 @@ namespace armarx::armem::ltm
nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(*res));
for (unsigned int i = 0; i < json.at("instances").size(); ++i)
{
- auto wms = _container.emplace_back(id().withInstanceIndex(i));
+ _container.emplace_back(id().withInstanceIndex(i));
}
-
- ARMARX_INFO << "Entity '" + id().str() + "': Found instances in LTM: " << json.at("instances").size();
}
void EntitySnapshot::setTo(const wm::EntitySnapshot& m, const armem::Time& t)
@@ -91,18 +87,20 @@ namespace armarx::armem::ltm
auto array_builder = bsoncxx::builder::basic::array{};
int i = 0;
- for (const auto& s : m.container())
+ m.forEachInstance([this, &array_builder, &i](const wm::EntityInstance & instance)
{
auto wms = _container.emplace_back(id().withInstanceIndex(i++));
auto aron = std::make_shared<aron::datanavigator::DictNavigator>();
- to_aron(aron, s);
+ to_aron(aron, instance);
nlohmann::json j;
from_aron(aron, j);
auto doc_value = bsoncxx::from_json(j.dump(2));
array_builder.append(doc_value);
- }
+
+ return true;
+ });
auto after_array = in_array << array_builder;
bsoncxx::document::value doc = after_array << bsoncxx::builder::stream::finalize;
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h
index 02361d2e708365303fbce81f56eaa9b49fb07a61..3029f8a7ca0278b9a505ee2fa5a744cfab327359 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h
@@ -1,10 +1,12 @@
#pragma once
-#include "../base/EntitySnapshotBase.h"
-
#include "EntityInstance.h"
+#include "mongodb/MongoDBConnectionManager.h"
+
+#include <RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include "../workingmemory/EntitySnapshot.h"
+#include <RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h>
namespace armarx::armem::ltm
@@ -21,7 +23,6 @@ namespace armarx::armem::ltm
public:
using Base::EntitySnapshotBase;
- using Base::operator=;
// Conversion
@@ -31,20 +32,10 @@ namespace armarx::armem::ltm
void reload();
void setTo(const wm::EntitySnapshot&, const armem::Time& t);
- protected:
- virtual void _copySelf(EntitySnapshot& other) const override
- {
- Base::_copySelf(other);
- other.dbsettings = dbsettings;
- }
-
- virtual void _copySelfEmpty(EntitySnapshot& other) const override
- {
- Base::_copySelfEmpty(other);
- other.dbsettings = dbsettings;
- }
public:
+
MongoDBConnectionManager::MongoDBSettings dbsettings;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
index fbea444e9d44008407703fe7b7f8b007179726b6..ab4cb5d13ec0a30c0e8dc04125b61bd4b031df99 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
@@ -1,44 +1,106 @@
#include "Memory.h"
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
#include "error.h"
+#include <ArmarXCore/core/application/properties/PluginAll.h>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
namespace armarx::armem::ltm
{
+ Memory::Memory(const Memory& other) :
+ Base(other),
+ dbsettings(other.dbsettings),
+ periodicTransferSettings(other.periodicTransferSettings),
+ onFullTransferSettings(other.onFullTransferSettings),
+ mongoDBMutex()
+ {
+ // Do not copy _mutex.
+ }
+
+
+ Memory::Memory(Memory&& other) :
+ Base(std::move(other)),
+ dbsettings(other.dbsettings),
+ periodicTransferSettings(other.periodicTransferSettings),
+ onFullTransferSettings(other.onFullTransferSettings),
+ reloaded(other.reloaded)
+ {
+ // Do not move _mutex.
+ }
+
+
+ Memory& Memory::operator=(const Memory& other)
+ {
+ Base::operator=(other);
+
+ dbsettings = other.dbsettings;
+ periodicTransferSettings = other.periodicTransferSettings;
+ onFullTransferSettings = other.onFullTransferSettings;
+
+ // Don't copy _mutex.
+ return *this;
+ }
+
+
+ Memory& Memory::operator=(Memory&& other)
+ {
+ Base::operator=(std::move(other));
+
+ dbsettings = std::move(other.dbsettings);
+ periodicTransferSettings = std::move(other.periodicTransferSettings);
+ onFullTransferSettings = std::move(other.onFullTransferSettings);
+ reloaded = other.reloaded;
+
+ // Don't move _mutex.
+ return *this;
+ }
+
bool Memory::checkConnection() const
{
// Check connection:
+ ARMARX_INFO << "Checking connection";
if (!MongoDBConnectionManager::ConnectionIsValid(dbsettings))
{
ARMARX_WARNING << deactivateSpam("ConnectionIsNotValid")
<< "The connection to mongocxx for memory '" << name() << "' is not valid. Settings are: " << dbsettings.toString()
<< "\nTo start it, run e.g.: \n"
- << "mongod --port " << dbsettings.port << " --dbpath \"/tmp/\""
+ << "armarx memory start"
<< "\n\n";
return false;
}
- ARMARX_INFO << "Checking connection";
return true;
}
+ void Memory::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(dbsettings.host, prefix + ".ltm.10_host");
+ defs->optional(dbsettings.port, prefix + "ltm.11_port");
+ defs->optional(dbsettings.user, prefix + "ltm.20_user");
+ defs->optional(dbsettings.password, prefix + "ltm.21_password");
+ defs->optional(dbsettings.database, prefix + "ltm.22_database");
+ defs->optional(periodicTransferSettings.enabled, prefix + "ltm.30_enablePeriodicTransfer", "Enable transfer based on periodic interval.");
+ defs->optional(onFullTransferSettings.enabled, prefix + "ltm.31_enableOnFullTransfer", "Enable transfer whenever the wm is full (see maxHistorySize).");
+ }
+
wm::Memory Memory::convert() const
{
+ wm::Memory m(id());
+
+ std::lock_guard l(mongoDBMutex);
if (!checkConnection())
{
- wm::Memory m(id());
return m;
}
- ARMARX_INFO << "Converting with connection to: " << dbsettings.toString();
+ ARMARX_INFO << "Converting Memory with connection to: " << dbsettings.toString();
TIMING_START(LTM_Convert);
- wm::Memory m(id());
for (const auto& [_, s] : _container)
{
m.addCoreSegment(s.convert());
@@ -50,20 +112,41 @@ namespace armarx::armem::ltm
void Memory::reload()
{
+ std::lock_guard l(mongoDBMutex);
+ reloaded = false;
+
if (!checkConnection())
{
return;
}
ARMARX_INFO << "(Re)Establishing connection to: " << dbsettings.toString();
+
+ TIMING_START(LTM_Reload);
_container.clear();
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
+ if (!client)
+ {
+ ARMARX_ERROR << "A client has died. Could not reload.";
+ return;
+ }
+
+ auto databases = client.list_databases();
+ for (const auto& doc : databases)
+ {
+ nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(doc));
+ ARMARX_INFO << "Found the database: " << json.at("name");
+ }
+
mongocxx::database db = client[dbsettings.database];
+ ARMARX_INFO << "Getting collection for id: " << id().str();
mongocxx::collection coll = db[id().str()];
+ ARMARX_IMPORTANT << "Memory Container size is: " << _container.size();
+
mongocxx::cursor cursor = coll.find({});
- for (auto doc : cursor)
+ for (const auto& doc : cursor)
{
nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(doc));
ARMARX_INFO << "Memory: Found foreign key: " << json.at("foreign_key");
@@ -76,9 +159,9 @@ namespace armarx::armem::ltm
std::string k = i.coreSegmentName;
- if (const auto& it = _container.find(k); it != _container.end())
+ if (auto it = _container.find(k); it != _container.end())
{
- throw error::ArMemError("Somehow after clearing the container a key k = " + k + " was found. Do you have double entries in mongodb?");
+ throw error::ArMemError("Somehow after clearing the (memory) container a key k = " + k + " was found. Do you have double entries in mongodb?");
}
else
{
@@ -87,16 +170,26 @@ namespace armarx::armem::ltm
wms.first->second.reload();
}
}
+
+ reloaded = true;
+ for (const auto& m : toAppendQueue)
+ {
+ _append(m);
+ }
+
+ TIMING_END(LTM_Reload);
+ ARMARX_INFO << "After reload memory " << id().str() << " size: " << _container.size() << ". Setting reloaded: " << reloaded;
}
- void Memory::append(const wm::Memory& m)
+ void Memory::_append(const wm::Memory& m)
{
- if (!checkConnection())
+ if (!checkConnection() || !reloaded)
{
+ // We ignore if not fully loaded yet
return;
}
- ARMARX_INFO << "Merge memory with name '" << m.name() << "' into the LTM with name '" << name() << "'";
+ //ARMARX_INFO << "Merge memory with name '" << m.name() << "' into the LTM with name '" << name() << "'";
TIMING_START(LTM_Append);
@@ -104,9 +197,9 @@ namespace armarx::armem::ltm
mongocxx::database db = client[dbsettings.database];
mongocxx::collection coll = db[id().str()];
- for (const auto& [k, s] : m.container())
+ m.forEachCoreSegment([this, &coll](const wm::CoreSegment & s)
{
- if (const auto& it = _container.find(k); it != _container.end())
+ if (auto it = _container.find(s.name()); it != _container.end())
{
// TODO check if foreign key exists
it->second.append(s);
@@ -115,16 +208,29 @@ namespace armarx::armem::ltm
{
auto builder = bsoncxx::builder::stream::document{};
bsoncxx::document::value foreign_key = builder
- << "foreign_key" << s.id().withCoreSegmentName(k).str()
+ << "foreign_key" << s.id().withCoreSegmentName(s.name()).str()
<< bsoncxx::builder::stream::finalize;
coll.insert_one(foreign_key.view());
- auto wms = _container.emplace(k, id().withCoreSegmentName(k));
+ auto wms = _container.emplace(s.name(), id().withCoreSegmentName(s.name()));
wms.first->second.dbsettings = dbsettings;
wms.first->second.append(s);
}
- }
+
+ return true;
+ });
TIMING_END(LTM_Append);
}
+
+ void Memory::append(const wm::Memory& m)
+ {
+ std::lock_guard l(mongoDBMutex);
+ if (!checkConnection() || !reloaded)
+ {
+ toAppendQueue.push_back(m);
+ return;
+ }
+ _append(m);
+ }
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
index 86c00cc618e23620540428ba3b024743ecc272ee..41e82bce38b50f9e122e34056c2b5fd74cec4bac 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
@@ -1,10 +1,15 @@
#pragma once
-#include "../base/MemoryBase.h"
-
#include "CoreSegment.h"
+#include "mongodb/MongoDBConnectionManager.h"
+
+#include <RobotAPI/libraries/armem/core/base/MemoryBase.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
+
+#include <mutex>
-#include "../workingmemory/Memory.h"
namespace armarx::armem::ltm
{
@@ -18,32 +23,41 @@ namespace armarx::armem::ltm
public:
- using Base::MemoryBase;
- using Base::operator=;
-
- enum class PeriodicTransferMode
+ struct TransferSettings
{
- MANUAL,
- TRANSFER_PERIODIC_KEEP,
- TRANSFER_PERIODIC_DELETE,
+ bool enabled = false;
};
- enum class FilledTransferMode
+ struct PeriodicTransferSettings : public TransferSettings
{
- TRANSFER_LATEST,
- TRANSFER_LEAST_USED
+ bool deleteFromWMOnTransfer = false;
+ int frequencyHz = 1;
};
- struct PeriodicTransfer
+ struct OnFullTransferSettings : public TransferSettings
{
- PeriodicTransferMode mode = PeriodicTransferMode::MANUAL;
+ enum class Mode
+ {
+ TRANSFER_LATEST,
+ TRANSFER_LEAST_USED
+ };
+
+ Mode mode;
+ int batch_size = 20;
};
- struct FilledTransfer
- {
- FilledTransferMode mode = FilledTransferMode::TRANSFER_LATEST;
- };
+ public:
+
+ using Base::MemoryBase;
+
+ Memory(const Memory& other);
+ Memory(Memory&& other);
+ Memory& operator=(const Memory& other);
+ Memory& operator=(Memory&& other);
+
+ // PropertyDefinitions related to LTM
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
// Conversion
wm::Memory convert() const;
@@ -52,26 +66,22 @@ namespace armarx::armem::ltm
void reload();
void append(const wm::Memory&);
- protected:
- virtual void _copySelf(Memory& other) const override
- {
- Base::_copySelf(other);
- other.dbsettings = dbsettings;
- }
-
- virtual void _copySelfEmpty(Memory& other) const override
- {
- Base::_copySelfEmpty(other);
- other.dbsettings = dbsettings;
- }
private:
bool checkConnection() const;
+ void _append(const wm::Memory&);
+
public:
MongoDBConnectionManager::MongoDBSettings dbsettings;
- PeriodicTransfer periodic_transfer;
- FilledTransferMode filled_transfer;
+ PeriodicTransferSettings periodicTransferSettings;
+ OnFullTransferSettings onFullTransferSettings;
+
+ private:
+ bool reloaded = false;
+ mutable std::mutex mongoDBMutex;
+
+ std::vector<wm::Memory> toAppendQueue;
};
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
index aac80459904cae59550ecdf33f7057f92ed03aa4..8126867e4db4be01c91dfb582b67c94303751927 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
@@ -1,8 +1,10 @@
#include "ProviderSegment.h"
+#include "error.h"
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "error.h"
+#include <SimoxUtility/json/json.hpp>
namespace armarx::armem::ltm
@@ -10,8 +12,6 @@ namespace armarx::armem::ltm
wm::ProviderSegment ProviderSegment::convert() const
{
- ARMARX_INFO << "ProviderSegment: Converting with connection to: " << dbsettings.toString();
-
wm::ProviderSegment m(id());
for (const auto& [_, s] : _container)
{
@@ -24,8 +24,6 @@ namespace armarx::armem::ltm
{
_container.clear();
- ARMARX_INFO << "ProviderSegment: (Re)Establishing connection to: " << dbsettings.toString();
-
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
mongocxx::collection coll = db[id().str()];
@@ -34,7 +32,6 @@ namespace armarx::armem::ltm
for (auto doc : cursor)
{
nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(doc));
- ARMARX_INFO << "ProviderSegment: Found foreign key: " << json.at("foreign_key");
MemoryID i((std::string) json.at("foreign_key"));
if (i.providerSegmentName != id().providerSegmentName)
@@ -44,9 +41,9 @@ namespace armarx::armem::ltm
std::string k = i.entityName;
- if (const auto& it = _container.find(k); it != _container.end())
+ if (auto it = _container.find(k); it != _container.end())
{
- throw error::ArMemError("Somehow after clearing the container a key k = " + k + " was found. Do you have double entries in mongodb?");
+ throw error::ArMemError("Somehow after clearing the (provvider) container a key k = " + k + " was found. Do you have double entries in mongodb?");
}
else
{
@@ -55,6 +52,8 @@ namespace armarx::armem::ltm
wms.first->second.reload();
}
}
+
+ ARMARX_INFO << "After reload has provider segment " << id().str() << " size: " << _container.size();
}
void ProviderSegment::append(const wm::ProviderSegment& m)
@@ -63,9 +62,9 @@ namespace armarx::armem::ltm
mongocxx::database db = client[dbsettings.database];
mongocxx::collection coll = db[id().str()];
- for (const auto& [k, s] : m.container())
+ m.forEachEntity([this, &coll](const wm::Entity & s)
{
- if (const auto& it = _container.find(k); it != _container.end())
+ if (auto it = _container.find(s.name()); it != _container.end())
{
it->second.append(s);
}
@@ -73,14 +72,16 @@ namespace armarx::armem::ltm
{
auto builder = bsoncxx::builder::stream::document{};
bsoncxx::document::value foreign_key = builder
- << "foreign_key" << s.id().withEntityName(k).str()
+ << "foreign_key" << s.id().withEntityName(s.name()).str()
<< bsoncxx::builder::stream::finalize;
coll.insert_one(foreign_key.view());
- auto wms = _container.emplace(k, id().withEntityName(k));
+ auto wms = _container.emplace(s.name(), id().withEntityName(s.name()));
wms.first->second.dbsettings = dbsettings;
wms.first->second.append(s);
}
- }
+
+ return true;
+ });
}
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h
index bb49f232e9fe231e1cd500e3f9739ddaa0d7d83f..bf1d5e3a9f4cbd816e087957d0bb3d6351b0b6cd 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h
@@ -1,10 +1,10 @@
#pragma once
-#include "../base/ProviderSegmentBase.h"
-
#include "Entity.h"
+#include "mongodb/MongoDBConnectionManager.h"
-#include "../workingmemory/ProviderSegment.h"
+#include <RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
namespace armarx::armem::ltm
@@ -21,7 +21,6 @@ namespace armarx::armem::ltm
public:
using Base::ProviderSegmentBase;
- using Base::operator=;
// Conversion
@@ -31,19 +30,6 @@ namespace armarx::armem::ltm
void reload();
void append(const wm::ProviderSegment&);
- protected:
- virtual void _copySelf(ProviderSegment& other) const override
- {
- Base::_copySelf(other);
- other.dbsettings = dbsettings;
- }
-
- virtual void _copySelfEmpty(ProviderSegment& other) const override
- {
- Base::_copySelfEmpty(other);
- other.dbsettings = dbsettings;
- }
-
public:
MongoDBConnectionManager::MongoDBSettings dbsettings;
};
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/mongodb/MongoDBConnectionManager.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/mongodb/MongoDBConnectionManager.cpp
index 6ad44836900e04765790a9a3c2a19881e1288656..fd2bbbb6ccb8aee93c4d343b818dc55872f7838a 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/mongodb/MongoDBConnectionManager.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/mongodb/MongoDBConnectionManager.cpp
@@ -20,7 +20,7 @@ namespace armarx::armem::ltm
initialize_if();
const auto uri_str = settings.uri();
- const auto& it = Connections.find(uri_str);
+ auto it = Connections.find(uri_str);
if (it == Connections.end())
{
mongocxx::uri uri(uri_str);
@@ -43,7 +43,7 @@ namespace armarx::armem::ltm
if (!force)
{
const auto uri_str = settings.uri();
- const auto& it = Connections.find(uri_str);
+ auto it = Connections.find(uri_str);
if (it != Connections.end())
{
auto admin = it->second["admin"];
@@ -70,7 +70,7 @@ namespace armarx::armem::ltm
initialize_if();
const auto uri_str = settings.uri();
- const auto& it = Connections.find(uri_str);
+ auto it = Connections.find(uri_str);
return it != Connections.end();
}
}
diff --git a/source/RobotAPI/libraries/armem/core/operations.cpp b/source/RobotAPI/libraries/armem/core/operations.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b9ebedd316a41494d93bf99b1700fd83c475ba71
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/operations.cpp
@@ -0,0 +1,32 @@
+#include "operations.h"
+
+
+namespace armarx
+{
+
+ std::vector<aron::datanavigator::DictNavigatorPtr>
+ armem::getAronData(const wm::EntitySnapshot& snapshot)
+ {
+ std::vector<aron::datanavigator::DictNavigatorPtr> result;
+ snapshot.forEachChild([&result](const wm::EntityInstance & instance)
+ {
+ result.push_back(instance.data());
+ return true;
+ });
+ return result;
+ }
+
+
+ armem::EntityUpdate armem::toEntityUpdate(const wm::EntitySnapshot& snapshot)
+ {
+ EntityUpdate up;
+ up.entityID = snapshot.id().getEntityID();
+ up.timeCreated = snapshot.time();
+ up.instancesData = getAronData(snapshot);
+ return up;
+ }
+
+}
+
+
+
diff --git a/source/RobotAPI/libraries/armem/core/operations.h b/source/RobotAPI/libraries/armem/core/operations.h
new file mode 100644
index 0000000000000000000000000000000000000000..3b0182e324f32b832c97aaafa04ada90e604c9fa
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/operations.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include "wm/memory_definitions.h"
+
+
+/*
+ * Functions performing some "advanced" operations on the memory
+ * data structure, which are using their public API but should not
+ * be part of it.
+ */
+
+namespace armarx::armem
+{
+
+ std::vector<aron::datanavigator::DictNavigatorPtr>
+ getAronData(const wm::EntitySnapshot& snapshot);
+
+ EntityUpdate toEntityUpdate(const wm::EntitySnapshot& snapshot);
+
+
+ template <class ContainerT>
+ Commit toCommit(const ContainerT& container)
+ {
+ Commit commit;
+ container.forEachSnapshot([&commit](const wm::EntitySnapshot & snapshot)
+ {
+ commit.add(toEntityUpdate(snapshot));
+ return true;
+ });
+ return commit;
+ }
+
+}
+
diff --git a/source/RobotAPI/libraries/armem/core/wm.h b/source/RobotAPI/libraries/armem/core/wm.h
new file mode 100644
index 0000000000000000000000000000000000000000..83776ed701261bde89ed1fe2fd1e4ff479f06f44
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/wm.h
@@ -0,0 +1,10 @@
+#pragma once
+
+#include "wm/memory_definitions.h"
+
+
+namespace armarx::armem::wm
+{
+
+}
+
diff --git a/source/RobotAPI/libraries/armem/core/wm/aron_conversions.cpp b/source/RobotAPI/libraries/armem/core/wm/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a4a5faa1404129feed5f2573c706c22900c108df
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/wm/aron_conversions.cpp
@@ -0,0 +1,69 @@
+#include "aron_conversions.h"
+
+#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
+
+
+namespace armarx::armem
+{
+
+ constexpr const char* DATA_WRAPPER_DATA_FIELD = "__ARON_DATA";
+ constexpr const char* DATA_WRAPPER_TIME_STORED_FIELD = "__WRITER_METADATA__TIME_STORED";
+ constexpr const char* DATA_WRAPPER_TIME_CREATED_FIELD = "__ENTITY_METADATA__TIME_CREATED";
+ constexpr const char* DATA_WRAPPER_TIME_SENT_FIELD = "__ENTITY_METADATA__TIME_SENT";
+ constexpr const char* DATA_WRAPPER_TIME_ARRIVED_FIELD = "__ENTITY_METADATA__TIME_ARRIVED";
+ constexpr const char* DATA_WRAPPER_CONFIDENCE_FIELD = "__ENTITY_METADATA__CONFIDENCE";
+
+}
+
+
+void armarx::armem::from_aron(const aron::datanavigator::DictNavigatorPtr& dataWrapped, wm::EntityInstance& e)
+{
+ wm::EntityInstanceMetadata& metadata = e.metadata();
+
+ if (dataWrapped->hasElement(DATA_WRAPPER_DATA_FIELD))
+ {
+ e.data() = aron::datanavigator::DictNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_DATA_FIELD));
+ }
+
+ auto timeCreated = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_TIME_CREATED_FIELD));
+ metadata.timeCreated = Time::microSeconds(timeCreated->toAronLongPtr()->value);
+
+ auto timeSent = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_TIME_SENT_FIELD));
+ metadata.timeSent = Time::microSeconds(timeSent->toAronLongPtr()->value);
+
+ auto timeArrived = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_TIME_ARRIVED_FIELD));
+ metadata.timeArrived = Time::microSeconds(timeArrived->toAronLongPtr()->value);
+
+ auto confidence = aron::datanavigator::DoubleNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_CONFIDENCE_FIELD));
+ metadata.confidence = static_cast<float>(confidence->toAronDoublePtr()->value);
+}
+
+
+void armarx::armem::to_aron(aron::datanavigator::DictNavigatorPtr& a, const wm::EntityInstance& e)
+{
+ if (e.data())
+ {
+ a->addElement(DATA_WRAPPER_DATA_FIELD, e.data());
+ }
+
+ auto timeWrapped = std::make_shared<aron::datanavigator::LongNavigator>();
+ timeWrapped->setValue(Time::now().toMicroSeconds());
+ a->addElement(DATA_WRAPPER_TIME_STORED_FIELD, timeWrapped);
+
+ const wm::EntityInstanceMetadata& metadata = e.metadata();
+ auto timeCreated = std::make_shared<aron::datanavigator::LongNavigator>();
+ timeCreated->setValue(metadata.timeCreated.toMicroSeconds());
+ a->addElement(DATA_WRAPPER_TIME_CREATED_FIELD, timeCreated);
+
+ auto timeSent = std::make_shared<aron::datanavigator::LongNavigator>();
+ timeSent->setValue(metadata.timeSent.toMicroSeconds());
+ a->addElement(DATA_WRAPPER_TIME_SENT_FIELD, timeSent);
+
+ auto timeArrived = std::make_shared<aron::datanavigator::LongNavigator>();
+ timeArrived->setValue(metadata.timeArrived.toMicroSeconds());
+ a->addElement(DATA_WRAPPER_TIME_ARRIVED_FIELD, timeArrived);
+
+ auto confidence = std::make_shared<aron::datanavigator::DoubleNavigator>();
+ confidence->setValue(static_cast<double>(metadata.confidence));
+ a->addElement(DATA_WRAPPER_CONFIDENCE_FIELD, confidence);
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/entityInstance_conversions.h b/source/RobotAPI/libraries/armem/core/wm/aron_conversions.h
similarity index 59%
rename from source/RobotAPI/libraries/armem/core/workingmemory/entityInstance_conversions.h
rename to source/RobotAPI/libraries/armem/core/wm/aron_conversions.h
index f3b25566f110dca844e24dcac4c997f724c74548..f6b43dc8bd468008b9fe8ee470564f5de89e16cb 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/entityInstance_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/wm/aron_conversions.h
@@ -1,6 +1,9 @@
#pragma once
-#include "Memory.h"
+#include <RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h>
+
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+
namespace armarx::armem
{
diff --git a/source/RobotAPI/libraries/armem/core/wm/ice_conversions.cpp b/source/RobotAPI/libraries/armem/core/wm/ice_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..70a905a931fd331a6ecdfbc2c278924b0da9144a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/wm/ice_conversions.cpp
@@ -0,0 +1,64 @@
+#include "ice_conversions.h"
+
+#include <RobotAPI/libraries/armem/core/base/ice_conversions.h>
+
+
+namespace armarx::armem
+{
+
+ void wm::toIce(data::EntityInstance& ice, const EntityInstance& data)
+ {
+ base::toIce(ice, data);
+ }
+ void wm::fromIce(const data::EntityInstance& ice, EntityInstance& data)
+ {
+ base::fromIce(ice, data);
+ }
+
+
+ void wm::toIce(data::EntitySnapshot& ice, const EntitySnapshot& snapshot)
+ {
+ base::toIce(ice, snapshot);
+ }
+ void wm::fromIce(const data::EntitySnapshot& ice, EntitySnapshot& snapshot)
+ {
+ base::fromIce(ice, snapshot);
+ }
+
+ void wm::toIce(data::Entity& ice, const Entity& entity)
+ {
+ base::toIce(ice, entity);
+ }
+ void wm::fromIce(const data::Entity& ice, Entity& entity)
+ {
+ base::fromIce(ice, entity);
+ }
+
+ void wm::toIce(data::ProviderSegment& ice, const ProviderSegment& providerSegment)
+ {
+ base::toIce(ice, providerSegment);
+ }
+ void wm::fromIce(const data::ProviderSegment& ice, ProviderSegment& providerSegment)
+ {
+ base::fromIce(ice, providerSegment);
+ }
+
+ void wm::toIce(data::CoreSegment& ice, const CoreSegment& coreSegment)
+ {
+ base::toIce(ice, coreSegment);
+ }
+ void wm::fromIce(const data::CoreSegment& ice, CoreSegment& coreSegment)
+ {
+ base::fromIce(ice, coreSegment);
+ }
+
+ void wm::toIce(data::Memory& ice, const Memory& memory)
+ {
+ base::toIce(ice, memory);
+ }
+ void wm::fromIce(const data::Memory& ice, Memory& memory)
+ {
+ base::fromIce(ice, memory);
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/wm/ice_conversions.h b/source/RobotAPI/libraries/armem/core/wm/ice_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..4a58f16aaa6eef4c6f6685a86ed66298c1407454
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/wm/ice_conversions.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include <RobotAPI/interface/armem/commit.h>
+#include <RobotAPI/interface/armem/memory.h>
+
+#include "memory_definitions.h"
+
+
+namespace armarx::armem::wm
+{
+
+ void toIce(data::EntityInstance& ice, const EntityInstance& data);
+ void fromIce(const data::EntityInstance& ice, EntityInstance& data);
+
+
+ void toIce(data::EntitySnapshot& ice, const EntitySnapshot& snapshot);
+ void fromIce(const data::EntitySnapshot& ice, EntitySnapshot& snapshot);
+
+ void toIce(data::Entity& ice, const Entity& entity);
+ void fromIce(const data::Entity& ice, Entity& entity);
+
+
+ void toIce(data::ProviderSegment& ice, const ProviderSegment& providerSegment);
+ void fromIce(const data::ProviderSegment& ice, ProviderSegment& providerSegment);
+
+ void toIce(data::CoreSegment& ice, const CoreSegment& coreSegment);
+ void fromIce(const data::CoreSegment& ice, CoreSegment& coreSegment);
+
+ void toIce(data::Memory& ice, const Memory& memory);
+ void fromIce(const data::Memory& ice, Memory& memory);
+}
+
+// Must be included after the prototypes. Otherwise the compiler cannot find the correct methods in ice_coversion_templates.h
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/json_conversions.cpp b/source/RobotAPI/libraries/armem/core/wm/json_conversions.cpp
similarity index 64%
rename from source/RobotAPI/libraries/armem/core/workingmemory/json_conversions.cpp
rename to source/RobotAPI/libraries/armem/core/wm/json_conversions.cpp
index e7499d8b42edf895bbbdb2f0d06a1fe3ed2ee823..fd67c5f822cbfa3783bf1fc23b3de24f06a27f88 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/json_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/wm/json_conversions.cpp
@@ -1,10 +1,14 @@
#include "json_conversions.h"
#include <RobotAPI/libraries/aron/core/Debug.h>
+#include <RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.h>
+#include <RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.h>
+#include <RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h>
+#include <RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
+
namespace armarx::armem
{
-
void from_aron(const aron::datanavigator::DictNavigatorPtr& aron, nlohmann::json& j)
{
aron::dataIO::writer::NlohmannJSONWriter dataWriter;
@@ -12,7 +16,8 @@ namespace armarx::armem
j = dataWriter.getResult();
}
- void to_aron(aron::datanavigator::DictNavigatorPtr& a, const nlohmann::json& e, const aron::typenavigator::NavigatorPtr& expectedStructure)
+ void to_aron(aron::datanavigator::DictNavigatorPtr& a, const nlohmann::json& e,
+ const aron::typenavigator::NavigatorPtr& expectedStructure)
{
aron::dataIO::reader::NlohmannJSONReader dataReader(e);
aron::dataIO::writer::NavigatorWriter navWriter;
diff --git a/source/RobotAPI/libraries/armem/core/wm/json_conversions.h b/source/RobotAPI/libraries/armem/core/wm/json_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..4ce93812b606688e642eecc0b7856210adce1a90
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/wm/json_conversions.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h>
+
+#include <SimoxUtility/json/json.hpp>
+
+
+namespace armarx::armem
+{
+ void from_aron(const aron::datanavigator::DictNavigatorPtr&, nlohmann::json&);
+ void to_aron(aron::datanavigator::DictNavigatorPtr&, const nlohmann::json&,
+ const aron::typenavigator::NavigatorPtr& expectedStructure = nullptr);
+}
diff --git a/source/RobotAPI/libraries/armem/core/wm/memory_definitions.cpp b/source/RobotAPI/libraries/armem/core/wm/memory_definitions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c9516fc3b74ce05797d7d8c39b12f83b0bdf740d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/wm/memory_definitions.cpp
@@ -0,0 +1,92 @@
+#include "memory_definitions.h"
+
+#include "error.h"
+
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <map>
+#include <vector>
+
+
+namespace armarx::armem::wm
+{
+
+ bool EntityInstance::equalsDeep(const EntityInstance& other) const
+ {
+ if (_data and other.data())
+ {
+ return id() == other.id()
+ && _metadata == other.metadata()
+ && *_data == *other.data();
+ }
+ if (_data or other.data())
+ {
+ return false;
+ }
+ return id() == other.id() && _metadata == other.metadata();
+ }
+
+
+ void EntityInstance::update(const EntityUpdate& update)
+ {
+ ARMARX_CHECK_FITS_SIZE(this->index(), update.instancesData.size());
+
+ this->data() = update.instancesData.at(size_t(this->index()));
+
+ this->_metadata.confidence = update.confidence;
+
+ this->_metadata.timeCreated = update.timeCreated;
+ this->_metadata.timeSent = update.timeSent;
+ this->_metadata.timeArrived = update.timeArrived;
+ }
+
+
+
+ std::optional<wm::EntitySnapshot> CoreSegment::getLatestEntitySnapshot(const MemoryID& entityID) const
+ {
+ const wm::Entity& entity = this->getEntity(entityID);
+ if (entity.empty())
+ {
+ return std::nullopt;
+ }
+ else
+ {
+ return entity.getLatestSnapshot();
+ }
+ }
+
+
+ std::optional<wm::EntityInstance> CoreSegment::getLatestEntityInstance(
+ const MemoryID& entityID, int instanceIndex) const
+ {
+ auto snapshot = getLatestEntitySnapshot(entityID);
+ if (snapshot.has_value()
+ and instanceIndex >= 0
+ and static_cast<size_t>(instanceIndex) < snapshot->size())
+ {
+ return snapshot->getInstance(instanceIndex);
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+ armarx::aron::datanavigator::DictNavigatorPtr
+ CoreSegment::getLatestEntityInstanceData(const MemoryID& entityID, int instanceIndex) const
+ {
+ auto instance = getLatestEntityInstance(entityID, instanceIndex);
+ if (instance.has_value())
+ {
+ return instance->data();
+ }
+ else
+ {
+ return nullptr;
+ }
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/wm/memory_definitions.h b/source/RobotAPI/libraries/armem/core/wm/memory_definitions.h
new file mode 100644
index 0000000000000000000000000000000000000000..679153b47809682fec38adb5baea609dd99c5c28
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/wm/memory_definitions.h
@@ -0,0 +1,135 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/base/EntityInstanceBase.h>
+#include <RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h>
+#include <RobotAPI/libraries/armem/core/base/EntityBase.h>
+#include <RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h>
+#include <RobotAPI/libraries/armem/core/base/CoreSegmentBase.h>
+#include <RobotAPI/libraries/armem/core/base/MemoryBase.h>
+
+#include <optional>
+
+
+namespace armarx::armem::wm
+{
+
+ using EntityInstanceMetadata = base::EntityInstanceMetadata;
+ using EntityInstanceData = armarx::aron::datanavigator::DictNavigator;
+ using EntityInstanceDataPtr = armarx::aron::datanavigator::DictNavigatorPtr;
+
+
+ /// @see base::EntityInstanceBase
+ class EntityInstance :
+ public base::EntityInstanceBase<EntityInstanceDataPtr, EntityInstanceMetadata>
+ {
+ using Base = base::EntityInstanceBase<EntityInstanceDataPtr, EntityInstanceMetadata>;
+
+ public:
+
+ using Base::EntityInstanceBase;
+
+
+ /**
+ * @brief Fill `*this` with the update's values.
+ * @param update The update.
+ * @param index The instances index.
+ */
+ void update(const EntityUpdate& update);
+
+ bool equalsDeep(const EntityInstance& other) const;
+
+ };
+
+
+
+ /// @see base::EntitySnapshotBase
+ class EntitySnapshot :
+ public base::EntitySnapshotBase<EntityInstance, EntitySnapshot>
+ {
+ public:
+
+ using base::EntitySnapshotBase<EntityInstance, EntitySnapshot>::EntitySnapshotBase;
+
+ };
+
+
+ /// @see base::EntityBase
+ class Entity :
+ public base::EntityBase<EntitySnapshot, Entity>
+ {
+ public:
+
+ using base::EntityBase<EntitySnapshot, Entity>::EntityBase;
+
+ };
+
+
+
+ /// @see base::ProviderSegmentBase
+ class ProviderSegment :
+ public base::ProviderSegmentBase<Entity, ProviderSegment>
+ {
+ public:
+
+ using base::ProviderSegmentBase<Entity, ProviderSegment>::ProviderSegmentBase;
+
+ };
+
+
+
+ /// @see base::CoreSegmentBase
+ class CoreSegment :
+ public base::CoreSegmentBase<ProviderSegment, CoreSegment>
+ {
+ using Base = base::CoreSegmentBase<ProviderSegment, CoreSegment>;
+
+ public:
+
+ using Base::CoreSegmentBase;
+
+
+ // Non-locking interface
+
+ std::optional<wm::EntitySnapshot> getLatestEntitySnapshot(
+ const MemoryID& entityID) const;
+ std::optional<wm::EntityInstance> getLatestEntityInstance(
+ const MemoryID& entityID, int instanceIndex = 0) const;
+ armarx::aron::datanavigator::DictNavigatorPtr getLatestEntityInstanceData(
+ const MemoryID& entityID, int instanceIndex = 0) const;
+
+
+ template <class AronDtoT>
+ std::optional<AronDtoT> getLatestEntityInstanceDataAs(
+ const MemoryID& entityID, int instanceIndex = 0) const
+ {
+ wm::EntityInstanceDataPtr data = getLatestEntityInstanceData(entityID, instanceIndex);
+ if (data)
+ {
+ AronDtoT aron;
+ aron.fromAron(data);
+ return aron;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+ };
+
+
+
+ /// @see base::MemoryBase
+ class Memory :
+ public base::MemoryBase<CoreSegment, Memory>
+ {
+ using Base = base::MemoryBase<CoreSegment, Memory>;
+
+ public:
+
+ using Base::MemoryBase;
+
+ };
+
+}
+
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.cpp b/source/RobotAPI/libraries/armem/core/wm/visitor/FunctionalVisitor.cpp
similarity index 82%
rename from source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.cpp
rename to source/RobotAPI/libraries/armem/core/wm/visitor/FunctionalVisitor.cpp
index 53016fc6c83d9ef6e24a2e75942a12bced7d0eaa..f72b37c1a7ab68320c8ff2ec8c071dd741e61f22 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.cpp
+++ b/source/RobotAPI/libraries/armem/core/wm/visitor/FunctionalVisitor.cpp
@@ -2,7 +2,7 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/core/error.h>
@@ -18,6 +18,4 @@ namespace armarx::armem::wm
{
}
-
-
}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h b/source/RobotAPI/libraries/armem/core/wm/visitor/FunctionalVisitor.h
similarity index 82%
rename from source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h
rename to source/RobotAPI/libraries/armem/core/wm/visitor/FunctionalVisitor.h
index 1c1b46e138c7eb2d713ba15b76ba77a2588ea0ce..34390a1823a6f3f1651bbe59ec815a0c1ab48bd6 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h
+++ b/source/RobotAPI/libraries/armem/core/wm/visitor/FunctionalVisitor.h
@@ -53,28 +53,28 @@ namespace armarx::armem::wm
bool visitEnter(const Memory& memory) override
{
- return memoryFn ? memoryConstFn(memory) : Visitor::visitEnter(memory);
+ return memoryConstFn ? memoryConstFn(memory) : Visitor::visitEnter(memory);
}
bool visitEnter(const CoreSegment& coreSegment) override
{
- return coreSegmentFn ? coreSegmentConstFn(coreSegment) : Visitor::visitEnter(coreSegment);
+ return coreSegmentConstFn ? coreSegmentConstFn(coreSegment) : Visitor::visitEnter(coreSegment);
}
bool visitEnter(const ProviderSegment& providerSegment) override
{
- return providerSegmentFn ? providerSegmentConstFn(providerSegment) : Visitor::visitEnter(providerSegment);
+ return providerSegmentConstFn ? providerSegmentConstFn(providerSegment) : Visitor::visitEnter(providerSegment);
}
bool visitEnter(const Entity& entity) override
{
- return entityFn ? entityConstFn(entity) : Visitor::visitEnter(entity);
+ return entityConstFn ? entityConstFn(entity) : Visitor::visitEnter(entity);
}
bool visitEnter(const EntitySnapshot& snapshot) override
{
- return memoryFn ? snapshotConstFn(snapshot) : Visitor::visitEnter(snapshot);
+ return snapshotConstFn ? snapshotConstFn(snapshot) : Visitor::visitEnter(snapshot);
}
bool visit(const EntityInstance& instance) override
{
- return instanceFn ? instanceConstFn(instance) : Visitor::visit(instance);
+ return instanceConstFn ? instanceConstFn(instance) : Visitor::visit(instance);
}
diff --git a/source/RobotAPI/libraries/armem/core/wm/visitor/Visitor.cpp b/source/RobotAPI/libraries/armem/core/wm/visitor/Visitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1140bc11be0a43e41e2e5c690ae8a73132440a6b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/wm/visitor/Visitor.cpp
@@ -0,0 +1,141 @@
+#include "Visitor.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+
+
+namespace armarx::armem::wm
+{
+
+ Visitor::Visitor()
+ {
+ }
+
+ Visitor::~Visitor()
+ {
+ }
+
+ bool Visitor::applyTo(Memory& memory)
+ {
+ visitEnter(memory);
+ bool cont = memory.forEachCoreSegment([this](CoreSegment & coreSeg)
+ {
+ return applyTo(coreSeg);
+ });
+ visitExit(memory);
+ return cont;
+ }
+
+ bool Visitor::applyTo(CoreSegment& coreSegment)
+ {
+ visitEnter(coreSegment);
+ bool cont = coreSegment.forEachProviderSegment([this](ProviderSegment & provSeg)
+ {
+ return applyTo(provSeg);
+ });
+ visitExit(coreSegment);
+ return cont;
+ }
+
+ bool Visitor::applyTo(ProviderSegment& providerSegment)
+ {
+ visitEnter(providerSegment);
+ bool cont = providerSegment.forEachEntity([this](Entity & entity)
+ {
+ return applyTo(entity);
+ });
+ visitExit(providerSegment);
+ return cont;
+ }
+
+ bool Visitor::applyTo(Entity& entity)
+ {
+ visitEnter(entity);
+ bool cont = entity.forEachSnapshot([this](EntitySnapshot & snapshot)
+ {
+ return applyTo(snapshot);
+ });
+ visitExit(entity);
+ return cont;
+ }
+
+ bool Visitor::applyTo(EntitySnapshot& snapshot)
+ {
+ visitEnter(snapshot);
+ bool cont = snapshot.forEachInstance([this](EntityInstance & instance)
+ {
+ return applyTo(instance);
+ });
+ visitExit(snapshot);
+ return cont;
+ }
+
+ bool Visitor::applyTo(EntityInstance& instance)
+ {
+ return visit(instance);
+ }
+
+
+ bool Visitor::applyTo(const Memory& memory)
+ {
+ visitEnter(memory);
+ bool cont = memory.forEachCoreSegment([this](const CoreSegment & coreSeg)
+ {
+ return applyTo(coreSeg);
+ });
+ visitExit(memory);
+ return cont;
+ }
+
+ bool Visitor::applyTo(const CoreSegment& coreSegment)
+ {
+ visitEnter(coreSegment);
+ bool cont = coreSegment.forEachProviderSegment([this](const ProviderSegment & provSeg)
+ {
+ return applyTo(provSeg);
+ });
+ visitExit(coreSegment);
+ return cont;
+ }
+
+ bool Visitor::applyTo(const ProviderSegment& providerSegment)
+ {
+ visitEnter(providerSegment);
+ bool cont = providerSegment.forEachEntity([this](const Entity & entity)
+ {
+ return applyTo(entity);
+ });
+ visitExit(providerSegment);
+ return cont;
+ }
+
+ bool Visitor::applyTo(const Entity& entity)
+ {
+ visitEnter(entity);
+ bool cont = entity.forEachSnapshot([this](const EntitySnapshot & snapshot)
+ {
+ return applyTo(snapshot);
+ });
+ visitExit(entity);
+ return cont;
+ }
+
+ bool Visitor::applyTo(const EntitySnapshot& snapshot)
+ {
+ visitEnter(snapshot);
+ bool cont = snapshot.forEachInstance([this](const EntityInstance & instance)
+ {
+ return applyTo(instance);
+ });
+ visitExit(snapshot);
+ return cont;
+ }
+
+ bool Visitor::applyTo(const EntityInstance& instance)
+ {
+ return visit(instance);
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/visitor/Visitor.h b/source/RobotAPI/libraries/armem/core/wm/visitor/Visitor.h
similarity index 100%
rename from source/RobotAPI/libraries/armem/core/workingmemory/visitor/Visitor.h
rename to source/RobotAPI/libraries/armem/core/wm/visitor/Visitor.h
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
deleted file mode 100644
index 285235b76e397952301c23d8218abb11f1f7aeaf..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
+++ /dev/null
@@ -1,30 +0,0 @@
-#include "CoreSegment.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "error.h"
-
-
-namespace armarx::armem::wm
-{
-
- Commit CoreSegment::toCommit() const
- {
- Commit c;
- for (const auto& [k, s] : _container)
- {
- c.append(s.toCommit());
- }
- return c;
- }
-
- void CoreSegment::_copySelfWithoutData(CoreSegment& other) const
- {
- other.id() = _id;
- other.setMaxHistorySize(_maxHistorySize);
- for (const auto& [k, s] : _container)
- {
- other.addProviderSegment(s.copyWithoutData());
- }
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
deleted file mode 100644
index 7847bbb71cc6a1a269556aeec07738c396e42a68..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
+++ /dev/null
@@ -1,42 +0,0 @@
-#pragma once
-
-#include "../base/CoreSegmentBase.h"
-
-#include "ProviderSegment.h"
-#include "detail/CopyWithoutData.h"
-
-
-namespace armarx::armem::wm
-{
-
- /**
- * @brief Data of a core segment containing multiple provider segments.
- */
- class CoreSegment :
- public base::CoreSegmentBase<ProviderSegment, CoreSegment>,
- public detail::CopyWithoutData<CoreSegment>
- {
- using Base = base::CoreSegmentBase<ProviderSegment, CoreSegment>;
-
- public:
-
- using Base::CoreSegmentBase;
-
- CoreSegment(const CoreSegment& other) = default;
- CoreSegment(CoreSegment&& other) = default;
- CoreSegment& operator=(const CoreSegment& other) = default;
- CoreSegment& operator=(CoreSegment&& other) = default;
-
- /**
- * @brief Convert the content of this segmnet into a commit
- * @return The resulting commit
- */
- Commit toCommit() const;
-
- protected:
-
- virtual void _copySelfWithoutData(CoreSegment& other) const override;
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Entity.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/Entity.cpp
deleted file mode 100644
index e704e96d5454d27bfb095120c1901e000611122a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Entity.cpp
+++ /dev/null
@@ -1,28 +0,0 @@
-#include "Entity.h"
-
-namespace armarx::armem::wm
-{
-
- Commit Entity::toCommit() const
- {
- Commit c;
- for (const auto& [k, s] : _container)
- {
- EntityUpdate& up = c.add();
- up.entityID = id();
- up.timeCreated = k;
- up.instancesData = s.getAronData();
- }
- return c;
- }
-
- void Entity::_copySelfWithoutData(Entity& other) const
- {
- other.id() = _id;
- other.setMaxHistorySize(_maxHistorySize);
- for (const auto& [k, s] : _container)
- {
- other.addSnapshot(s.copyWithoutData());
- }
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h b/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h
deleted file mode 100644
index a6a7621e21272db6447e9e09b695900eefd263c1..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h
+++ /dev/null
@@ -1,57 +0,0 @@
-#pragma once
-
-#include "../base/EntityBase.h"
-
-#include "EntitySnapshot.h"
-#include "detail/CopyWithoutData.h"
-
-
-namespace armarx::armem::wm
-{
- /**
- * @brief An entity over a period of time.
- *
- * An entity should be a physical thing or abstract concept existing
- * (and potentially evolving) over some time.
- *
- * Examples are:
- * - objects (the green box)
- * - agents (robot, human)
- * - locations (frige, sink)
- * - grasp affordances (general, or for a specific object)
- * - images
- * - point clouds
- * - other sensory values
- *
- * At each point in time (`EntitySnapshot`), the entity can have a
- * (potentially variable) number of instances (`EntityInstance`),
- * each containing a single `AronData` object of a specific `AronType`.
- */
- class Entity :
- public base::EntityBase<EntitySnapshot, Entity>,
- public detail::CopyWithoutData<Entity>
- {
- using Base = base::EntityBase<EntitySnapshot, Entity>;
-
- public:
-
- using Base::EntityBase;
-
- Entity(const Entity& other) = default;
- Entity(Entity&& other) = default;
- Entity& operator=(const Entity& other) = default;
- Entity& operator=(Entity&& other) = default;
-
- /**
- * @brief Convert the content of this segmnet into a commit
- * @return The resulting commit
- */
- Commit toCommit() const;
-
- protected:
-
- virtual void _copySelfWithoutData(Entity& other) const override;
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.cpp
deleted file mode 100644
index 6149449c77533abd656a460097969f0066d8d88f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.cpp
+++ /dev/null
@@ -1,77 +0,0 @@
-#include "EntityInstance.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-
-std::ostream& armarx::armem::wm::operator<<(std::ostream& os, const EntityInstanceMetadata& d)
-{
- os << "EntityInstanceMetadata: "
- << "\n- t_create = \t" << armem::toStringMicroSeconds(d.timeCreated) << " us"
- << "\n- t_sent = \t" << armem::toStringMicroSeconds(d.timeSent) << " us"
- << "\n- t_arrived = \t" << armem::toStringMicroSeconds(d.timeArrived) << " us"
- << "\n- confidence = \t" << d.confidence << " us"
- ;
- return os;
-}
-
-namespace armarx::armem::wm
-{
-
- bool EntityInstanceMetadata::operator==(const EntityInstanceMetadata& other) const
- {
- return timeCreated == other.timeCreated
- && timeSent == other.timeSent
- && timeArrived == other.timeArrived
- && std::abs(confidence - other.confidence) < 1e-6f;
- }
-
-
- bool EntityInstance::equalsDeep(const EntityInstance& other) const
- {
- if (_data and other.data())
- {
- return id() == other.id()
- && _metadata == other.metadata()
- && *_data == *other.data();
- }
- if (_data or other.data())
- {
- return false;
- }
- return id() == other.id() && _metadata == other.metadata();
- }
-
- void EntityInstance::update(const EntityUpdate& update, int index)
- {
- ARMARX_CHECK_FITS_SIZE(index, update.instancesData.size());
-
- this->index() = index;
- setData(update.instancesData.at(size_t(index)));
-
- this->_metadata.confidence = update.confidence;
-
- this->_metadata.timeCreated = update.timeCreated;
- this->_metadata.timeSent = update.timeSent;
- this->_metadata.timeArrived = update.timeArrived;
- }
-
- EntityInstance EntityInstance::copy() const
- {
- EntityInstance d;
- this->_copySelf(d);
- return d;
- }
-
- void EntityInstance::_copySelf(EntityInstance& other) const
- {
- Base::_copySelf(other);
- other._metadata = _metadata;
- other._data = _data;
- }
-
- void EntityInstance::_copySelfWithoutData(EntityInstance& other) const
- {
- Base::_copySelf(other);
- other._metadata = _metadata;
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h b/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h
deleted file mode 100644
index 8e6eb1afe7e673baeab933397ebd16aa1781c18d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h
+++ /dev/null
@@ -1,103 +0,0 @@
-#pragma once
-
-#include "../base/EntityInstanceBase.h"
-
-#include "detail/CopyWithoutData.h"
-
-
-namespace armarx::armem::wm
-{
-
- /**
- * @brief Metadata of an entity instance.
- */
- struct EntityInstanceMetadata
- {
- /// Time when this value was created.
- Time timeCreated;
- /// Time when this value was sent to the memory.
- Time timeSent;
- /// Time when this value has arrived at the memory.
- Time timeArrived;
-
- /// An optional confidence, may be used for things like decay.
- float confidence = 1.0;
-
-
- bool operator==(const EntityInstanceMetadata& other) const;
- inline bool operator!=(const EntityInstanceMetadata& other) const
- {
- return !(*this == other);
- }
- };
-
- std::ostream& operator<<(std::ostream& os, const EntityInstanceMetadata& rhs);
-
-
- /**
- * @brief Data of a single entity instance.
- */
- class EntityInstance :
- public base::EntityInstanceBase<EntityInstance>,
- public detail::CopyWithoutData<EntityInstance>
- {
- using Base = base::EntityInstanceBase<EntityInstance>;
-
- public:
-
- using Base::EntityInstanceBase;
-
- EntityInstance(const EntityInstance& other) = default;
- EntityInstance(EntityInstance&& other) = default;
- EntityInstance& operator=(const EntityInstance& other) = default;
- EntityInstance& operator=(EntityInstance&& other) = default;
-
-
- EntityInstanceMetadata& metadata()
- {
- return _metadata;
- }
- inline const EntityInstanceMetadata& metadata() const
- {
- return _metadata;
- }
-
- inline armarx::aron::datanavigator::DictNavigatorPtr data() const
- {
- return _data;
- }
-
- void setData(const aron::datanavigator::DictNavigatorPtr& data)
- {
- this->_data = data;
- }
-
-
- /**
- * @brief Fill `*this` with the update's values.
- * @param update The update.
- * @param index The instances index.
- */
- virtual void update(const EntityUpdate& update, int index) override;
-
- virtual bool equalsDeep(const EntityInstance& other) const override;
-
- virtual EntityInstance copy() const override;
-
-
- protected:
-
- virtual void _copySelf(EntityInstance& other) const override;
- virtual void _copySelfWithoutData(EntityInstance& other) const override;
-
-
- private:
-
- /// The metadata.
- EntityInstanceMetadata _metadata;
-
- /// The data. May be nullptr.
- armarx::aron::datanavigator::DictNavigatorPtr _data;
-
- };
-}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.cpp
deleted file mode 100644
index 76d5400131a7ec6a57478f13afb0dfc38a4da174..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.cpp
+++ /dev/null
@@ -1,28 +0,0 @@
-#include "EntitySnapshot.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "error.h"
-
-
-namespace armarx::armem::wm
-{
- std::vector<aron::datanavigator::DictNavigatorPtr> EntitySnapshot::getAronData() const
- {
- std::vector<aron::datanavigator::DictNavigatorPtr> ret;
- for (const auto& aron : _container)
- {
- ret.push_back(aron.data());
- }
- return ret;
- }
-
- void EntitySnapshot::_copySelfWithoutData(EntitySnapshot& other) const
- {
- other.id() = _id;
- for (const auto& s : _container)
- {
- other.addInstance(s.copyWithoutData());
- }
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h b/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h
deleted file mode 100644
index 3eedb48d0b4aaa5191541d8ca1586ffc59ab5998..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h
+++ /dev/null
@@ -1,33 +0,0 @@
-#pragma once
-
-#include "../base/EntitySnapshotBase.h"
-
-#include "EntityInstance.h"
-#include "detail/CopyWithoutData.h"
-
-
-namespace armarx::armem::wm
-{
-
- /**
- * @brief Data of an entity at one point in time.
- */
- class EntitySnapshot :
- public base::EntitySnapshotBase<EntityInstance, EntitySnapshot>,
- public detail::CopyWithoutData<EntitySnapshot>
- {
- using Base = base::EntitySnapshotBase<EntityInstance, EntitySnapshot>;
-
- public:
-
- using Base::EntitySnapshotBase;
- using Base::operator=;
-
- std::vector<aron::datanavigator::DictNavigatorPtr> getAronData() const;
-
- protected:
-
- virtual void _copySelfWithoutData(EntitySnapshot& other) const override;
-
- };
-}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
deleted file mode 100644
index 3d2efd2249f969308b126608d01533500f3bfa73..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-#include "Memory.h"
-
-
-namespace armarx::armem::wm
-{
- Commit Memory::toCommit() const
- {
- Commit c;
- for (const auto& [k, s] : _container)
- {
- c.append(s.toCommit());
- }
- return c;
- }
-
- void Memory::_copySelfWithoutData(Memory& other) const
- {
- other.id() = _id;
- for (const auto& [k, s] : _container)
- {
- other.addCoreSegment(s.copyWithoutData());
- }
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
deleted file mode 100644
index 3c64c0841b46a5c95f7f7365af26a1b20226f4dc..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
+++ /dev/null
@@ -1,41 +0,0 @@
-#pragma once
-
-#include "../base/MemoryBase.h"
-
-#include "CoreSegment.h"
-#include "detail/CopyWithoutData.h"
-
-
-namespace armarx::armem::wm
-{
-
- /**
- * @brief Data of a memory consisting of multiple core segments.
- */
- class Memory :
- public base::MemoryBase<CoreSegment, Memory>,
- public detail::CopyWithoutData<Memory>
- {
- using Base = base::MemoryBase<CoreSegment, Memory>;
-
- public:
-
- using Base::MemoryBase;
-
- Memory(const Memory& other) = default;
- Memory(Memory&& other) = default;
- Memory& operator=(const Memory& other) = default;
- Memory& operator=(Memory&& other) = default;
-
- /**
- * @brief Convert the content of this memory into a commit
- * @return The resulting commit
- */
- Commit toCommit() const;
-
- protected:
-
- virtual void _copySelfWithoutData(Memory& other) const override;
-
- };
-}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.cpp
deleted file mode 100644
index bd03cf21cd8b616a3afcc4d531377288e55c5f2f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.cpp
+++ /dev/null
@@ -1,31 +0,0 @@
-#include "ProviderSegment.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "error.h"
-
-
-namespace armarx::armem::wm
-{
-
- Commit ProviderSegment::toCommit() const
- {
- Commit c;
- for (const auto& [k, s] : _container)
- {
- c.append(s.toCommit());
- }
- return c;
- }
-
- void ProviderSegment::_copySelfWithoutData(ProviderSegment& other) const
- {
- other.id() = _id;
- other.setMaxHistorySize(_maxHistorySize);
- for (const auto& [k, s] : _container)
- {
- other.addEntity(s.copyWithoutData());
- }
- }
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h b/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h
deleted file mode 100644
index 7d1e9db335ee39433351a8e15113059836bf825d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h
+++ /dev/null
@@ -1,44 +0,0 @@
-#pragma once
-
-#include "../base/ProviderSegmentBase.h"
-
-#include "Entity.h"
-#include "detail/CopyWithoutData.h"
-
-
-namespace armarx::armem::wm
-{
-
- /**
- * @brief Data of a provider segment containing multiple entities.
- */
- class ProviderSegment :
- public base::ProviderSegmentBase<Entity, ProviderSegment>,
- public detail::CopyWithoutData<ProviderSegment>
-
- {
- using Base = base::ProviderSegmentBase<Entity, ProviderSegment>;
-
- public:
-
- using Base::ProviderSegmentBase;
-
- ProviderSegment(const ProviderSegment& other) = default;
- ProviderSegment(ProviderSegment&& other) = default;
- ProviderSegment& operator=(const ProviderSegment& other) = default;
- ProviderSegment& operator=(ProviderSegment&& other) = default;
-
- /**
- * @brief Convert the content of this segmnet into a commit
- * @return The resulting commit
- */
- Commit toCommit() const;
-
-
- protected:
-
- virtual void _copySelfWithoutData(ProviderSegment& other) const override;
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/detail/CopyWithoutData.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/detail/CopyWithoutData.cpp
deleted file mode 100644
index 70441033dd0fcb24a502bcd4e552764c7c2f5d3c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/detail/CopyWithoutData.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "CopyWithoutData.h"
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/detail/CopyWithoutData.h b/source/RobotAPI/libraries/armem/core/workingmemory/detail/CopyWithoutData.h
deleted file mode 100644
index caabd1e6002c803aa6a1015afd7ce660abd2482f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/detail/CopyWithoutData.h
+++ /dev/null
@@ -1,30 +0,0 @@
-#pragma once
-
-
-namespace armarx::armem::wm::detail
-{
-
- /**
- * @class Allows copying `*this` without data in the leaf
- * data structures.
- */
- template <class DerivedT>
- class CopyWithoutData
- {
- public:
-
- /// Get a copy of `this` without data.
- virtual DerivedT copyWithoutData() const
- {
- DerivedT t;
- _copySelfWithoutData(t);
- return t;
- }
-
-
- protected:
-
- virtual void _copySelfWithoutData(DerivedT& other) const = 0;
-
- };
-}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/entityInstance_conversions.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/entityInstance_conversions.cpp
deleted file mode 100644
index 31538f8d8e2f1eeaf932ed16e50b7040520f0a99..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/entityInstance_conversions.cpp
+++ /dev/null
@@ -1,64 +0,0 @@
-#include "json_conversions.h"
-
-namespace armarx::armem
-{
-
- constexpr const char* DATA_WRAPPER_DATA_FIELD = "__ARON_DATA";
- constexpr const char* DATA_WRAPPER_TIME_STORED_FIELD = "__WRITER_METADATA__TIME_STORED";
- constexpr const char* DATA_WRAPPER_TIME_CREATED_FIELD = "__ENTITY_METADATA__TIME_CREATED";
- constexpr const char* DATA_WRAPPER_TIME_SENT_FIELD = "__ENTITY_METADATA__TIME_SENT";
- constexpr const char* DATA_WRAPPER_TIME_ARRIVED_FIELD = "__ENTITY_METADATA__TIME_ARRIVED";
- constexpr const char* DATA_WRAPPER_CONFIDENCE_FIELD = "__ENTITY_METADATA__CONFIDENCE";
-
- void from_aron(const aron::datanavigator::DictNavigatorPtr& dataWrapped, wm::EntityInstance& e)
- {
- wm::EntityInstanceMetadata& metadata = e.metadata();
-
- if (dataWrapped->hasElement(DATA_WRAPPER_DATA_FIELD))
- {
- aron::datanavigator::DictNavigatorPtr data = aron::datanavigator::DictNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_DATA_FIELD));
- e.setData(data);
- }
-
- auto timeCreated = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_TIME_CREATED_FIELD));
- metadata.timeCreated = Time::microSeconds(timeCreated->toAronLongPtr()->value);
-
- auto timeSent = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_TIME_SENT_FIELD));
- metadata.timeSent = Time::microSeconds(timeSent->toAronLongPtr()->value);
-
- auto timeArrived = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_TIME_ARRIVED_FIELD));
- metadata.timeArrived = Time::microSeconds(timeArrived->toAronLongPtr()->value);
-
- auto confidence = aron::datanavigator::DoubleNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_CONFIDENCE_FIELD));
- metadata.confidence = static_cast<float>(confidence->toAronDoublePtr()->value);
- }
-
- void to_aron(aron::datanavigator::DictNavigatorPtr& a, const wm::EntityInstance& e)
- {
- if (e.data())
- {
- a->addElement(DATA_WRAPPER_DATA_FIELD, e.data());
- }
-
- auto timeWrapped = std::make_shared<aron::datanavigator::LongNavigator>();
- timeWrapped->setValue(Time::now().toMicroSeconds());
- a->addElement(DATA_WRAPPER_TIME_STORED_FIELD, timeWrapped);
-
- const wm::EntityInstanceMetadata& metadata = e.metadata();
- auto timeCreated = std::make_shared<aron::datanavigator::LongNavigator>();
- timeCreated->setValue(metadata.timeCreated.toMicroSeconds());
- a->addElement(DATA_WRAPPER_TIME_CREATED_FIELD, timeCreated);
-
- auto timeSent = std::make_shared<aron::datanavigator::LongNavigator>();
- timeSent->setValue(metadata.timeSent.toMicroSeconds());
- a->addElement(DATA_WRAPPER_TIME_SENT_FIELD, timeSent);
-
- auto timeArrived = std::make_shared<aron::datanavigator::LongNavigator>();
- timeArrived->setValue(metadata.timeArrived.toMicroSeconds());
- a->addElement(DATA_WRAPPER_TIME_ARRIVED_FIELD, timeArrived);
-
- auto confidence = std::make_shared<aron::datanavigator::DoubleNavigator>();
- confidence->setValue(static_cast<double>(metadata.confidence));
- a->addElement(DATA_WRAPPER_CONFIDENCE_FIELD, confidence);
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.cpp
deleted file mode 100644
index bfc049995a6553e53a78be00294cb9c162e80a0d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.cpp
+++ /dev/null
@@ -1,128 +0,0 @@
-#include "ice_conversions.h"
-
-namespace armarx::armem
-{
- void toIce(armarx::armem::data::EntityInstanceMetadata& ice, const armarx::armem::wm::EntityInstanceMetadata& metadata)
- {
- ice.confidence = metadata.confidence;
- toIce(ice.timeArrivedMicroSeconds, metadata.timeArrived);
- toIce(ice.timeCreatedMicroSeconds, metadata.timeCreated);
- toIce(ice.timeSentMicroSeconds, metadata.timeSent);
- }
- void fromIce(const armarx::armem::data::EntityInstanceMetadata& ice, armarx::armem::wm::EntityInstanceMetadata& metadata)
- {
- metadata.confidence = ice.confidence;
- fromIce(ice.timeArrivedMicroSeconds, metadata.timeArrived);
- fromIce(ice.timeCreatedMicroSeconds, metadata.timeCreated);
- fromIce(ice.timeSentMicroSeconds, metadata.timeSent);
- }
-
- void toIce(data::EntityInstance& ice, const wm::EntityInstance& data)
- {
- detail::toIceItem(ice, data);
-
- if (data.data())
- {
- ice.data = data.data()->toAronDictPtr();
- }
- toIce(ice.metadata, data.metadata());
- }
- void fromIce(const data::EntityInstance& ice, wm::EntityInstance& data)
- {
- detail::fromIceItem(ice, data);
-
- if (ice.data)
- {
- data.setData(aron::datanavigator::DictNavigator::FromAronDictPtr(ice.data));
- }
- fromIce(ice.metadata, data.metadata());
- }
-
-
- void toIce(data::EntitySnapshot& ice, const wm::EntitySnapshot& snapshot)
- {
- detail::toIceItem(ice, snapshot);
-
- toIce(ice.instances, snapshot.instances());
- }
- void fromIce(const data::EntitySnapshot& ice, wm::EntitySnapshot& snapshot)
- {
- detail::fromIceItem(ice, snapshot);
-
- fromIce(ice.instances, snapshot.instances());
- }
-
- void toIce(data::Entity& ice, const wm::Entity& entity)
- {
- detail::toIceItem(ice, entity);
-
- toIce(ice.history, entity.history());
- }
- void fromIce(const data::Entity& ice, wm::Entity& entity)
- {
- detail::fromIceItem(ice, entity);
-
- fromIce(ice.history, entity.history());
- }
-
-
- void toIce(data::ProviderSegment& ice, const wm::ProviderSegment& providerSegment)
- {
- detail::toIceItem(ice, providerSegment);
-
- if (providerSegment.hasAronType())
- {
- ice.aronType = providerSegment.aronType()->toAronPtr();
- }
- ARMARX_CHECK(!providerSegment.aronType() || ice.aronType);
- toIce(ice.entities, providerSegment.entities());
- }
- void fromIce(const data::ProviderSegment& ice, wm::ProviderSegment& providerSegment)
- {
- detail::fromIceItem(ice, providerSegment);
-
- if (ice.aronType)
- {
- providerSegment.aronType() = aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(aron::typenavigator::Navigator::FromAronType(ice.aronType));
- }
- ARMARX_CHECK(!providerSegment.aronType() || ice.aronType);
- fromIce(ice.entities, providerSegment.entities());
- }
-
- void toIce(data::CoreSegment& ice, const wm::CoreSegment& coreSegment)
- {
- detail::toIceItem(ice, coreSegment);
-
- if (coreSegment.hasAronType())
- {
- ice.aronType = coreSegment.aronType()->toAronPtr();
- }
- ARMARX_CHECK(!coreSegment.aronType() || ice.aronType);
- toIce(ice.providerSegments, coreSegment.providerSegments());
- }
- void fromIce(const data::CoreSegment& ice, wm::CoreSegment& coreSegment)
- {
- detail::fromIceItem(ice, coreSegment);
-
- if (ice.aronType)
- {
- coreSegment.aronType() = aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(aron::typenavigator::Navigator::FromAronType(ice.aronType));
- }
- ARMARX_CHECK(!coreSegment.aronType() || ice.aronType);
- fromIce(ice.providerSegments, coreSegment.providerSegments());
- }
-
- void toIce(data::Memory& ice, const wm::Memory& memory)
- {
- detail::toIceItem(ice, memory);
-
- toIce(ice.coreSegments, memory.coreSegments());
- }
- void fromIce(const data::Memory& ice, wm::Memory& memory)
- {
- detail::fromIceItem(ice, memory);
-
- fromIce(ice.coreSegments, memory.coreSegments());
- }
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h b/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h
deleted file mode 100644
index 4d14e6b0d04e9086e701cc38a159e516e533d95c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h
+++ /dev/null
@@ -1,36 +0,0 @@
-#pragma once
-
-#include <RobotAPI/interface/armem/commit.h>
-#include <RobotAPI/interface/armem/memory.h>
-
-#include "Memory.h"
-
-
-namespace armarx::armem
-{
- void toIce(data::EntityInstanceMetadata& ice, const wm::EntityInstanceMetadata& metadata);
- void fromIce(const data::EntityInstanceMetadata& ice, wm::EntityInstanceMetadata& metadata);
-
- void toIce(data::EntityInstance& ice, const wm::EntityInstance& data);
- void fromIce(const data::EntityInstance& ice, wm::EntityInstance& data);
-
-
- void toIce(data::EntitySnapshot& ice, const wm::EntitySnapshot& snapshot);
- void fromIce(const data::EntitySnapshot& ice, wm::EntitySnapshot& snapshot);
-
- void toIce(data::Entity& ice, const wm::Entity& entity);
- void fromIce(const data::Entity& ice, wm::Entity& entity);
-
-
- void toIce(data::ProviderSegment& ice, const wm::ProviderSegment& providerSegment);
- void fromIce(const data::ProviderSegment& ice, wm::ProviderSegment& providerSegment);
-
- void toIce(data::CoreSegment& ice, const wm::CoreSegment& coreSegment);
- void fromIce(const data::CoreSegment& ice, wm::CoreSegment& coreSegment);
-
- void toIce(data::Memory& ice, const wm::Memory& memory);
- void fromIce(const data::Memory& ice, wm::Memory& memory);
-}
-
-// Must be included after the prototypes. Otherwise the compiler cannot find the correct methods in ice_coversion_templates.h
-#include "../ice_conversions.h"
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/json_conversions.h b/source/RobotAPI/libraries/armem/core/workingmemory/json_conversions.h
deleted file mode 100644
index d132d88f7d7fb694be8b3dc086c87dcafa276e6a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/json_conversions.h
+++ /dev/null
@@ -1,15 +0,0 @@
-#pragma once
-
-#include "Memory.h"
-
-#include <RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
-
-
-namespace armarx::armem
-{
- void from_aron(const aron::datanavigator::DictNavigatorPtr&, nlohmann::json&);
- void to_aron(aron::datanavigator::DictNavigatorPtr&, const nlohmann::json&, const aron::typenavigator::NavigatorPtr& expectedStructure = nullptr);
-}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/visitor.h b/source/RobotAPI/libraries/armem/core/workingmemory/visitor.h
deleted file mode 100644
index 815ee1b34908dc5916e7d909e709bc735409f63c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/visitor.h
+++ /dev/null
@@ -1,4 +0,0 @@
-#pragma once
-
-#include "visitor/FunctionalVisitor.h"
-#include "visitor/Visitor.h"
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/visitor/Visitor.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/Visitor.cpp
deleted file mode 100644
index ede63a494a7aff26c54ba1d7149200c91be7d987..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/visitor/Visitor.cpp
+++ /dev/null
@@ -1,191 +0,0 @@
-#include "Visitor.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
-#include <RobotAPI/libraries/armem/core/error.h>
-
-
-namespace armarx::armem::wm
-{
-
- Visitor::Visitor()
- {
- }
-
- Visitor::~Visitor()
- {
- }
-
- bool Visitor::applyTo(Memory& memory)
- {
- bool cont = true;
- visitEnter(memory);
- for (auto& [_, coreSeg] : memory)
- {
- if (!applyTo(coreSeg))
- {
- cont = false;
- break;
- }
- }
- visitExit(memory);
- return cont;
- }
-
- bool Visitor::applyTo(CoreSegment& coreSegment)
- {
- bool cont = true;
- visitEnter(coreSegment);
- for (auto& [_, provSeg] : coreSegment)
- {
- if (!applyTo(provSeg))
- {
- cont = false;
- break;
- }
- }
- visitExit(coreSegment);
- return cont;
- }
-
- bool Visitor::applyTo(ProviderSegment& providerSegment)
- {
- bool cont = true;
- visitEnter(providerSegment);
- for (auto& [_, entity] : providerSegment)
- {
- if (!applyTo(entity))
- {
- cont = false;
- break;
- }
- }
- visitExit(providerSegment);
- return cont;
- }
-
- bool Visitor::applyTo(Entity& entity)
- {
- bool cont = true;
- visitEnter(entity);
- for (auto& [_, snapshot] : entity)
- {
- if (!applyTo(snapshot))
- {
- cont = false;
- break;
- }
- }
- visitExit(entity);
- return cont;
- }
-
- bool Visitor::applyTo(EntitySnapshot& snapshot)
- {
- bool cont = true;
- visitEnter(snapshot);
- for (auto& instance : snapshot)
- {
- if (!applyTo(instance))
- {
- cont = false;
- break;
- }
- }
- visitExit(snapshot);
- return cont;
- }
-
- bool Visitor::applyTo(EntityInstance& instance)
- {
- return visit(instance);
- }
-
-
- bool Visitor::applyTo(const Memory& memory)
- {
- bool cont = true;
- visitEnter(memory);
- for (const auto& [_, coreSeg] : memory)
- {
- if (!applyTo(coreSeg))
- {
- cont = false;
- break;
- }
- }
- visitExit(memory);
- return cont;
- }
-
- bool Visitor::applyTo(const CoreSegment& coreSegment)
- {
- bool cont = true;
- visitEnter(coreSegment);
- for (const auto& [_, provSeg] : coreSegment)
- {
- if (!applyTo(provSeg))
- {
- cont = false;
- break;
- }
- }
- visitExit(coreSegment);
- return cont;
- }
-
- bool Visitor::applyTo(const ProviderSegment& providerSegment)
- {
- bool cont = true;
- visitEnter(providerSegment);
- for (const auto& [_, entity] : providerSegment)
- {
- if (!applyTo(entity))
- {
- cont = false;
- break;
- }
- }
- visitExit(providerSegment);
- return cont;
- }
-
- bool Visitor::applyTo(const Entity& entity)
- {
- bool cont = true;
- visitEnter(entity);
- for (const auto& [_, snapshot] : entity)
- {
- if (!applyTo(snapshot))
- {
- cont = false;
- break;
- }
- }
- visitExit(entity);
- return cont;
- }
-
- bool Visitor::applyTo(const EntitySnapshot& snapshot)
- {
- bool cont = true;
- visitEnter(snapshot);
- for (const auto& instance : snapshot)
- {
- if (!applyTo(instance))
- {
- cont = false;
- break;
- }
- }
- visitExit(snapshot);
- return cont;
- }
-
- bool Visitor::applyTo(const EntityInstance& instance)
- {
- return visit(instance);
- }
-
-}
diff --git a/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp b/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
index 7b3a1d05817c9c25675abe2e5b8ab011aba1d92d..ae847a803dc64e68b53c432b222cb6ddbe2fcd86 100644
--- a/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
@@ -1,18 +1,16 @@
#include "ComponentPlugin.h"
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include "MemoryToIceAdapter.h"
#include <RobotAPI/libraries/armem/core/error.h>
-#include "MemoryToIceAdapter.h"
-
-//#include <RobotAPI/libraries/armem/core/io/diskWriter/NlohmannJSON/NlohmannJSONDiskWriter.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
namespace armarx::armem::server::plugins
{
- ComponentPlugin::~ComponentPlugin()
- {}
+ ComponentPlugin::~ComponentPlugin() = default;
void ComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
@@ -22,12 +20,8 @@ namespace armarx::armem::server::plugins
ComponentPluginUser& parent = this->parent<ComponentPluginUser>();
properties->topic(memoryListener, parent.memoryListenerDefaultName);
- properties->optional(parent.longtermMemoryEnabled, "ltm.00_enabled");
- properties->optional(parent.longtermMemory.dbsettings.host, "ltm.10_host");
- properties->optional(parent.longtermMemory.dbsettings.port, "ltm.11_port");
- properties->optional(parent.longtermMemory.dbsettings.user, "ltm.20_user");
- properties->optional(parent.longtermMemory.dbsettings.password, "ltm.21_password");
- properties->optional(parent.longtermMemory.dbsettings.database, "ltm.22_database");
+ properties->optional(parent.longtermMemoryEnabled, "memory.ltm.00_enabled");
+ parent.longtermMemory.defineProperties(properties, "memory");
}
@@ -118,6 +112,8 @@ namespace armarx::armem::server
addPlugin(plugin);
}
+ ComponentPluginUser::~ComponentPluginUser() = default;
+
// WRITING
data::AddSegmentsResult ComponentPluginUser::addSegments(const data::AddSegmentsInput& input, const Ice::Current&)
@@ -128,7 +124,7 @@ namespace armarx::armem::server
data::AddSegmentsResult ComponentPluginUser::addSegments(const data::AddSegmentsInput& input, bool addCoreSegments)
{
- std::scoped_lock lock(workingMemoryMutex);
+ // std::scoped_lock lock(workingMemoryMutex);
data::AddSegmentsResult result = iceMemory.addSegments(input, addCoreSegments);
return result;
}
@@ -136,7 +132,7 @@ namespace armarx::armem::server
data::CommitResult ComponentPluginUser::commit(const data::Commit& commitIce, const Ice::Current&)
{
- std::scoped_lock lock(workingMemoryMutex);
+ // std::scoped_lock lock(workingMemoryMutex);
return iceMemory.commit(commitIce);
}
@@ -144,19 +140,20 @@ namespace armarx::armem::server
// READING
armem::query::data::Result ComponentPluginUser::query(const armem::query::data::Input& input, const Ice::Current&)
{
- std::scoped_lock lock(workingMemoryMutex);
+ // std::scoped_lock lock(workingMemoryMutex);
return iceMemory.query(input);
}
- // LTM LOADING
+
+ // LTM STORING
data::StoreResult ComponentPluginUser::store(const data::StoreInput& input, const Ice::Current&)
{
- std::scoped_lock lock(workingMemoryMutex, longtermMemoryMutex);
+ std::scoped_lock lock(/*workingMemoryMutex,*/ longtermMemoryMutex);
return iceMemory.store(input);
}
- // LTM STORING
+ // LTM LOADING
armem::query::data::Result ComponentPluginUser::load(const armem::query::data::Input& input, const Ice::Current&)
{
std::scoped_lock lock(longtermMemoryMutex);
diff --git a/source/RobotAPI/libraries/armem/server/ComponentPlugin.h b/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
index 5f9ef1d574df06b1644d733e494273ce0660dbbf..0b2b872c1ff4686eecefd8ae8508bcfc45600d5f 100644
--- a/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
@@ -2,16 +2,16 @@
#include <mutex>
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
-
#include <RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h>
+
#include "MemoryToIceAdapter.h"
@@ -73,7 +73,7 @@ namespace armarx::armem::server
public:
ComponentPluginUser();
- virtual ~ComponentPluginUser() override = default;
+ virtual ~ComponentPluginUser() override;
// WritingInterface interface
@@ -96,9 +96,11 @@ namespace armarx::armem::server
public:
+
/// The actual memory.
- wm::Memory workingMemory;
- std::mutex workingMemoryMutex;
+ server::wm::Memory workingMemory;
+ // [[deprecated ("The global working memory mutex is deprecated. Use the core segment mutexes instead.")]]
+ // std::mutex workingMemoryMutex;
bool longtermMemoryEnabled = true;
ltm::Memory longtermMemory;
@@ -112,6 +114,7 @@ namespace armarx::armem::server
private:
+
plugins::ComponentPlugin* plugin = nullptr;
};
diff --git a/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp b/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp
index 9ab34efd946541a37ae825d6338a355aec0dd722..6664288a0fa5f86da4df906819cba3be385cbcb9 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp
@@ -1,107 +1,96 @@
#include "MemoryRemoteGui.h"
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include "RemoteGuiAronDataVisitor.h"
#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
#include <RobotAPI/libraries/aron/core/navigator/visitors/DataVisitor.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
#include <SimoxUtility/meta/type_name.h>
-#include "RemoteGuiAronDataVisitor.h"
+#include <mutex>
namespace armarx::armem::server
{
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const wm::Memory& memory) const
+ template <class ...Args>
+ MemoryRemoteGui::GroupBox MemoryRemoteGui::_makeGroupBox(const armem::base::MemoryBase<Args...>& memory) const
{
GroupBox group;
- group.setLabel(makeGroupLabel("Memory", memory.name(), memory.coreSegments().size()));
+ group.setLabel(makeGroupLabel("Memory", memory.name(), memory.size()));
- if (memory.coreSegments().empty())
+ if (memory.empty())
{
group.addChild(Label(makeNoItemsMessage("core segments")));
}
- for (const auto& [name, coreSegment] : memory.coreSegments())
+ memory.forEachCoreSegment([this, &group](const auto & coreSegment)
{
- group.addChild(makeGroupBox(coreSegment));
- }
-
+ group.addChild(this->makeGroupBox(coreSegment));
+ });
return group;
}
-
-
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const wm::CoreSegment& coreSegment) const
+ template <class ...Args>
+ MemoryRemoteGui::GroupBox MemoryRemoteGui::_makeGroupBox(const armem::base::CoreSegmentBase<Args...>& coreSegment) const
{
GroupBox group;
- group.setLabel(makeGroupLabel("Core Segment", coreSegment.name(), coreSegment.providerSegments().size()));
+ group.setLabel(makeGroupLabel("Core Segment", coreSegment.name(), coreSegment.size()));
- if (coreSegment.providerSegments().empty())
+ if (coreSegment.empty())
{
group.addChild(Label(makeNoItemsMessage("provider segments")));
}
- for (const auto& [name, providerSegment] : coreSegment.providerSegments())
+ coreSegment.forEachProviderSegment([this, &group](const auto & providerSegment)
{
- group.addChild(makeGroupBox(providerSegment));
- }
-
+ group.addChild(this->makeGroupBox(providerSegment));
+ });
return group;
}
-
-
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const wm::ProviderSegment& providerSegment) const
+ template <class ...Args>
+ MemoryRemoteGui::GroupBox MemoryRemoteGui::_makeGroupBox(const armem::base::ProviderSegmentBase<Args...>& providerSegment) const
{
GroupBox group;
- group.setLabel(makeGroupLabel("Provider Segment", providerSegment.name(), providerSegment.entities().size()));
+ group.setLabel(makeGroupLabel("Provider Segment", providerSegment.name(), providerSegment.size()));
- if (providerSegment.entities().empty())
+ if (providerSegment.empty())
{
group.addChild(Label(makeNoItemsMessage("entities")));
}
- for (const auto& [name, entity] : providerSegment.entities())
+ providerSegment.forEachEntity([this, &group](const auto & entity)
{
- group.addChild(makeGroupBox(entity));
- }
-
+ group.addChild(this->makeGroupBox(entity));
+ });
return group;
}
-
-
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const wm::Entity& entity) const
+ template <class ...Args>
+ MemoryRemoteGui::GroupBox MemoryRemoteGui::_makeGroupBox(const armem::base::EntityBase<Args...>& entity) const
{
GroupBox group;
- group.setLabel(makeGroupLabel("Entity", entity.name(), entity.history().size()));
+ group.setLabel(makeGroupLabel("Entity", entity.name(), entity.size()));
- if (entity.history().empty())
+ if (entity.empty())
{
group.addChild(Label(makeNoItemsMessage("snapshots")));
}
- if (int(entity.history().size()) <= maxHistorySize)
+
+ auto addChild = [this, &group](const armem::wm::EntitySnapshot & snapshot)
{
- for (const auto& [time, snapshot] : entity.history())
- {
- group.addChild(makeGroupBox(snapshot));
- }
+ group.addChild(makeGroupBox(snapshot));
+ };
+
+ if (int(entity.size()) <= maxHistorySize)
+ {
+ entity.forEachSnapshot(addChild);
}
else
{
- int margin = 2;
- auto it = entity.history().begin();
- auto rit = entity.history().end();
- --rit;
- for (int i = 0; i < margin; ++i, ++it)
- {
- group.addChild(makeGroupBox(it->second));
- --rit;
- }
- group.addChild(Label("..."));
- for (; rit != entity.history().end(); ++rit)
- {
- group.addChild(makeGroupBox(rit->second));
- }
+ const int margin = 2;
+ entity.forEachSnapshotInIndexRange(0, margin, addChild);
+ entity.forEachSnapshotInIndexRange(-margin, -1, addChild);
}
group.setCollapsed(true);
@@ -109,27 +98,76 @@ namespace armarx::armem::server
}
+ MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const armem::server::wm::Memory& memory) const
+ {
+ return this->_makeGroupBox(memory);
+ }
+
+ MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const armem::wm::Memory& memory) const
+ {
+ return this->_makeGroupBox(memory);
+ }
+
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const wm::EntitySnapshot& snapshot) const
+ MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const armem::server::wm::CoreSegment& coreSegment) const
+ {
+ std::scoped_lock lock(coreSegment.mutex());
+ return this->_makeGroupBox(coreSegment);
+ }
+
+
+ MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const armem::wm::CoreSegment& coreSegment) const
+ {
+ return this->_makeGroupBox(coreSegment);
+ }
+
+
+ MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const armem::server::wm::ProviderSegment& providerSegment) const
+ {
+ return this->_makeGroupBox(providerSegment);
+ }
+
+
+ MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const armem::wm::ProviderSegment& providerSegment) const
+ {
+ return this->_makeGroupBox(providerSegment);
+ }
+
+
+ MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const armem::wm::Entity& entity) const
+ {
+ return this->_makeGroupBox(entity);
+ }
+
+
+ MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const armem::server::wm::Entity& entity) const
+ {
+ return this->_makeGroupBox(entity);
+ }
+
+
+ MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const armem::wm::EntitySnapshot& snapshot) const
{
GroupBox group;
group.setLabel(makeGroupLabel("t", armem::toDateTimeMilliSeconds(snapshot.time()),
- snapshot.instances().size(), " = ", ""));
+ snapshot.size(), " = ", ""));
- if (snapshot.instances().empty())
+ if (snapshot.empty())
{
group.addChild(Label(makeNoItemsMessage("instances")));
}
- for (const wm::EntityInstance& instance : snapshot.instances())
+ snapshot.forEachInstance([this, &group](const armem::wm::EntityInstance & instance)
{
group.addChild(makeGroupBox(instance));
- }
+ return true;
+ });
group.setCollapsed(true);
return group;
}
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const wm::EntityInstance& instance) const
+
+ MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const armem::wm::EntityInstance& instance) const
{
GroupBox group;
@@ -153,7 +191,6 @@ namespace armarx::armem::server
}
-
std::string MemoryRemoteGui::makeGroupLabel(
const std::string& term, const std::string& name, size_t size,
const std::string& namePrefix, const std::string& nameSuffix) const
diff --git a/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.h b/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.h
index f4a7fad95f1efa38d8ad7044d661e77fef60be31..c6aef5c51546ac203c50d38f80696b89f3b6f3b9 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.h
+++ b/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.h
@@ -2,7 +2,7 @@
#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
-#include "../core/workingmemory/Memory.h"
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
namespace armarx::armem::server
@@ -17,12 +17,18 @@ namespace armarx::armem::server
using GroupBox = armarx::RemoteGui::Client::GroupBox;
using Label = armarx::RemoteGui::Client::Label;
- GroupBox makeGroupBox(const wm::Memory& memory) const;
- GroupBox makeGroupBox(const wm::CoreSegment& coreSegment) const;
- GroupBox makeGroupBox(const wm::ProviderSegment& providerSegment) const;
- GroupBox makeGroupBox(const wm::Entity& entity) const;
- GroupBox makeGroupBox(const wm::EntitySnapshot& entitySnapshot) const;
- GroupBox makeGroupBox(const wm::EntityInstance& instance) const;
+
+ GroupBox makeGroupBox(const armem::wm::Memory& memory) const;
+ GroupBox makeGroupBox(const armem::wm::CoreSegment& coreSegment) const;
+ GroupBox makeGroupBox(const armem::wm::ProviderSegment& providerSegment) const;
+ GroupBox makeGroupBox(const armem::wm::Entity& entity) const;
+ GroupBox makeGroupBox(const armem::wm::EntitySnapshot& entitySnapshot) const;
+ GroupBox makeGroupBox(const armem::wm::EntityInstance& instance) const;
+
+ GroupBox makeGroupBox(const armem::server::wm::Memory& memory) const;
+ GroupBox makeGroupBox(const armem::server::wm::CoreSegment& coreSegment) const;
+ GroupBox makeGroupBox(const armem::server::wm::ProviderSegment& providerSegment) const;
+ GroupBox makeGroupBox(const armem::server::wm::Entity& entity) const;
std::string makeGroupLabel(const std::string& term, const std::string& name, size_t size,
@@ -33,6 +39,17 @@ namespace armarx::armem::server
int maxHistorySize = 10;
+ private:
+
+ template <class ...Args>
+ MemoryRemoteGui::GroupBox _makeGroupBox(const armem::base::MemoryBase<Args...>& memory) const;
+ template <class ...Args>
+ MemoryRemoteGui::GroupBox _makeGroupBox(const armem::base::CoreSegmentBase<Args...>& coreSegment) const;
+ template <class ...Args>
+ MemoryRemoteGui::GroupBox _makeGroupBox(const armem::base::ProviderSegmentBase<Args...>& providerSegment) const;
+ template <class ...Args>
+ MemoryRemoteGui::GroupBox _makeGroupBox(const armem::base::EntityBase<Args...>& entity) const;
+
};
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index a846cffac89c6118b6c476d3531e47cd49f96f8a..52fb496592390a1f7df893d0709c73937da55ae5 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -1,12 +1,17 @@
#include "MemoryToIceAdapter.h"
-#include "ArmarXCore/core/logging/Logging.h"
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include "query_proc/wm.h"
+#include "query_proc/ltm.h"
+
+#include <RobotAPI/libraries/armem/server/wm/ice_conversions.h>
+
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
-#include "../error.h"
-#include "../core/workingmemory/ice_conversions.h"
-#include "query_proc/workingmemory/MemoryQueryProcessor.h"
-#include "query_proc/longtermmemory/MemoryQueryProcessor.h"
+#include <RobotAPI/libraries/aron/core/Exception.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
namespace armarx::armem::server
@@ -33,7 +38,7 @@ namespace armarx::armem::server
data::AddSegmentResult output;
- armem::wm::CoreSegment* coreSegment = nullptr;
+ server::wm::CoreSegment* coreSegment = nullptr;
try
{
coreSegment = &workingMemory->getCoreSegment(input.coreSegmentName);
@@ -55,6 +60,7 @@ namespace armarx::armem::server
if (input.providerSegmentName.size() > 0)
{
+ std::scoped_lock lock(coreSegment->mutex());
try
{
coreSegment->addProviderSegment(input.providerSegmentName);
@@ -64,7 +70,7 @@ namespace armarx::armem::server
// This is ok.
if (input.clearWhenExists)
{
- wm::ProviderSegment& provider = coreSegment->getProviderSegment(input.providerSegmentName);
+ server::wm::ProviderSegment& provider = coreSegment->getProviderSegment(input.providerSegmentName);
provider.clear();
}
}
@@ -94,6 +100,7 @@ namespace armarx::armem::server
return output;
}
+
data::CommitResult
MemoryToIceAdapter::commit(const data::Commit& commitIce, Time timeArrived)
{
@@ -140,9 +147,21 @@ namespace armarx::armem::server
armem::CommitResult
MemoryToIceAdapter::commit(const armem::Commit& commit)
+ {
+ return this->_commit(commit, false);
+ }
+
+
+ armem::CommitResult MemoryToIceAdapter::commitLocking(const armem::Commit& commit)
+ {
+ return this->_commit(commit, true);
+ }
+
+
+ armem::CommitResult MemoryToIceAdapter::_commit(const armem::Commit& commit, bool locking)
{
std::vector<data::MemoryID> updatedIDs;
- const bool publishUpdates = memoryListenerTopic;
+ const bool publishUpdates = bool(memoryListenerTopic);
CommitResult commitResult;
for (const EntityUpdate& update : commit.updates)
@@ -150,13 +169,22 @@ namespace armarx::armem::server
EntityUpdateResult& result = commitResult.results.emplace_back();
try
{
- auto updateResult = workingMemory->update(update);
+ auto updateResult = locking
+ ? workingMemory->updateLocking(update)
+ : workingMemory->update(update);
result.success = true;
result.snapshotID = updateResult.id;
result.timeArrived = update.timeArrived;
- // TODO: Add if and param here (fabian.peller)
+ // also store in ltm if transfermode is set to always
+ // TODO: Move outside of loop?
+ if (longtermMemory)
+ {
+
+ }
+
+ // TODO: Consollidate to ltm if onFilledTransfer is enabled (fabian.peller)
for (const auto& snapshot : updateResult.removedSnapshots)
{
ARMARX_DEBUG << "The id " << snapshot.id() << " was removed from wm";
@@ -200,40 +228,60 @@ namespace armarx::armem::server
{
ARMARX_CHECK_NOT_NULL(workingMemory);
- armem::wm::query_proc::MemoryQueryProcessor wm_processor(
- input.withData ? armem::DataMode::WithData
- : armem::DataMode::NoData);
-
- armem::query::data::Result result;
- wm::Memory wm_result = wm_processor.process(input, *workingMemory, /* execute if: */ { query::data::QueryTarget::WM });
-
- armem::ltm::query_proc::MemoryQueryProcessor ltm_processor;
- ltm::Memory ltm_result = ltm_processor.process(input, *longtermMemory, /* execute if: */ { query::data::QueryTarget::LTM });
+ // Core segment processors will aquire the core segment locks.
+ query_proc::wm_server::MemoryQueryProcessor wmServerProcessor(
+ input.withData ? armem::DataMode::WithData : armem::DataMode::NoData);
+ armem::wm::Memory wmResult = wmServerProcessor.process(input.memoryQueries, *workingMemory);
+ query_proc::ltm::MemoryQueryProcessor ltmProcessor;
+ ltm::Memory ltmResult = ltmProcessor.process(input, *longtermMemory);
- if (ltm_result.hasData())
+ armem::query::data::Result result;
+ if (not ltmResult.empty())
{
+ ARMARX_INFO << "The LTM returned data after query";
+
// ATTENTION: This code block moves data from LTM back into WM.
// However, since some segments are constrained, the WM might send data back to LTM.
// This may also affect the data returned by the current query.
// However, this is expected behavior, since we copy the data in the processor (copyEmpty) we can safely return the copy and
// remove the original memory reference from WM here.
- wm::Memory ltm_converted = ltm_result.convert();
- wm_result.append(ltm_converted);
+ armem::wm::Memory ltmConverted = ltmResult.convert();
+ if (ltmConverted.empty())
+ {
+ ARMARX_ERROR << "A converted memory contains no data although the original memory contained data. This indicates that something is wrong.";
+ }
+
+ wmResult.append(ltmConverted);
+ if (wmResult.empty())
+ {
+ ARMARX_ERROR << "A merged Memory has no data although at least the LTM result contains data. This indicates that something is wrong.";
+ }
// query again to limit output size (TODO: Skip if querytype is all)
- wm::Memory merged_result = wm_processor.process(input, wm_result);
+ auto queryInput = armem::client::QueryInput::fromIce(input);
+ queryInput.replaceQueryTarget(query::data::QueryTarget::LTM, query::data::QueryTarget::WM);
+
+ query_proc::wm::MemoryQueryProcessor wm2wmProcessor(
+ input.withData ? armem::DataMode::WithData : armem::DataMode::NoData);
+ armem::wm::Memory merged_result = wm2wmProcessor.process(queryInput.toIce(), wmResult);
+ if (merged_result.empty())
+ {
+ ARMARX_ERROR << "A merged and postprocessed Memory has no data although at least the LTM result contains data. This indicates that something is wrong.";
+ }
+
result.memory = toIce<data::MemoryPtr>(merged_result);
// also move results of ltm to wm
- //this->commit(ltm_converted.toCommit());
+ //this->commit(toCommit(ltm_converted));
// mark removed entries of wm in viewer
// TODO
}
else
{
- result.memory = toIce<data::MemoryPtr>(wm_result);
+ ARMARX_VERBOSE << "The LTM did not return data after query";
+ result.memory = toIce<data::MemoryPtr>(wmResult);
}
result.success = true;
@@ -245,6 +293,13 @@ namespace armarx::armem::server
return result;
}
+
+ client::QueryResult MemoryToIceAdapter::query(const client::QueryInput& input)
+ {
+ return client::QueryResult::fromIce(query(input.toIce()));
+ }
+
+
// LTM LOADING FROM LTM
query::data::Result MemoryToIceAdapter::load(const armem::query::data::Input& query)
{
@@ -255,6 +310,7 @@ namespace armarx::armem::server
return output;
}
+
// LTM STORING
data::StoreResult MemoryToIceAdapter::store(const armem::data::StoreInput& input)
{
@@ -274,7 +330,7 @@ namespace armarx::armem::server
if (queryResult.success)
{
- wm::Memory m;
+ armem::wm::Memory m;
fromIce(queryResult.memory, m);
longtermMemory->append(m);
}
@@ -282,9 +338,8 @@ namespace armarx::armem::server
return output;
}
- client::QueryResult MemoryToIceAdapter::query(const client::QueryInput& input)
- {
- return client::QueryResult::fromIce(query(input.toIce()));
- }
+
+
+
}
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
index 919c5fc8dc74ec3f742ac1c33365bc2c680ac768..6aa68b5714ad369e3b2bbad08d7c1ca06ab04bb8 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
@@ -3,15 +3,15 @@
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
#include <RobotAPI/libraries/armem/client/Query.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
namespace armarx::armem::server
{
-
/**
* @brief Helps connecting a Memory server to the Ice interface.
*
@@ -23,7 +23,8 @@ namespace armarx::armem::server
public:
/// Construct an MemoryToIceAdapter from an existing Memory.
- MemoryToIceAdapter(wm::Memory* workingMemory = nullptr, ltm::Memory* longtermMemory = nullptr);
+ MemoryToIceAdapter(server::wm::Memory* workingMemory = nullptr,
+ ltm::Memory* longtermMemory = nullptr);
void setMemoryListener(client::MemoryListenerInterfacePrx memoryListenerTopic);
@@ -39,6 +40,8 @@ namespace armarx::armem::server
data::CommitResult commit(const data::Commit& commitIce, Time timeArrived);
data::CommitResult commit(const data::Commit& commitIce);
armem::CommitResult commit(const armem::Commit& commit);
+ armem::CommitResult commitLocking(const armem::Commit& commit);
+
// READING
query::data::Result query(const armem::query::data::Input& input);
@@ -52,11 +55,16 @@ namespace armarx::armem::server
public:
- wm::Memory* workingMemory;
+ server::wm::Memory* workingMemory;
ltm::Memory* longtermMemory;
client::MemoryListenerInterfacePrx memoryListenerTopic;
+
+ private:
+
+ armem::CommitResult _commit(const armem::Commit& commit, bool locking);
+
};
diff --git a/source/RobotAPI/libraries/armem/server/Segment.cpp b/source/RobotAPI/libraries/armem/server/Segment.cpp
deleted file mode 100644
index ec25ce7843a6cca3555dc9789f3570e0bfb51425..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/Segment.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "Segment.h"
diff --git a/source/RobotAPI/libraries/armem/server/Segment.h b/source/RobotAPI/libraries/armem/server/Segment.h
deleted file mode 100644
index 5d1c31f4179773a5fa2e96d9ebd211507e91893d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/Segment.h
+++ /dev/null
@@ -1,160 +0,0 @@
-#pragma once
-
-// STD/STL
-#include <mutex>
-#include <string>
-
-// ArmarX
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-
-#include "MemoryToIceAdapter.h"
-
-
-namespace armarx::armem::server
-{
- /**
- * @brief A base class for memory servers to manage their segments.
- * A segment can inherit this base class and add segment specific code.
- * TODO (fabian.peller): add concept to only accept coresegments, providersegments or entitysegments
- */
- template <class SegmentType>
- class SegmentBase : public armarx::Logging
- {
- public:
- SegmentBase() = delete;
- SegmentBase(armem::server::MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex) :
- iceMemory(iceMemory),
- memoryMutex(memoryMutex)
- {
- Logging::setTag("armarx::armem::Segment");
- }
-
- virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") = 0;
- virtual void onInit() = 0;
- virtual void onConnect() = 0;
-
- protected:
- private:
-
- public:
- protected:
- // Memory connection
- armem::server::MemoryToIceAdapter& iceMemory;
- std::mutex& memoryMutex;
-
- SegmentType* segment;
-
- private:
- };
-
- namespace wm
- {
- template <class BusinessClassObject>
- class CoreSegmentBase : public SegmentBase<armarx::armem::wm::CoreSegment>
- {
- using Base = SegmentBase<armarx::armem::wm::CoreSegment>;
-
- public:
- CoreSegmentBase(armem::server::MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex):
- Base(iceMemory, memoryMutex)
- {
- Logging::setTag("armarx::armem::wm::Segment");
- }
-
- virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override
- {
- ARMARX_CHECK_NOT_NULL(defs);
-
- defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the '" + p.coreSegmentName + "' core segment.");
- defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of '" + p.coreSegmentName + "' history (-1 for infinite).");
- }
-
- virtual void onInit() override
- {
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
-
- segment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, BusinessClassObject::toAronType());
- segment->setMaxHistorySize(p.maxHistorySize);
- }
-
- virtual void onConnect() override
- {
-
- }
-
- protected:
- private:
-
- public:
- protected:
- struct Properties
- {
- std::string coreSegmentName = "";
- int maxHistorySize = -1;
- };
- Properties p;
- };
-
-
- template <class BusinessClassObject>
- class ProviderSegmentBase : public SegmentBase<armarx::armem::wm::ProviderSegment>
- {
- using Base = SegmentBase<armarx::armem::wm::ProviderSegment>;
-
- public:
- ProviderSegmentBase(armem::server::MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex):
- Base(iceMemory, memoryMutex)
- {
- Logging::setTag("armarx::armem::wm::Segment");
- }
-
- virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override
- {
- ARMARX_CHECK_NOT_NULL(defs);
-
- defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the core segment.");
- defs->optional(p.providerSegmentName, prefix + "ProviderSegmentName", "Name of the provider segment.");
- defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of history (-1 for infinite).");
- }
-
- virtual void onInit() override
- {
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
-
- if (!iceMemory.workingMemory->hasCoreSegment(p.coreSegmentName))
- {
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName);
- coreSegment->setMaxHistorySize(p.maxHistorySize);
- }
- else
- {
- coreSegment = &iceMemory.workingMemory->getCoreSegment(p.coreSegmentName);
- }
-
- segment = &coreSegment->addProviderSegment(p.providerSegmentName, BusinessClassObject::toAronType());
- segment->setMaxHistorySize(p.maxHistorySize);
- }
-
- virtual void onConnect() override
- {
-
- }
-
- protected:
- private:
-
- public:
- protected:
- struct Properties
- {
- std::string coreSegmentName = "";
- std::string providerSegmentName = "";
- int maxHistorySize = -1;
- };
- Properties p;
-
- armarx::armem::wm::CoreSegment* coreSegment;
- };
- }
-}
diff --git a/source/RobotAPI/libraries/armem/server/forward_declarations.h b/source/RobotAPI/libraries/armem/server/forward_declarations.h
new file mode 100644
index 0000000000000000000000000000000000000000..6872e2584352195ca8e587043e275acfbffb3739
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/forward_declarations.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+
+
+namespace armarx::armem::server
+{
+ class MemoryToIceAdapter;
+}
+namespace armarx::armem::server::wm
+{
+ using EntityInstance = armem::wm::EntityInstance;
+ using EntitySnapshot = armem::wm::EntitySnapshot;
+ class Entity;
+ class ProviderSegment;
+ class CoreSegment;
+ class Memory;
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base.h b/source/RobotAPI/libraries/armem/server/query_proc/base.h
new file mode 100644
index 0000000000000000000000000000000000000000..1d826798cecac7371882ce0783ddc05414a4f237
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base.h
@@ -0,0 +1,12 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h>
+#include <RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h>
+#include <RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h>
+#include <RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h>
+
+
+namespace armarx::armem::server::query_proc::base
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.cpp b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.cpp
index ce437fd67bdd2ba3aa6ac9943093525db99a581e..a3dfb4eb4102e21ee0cc97d7a9919b55c7c83f35 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.cpp
@@ -1 +1,21 @@
#include "BaseQueryProcessorBase.h"
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+
+namespace armarx::armem::server::query_proc::base
+{
+
+ std::set<query::data::QueryTarget>
+ detail::getTargets(const std::vector<query::data::QueryTarget>& _targets)
+ {
+ std::set<query::data::QueryTarget> targets(_targets.begin(), _targets.end());
+ if (targets.empty())
+ {
+ ARMARX_DEBUG << "Query has no targets - using WM as default.";
+ targets.insert(query::data::QueryTarget::WM);
+ }
+ return targets;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
index f71aede29731259471cf5e8dd660917b3e848bb1..73c662e04ebd734cbe79c3b19f676f684223a88c 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
@@ -1,88 +1,91 @@
#pragma once
-#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/interface/armem/query.h>
+#include <Ice/Handle.h>
-namespace armarx::armem::base::query_proc
+#include <set>
+#include <vector>
+
+
+namespace armarx::armem::server::query_proc::base::detail
+{
+ // If empty, e.g. when receiving queries from python, we use WM as default.
+ // We do it here as (Sl)ice does not support default values for vectors.
+ std::set<query::data::QueryTarget> getTargets(const std::vector<query::data::QueryTarget>& _targets);
+}
+namespace armarx::armem::server::query_proc::base
{
+ using QueryTarget = query::data::QueryTarget;
+
+
/**
* @brief Base class for memory query processors.
*/
- template <class DataT, class QueryT>
+ template <QueryTarget queryTarget, class DataT, class ResultT, class QueryT>
class BaseQueryProcessorBase
{
public:
+
using QueryPtrT = ::IceInternal::Handle<QueryT>;
using QuerySeqT = std::vector<QueryPtrT>;
+
public:
- DataT process(const QueryT& query, const DataT& data, const query::data::QueryTargets& executeIf = {}) const
- {
- DataT result = data.copyEmpty();
+ virtual ~BaseQueryProcessorBase() = default;
- for (const auto queryTarget : query.targets)
+
+ ResultT process(const QueryT& query, const DataT& data) const
+ {
+ ResultT result { data.id() };
+ if (detail::getTargets(query.targets).count(queryTarget))
{
- if (std::find(executeIf.begin(), executeIf.end(), queryTarget) != executeIf.end())
- {
- this->process(result, query, data);
- break;
- }
+ this->process(result, query, data);
}
return result;
}
- DataT process(const QueryPtrT& query, const DataT& data, const query::data::QueryTargets& executeIf = {}) const
+ ResultT process(const QueryPtrT& query, const DataT& data) const
{
- return this->process(*query, *data, executeIf);
+ return this->process(*query, *data);
}
- DataT process(const QuerySeqT& queries, const DataT& data, const query::data::QueryTargets& executeIf = {}) const
+ ResultT process(const QuerySeqT& queries, const DataT& data) const
{
- DataT result = data.copyEmpty();
- this->process(result, queries, data, executeIf);
+ ResultT result { data.id() };
+ this->process(result, queries, data);
return result;
}
- void process(DataT& result, const QuerySeqT& queries, const DataT& data, const query::data::QueryTargets& executeIf = {}) const
+ void process(ResultT& result, const QuerySeqT& queries, const DataT& data) const
{
if (queries.empty())
{
- ARMARX_DEBUG << "There are no queries to process.";
- return;
- }
-
- if (executeIf.empty())
- {
- ARMARX_DEBUG << "Could not execute query. ExecuteIf s empty.";
return;
}
for (const auto& query : queries)
{
- if (query->targets.empty())
- {
- ARMARX_DEBUG << "The targets of a query are empty";
- continue;
- }
-
- for (const auto queryTarget : query->targets)
+ if (detail::getTargets(query->targets).count(queryTarget))
{
- if (std::find(executeIf.begin(), executeIf.end(), queryTarget) != executeIf.end())
- {
- this->process(result, *query, data);
- break;
- }
- else
- {
- ARMARX_DEBUG << "The query target " << queryTarget << " was not found in executeIf: " << executeIf;
- }
+ this->process(result, *query, data);
}
}
}
- virtual void process(DataT& result, const QueryT& query, const DataT& data) const = 0;
+
+ /**
+ * @brief Process the query and populate `result`.
+ *
+ * @param result The result container.
+ * @param query The query.
+ * @param data The source container.
+ */
+ virtual void process(ResultT& result, const QueryT& query, const DataT& data) const = 0;
+
};
+
+
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h
index f8ab8c113e510e57a1ab2b53af6a06e60e393ce1..3050e6e4e91a90bfbfa97feff7f3afc6cb804f41 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h
@@ -1,41 +1,54 @@
#pragma once
-#include <RobotAPI/interface/armem/query.h>
-
-#include "BaseQueryProcessorBase.h"
-#include "ProviderSegmentQueryProcessorBase.h"
-
#include <regex>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/interface/armem/query.h>
#include <RobotAPI/libraries/armem/core/error.h>
+#include "BaseQueryProcessorBase.h"
-namespace armarx::armem::base::query_proc
+namespace armarx::armem::server::query_proc::base
{
/**
* @brief Handles memory queries.
*/
- template <class _CoreSegmentT>
+ template <QueryTarget queryTarget, class _CoreSegmentT, class _ResultCoreSegmentT, class _ChildProcessorT>
class CoreSegmentQueryProcessorBase :
- virtual public BaseQueryProcessorBase<_CoreSegmentT, armem::query::data::CoreSegmentQuery>
+ public BaseQueryProcessorBase<queryTarget, _CoreSegmentT, _ResultCoreSegmentT, armem::query::data::CoreSegmentQuery>
{
- using Base = BaseQueryProcessorBase<_CoreSegmentT, armem::query::data::CoreSegmentQuery>;
+ using Base = BaseQueryProcessorBase<queryTarget, _CoreSegmentT, _ResultCoreSegmentT, armem::query::data::CoreSegmentQuery>;
public:
+
using CoreSegmentT = _CoreSegmentT;
- using ProviderSegmentT = typename _CoreSegmentT::ProviderSegmentT;
- using EntityT = typename ProviderSegmentT::EntityT;
- using EntitySnapshotT = typename EntityT::EntitySnapshotT;
+ using ProviderSegmentT = typename CoreSegmentT::ProviderSegmentT;
+
+ using ResultCoreSegmentT = _ResultCoreSegmentT;
+ using ResultProviderSegmentT = typename ResultCoreSegmentT::ProviderSegmentT;
+
+ using ChildProcessorT = _ChildProcessorT;
+
+
+ public:
+
+ CoreSegmentQueryProcessorBase()
+ {
+ }
+ CoreSegmentQueryProcessorBase(ChildProcessorT&& childProcessor) :
+ childProcessor(childProcessor)
+ {
+ }
+
using Base::process;
- void process(_CoreSegmentT& result,
- const armem::query::data::CoreSegmentQuery& query,
- const _CoreSegmentT& coreSegment) const override
+ virtual void process(ResultCoreSegmentT& result,
+ const armem::query::data::CoreSegmentQuery& query,
+ const CoreSegmentT& coreSegment) const override
{
if (auto q = dynamic_cast<const armem::query::data::core::All*>(&query))
{
@@ -55,24 +68,24 @@ namespace armarx::armem::base::query_proc
}
}
- void process(_CoreSegmentT& result,
+ void process(ResultCoreSegmentT& result,
const armem::query::data::core::All& query,
- const _CoreSegmentT& coreSegment) const
+ const CoreSegmentT& coreSegment) const
{
- for (const auto& [name, providerSegment] : coreSegment.providerSegments())
+ coreSegment.forEachProviderSegment([this, &query, &result](const ProviderSegmentT & providerSegment)
{
- result.addProviderSegment(providerSegmentProcessorProcess(query.providerSegmentQueries, providerSegment));
- }
+ childProcessor.process(result.addProviderSegment(providerSegment.name()), query.providerSegmentQueries, providerSegment);
+ });
}
- void process(_CoreSegmentT& result,
+ void process(ResultCoreSegmentT& result,
const armem::query::data::core::Single& query,
- const _CoreSegmentT& coreSegment) const
+ const CoreSegmentT& coreSegment) const
{
try
{
const ProviderSegmentT& providerSegment = coreSegment.getProviderSegment(query.providerSegmentName);
- result.addProviderSegment(providerSegmentProcessorProcess(query.providerSegmentQueries, providerSegment));
+ childProcessor.process(result.addProviderSegment(providerSegment.name()), query.providerSegmentQueries, providerSegment);
}
catch (const error::MissingEntry&)
{
@@ -80,21 +93,25 @@ namespace armarx::armem::base::query_proc
}
}
- void process(_CoreSegmentT& result,
+ void process(ResultCoreSegmentT& result,
const armem::query::data::core::Regex& query,
- const _CoreSegmentT& coreSegment) const
+ const CoreSegmentT& coreSegment) const
{
std::regex regex(query.providerSegmentNameRegex);
- for (const auto& [name, providerSegment] : coreSegment.providerSegments())
+ coreSegment.forEachProviderSegment(
+ [this, &result, &query, ®ex](const ProviderSegmentT & providerSegment)
{
if (std::regex_search(providerSegment.name(), regex))
{
- result.addProviderSegment(providerSegmentProcessorProcess(query.providerSegmentQueries, providerSegment));
+ childProcessor.process(result.addProviderSegment(providerSegment.name()), query.providerSegmentQueries, providerSegment);
}
- }
+ });
}
+
protected:
- virtual ProviderSegmentT providerSegmentProcessorProcess(const armem::query::data::ProviderSegmentQuerySeq& a, const ProviderSegmentT& o) const = 0;
+
+ ChildProcessorT childProcessor;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
index 40113207f194c76301fbe8768a6d42c22f8b01db..0c4a842fa2772ae18b475b9e28ad3b13077bb49d 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
@@ -1,41 +1,42 @@
#pragma once
-#include <cstdint>
-#include <iterator>
+#include "BaseQueryProcessorBase.h"
+
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/interface/armem/query.h>
-#include <ArmarXCore/core/exceptions/LocalException.h>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
-
-#include "BaseQueryProcessorBase.h"
-#include "RobotAPI/libraries/armem/core/Time.h"
+#include <cstdint>
+#include <iterator>
-namespace armarx::armem::base::query_proc
+namespace armarx::armem::server::query_proc::base
{
- /**
- * @brief Handles memory queries.
- */
- template <class _EntityT>
+ template <QueryTarget queryTarget, class _EntityT, class _ResultEntityT>
class EntityQueryProcessorBase :
- virtual public BaseQueryProcessorBase<_EntityT, armem::query::data::EntityQuery>
+ public BaseQueryProcessorBase<queryTarget, _EntityT, _ResultEntityT, armem::query::data::EntityQuery>
{
- using Base = BaseQueryProcessorBase<_EntityT, armem::query::data::EntityQuery>;
+ using Base = BaseQueryProcessorBase<queryTarget, _EntityT, _ResultEntityT, armem::query::data::EntityQuery>;
public:
+
using EntityT = _EntityT;
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
+ using ResultEntityT = _ResultEntityT;
+ using ResultSnapshotT = typename ResultEntityT::EntitySnapshotT;
+
+
using Base::process;
- void process(_EntityT& result,
- const armem::query::data::EntityQuery& query,
- const _EntityT& entity) const override
+ virtual void process(ResultEntityT& result,
+ const armem::query::data::EntityQuery& query,
+ const EntityT& entity) const override
{
if (auto q = dynamic_cast<const armem::query::data::entity::All*>(&query))
{
@@ -71,22 +72,24 @@ namespace armarx::armem::base::query_proc
}
}
- void process(_EntityT& result,
+
+ void process(ResultEntityT& result,
const armem::query::data::entity::All& query,
- const _EntityT& entity) const
+ const EntityT& entity) const
{
(void) query;
// Copy this entitiy and its contents.
- for (const auto& [time, snapshot] : entity)
+ entity.forEachSnapshot([this, &result](const EntitySnapshotT & snapshot)
{
addResultSnapshot(result, snapshot);
- }
+ });
}
- void process(_EntityT& result,
+
+ void process(ResultEntityT& result,
const armem::query::data::entity::Single& query,
- const _EntityT& entity) const
+ const EntityT& entity) const
{
if (query.timestamp < 0)
{
@@ -131,9 +134,10 @@ namespace armarx::armem::base::query_proc
}
}
- void process(_EntityT& result,
+
+ void process(ResultEntityT& result,
const armem::query::data::entity::TimeRange& query,
- const _EntityT& entity) const
+ const EntityT& entity) const
{
if (query.minTimestamp <= query.maxTimestamp || query.minTimestamp < 0 || query.maxTimestamp < 0)
{
@@ -143,54 +147,39 @@ namespace armarx::armem::base::query_proc
}
}
- void process(_EntityT& result,
+
+ void process(ResultEntityT& result,
const armem::query::data::entity::IndexRange& query,
- const _EntityT& entity) const
+ const EntityT& entity) const
{
- if (entity.empty())
- {
- return;
- }
-
- size_t first = negativeIndexSemantics(query.first, entity.history().size());
- size_t last = negativeIndexSemantics(query.last, entity.history().size());
-
- if (first <= last)
+ entity.forEachSnapshotInIndexRange(
+ query.first, query.last,
+ [this, &result](const EntitySnapshotT & snapshot)
{
- auto it = entity.begin();
- std::advance(it, first);
-
- size_t num = last - first + 1; // +1 to make last inclusive
- for (size_t i = 0; i < num; ++i, ++it)
- {
- addResultSnapshot(result, it);
- }
- }
+ addResultSnapshot(result, snapshot);
+ });
}
- void process(_EntityT& result,
+
+ void process(ResultEntityT& result,
const Time& min,
const Time& max,
- const _EntityT& entity,
+ const EntityT& entity,
const armem::query::data::EntityQuery& query) const
{
(void) query;
-
- // Returns an iterator pointing to the first element that is not less than (i.e. greater or equal to) key.
- auto begin = min.toMicroSeconds() > 0 ? entity.history().lower_bound(min) : entity.history().begin();
- // Returns an iterator pointing to the first element that is *greater than* key.
- auto end = max.toMicroSeconds() > 0 ? entity.history().upper_bound(max) : entity.history().end();
-
- for (auto it = begin; it != end && it != entity.history().end(); ++it)
+ entity.forEachSnapshotInTimeRange(
+ min, max,
+ [this, &result](const EntitySnapshotT & snapshot)
{
- addResultSnapshot(result, it);
- }
+ addResultSnapshot(result, snapshot);
+ });
}
- void process(_EntityT& result,
+ void process(ResultEntityT& result,
const armem::query::data::entity::BeforeOrAtTime& query,
- const _EntityT& entity) const
+ const EntityT& entity) const
{
const auto referenceTimestamp = fromIce<Time>(query.timestamp);
ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
@@ -207,39 +196,40 @@ namespace armarx::armem::base::query_proc
}
- void process(_EntityT& result,
+ void process(ResultEntityT& result,
const armem::query::data::entity::BeforeTime& query,
- const _EntityT& entity) const
+ const EntityT& entity) const
{
- const auto referenceTimestamp = fromIce<Time>(query.timestamp);
- ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
+ const armem::Time referenceTimestamp = fromIce<Time>(query.timestamp);
+ ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp must be non-negative.";
- const auto maxEntries = fromIce<std::int64_t>(query.maxEntries);
-
- try
+ std::vector<const EntitySnapshotT*> befores;
+ entity.forEachSnapshotBefore(referenceTimestamp, [&befores](const EntitySnapshotT & s)
{
- const auto before = entity.getSnapshotsBefore(referenceTimestamp);
+ befores.push_back(&s);
+ });
- int i = 0;
- for (const auto& snapshot : before)
- {
- if (maxEntries >= 0 && i >= maxEntries)
- {
- break;
- }
- addResultSnapshot(result, snapshot);
- ++i;
- }
+ size_t num = 0;
+ if (query.maxEntries < 0)
+ {
+ num = befores.size();
}
- catch (const error::MissingEntry&)
+ else
{
- // Leave empty.
+ num = std::min(befores.size(), static_cast<size_t>(query.maxEntries));
+ }
+
+ for (size_t r = 0; r < num; ++r)
+ {
+ size_t i = befores.size() - 1 - r;
+ addResultSnapshot(result, *befores[i]);
}
}
- void process(_EntityT& result,
+
+ void process(ResultEntityT& result,
const armem::query::data::entity::TimeApprox& query,
- const _EntityT& entity) const
+ const EntityT& entity) const
{
const auto referenceTimestamp = fromIce<Time>(query.timestamp);
ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
@@ -285,26 +275,16 @@ namespace armarx::armem::base::query_proc
}
catch (const armem::error::MissingEntry&)
{
-
}
}
- static size_t negativeIndexSemantics(long index, size_t size)
- {
- const size_t max = size > 0 ? size - 1 : 0;
- if (index >= 0)
- {
- return std::clamp<size_t>(static_cast<size_t>(index), 0, max);
- }
- else
- {
- return static_cast<size_t>(std::clamp<long>(static_cast<long>(size) + index, 0, static_cast<long>(max)));
- }
- }
protected:
- virtual void addResultSnapshot(_EntityT& result, typename _EntityT::ContainerT::const_iterator it) const = 0;
- virtual void addResultSnapshot(_EntityT& result, const typename _EntityT::EntitySnapshotT& snapshot) const = 0;
+
+ void addResultSnapshot(ResultEntityT& result, const EntitySnapshotT& snapshot) const
+ {
+ result.addSnapshot(snapshot);
+ }
};
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
index cc06defc93976ea018ab8a7d3823a00d6121543d..df71683c29f2cb17db976e8be971725934b03348 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
@@ -1,47 +1,56 @@
#pragma once
-#include <regex>
-
-#include <RobotAPI/interface/armem/query.h>
-
#include "BaseQueryProcessorBase.h"
-#include "CoreSegmentQueryProcessorBase.h"
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/interface/armem/query.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <RobotAPI/libraries/armem/core/error.h>
+#include <regex>
-namespace armarx::armem::base::query_proc
+namespace armarx::armem::server::query_proc::base
{
- /**
- * @brief Handles memory queries.
- */
- template <class _MemoryT>
+
+ template <QueryTarget queryTarget, class _MemoryT, class _ResultMemoryT, class _ChildProcessorT>
class MemoryQueryProcessorBase :
- virtual public BaseQueryProcessorBase<_MemoryT, armem::query::data::MemoryQuery>
+ public BaseQueryProcessorBase<queryTarget, _MemoryT, _ResultMemoryT, armem::query::data::MemoryQuery>
{
- using Base = BaseQueryProcessorBase<_MemoryT, armem::query::data::MemoryQuery>;
+ using Base = BaseQueryProcessorBase<queryTarget, _MemoryT, _ResultMemoryT, armem::query::data::MemoryQuery>;
public:
+
using MemoryT = _MemoryT;
using CoreSegmentT = typename MemoryT::CoreSegmentT;
- using ProviderSegmentT = typename CoreSegmentT::ProviderSegmentT;
- using EntityT = typename ProviderSegmentT::EntityT;
- using EntitySnapshotT = typename EntityT::EntitySnapshotT;
+
+ using ResultMemoryT = _ResultMemoryT;
+ using ResultCoreSegmentT = typename ResultMemoryT::CoreSegmentT;
+
+ using ChildProcessorT = _ChildProcessorT;
+
+
+ public:
+
+ MemoryQueryProcessorBase()
+ {
+ }
+ MemoryQueryProcessorBase(ChildProcessorT&& childProcessor) :
+ childProcessor(childProcessor)
+ {
+ }
using Base::process;
- _MemoryT process(const armem::query::data::Input& input, const _MemoryT& memory, const std::vector<query::data::QueryTarget>& executeIf = {}) const
+ ResultMemoryT process(const armem::query::data::Input& input, const MemoryT& memory) const
{
- return this->process(input.memoryQueries, memory, executeIf);
+ return this->process(input.memoryQueries, memory);
}
- void process(_MemoryT& result,
- const armem::query::data::MemoryQuery& query,
- const _MemoryT& memory) const override
+ virtual void process(ResultMemoryT& result,
+ const armem::query::data::MemoryQuery& query,
+ const MemoryT& memory) const override
{
if (auto q = dynamic_cast<const armem::query::data::memory::All*>(&query))
{
@@ -57,49 +66,52 @@ namespace armarx::armem::base::query_proc
}
else
{
- throw armem::error::UnknownQueryType("memory segment", query);
+ throw armem::error::UnknownQueryType(MemoryT::getLevelName(), query);
}
}
- void process(_MemoryT& result,
+ void process(ResultMemoryT& result,
const armem::query::data::memory::All& query,
- const _MemoryT& memory) const
+ const MemoryT& memory) const
{
- for (const auto& [name, coreSegment] : memory.coreSegments())
+ memory.forEachCoreSegment([this, &result, &query](const CoreSegmentT & coreSegment)
{
- result.addCoreSegment(coreSegmentProcessorProcess(query.coreSegmentQueries, coreSegment));
- }
+ childProcessor.process(result.addCoreSegment(coreSegment.name()), query.coreSegmentQueries, coreSegment);
+ });
}
- void process(_MemoryT& result,
+ void process(ResultMemoryT& result,
const armem::query::data::memory::Single& query,
- const _MemoryT& memory) const
+ const MemoryT& memory) const
{
try
{
const CoreSegmentT& coreSegment = memory.getCoreSegment(query.coreSegmentName);
- result.addCoreSegment(coreSegmentProcessorProcess(query.coreSegmentQueries, coreSegment));
+ childProcessor.process(result.addCoreSegment(coreSegment.name()), query.coreSegmentQueries, coreSegment);
}
catch (const error::MissingEntry&)
{
}
}
- void process(_MemoryT& result,
+ void process(ResultMemoryT& result,
const armem::query::data::memory::Regex& query,
- const _MemoryT& memory) const
+ const MemoryT& memory) const
{
- std::regex regex(query.coreSegmentNameRegex);
- for (const auto& [name, coreSegment] : memory.coreSegments())
+ const std::regex regex(query.coreSegmentNameRegex);
+ memory.forEachCoreSegment([this, &result, &query, ®ex](const CoreSegmentT & coreSegment)
{
if (std::regex_search(coreSegment.name(), regex))
{
- result.addCoreSegment(coreSegmentProcessorProcess(query.coreSegmentQueries, coreSegment));
+ childProcessor.process(result.addCoreSegment(coreSegment.name()), query.coreSegmentQueries, coreSegment);
}
- }
+ });
}
+
protected:
- virtual CoreSegmentT coreSegmentProcessorProcess(const armem::query::data::CoreSegmentQuerySeq& a, const CoreSegmentT& o) const = 0;
+
+ ChildProcessorT childProcessor;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h
index b3b7da7ffa9cc1d06aeadabef58c5aebba3974de..5102a12829c106e58159e7dcc35a7df0890f93d5 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h
@@ -1,39 +1,50 @@
#pragma once
-#include <RobotAPI/interface/armem/query.h>
-
#include "BaseQueryProcessorBase.h"
-#include "EntityQueryProcessorBase.h"
-#include <regex>
+#include <RobotAPI/libraries/armem/core/error.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/interface/armem/query.h>
-#include <RobotAPI/libraries/armem/core/error.h>
+#include <regex>
-namespace armarx::armem::base::query_proc
+namespace armarx::armem::server::query_proc::base
{
- /**
- * @brief Handles memory queries.
- */
- template <class _ProviderSegmentT>
+ template <QueryTarget queryTarget, class _ProviderSegmentT, class _ResultProviderSegmentT, class _ChildProcessorT>
class ProviderSegmentQueryProcessorBase :
- virtual public BaseQueryProcessorBase<_ProviderSegmentT, armem::query::data::ProviderSegmentQuery>
+ public BaseQueryProcessorBase<queryTarget, _ProviderSegmentT, _ResultProviderSegmentT, armem::query::data::ProviderSegmentQuery>
{
- using Base = BaseQueryProcessorBase<_ProviderSegmentT, armem::query::data::ProviderSegmentQuery>;
+ using Base = BaseQueryProcessorBase<queryTarget, _ProviderSegmentT, _ResultProviderSegmentT, armem::query::data::ProviderSegmentQuery>;
+
public:
+
using ProviderSegmentT = _ProviderSegmentT;
using EntityT = typename ProviderSegmentT::EntityT;
- using EntitySnapshotT = typename EntityT::EntitySnapshotT;
+
+ using ResultProviderSegmentT = _ResultProviderSegmentT;
+ using ResultEntityT = typename ResultProviderSegmentT::EntityT;
+
+ using ChildProcessorT = _ChildProcessorT;
+
+
+ public:
+
+ ProviderSegmentQueryProcessorBase()
+ {
+ }
+ ProviderSegmentQueryProcessorBase(ChildProcessorT&& childProcessor) :
+ childProcessor(childProcessor)
+ {
+ }
+
using Base::process;
- void process(_ProviderSegmentT& result,
- const armem::query::data::ProviderSegmentQuery& query,
- const _ProviderSegmentT& providerSegment) const override
+ virtual void process(ResultProviderSegmentT& result,
+ const armem::query::data::ProviderSegmentQuery& query,
+ const ProviderSegmentT& providerSegment) const override
{
if (auto q = dynamic_cast<const armem::query::data::provider::All*>(&query))
{
@@ -49,28 +60,28 @@ namespace armarx::armem::base::query_proc
}
else
{
- throw armem::error::UnknownQueryType("provider segment", query);
+ throw armem::error::UnknownQueryType(ProviderSegmentT::getLevelName(), query);
}
}
- void process(_ProviderSegmentT& result,
+ void process(ResultProviderSegmentT& result,
const armem::query::data::provider::All& query,
- const _ProviderSegmentT& providerSegment) const
+ const ProviderSegmentT& providerSegment) const
{
- for (const auto& [name, entity] : providerSegment.entities())
+ providerSegment.forEachEntity([this, &result, &query](const EntityT & entity)
{
- result.addEntity(entityProcessorProcess(query.entityQueries, entity));
- }
+ childProcessor.process(result.addEntity(entity.name()), query.entityQueries, entity);
+ });
}
- void process(_ProviderSegmentT& result,
+ void process(ResultProviderSegmentT& result,
const armem::query::data::provider::Single& query,
- const _ProviderSegmentT& providerSegment) const
+ const ProviderSegmentT& providerSegment) const
{
try
{
const EntityT& entity = providerSegment.getEntity(query.entityName);
- result.addEntity(entityProcessorProcess(query.entityQueries, entity));
+ childProcessor.process(result.addEntity(entity.name()), query.entityQueries, entity);
}
catch (const error::MissingEntry&)
{
@@ -78,21 +89,25 @@ namespace armarx::armem::base::query_proc
}
}
- void process(_ProviderSegmentT& result,
+ void process(ResultProviderSegmentT& result,
const armem::query::data::provider::Regex& query,
- const _ProviderSegmentT& providerSegment) const
+ const ProviderSegmentT& providerSegment) const
{
- std::regex regex(query.entityNameRegex);
- for (const auto& [name, entity] : providerSegment.entities())
+ const std::regex regex(query.entityNameRegex);
+ providerSegment.forEachEntity([this, &result, &query, ®ex](const EntityT & entity)
{
if (std::regex_search(entity.name(), regex))
{
- result.addEntity(entityProcessorProcess(query.entityQueries, entity));
+ childProcessor.process(result.addEntity(entity.name()), query.entityQueries, entity);
}
- }
+ return true;
+ });
}
+
protected:
- virtual EntityT entityProcessorProcess(const armem::query::data::EntityQuerySeq& a, const EntityT& o) const = 0;
+
+ ChildProcessorT childProcessor;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory.cpp b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a695cf43d51dcbbc2329fa1c48fbe8e63565c24f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory.cpp
@@ -0,0 +1,7 @@
+#include "diskmemory.h"
+
+
+namespace armarx::armem::server::query_proc::d_ltm
+{
+}
+
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory.h b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory.h
new file mode 100644
index 0000000000000000000000000000000000000000..0f2ffe791d192b9bc2b5f2a2be311735808427a4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory.h
@@ -0,0 +1,32 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/diskmemory/Memory.h>
+#include <RobotAPI/libraries/armem/server/query_proc/base.h>
+
+
+namespace armarx::armem::server::query_proc::d_ltm
+{
+ static const base::QueryTarget queryTarget = query::data::QueryTarget::LTM;
+
+
+ class EntityQueryProcessor :
+ public base::EntityQueryProcessorBase<queryTarget, armem::d_ltm::Entity, armem::d_ltm::Entity>
+ {
+ };
+
+ class ProviderSegmentQueryProcessor :
+ public base::ProviderSegmentQueryProcessorBase <queryTarget, armem::d_ltm::ProviderSegment, armem::d_ltm::ProviderSegment, EntityQueryProcessor >
+ {
+ };
+
+ class CoreSegmentQueryProcessor :
+ public base::CoreSegmentQueryProcessorBase<queryTarget, armem::d_ltm::CoreSegment, armem::d_ltm::CoreSegment, ProviderSegmentQueryProcessor>
+ {
+ };
+
+ class MemoryQueryProcessor :
+ public base::MemoryQueryProcessorBase <queryTarget, armem::d_ltm::Memory, armem::d_ltm::Memory, CoreSegmentQueryProcessor >
+ {
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/BaseQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/BaseQueryProcessor.cpp
deleted file mode 100644
index 68821bcd95cdf2aaccf7126d4055495d39d0393d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/BaseQueryProcessor.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "BaseQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/BaseQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/BaseQueryProcessor.h
deleted file mode 100644
index 84f967795fd26d79d173a14e71498dbec7d56ee1..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/BaseQueryProcessor.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#pragma once
-
-#include <RobotAPI/interface/armem/query.h>
-#include <RobotAPI/libraries/armem/core/DataMode.h>
-
-#include "../base/BaseQueryProcessorBase.h"
-
-
-namespace armarx::armem::d_ltm::query_proc
-{
- /**
- * @brief Base class for memory query processors.
- */
- template <class DataT, class QueryT>
- class BaseQueryProcessor :
- virtual public base::query_proc::BaseQueryProcessorBase<DataT, QueryT>
- {
- using Base = base::query_proc::BaseQueryProcessorBase<DataT, QueryT>;
-
- public:
- BaseQueryProcessor()
- {}
-
- protected:
- };
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/CoreSegmentQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/CoreSegmentQueryProcessor.cpp
deleted file mode 100644
index afbe35ad15eea0342b0b0d4df0200aaf0d32aa2a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/CoreSegmentQueryProcessor.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "CoreSegmentQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/CoreSegmentQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/CoreSegmentQueryProcessor.h
deleted file mode 100644
index 1c40f7fc64b0a2c59fdc3e54d968c6627480cbb0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/CoreSegmentQueryProcessor.h
+++ /dev/null
@@ -1,36 +0,0 @@
-#pragma once
-
-#include "BaseQueryProcessor.h"
-#include "../base/CoreSegmentQueryProcessorBase.h"
-
-#include "../../../core/diskmemory/CoreSegment.h"
-
-#include "ProviderSegmentQueryProcessor.h"
-
-
-namespace armarx::armem::d_ltm::query_proc
-{
- /**
- * @brief Handles memory queries.
- */
- class CoreSegmentQueryProcessor :
- virtual public BaseQueryProcessor<d_ltm::CoreSegment, armem::query::data::CoreSegmentQuery>,
- virtual public base::query_proc::CoreSegmentQueryProcessorBase<d_ltm::CoreSegment>
- {
- using Base = BaseQueryProcessor<d_ltm::CoreSegment, armem::query::data::CoreSegmentQuery>;
-
- public:
- CoreSegmentQueryProcessor() :
- Base()
- {}
-
- protected:
- virtual ProviderSegmentT providerSegmentProcessorProcess(const armem::query::data::ProviderSegmentQuerySeq& q, const ProviderSegmentT& s) const override
- {
- return providerSegmentProcessor.process(q, s, {armem::query::data::QueryTarget::Disk});
- }
-
- private:
- ProviderSegmentQueryProcessor providerSegmentProcessor;
- };
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/EntityQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/EntityQueryProcessor.cpp
deleted file mode 100644
index c1c321b026b173c6552758fb9d8b9fdf722ea5a4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/EntityQueryProcessor.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "EntityQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/EntityQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/EntityQueryProcessor.h
deleted file mode 100644
index 4d1b9e14e21804f1e44f1a2fd6456da37a113c03..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/EntityQueryProcessor.h
+++ /dev/null
@@ -1,39 +0,0 @@
-#pragma once
-
-#include "BaseQueryProcessor.h"
-#include "../base/EntityQueryProcessorBase.h"
-
-#include "../../../core/diskmemory/Entity.h"
-
-#include "EntityQueryProcessor.h"
-
-namespace armarx::armem::d_ltm::query_proc
-{
- /**
- * @brief Handles memory queries.
- */
- class EntityQueryProcessor :
- virtual public BaseQueryProcessor<d_ltm::Entity, armem::query::data::EntityQuery>,
- virtual public base::query_proc::EntityQueryProcessorBase<d_ltm::Entity>
- {
- using Base = BaseQueryProcessor<d_ltm::Entity, armem::query::data::EntityQuery>;
-
- public:
- EntityQueryProcessor() :
- Base()
- {}
-
- private:
- void addResultSnapshot(d_ltm::Entity& result, d_ltm::Entity::ContainerT::const_iterator it) const override
- {
- addResultSnapshot(result, it->second);
- }
-
- void addResultSnapshot(d_ltm::Entity& result, const d_ltm::EntitySnapshot& snapshot) const override
- {
- result.addSnapshot(snapshot.copy());
- }
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/MemoryQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/MemoryQueryProcessor.cpp
deleted file mode 100644
index 69b04de6c9e623286a5bda836dddfdc8b551b64a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/MemoryQueryProcessor.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "MemoryQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/MemoryQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/MemoryQueryProcessor.h
deleted file mode 100644
index 2abdcc1ccc7c25a317edc67e36835cb4a4df2f9b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/MemoryQueryProcessor.h
+++ /dev/null
@@ -1,35 +0,0 @@
-#pragma once
-
-#include "BaseQueryProcessor.h"
-#include "../base/MemoryQueryProcessorBase.h"
-
-#include "../../../core/diskmemory/Memory.h"
-
-#include "CoreSegmentQueryProcessor.h"
-
-namespace armarx::armem::d_ltm::query_proc
-{
- /**
- * @brief Handles memory queries.
- */
- class MemoryQueryProcessor :
- virtual public BaseQueryProcessor<d_ltm::Memory, armem::query::data::MemoryQuery>,
- virtual public base::query_proc::MemoryQueryProcessorBase<d_ltm::Memory>
- {
- using Base = BaseQueryProcessor<d_ltm::Memory, armem::query::data::MemoryQuery>;
-
- public:
- MemoryQueryProcessor() :
- Base()
- {}
-
- protected:
- virtual CoreSegmentT coreSegmentProcessorProcess(const armem::query::data::CoreSegmentQuerySeq& q, const CoreSegmentT& s) const override
- {
- return coreSegmentProcessor.process(q, s, {armem::query::data::QueryTarget::Disk});
- }
-
- private:
- CoreSegmentQueryProcessor coreSegmentProcessor;
- };
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/ProviderSegmentQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/ProviderSegmentQueryProcessor.cpp
deleted file mode 100644
index 9a2a4405001f0904b74fc6afcf96813eef0879cd..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/ProviderSegmentQueryProcessor.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "ProviderSegmentQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/ProviderSegmentQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/ProviderSegmentQueryProcessor.h
deleted file mode 100644
index 94c2e756875c1152f8018f70c5e0d691a5704582..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/ProviderSegmentQueryProcessor.h
+++ /dev/null
@@ -1,37 +0,0 @@
-#pragma once
-
-#include "BaseQueryProcessor.h"
-#include "../base/ProviderSegmentQueryProcessorBase.h"
-
-#include "../../../core/diskmemory/ProviderSegment.h"
-
-#include "EntityQueryProcessor.h"
-
-namespace armarx::armem::d_ltm::query_proc
-{
- /**
- * @brief Handles memory queries.
- */
- class ProviderSegmentQueryProcessor :
- virtual public BaseQueryProcessor<d_ltm::ProviderSegment, armem::query::data::ProviderSegmentQuery>,
- virtual public base::query_proc::ProviderSegmentQueryProcessorBase<d_ltm::ProviderSegment>
- {
- using Base = BaseQueryProcessor<d_ltm::ProviderSegment, armem::query::data::ProviderSegmentQuery>;
-
- public:
- ProviderSegmentQueryProcessor() :
- Base()
- {}
-
- protected:
- virtual EntityT entityProcessorProcess(const armem::query::data::EntityQuerySeq& q, const EntityT& s) const override
- {
- return entityProcessor.process(q, s, {armem::query::data::QueryTarget::Disk});
- }
-
- private:
- EntityQueryProcessor entityProcessor;
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/BaseQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/BaseQueryProcessor.cpp
deleted file mode 100644
index 68821bcd95cdf2aaccf7126d4055495d39d0393d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/BaseQueryProcessor.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "BaseQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/BaseQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/BaseQueryProcessor.h
deleted file mode 100644
index 458128e5305139db33a6c25d6ff19e3e92772fd5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/BaseQueryProcessor.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#pragma once
-
-#include <RobotAPI/interface/armem/query.h>
-#include <RobotAPI/libraries/armem/core/DataMode.h>
-
-#include "../base/BaseQueryProcessorBase.h"
-
-
-namespace armarx::armem::ltm::query_proc
-{
- /**
- * @brief Base class for memory query processors.
- */
- template <class DataT, class QueryT>
- class BaseQueryProcessor :
- virtual public base::query_proc::BaseQueryProcessorBase<DataT, QueryT>
- {
- using Base = base::query_proc::BaseQueryProcessorBase<DataT, QueryT>;
-
- public:
- BaseQueryProcessor()
- {}
-
- protected:
- };
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/CoreSegmentQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/CoreSegmentQueryProcessor.cpp
deleted file mode 100644
index afbe35ad15eea0342b0b0d4df0200aaf0d32aa2a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/CoreSegmentQueryProcessor.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "CoreSegmentQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/CoreSegmentQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/CoreSegmentQueryProcessor.h
deleted file mode 100644
index e333dd816aedabbb8dfa7209c687caf727d9ccf9..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/CoreSegmentQueryProcessor.h
+++ /dev/null
@@ -1,36 +0,0 @@
-#pragma once
-
-#include "BaseQueryProcessor.h"
-#include "../base/CoreSegmentQueryProcessorBase.h"
-
-#include "../../../core/longtermmemory/CoreSegment.h"
-
-#include "ProviderSegmentQueryProcessor.h"
-
-
-namespace armarx::armem::ltm::query_proc
-{
- /**
- * @brief Handles memory queries.
- */
- class CoreSegmentQueryProcessor :
- virtual public BaseQueryProcessor<ltm::CoreSegment, armem::query::data::CoreSegmentQuery>,
- virtual public base::query_proc::CoreSegmentQueryProcessorBase<ltm::CoreSegment>
- {
- using Base = BaseQueryProcessor<ltm::CoreSegment, armem::query::data::CoreSegmentQuery>;
-
- public:
- CoreSegmentQueryProcessor() :
- Base()
- {}
-
- protected:
- virtual ProviderSegmentT providerSegmentProcessorProcess(const armem::query::data::ProviderSegmentQuerySeq& q, const ProviderSegmentT& s) const override
- {
- return providerSegmentProcessor.process(q, s, {armem::query::data::QueryTarget::LTM});
- }
-
- private:
- ProviderSegmentQueryProcessor providerSegmentProcessor;
- };
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/EntityQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/EntityQueryProcessor.cpp
deleted file mode 100644
index c1c321b026b173c6552758fb9d8b9fdf722ea5a4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/EntityQueryProcessor.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "EntityQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/EntityQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/EntityQueryProcessor.h
deleted file mode 100644
index 5ec4c5dde481e16f0af30589d57ff2a447d81221..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/EntityQueryProcessor.h
+++ /dev/null
@@ -1,39 +0,0 @@
-#pragma once
-
-#include "BaseQueryProcessor.h"
-#include "../base/EntityQueryProcessorBase.h"
-
-#include "../../../core/longtermmemory/Entity.h"
-
-#include "EntityQueryProcessor.h"
-
-namespace armarx::armem::ltm::query_proc
-{
- /**
- * @brief Handles memory queries.
- */
- class EntityQueryProcessor :
- virtual public BaseQueryProcessor<ltm::Entity, armem::query::data::EntityQuery>,
- virtual public base::query_proc::EntityQueryProcessorBase<ltm::Entity>
- {
- using Base = BaseQueryProcessor<ltm::Entity, armem::query::data::EntityQuery>;
-
- public:
- EntityQueryProcessor() :
- Base()
- {}
-
- private:
- void addResultSnapshot(ltm::Entity& result, ltm::Entity::ContainerT::const_iterator it) const override
- {
- addResultSnapshot(result, it->second);
- }
-
- void addResultSnapshot(ltm::Entity& result, const ltm::EntitySnapshot& snapshot) const override
- {
- result.addSnapshot(snapshot.copy());
- }
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/MemoryQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/MemoryQueryProcessor.cpp
deleted file mode 100644
index 69b04de6c9e623286a5bda836dddfdc8b551b64a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/MemoryQueryProcessor.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "MemoryQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/MemoryQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/MemoryQueryProcessor.h
deleted file mode 100644
index 569014d6579eeb6c99996e4be5b7cb68f65457b1..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/MemoryQueryProcessor.h
+++ /dev/null
@@ -1,35 +0,0 @@
-#pragma once
-
-#include "BaseQueryProcessor.h"
-#include "../base/MemoryQueryProcessorBase.h"
-
-#include "../../../core/longtermmemory/Memory.h"
-
-#include "CoreSegmentQueryProcessor.h"
-
-namespace armarx::armem::ltm::query_proc
-{
- /**
- * @brief Handles memory queries.
- */
- class MemoryQueryProcessor :
- virtual public BaseQueryProcessor<ltm::Memory, armem::query::data::MemoryQuery>,
- virtual public base::query_proc::MemoryQueryProcessorBase<ltm::Memory>
- {
- using Base = BaseQueryProcessor<ltm::Memory, armem::query::data::MemoryQuery>;
-
- public:
- MemoryQueryProcessor() :
- Base()
- {}
-
- protected:
- virtual CoreSegmentT coreSegmentProcessorProcess(const armem::query::data::CoreSegmentQuerySeq& q, const CoreSegmentT& s) const override
- {
- return coreSegmentProcessor.process(q, s, {armem::query::data::QueryTarget::LTM});
- }
-
- private:
- CoreSegmentQueryProcessor coreSegmentProcessor;
- };
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.cpp
deleted file mode 100644
index 9a2a4405001f0904b74fc6afcf96813eef0879cd..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "ProviderSegmentQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.h
deleted file mode 100644
index 5443acfd6594b98250711cf2acfd434a98414066..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.h
+++ /dev/null
@@ -1,37 +0,0 @@
-#pragma once
-
-#include "BaseQueryProcessor.h"
-#include "../base/ProviderSegmentQueryProcessorBase.h"
-
-#include "../../../core/longtermmemory/ProviderSegment.h"
-
-#include "EntityQueryProcessor.h"
-
-namespace armarx::armem::ltm::query_proc
-{
- /**
- * @brief Handles memory queries.
- */
- class ProviderSegmentQueryProcessor :
- virtual public BaseQueryProcessor<ltm::ProviderSegment, armem::query::data::ProviderSegmentQuery>,
- virtual public base::query_proc::ProviderSegmentQueryProcessorBase<ltm::ProviderSegment>
- {
- using Base = BaseQueryProcessor<ltm::ProviderSegment, armem::query::data::ProviderSegmentQuery>;
-
- public:
- ProviderSegmentQueryProcessor() :
- Base()
- {}
-
- protected:
- virtual EntityT entityProcessorProcess(const armem::query::data::EntityQuerySeq& q, const EntityT& s) const override
- {
- return entityProcessor.process(q, s, {armem::query::data::QueryTarget::LTM});
- }
-
- private:
- EntityQueryProcessor entityProcessor;
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ltm.cpp b/source/RobotAPI/libraries/armem/server/query_proc/ltm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..238cc4b8d1601bf1ffae131caabbf8ca8435899a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/ltm.cpp
@@ -0,0 +1,7 @@
+#include "ltm.h"
+
+
+namespace armarx::armem::server::query_proc::ltm
+{
+}
+
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ltm.h b/source/RobotAPI/libraries/armem/server/query_proc/ltm.h
new file mode 100644
index 0000000000000000000000000000000000000000..ae0dc778c821ae138c98b8554a22237bf26e5989
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/ltm.h
@@ -0,0 +1,32 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
+#include <RobotAPI/libraries/armem/server/query_proc/base.h>
+
+
+namespace armarx::armem::server::query_proc::ltm
+{
+ static const base::QueryTarget queryTarget = query::data::QueryTarget::LTM;
+
+
+ class EntityQueryProcessor :
+ public base::EntityQueryProcessorBase<queryTarget, armem::ltm::Entity, armem::ltm::Entity>
+ {
+ };
+
+ class ProviderSegmentQueryProcessor :
+ public base::ProviderSegmentQueryProcessorBase <queryTarget, armem::ltm::ProviderSegment, armem::ltm::ProviderSegment, EntityQueryProcessor >
+ {
+ };
+
+ class CoreSegmentQueryProcessor :
+ public base::CoreSegmentQueryProcessorBase <queryTarget, armem::ltm::CoreSegment, armem::ltm::CoreSegment, ProviderSegmentQueryProcessor>
+ {
+ };
+
+ class MemoryQueryProcessor :
+ public base::MemoryQueryProcessorBase <queryTarget, armem::ltm::Memory, armem::ltm::Memory, CoreSegmentQueryProcessor >
+ {
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm.cpp b/source/RobotAPI/libraries/armem/server/query_proc/wm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4e968623d63aa57213507879608c81823d31c7d6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm.cpp
@@ -0,0 +1,70 @@
+#include "wm.h"
+
+
+
+namespace armarx::armem::server::query_proc::wm::detail
+{
+
+ HasDataMode::HasDataMode(armem::DataMode dataMode) : dataMode(dataMode)
+ {
+ }
+
+}
+
+
+namespace armarx::armem::server::query_proc::wm
+{
+
+ ProviderSegmentQueryProcessor::ProviderSegmentQueryProcessor(DataMode dataMode) :
+ ProviderSegmentQueryProcessorBase(dataMode), HasDataMode(dataMode)
+ {
+ }
+
+
+ CoreSegmentQueryProcessor::CoreSegmentQueryProcessor(DataMode dataMode) :
+ CoreSegmentQueryProcessorBase(dataMode), HasDataMode(dataMode)
+ {
+ }
+
+
+ MemoryQueryProcessor::MemoryQueryProcessor(DataMode dataMode) :
+ MemoryQueryProcessorBase(dataMode), HasDataMode(dataMode)
+ {
+ }
+
+}
+
+
+namespace armarx::armem::server::query_proc::wm_server
+{
+ ProviderSegmentQueryProcessor::ProviderSegmentQueryProcessor(DataMode dataMode) :
+ ProviderSegmentQueryProcessorBase(dataMode), HasDataMode(dataMode)
+ {
+ }
+
+
+ CoreSegmentQueryProcessor::CoreSegmentQueryProcessor(DataMode dataMode) :
+ CoreSegmentQueryProcessorBase(dataMode),
+ HasDataMode(dataMode)
+ {
+ }
+
+
+ void CoreSegmentQueryProcessor::process(
+ armem::wm::CoreSegment& result,
+ const armem::query::data::CoreSegmentQuery& query,
+ const CoreSegment& coreSegment) const
+ {
+ std::scoped_lock lock(coreSegment.mutex());
+ CoreSegmentQueryProcessorBase::process(result, query, coreSegment);
+ }
+
+
+ MemoryQueryProcessor::MemoryQueryProcessor(DataMode dataMode) :
+ MemoryQueryProcessorBase(dataMode),
+ HasDataMode(dataMode)
+ {
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm.h b/source/RobotAPI/libraries/armem/server/query_proc/wm.h
new file mode 100644
index 0000000000000000000000000000000000000000..aa52b4ab22fe1c6ef194ef37d4e38f5c7720d6c5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm.h
@@ -0,0 +1,164 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/DataMode.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/query_proc/base.h>
+
+
+namespace armarx::armem::server::query_proc::wm
+{
+ static const base::QueryTarget queryTarget = query::data::QueryTarget::WM;
+}
+namespace armarx::armem::server::query_proc::wm::detail
+{
+
+ class HasDataMode
+ {
+ public:
+
+ HasDataMode(armem::DataMode dataMode);
+
+
+ protected:
+
+ armem::DataMode dataMode;
+
+ };
+
+
+
+ template <class SourceEntityT>
+ class EntityQueryProcessor :
+ public base::EntityQueryProcessorBase<queryTarget, SourceEntityT, armem::wm::Entity>,
+ public HasDataMode
+ {
+ public:
+
+ EntityQueryProcessor(DataMode dataMode = DataMode::WithData) :
+ HasDataMode(dataMode)
+ {}
+
+
+ protected:
+
+ void addResultSnapshot(armem::wm::Entity& result, const typename SourceEntityT::EntitySnapshotT& snapshot) const
+ {
+ bool withData = (dataMode == DataMode::WithData);
+ if (withData)
+ {
+ result.addSnapshot(server::wm::EntitySnapshot{ snapshot });
+ }
+ else
+ {
+ server::wm::EntitySnapshot copy = snapshot;
+ copy.forEachInstance([](server::wm::EntityInstance & i)
+ {
+ i.data() = nullptr;
+ return true;
+ });
+ result.addSnapshot(std::move(copy));
+ }
+ }
+
+ };
+}
+
+
+namespace armarx::armem::server::query_proc::wm
+{
+
+ using EntityQueryProcessor = detail::EntityQueryProcessor<armem::wm::Entity>;
+
+
+ class ProviderSegmentQueryProcessor :
+ public base::ProviderSegmentQueryProcessorBase<queryTarget, armem::wm::ProviderSegment, armem::wm::ProviderSegment, EntityQueryProcessor >,
+ public detail::HasDataMode
+ {
+ public:
+
+ ProviderSegmentQueryProcessor(DataMode dataMode = DataMode::WithData);
+
+ };
+
+
+ class CoreSegmentQueryProcessor :
+ public base::CoreSegmentQueryProcessorBase <queryTarget, armem::wm::CoreSegment, armem::wm::CoreSegment, ProviderSegmentQueryProcessor >,
+ public detail::HasDataMode
+ {
+ using Base = base::CoreSegmentQueryProcessorBase<queryTarget, armem::wm::CoreSegment, armem::wm::CoreSegment, ProviderSegmentQueryProcessor>;
+ using CoreSegment = armem::wm::CoreSegment;
+ using ProviderSegment = armem::wm::ProviderSegment;
+
+ public:
+
+ CoreSegmentQueryProcessor(DataMode dataMode = DataMode::WithData);
+
+ };
+
+
+ class MemoryQueryProcessor :
+ public base::MemoryQueryProcessorBase<queryTarget, armem::wm::Memory, armem::wm::Memory, CoreSegmentQueryProcessor>,
+ public detail::HasDataMode
+ {
+ public:
+
+ MemoryQueryProcessor(DataMode dataMode = DataMode::WithData);
+
+ };
+
+}
+
+
+namespace armarx::armem::server::query_proc::wm_server
+{
+
+ using EntityQueryProcessor = wm::detail::EntityQueryProcessor<server::wm::Entity>;
+
+
+ class ProviderSegmentQueryProcessor :
+ public base::ProviderSegmentQueryProcessorBase<wm::queryTarget, server::wm::ProviderSegment, armem::wm::ProviderSegment, EntityQueryProcessor >,
+ public wm::detail::HasDataMode
+ {
+ public:
+
+ ProviderSegmentQueryProcessor(DataMode dataMode = DataMode::WithData);
+
+ };
+
+
+ class CoreSegmentQueryProcessor :
+ public base::CoreSegmentQueryProcessorBase <wm::queryTarget, server::wm::CoreSegment, armem::wm::CoreSegment, ProviderSegmentQueryProcessor >,
+ public wm::detail::HasDataMode
+ {
+ using Base = base::CoreSegmentQueryProcessorBase<wm::queryTarget, server::wm::CoreSegment, armem::wm::CoreSegment, ProviderSegmentQueryProcessor>;
+ using CoreSegment = server::wm::CoreSegment;
+ using ProviderSegment = server::wm::ProviderSegment;
+
+ public:
+
+ CoreSegmentQueryProcessor(DataMode dataMode = DataMode::WithData);
+
+
+ using Base::process;
+
+ /// Locks the core segment, then delegates back to `CoreSegmentQueryProcessorBase`.
+ void process(
+ armem::wm::CoreSegment& result,
+ const armem::query::data::CoreSegmentQuery& query,
+ const CoreSegment& coreSegment) const override;
+
+ };
+
+
+ class MemoryQueryProcessor :
+ public base::MemoryQueryProcessorBase<wm::queryTarget, server::wm::Memory, armem::wm::Memory, CoreSegmentQueryProcessor>,
+ public wm::detail::HasDataMode
+ {
+ public:
+
+ MemoryQueryProcessor(DataMode dataMode = DataMode::WithData);
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/BaseQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/BaseQueryProcessor.cpp
deleted file mode 100644
index 68821bcd95cdf2aaccf7126d4055495d39d0393d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/BaseQueryProcessor.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "BaseQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/BaseQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/BaseQueryProcessor.h
deleted file mode 100644
index 99811dcc72d168dfecb0c5864f0760c4d5634a6e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/BaseQueryProcessor.h
+++ /dev/null
@@ -1,30 +0,0 @@
-#pragma once
-
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
-
-#include <RobotAPI/interface/armem/query.h>
-#include <RobotAPI/libraries/armem/core/DataMode.h>
-
-#include "../base/BaseQueryProcessorBase.h"
-
-
-namespace armarx::armem::wm::query_proc
-{
- /**
- * @brief Base class for memory query processors.
- */
- template <class DataT, class QueryT>
- class BaseQueryProcessor :
- virtual public base::query_proc::BaseQueryProcessorBase<DataT, QueryT>
- {
- using Base = base::query_proc::BaseQueryProcessorBase<DataT, QueryT>;
-
- public:
- BaseQueryProcessor(DataMode dataMode = DataMode::WithData) :
- dataMode(dataMode)
- {}
-
- protected:
- DataMode dataMode;
- };
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.cpp
deleted file mode 100644
index afbe35ad15eea0342b0b0d4df0200aaf0d32aa2a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "CoreSegmentQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.h
deleted file mode 100644
index 390ab66f656ece6010457f12f09917d9fca413f6..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.h
+++ /dev/null
@@ -1,40 +0,0 @@
-#pragma once
-
-#include "BaseQueryProcessor.h"
-#include "../base/CoreSegmentQueryProcessorBase.h"
-
-#include "ProviderSegmentQueryProcessor.h"
-
-
-namespace armarx::armem::wm::query_proc
-{
- /**
- * @brief Handles memory queries.
- */
- class CoreSegmentQueryProcessor :
- virtual public BaseQueryProcessor<wm::CoreSegment, armem::query::data::CoreSegmentQuery>,
- virtual public base::query_proc::CoreSegmentQueryProcessorBase<wm::CoreSegment>
- {
- using Base = BaseQueryProcessor<wm::CoreSegment, armem::query::data::CoreSegmentQuery>;
-
- public:
- CoreSegmentQueryProcessor(DataMode dataMode = DataMode::WithData) :
- Base(dataMode), providerSegmentProcessor(dataMode)
- {}
-
- using Base::process;
- data::CoreSegment processToIce(const armem::query::data::CoreSegmentQuery& query, const wm::CoreSegment& coreSegment) const
- {
- return toIce<data::CoreSegment>(process(query, coreSegment));
- }
-
- protected:
- virtual ProviderSegmentT providerSegmentProcessorProcess(const armem::query::data::ProviderSegmentQuerySeq& q, const ProviderSegmentT& s) const override
- {
- return providerSegmentProcessor.process(q, s, {armem::query::data::QueryTarget::WM});
- }
-
- private:
- ProviderSegmentQueryProcessor providerSegmentProcessor;
- };
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.cpp
deleted file mode 100644
index c1c321b026b173c6552758fb9d8b9fdf722ea5a4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "EntityQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.h
deleted file mode 100644
index 94dc905e489be501257be508cff03c7efee8f8b8..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.h
+++ /dev/null
@@ -1,50 +0,0 @@
-#pragma once
-
-#include "BaseQueryProcessor.h"
-#include "../base/EntityQueryProcessorBase.h"
-
-#include "EntityQueryProcessor.h"
-
-namespace armarx::armem::wm::query_proc
-{
- /**
- * @brief Handles memory queries.
- */
- class EntityQueryProcessor :
- virtual public BaseQueryProcessor<wm::Entity, armem::query::data::EntityQuery>,
- virtual public base::query_proc::EntityQueryProcessorBase<wm::Entity>
- {
- using Base = BaseQueryProcessor<wm::Entity, armem::query::data::EntityQuery>;
-
- public:
- EntityQueryProcessor(DataMode dataMode = DataMode::WithData) :
- Base(dataMode)
- {}
-
- data::Entity processToIce(const armem::query::data::EntityQuery& query, const wm::Entity& entity) const
- {
- return toIce<data::Entity>(process(query, entity));
- }
-
- private:
- void addResultSnapshot(wm::Entity& result, wm::Entity::ContainerT::const_iterator it) const override
- {
- addResultSnapshot(result, it->second);
- }
-
- void addResultSnapshot(wm::Entity& result, const wm::EntitySnapshot& snapshot) const override
- {
- bool withData = (dataMode == DataMode::WithData);
- if (withData)
- {
- result.addSnapshot(snapshot.copy());
- }
- else
- {
- result.addSnapshot(snapshot.copyWithoutData());
- }
- }
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.cpp
deleted file mode 100644
index 69b04de6c9e623286a5bda836dddfdc8b551b64a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "MemoryQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h
deleted file mode 100644
index 8284df202bbcf602e2433639fe85388ca13d75ff..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h
+++ /dev/null
@@ -1,34 +0,0 @@
-#pragma once
-
-#include "BaseQueryProcessor.h"
-#include "../base/MemoryQueryProcessorBase.h"
-
-#include "CoreSegmentQueryProcessor.h"
-
-namespace armarx::armem::wm::query_proc
-{
- /**
- * @brief Handles memory queries.
- */
- class MemoryQueryProcessor :
- virtual public BaseQueryProcessor<wm::Memory, armem::query::data::MemoryQuery>,
- virtual public base::query_proc::MemoryQueryProcessorBase<wm::Memory>
- {
- using Base = BaseQueryProcessor<wm::Memory, armem::query::data::MemoryQuery>;
-
- public:
- MemoryQueryProcessor(DataMode dataMode = DataMode::WithData) :
- Base(dataMode), coreSegmentProcessor(dataMode)
- {}
-
- protected:
- virtual CoreSegmentT coreSegmentProcessorProcess(const armem::query::data::CoreSegmentQuerySeq& q, const CoreSegmentT& s) const override
- {
- return coreSegmentProcessor.process(q, s, {armem::query::data::QueryTarget::WM});
- }
-
-
- private:
- CoreSegmentQueryProcessor coreSegmentProcessor;
- };
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.cpp
deleted file mode 100644
index 9a2a4405001f0904b74fc6afcf96813eef0879cd..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "ProviderSegmentQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.h
deleted file mode 100644
index 468138d9e2bbd526db3632019beca6a5a29f8269..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.h
+++ /dev/null
@@ -1,39 +0,0 @@
-#pragma once
-
-#include "BaseQueryProcessor.h"
-#include "../base/ProviderSegmentQueryProcessorBase.h"
-
-#include "EntityQueryProcessor.h"
-
-namespace armarx::armem::wm::query_proc
-{
- /**
- * @brief Handles memory queries.
- */
- class ProviderSegmentQueryProcessor :
- virtual public BaseQueryProcessor<wm::ProviderSegment, armem::query::data::ProviderSegmentQuery>,
- virtual public base::query_proc::ProviderSegmentQueryProcessorBase<wm::ProviderSegment>
- {
- using Base = BaseQueryProcessor<wm::ProviderSegment, armem::query::data::ProviderSegmentQuery>;
-
- public:
- ProviderSegmentQueryProcessor(DataMode dataMode = DataMode::WithData) :
- Base(dataMode), entityProcessor(dataMode)
- {}
-
- using Base::process;
- data::ProviderSegment processToIce(const armem::query::data::ProviderSegmentQuery& query, const wm::ProviderSegment& providerSegment) const
- {
- return toIce<data::ProviderSegment>(process(query, providerSegment));
- }
-
- protected:
- virtual EntityT entityProcessorProcess(const armem::query::data::EntityQuerySeq& q, const EntityT& s) const override
- {
- return entityProcessor.process(q, s, {armem::query::data::QueryTarget::WM});
- }
-
- private:
- EntityQueryProcessor entityProcessor;
- };
-}
diff --git a/source/RobotAPI/libraries/armem/server/segment/Segment.cpp b/source/RobotAPI/libraries/armem/server/segment/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..050e77d392da77bc5c4e4edfcf11f76ee6e1e051
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/segment/Segment.cpp
@@ -0,0 +1,109 @@
+#include "Segment.h"
+
+#include <ArmarXCore/core/application/properties/PluginAll.h>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+
+namespace armarx::armem::server::segment
+{
+
+ wm::CoreSegmentBase::CoreSegmentBase(
+ armem::server::MemoryToIceAdapter& iceMemory,
+ const std::string& defaultCoreSegmentName,
+ aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType,
+ int defaultMaxHistorySize) :
+ Base(iceMemory),
+ p({defaultCoreSegmentName, defaultMaxHistorySize}),
+ coreSegmentAronType(coreSegmentAronType)
+ {
+ Logging::setTag("armarx::armem::wm::Segment");
+ }
+
+
+ wm::CoreSegmentBase::~CoreSegmentBase()
+ {
+ }
+
+
+ void wm::CoreSegmentBase::defineProperties(
+ armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix)
+ {
+ ARMARX_CHECK_NOT_NULL(defs);
+
+ defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the '" + p.coreSegmentName + "' core segment.");
+ defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of '" + p.coreSegmentName + "' history (-1 for infinite).");
+ }
+
+
+ void wm::CoreSegmentBase::onInit()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ segment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, coreSegmentAronType);
+ segment->setMaxHistorySize(p.maxHistorySize);
+ }
+
+
+ std::mutex& wm::CoreSegmentBase::mutex() const
+ {
+ ARMARX_CHECK_NOT_NULL(segment);
+ return segment->mutex();
+ }
+
+
+
+ wm::ProviderSegmentBase::ProviderSegmentBase(
+ armem::server::MemoryToIceAdapter& iceMemory,
+ const std::string& defaultProviderSegmentName,
+ aron::typenavigator::ObjectNavigatorPtr providerSegmentAronType,
+ const std::string& defaultCoreSegmentName,
+ aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType,
+ int defaultMaxHistorySize) :
+
+ Base(iceMemory),
+ p({defaultCoreSegmentName, defaultProviderSegmentName, defaultMaxHistorySize}),
+ coreSegmentAronType(coreSegmentAronType),
+ providerSegmentAronType(providerSegmentAronType)
+ {
+ Logging::setTag("armarx::armem::wm::Segment");
+ }
+
+
+ wm::ProviderSegmentBase::~ProviderSegmentBase()
+ {
+ }
+
+
+ void wm::ProviderSegmentBase::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ ARMARX_CHECK_NOT_NULL(defs);
+
+ defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the '" + p.coreSegmentName + "' core segment.");
+ defs->optional(p.providerSegmentName, prefix + "ProviderSegmentName", "Name of the '" + p.providerSegmentName + "' provider segment.");
+ defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of '" + p.providerSegmentName + "' history (-1 for infinite).");
+ }
+
+
+ void wm::ProviderSegmentBase::onInit()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ if (!iceMemory.workingMemory->hasCoreSegment(p.coreSegmentName))
+ {
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, coreSegmentAronType);
+ coreSegment->setMaxHistorySize(p.maxHistorySize);
+ }
+ else
+ {
+ coreSegment = &iceMemory.workingMemory->getCoreSegment(p.coreSegmentName);
+ }
+
+ segment = &coreSegment->addProviderSegment(p.providerSegmentName, providerSegmentAronType);
+ segment->setMaxHistorySize(p.maxHistorySize);
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/segment/Segment.h b/source/RobotAPI/libraries/armem/server/segment/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..7808e99d31f4df3543bfbd5b882a408dda143ed5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/segment/Segment.h
@@ -0,0 +1,199 @@
+#pragma once
+
+// STD/STL
+#include <mutex>
+#include <string>
+
+// ArmarX
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
+
+#include <RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h>
+
+
+namespace armarx::armem
+{
+ namespace server
+ {
+ class MemoryToIceAdapter;
+
+ namespace wm
+ {
+ class CoreSegment;
+ class ProviderSegment;
+ }
+ }
+}
+namespace armarx::armem::server::segment
+{
+ namespace detail
+ {
+ /**
+ * @brief A base class for memory servers to manage their segments.
+ * A segment can inherit this base class and add segment specific code.
+ * TODO (fabian.peller): add concept to only accept coresegments, providersegments or entitysegments
+ */
+ template <class SegmentType>
+ class SegmentBase : public armarx::Logging
+ {
+ public:
+
+ SegmentBase() = delete;
+ SegmentBase(MemoryToIceAdapter& iceMemory) :
+ iceMemory(iceMemory)
+ {
+ Logging::setTag("armarx::armem::Segment");
+ }
+
+ virtual ~SegmentBase() = default;
+
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") = 0;
+ virtual void onInit() = 0;
+ // virtual void onConnect() = 0;
+
+
+ protected:
+
+ // Memory connection
+ MemoryToIceAdapter& iceMemory;
+
+ SegmentType* segment;
+
+ };
+ }
+
+
+ namespace wm
+ {
+ /**
+ * @brief A base class for core segments
+ */
+ class CoreSegmentBase : public detail::SegmentBase<server::wm::CoreSegment>
+ {
+ using Base = detail::SegmentBase<server::wm::CoreSegment>;
+
+ public:
+
+ CoreSegmentBase(
+ MemoryToIceAdapter& iceMemory,
+ const std::string& defaultCoreSegmentName = "",
+ aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType = nullptr,
+ int defaultMaxHistorySize = -1);
+
+ virtual ~CoreSegmentBase() override;
+
+
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ virtual void onInit() override;
+ // virtual void onConnect() override;
+
+
+ std::mutex& mutex() const;
+
+
+ protected:
+
+ struct Properties
+ {
+ std::string coreSegmentName;
+ int maxHistorySize;
+ };
+ Properties p;
+
+ aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType;
+
+ };
+
+
+ /**
+ * @brief A base class for provider segments
+ */
+ class ProviderSegmentBase : public detail::SegmentBase<server::wm::ProviderSegment>
+ {
+ using Base = detail::SegmentBase<server::wm::ProviderSegment>;
+
+ public:
+
+ ProviderSegmentBase(
+ MemoryToIceAdapter& iceMemory,
+ const std::string& defaultProviderSegmentName = "",
+ aron::typenavigator::ObjectNavigatorPtr providerSegmentAronType = nullptr,
+ const std::string& defaultCoreSegmentName = "",
+ aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType = nullptr,
+ int defaultMaxHistorySize = -1);
+
+ virtual ~ProviderSegmentBase() override;
+
+
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ virtual void onInit() override;
+ // virtual void onConnect() override;
+
+
+ protected:
+
+ struct Properties
+ {
+ std::string coreSegmentName;
+ std::string providerSegmentName;
+ int maxHistorySize;
+ };
+ Properties p;
+
+ aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType;
+ aron::typenavigator::ObjectNavigatorPtr providerSegmentAronType;
+
+ server::wm::CoreSegment* coreSegment;
+ };
+
+
+
+ /**
+ * @brief A base class for provider segments with an aron business object
+ */
+ template <class BusinessClassObject>
+ class AronTypedCoreSegmentBase : public CoreSegmentBase
+ {
+ using Base = CoreSegmentBase;
+
+ public:
+
+ AronTypedCoreSegmentBase(
+ MemoryToIceAdapter& iceMemory,
+ const std::string& defaultCoreSegmentName = "",
+ int defaultMaxHistorySize = -1) :
+ Base(iceMemory, defaultCoreSegmentName, BusinessClassObject::toAronType(),
+ defaultMaxHistorySize)
+ {
+ }
+
+ virtual ~AronTypedCoreSegmentBase() = default;
+ };
+
+
+
+ /**
+ * @brief A base class for provider segments with an aron business object
+ */
+ template <class BusinessClassObject>
+ class AronTypedProviderSegmentBase : public ProviderSegmentBase
+ {
+ using Base = ProviderSegmentBase;
+
+ public:
+
+ AronTypedProviderSegmentBase(
+ MemoryToIceAdapter& iceMemory,
+ const std::string& defaultProviderSegmentName = "",
+ const std::string& defaultCoreSegmentName = "",
+ int defaultMaxHistorySize = -1) :
+ Base(iceMemory, defaultProviderSegmentName, BusinessClassObject::toAronType(),
+ defaultCoreSegmentName, nullptr, defaultMaxHistorySize)
+ {
+ }
+
+ virtual ~AronTypedProviderSegmentBase() = default;
+
+ };
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.cpp b/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8184e27c985877a19aaa873db07883685140b17b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.cpp
@@ -0,0 +1,79 @@
+#include "SpecializedSegment.h"
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/application/properties/PluginAll.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+
+namespace armarx::armem::server::segment
+{
+
+ SpecializedSegment::SpecializedSegment(
+ server::MemoryToIceAdapter& iceMemory,
+ aron::typenavigator::ObjectNavigatorPtr aronType,
+ const std::string& defaultCoreSegmentName,
+ int64_t defaultMaxHistorySize
+ ) :
+ iceMemory(iceMemory), aronType(aronType)
+ {
+ setDefaultCoreSegmentName(defaultCoreSegmentName);
+ setDefaultMaxHistorySize(defaultMaxHistorySize);
+ }
+
+
+ SpecializedSegment::~SpecializedSegment()
+ {
+ }
+
+
+ void SpecializedSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(properties.coreSegmentName,
+ prefix + "seg.CoreSegmentName",
+ "Name of the " + properties.coreSegmentName + " core segment.");
+
+ defs->optional(properties.maxHistorySize,
+ prefix + "seg.MaxHistorySize",
+ "Maximal size of the " + properties.coreSegmentName + " entity histories (-1 for infinite).");
+ }
+
+
+ void SpecializedSegment::init()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+ Logging::setTag(properties.coreSegmentName + " Core Segment");
+
+ ARMARX_INFO << "Adding core segment '" << properties.coreSegmentName << "'";
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(properties.coreSegmentName, aronType);
+ coreSegment->setMaxHistorySize(properties.maxHistorySize);
+ }
+
+
+ std::mutex& SpecializedSegment::mutex() const
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ return coreSegment->mutex();
+ }
+
+
+ void SpecializedSegment::setDefaultCoreSegmentName(const std::string& coreSegmentName)
+ {
+ this->properties.coreSegmentName = coreSegmentName;
+ }
+
+
+ void SpecializedSegment::setDefaultMaxHistorySize(int64_t maxHistorySize)
+ {
+ this->properties.maxHistorySize = maxHistorySize;
+ }
+
+
+ void SpecializedSegment::setAronType(aron::typenavigator::ObjectNavigatorPtr aronType)
+ {
+ this->aronType = aronType;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h b/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h
new file mode 100644
index 0000000000000000000000000000000000000000..460fbf3f0f0cd4ea6c2835d499071f2e43bf2a7d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h
@@ -0,0 +1,75 @@
+#pragma once
+
+#include <mutex>
+#include <string>
+
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h>
+
+
+namespace armarx::armem
+{
+ namespace server
+ {
+ class MemoryToIceAdapter;
+
+ namespace wm
+ {
+ class CoreSegment;
+ }
+ }
+}
+
+namespace armarx::armem::server::segment
+{
+
+ /**
+ * @brief Specialized management of a core segment.
+ */
+ class SpecializedSegment :
+ public armarx::Logging
+ {
+ public:
+
+ SpecializedSegment(
+ server::MemoryToIceAdapter& iceMemory,
+ aron::typenavigator::ObjectNavigatorPtr aronType = nullptr,
+ const std::string& defaultCoreSegmentName = "",
+ int64_t defaultMaxHistorySize = -1);
+
+ virtual ~SpecializedSegment();
+
+
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+ virtual void init();
+ // void connect();
+
+ std::mutex& mutex() const;
+
+
+ protected:
+
+ void setDefaultCoreSegmentName(const std::string& coreSegmentName);
+ void setDefaultMaxHistorySize(int64_t maxHistorySize);
+ void setAronType(aron::typenavigator::ObjectNavigatorPtr aronType);
+
+
+ protected:
+
+ server::MemoryToIceAdapter& iceMemory;
+ server::wm::CoreSegment* coreSegment = nullptr;
+ aron::typenavigator::ObjectNavigatorPtr aronType;
+
+ struct Properties
+ {
+ std::string coreSegmentName = "";
+ int64_t maxHistorySize = -1;
+ };
+ Properties properties;
+
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.cpp b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp
similarity index 72%
rename from source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.cpp
rename to source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp
index 2c3b0554c65523826db0a7709599ed8e5140e0a1..d90756691871b6ad444a67fa76a733d0086b5b29 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.cpp
+++ b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp
@@ -1,12 +1,8 @@
#include "MaxHistorySize.h"
-namespace armarx::armem::base::detail
+namespace armarx::armem::server::detail
{
- MaxHistorySize::~MaxHistorySize()
- {
- }
-
void MaxHistorySize::setMaxHistorySize(long maxSize)
{
this->_maxHistorySize = maxSize;
diff --git a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h
new file mode 100644
index 0000000000000000000000000000000000000000..e2f2d498c2945f4456c281abafe0f1ad66074e72
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h
@@ -0,0 +1,58 @@
+#pragma once
+
+
+namespace armarx::armem::server::detail
+{
+ // TODO: Replace by ConstrainedHistorySize (not only max entries, e.g. delete oldest / delete least accessed / ...)
+
+ class MaxHistorySize
+ {
+ public:
+
+ /**
+ * @brief Set the maximum number of snapshots to be contained in an entity.
+ * Affected entities are to be update right away.
+ */
+ void setMaxHistorySize(long maxSize);
+
+ long getMaxHistorySize() const;
+
+
+ protected:
+
+ /**
+ * @brief Maximum size of entity histories.
+ *
+ * If negative, the size of `history` is not limited.
+ *
+ * @see Entity::maxHstorySize
+ */
+ long _maxHistorySize = -1;
+
+ };
+
+
+
+ template <class DerivedT>
+ class MaxHistorySizeParent : public MaxHistorySize
+ {
+ public:
+
+ /**
+ * @brief Sets the maximum history size of entities in this container.
+ * This affects all current entities as well as new ones.
+ *
+ * @see MaxHistorySize::setMaxHistorySize()
+ */
+ void setMaxHistorySize(long maxSize)
+ {
+ MaxHistorySize::setMaxHistorySize(maxSize);
+ static_cast<DerivedT&>(*this).forEachChild([maxSize](auto & child)
+ {
+ child.setMaxHistorySize(maxSize);
+ return true;
+ });
+ }
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/wm/ice_conversions.cpp b/source/RobotAPI/libraries/armem/server/wm/ice_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1ba3352d6e8bb443693ebfa5752a46cad9c24267
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/wm/ice_conversions.cpp
@@ -0,0 +1,64 @@
+#include "ice_conversions.h"
+
+#include <RobotAPI/libraries/armem/core/base/ice_conversions.h>
+
+
+namespace armarx::armem::server
+{
+
+ void wm::toIce(data::EntityInstance& ice, const EntityInstance& data)
+ {
+ base::toIce(ice, data);
+ }
+ void wm::fromIce(const data::EntityInstance& ice, EntityInstance& data)
+ {
+ base::fromIce(ice, data);
+ }
+
+
+ void wm::toIce(data::EntitySnapshot& ice, const EntitySnapshot& snapshot)
+ {
+ base::toIce(ice, snapshot);
+ }
+ void wm::fromIce(const data::EntitySnapshot& ice, EntitySnapshot& snapshot)
+ {
+ base::fromIce(ice, snapshot);
+ }
+
+ void wm::toIce(data::Entity& ice, const Entity& entity)
+ {
+ base::toIce(ice, entity);
+ }
+ void wm::fromIce(const data::Entity& ice, Entity& entity)
+ {
+ base::fromIce(ice, entity);
+ }
+
+ void wm::toIce(data::ProviderSegment& ice, const ProviderSegment& providerSegment)
+ {
+ base::toIce(ice, providerSegment);
+ }
+ void wm::fromIce(const data::ProviderSegment& ice, ProviderSegment& providerSegment)
+ {
+ base::fromIce(ice, providerSegment);
+ }
+
+ void wm::toIce(data::CoreSegment& ice, const CoreSegment& coreSegment)
+ {
+ base::toIce(ice, coreSegment);
+ }
+ void wm::fromIce(const data::CoreSegment& ice, CoreSegment& coreSegment)
+ {
+ base::fromIce(ice, coreSegment);
+ }
+
+ void wm::toIce(data::Memory& ice, const Memory& memory)
+ {
+ base::toIce(ice, memory);
+ }
+ void wm::fromIce(const data::Memory& ice, Memory& memory)
+ {
+ base::fromIce(ice, memory);
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/wm/ice_conversions.h b/source/RobotAPI/libraries/armem/server/wm/ice_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..c25b16775bc577c61d0c804cf4525b23ecd5e5d1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/wm/ice_conversions.h
@@ -0,0 +1,30 @@
+#pragma once
+
+#include <RobotAPI/interface/armem/memory.h>
+
+#include "memory_definitions.h"
+
+
+namespace armarx::armem::server::wm
+{
+
+ void toIce(data::EntityInstance& ice, const EntityInstance& data);
+ void fromIce(const data::EntityInstance& ice, EntityInstance& data);
+
+
+ void toIce(data::EntitySnapshot& ice, const EntitySnapshot& snapshot);
+ void fromIce(const data::EntitySnapshot& ice, EntitySnapshot& snapshot);
+
+ void toIce(data::Entity& ice, const Entity& entity);
+ void fromIce(const data::Entity& ice, Entity& entity);
+
+
+ void toIce(data::ProviderSegment& ice, const ProviderSegment& providerSegment);
+ void fromIce(const data::ProviderSegment& ice, ProviderSegment& providerSegment);
+
+ void toIce(data::CoreSegment& ice, const CoreSegment& coreSegment);
+ void fromIce(const data::CoreSegment& ice, CoreSegment& coreSegment);
+
+ void toIce(data::Memory& ice, const Memory& memory);
+ void fromIce(const data::Memory& ice, Memory& memory);
+}
diff --git a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..58be8027db2561979d3510b7d481715678b61eae
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
@@ -0,0 +1,202 @@
+#include "memory_definitions.h"
+
+#include "error.h"
+
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <map>
+#include <vector>
+
+
+namespace armarx::armem::server::wm
+{
+
+ void Entity::setMaxHistorySize(long maxSize)
+ {
+ MaxHistorySize::setMaxHistorySize(maxSize);
+ truncate();
+ }
+
+
+ auto Entity::update(const EntityUpdate& update) -> UpdateResult
+ {
+ UpdateResult result = EntityBase::update(update);
+ result.removedSnapshots = this->truncate();
+ return result;
+ }
+
+
+ std::vector<EntitySnapshot> Entity::truncate()
+ {
+ std::vector<EntitySnapshot> removedElements;
+ if (_maxHistorySize >= 0)
+ {
+ while (this->_container.size() > size_t(_maxHistorySize))
+ {
+ removedElements.push_back(std::move(this->_container.begin()->second));
+ this->_container.erase(this->_container.begin());
+ }
+ ARMARX_CHECK_LESS_EQUAL(this->_container.size(), _maxHistorySize);
+ }
+ return removedElements;
+ }
+
+
+
+ Entity& ProviderSegment::addEntity(Entity&& entity)
+ {
+ Entity& added = ProviderSegmentBase::addEntity(std::move(entity));
+ added.setMaxHistorySize(this->getMaxHistorySize());
+ return added;
+ }
+
+
+
+ std::mutex& CoreSegment::mutex() const
+ {
+ return _mutex;
+ }
+
+
+ std::optional<wm::EntitySnapshot> CoreSegment::getLatestEntitySnapshot(const MemoryID& entityID) const
+ {
+ const wm::Entity& entity = this->getEntity(entityID);
+ if (entity.empty())
+ {
+ return std::nullopt;
+ }
+ else
+ {
+ return entity.getLatestSnapshot();
+ }
+ }
+
+
+ std::optional<wm::EntityInstance> CoreSegment::getLatestEntityInstance(
+ const MemoryID& entityID, int instanceIndex) const
+ {
+ auto snapshot = getLatestEntitySnapshot(entityID);
+ if (snapshot.has_value()
+ and instanceIndex >= 0
+ and static_cast<size_t>(instanceIndex) < snapshot->size())
+ {
+ return snapshot->getInstance(instanceIndex);
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+ armarx::aron::datanavigator::DictNavigatorPtr
+ CoreSegment::getLatestEntityInstanceData(const MemoryID& entityID, int instanceIndex) const
+ {
+ auto instance = getLatestEntityInstance(entityID, instanceIndex);
+ if (instance.has_value())
+ {
+ return instance->data();
+ }
+ else
+ {
+ return nullptr;
+ }
+ }
+
+
+ ProviderSegment& CoreSegment::addProviderSegment(ProviderSegment&& providerSegment)
+ {
+ ProviderSegmentT& added = CoreSegmentBase::addProviderSegment(std::move(providerSegment));
+ added.setMaxHistorySize(this->getMaxHistorySize());
+ return added;
+ }
+
+
+ std::optional<wm::EntitySnapshot>
+ CoreSegment::getLatestEntitySnapshotLocking(const MemoryID& entityID) const
+ {
+ std::scoped_lock lock(_mutex);
+ return getLatestEntitySnapshot(entityID);
+ }
+
+ std::optional<wm::EntityInstance>
+ CoreSegment::getLatestEntityInstanceLocking(const MemoryID& entityID, int instanceIndex) const
+ {
+ std::scoped_lock lock(_mutex);
+ return getLatestEntityInstance(entityID, instanceIndex);
+ }
+
+ armarx::aron::datanavigator::DictNavigatorPtr
+ CoreSegment::getLatestEntityInstanceDataLocking(const MemoryID& entityID, int instanceIndex) const
+ {
+ std::scoped_lock lock(_mutex);
+ return getLatestEntityInstanceData(entityID, instanceIndex);
+ }
+
+
+ // TODO: add core segment if param is set
+ std::vector<Memory::Base::UpdateResult>
+ Memory::updateLocking(const Commit& commit)
+ {
+ // Group updates by core segment, then update each core segment in a batch to only lock it once.
+ std::map<std::string, std::vector<const EntityUpdate*>> updatesPerCoreSegment;
+ for (const EntityUpdate& update : commit.updates)
+ {
+ updatesPerCoreSegment[update.entityID.coreSegmentName].push_back(&update);
+ }
+
+ std::vector<Memory::Base::UpdateResult> result;
+ // To throw an exception after the commit if a core segment is missing and the memory should not create new ones
+ std::vector<std::string> missingCoreSegmentNames;
+ for (const auto& [coreSegmentName, updates] : updatesPerCoreSegment)
+ {
+ auto it = this->_container.find(coreSegmentName);
+ if (it != this->_container.end())
+ {
+ CoreSegment& coreSegment = it->second;
+
+ // Lock the core segment for the whole batch.
+ std::scoped_lock lock(coreSegment.mutex());
+
+ for (const EntityUpdate* update : updates)
+ {
+ auto r = coreSegment.update(*update);
+ Base::UpdateResult ret { r };
+ ret.memoryUpdateType = UpdateType::UpdatedExisting;
+ result.push_back(ret);
+ }
+ }
+ else
+ {
+ // Perform the other updates first, then throw afterwards.
+ missingCoreSegmentNames.push_back(coreSegmentName);
+ }
+ }
+ // Throw an exception if something went wrong.
+ if (not missingCoreSegmentNames.empty())
+ {
+ // Just throw an exception for the first entry. We can extend this exception in the future.
+ throw armem::error::MissingEntry::create<CoreSegment>(missingCoreSegmentNames.front(), *this);
+ }
+ return result;
+ }
+
+
+ // TODO: Add core segment if param is set
+ Memory::Base::UpdateResult
+ Memory::updateLocking(const EntityUpdate& update)
+ {
+ this->_checkContainerName(update.entityID.memoryName, this->name());
+
+ CoreSegment& segment = getCoreSegment(update.entityID.coreSegmentName);
+ Base::UpdateResult result;
+ {
+ std::scoped_lock lock(segment.mutex());
+ result = segment.update(update);
+ }
+ result.memoryUpdateType = UpdateType::UpdatedExisting;
+ return result;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
new file mode 100644
index 0000000000000000000000000000000000000000..4eb6fdbb3105b0ae2ce3148c8457b915040a170a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
@@ -0,0 +1,189 @@
+#pragma once
+
+#include "detail/MaxHistorySize.h"
+
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/core/base/EntityInstanceBase.h>
+#include <RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h>
+#include <RobotAPI/libraries/armem/core/base/EntityBase.h>
+#include <RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h>
+#include <RobotAPI/libraries/armem/core/base/CoreSegmentBase.h>
+#include <RobotAPI/libraries/armem/core/base/MemoryBase.h>
+
+#include <mutex>
+#include <optional>
+
+
+namespace armarx::armem::server::wm
+{
+ using EntityInstanceMetadata = base::EntityInstanceMetadata;
+ using EntityInstanceData = armarx::aron::datanavigator::DictNavigator;
+ using EntityInstanceDataPtr = armarx::aron::datanavigator::DictNavigatorPtr;
+
+ using EntityInstance = armem::wm::EntityInstance;
+ using EntitySnapshot = armem::wm::EntitySnapshot;
+
+
+ /// @see base::EntityBase
+ class Entity :
+ public base::EntityBase<EntitySnapshot, Entity>,
+ public detail::MaxHistorySize
+ {
+ public:
+
+ using base::EntityBase<EntitySnapshot, Entity>::EntityBase;
+
+
+ /**
+ * @brief Sets the maximum history size.
+ *
+ * The current history is truncated if necessary.
+ */
+ void setMaxHistorySize(long maxSize);
+
+ UpdateResult update(const EntityUpdate& update);
+
+
+ protected:
+
+ /// If maximum size is set, ensure `history`'s is not higher.
+ std::vector<EntitySnapshotT> truncate();
+
+ };
+
+
+
+ /// @see base::ProviderSegmentBase
+ class ProviderSegment :
+ public base::ProviderSegmentBase<Entity, ProviderSegment>,
+ public detail::MaxHistorySizeParent<ProviderSegment>
+ {
+ public:
+
+ using base::ProviderSegmentBase<Entity, ProviderSegment>::ProviderSegmentBase;
+
+
+ using ProviderSegmentBase::addEntity;
+ EntityT& addEntity(EntityT&& entity);
+
+ };
+
+
+
+ /// @brief base::CoreSegmentBase
+ class CoreSegment :
+ public base::CoreSegmentBase<ProviderSegment, CoreSegment>,
+ public detail::MaxHistorySizeParent<CoreSegment>
+ {
+ using Base = base::CoreSegmentBase<ProviderSegment, CoreSegment>;
+
+ public:
+
+ using Base::CoreSegmentBase;
+
+ // ToDo: Replace by runLocked()
+ std::mutex& mutex() const;
+
+
+ // Non-locking interface
+
+ std::optional<wm::EntitySnapshot> getLatestEntitySnapshot(
+ const MemoryID& entityID) const;
+ std::optional<wm::EntityInstance> getLatestEntityInstance(
+ const MemoryID& entityID, int instanceIndex = 0) const;
+ armarx::aron::datanavigator::DictNavigatorPtr getLatestEntityInstanceData(
+ const MemoryID& entityID, int instanceIndex = 0) const;
+
+
+ template <class AronDtoT>
+ std::optional<AronDtoT> getLatestEntityInstanceDataAs(
+ const MemoryID& entityID, int instanceIndex = 0) const
+ {
+ wm::EntityInstanceDataPtr data = getLatestEntityInstanceData(entityID, instanceIndex);
+ if (data)
+ {
+ AronDtoT aron;
+ aron.fromAron(data);
+ return aron;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+ /// @see base::CoreSegmentBase::addProviderSegment()
+ using CoreSegmentBase::addProviderSegment;
+ ProviderSegment& addProviderSegment(ProviderSegment&& providerSegment);
+
+
+ // Locking interface
+
+ std::optional<wm::EntitySnapshot> getLatestEntitySnapshotLocking(
+ const MemoryID& entityID) const;
+ std::optional<wm::EntityInstance> getLatestEntityInstanceLocking(
+ const MemoryID& entityID, int instanceIndex = 0) const;
+ armarx::aron::datanavigator::DictNavigatorPtr getLatestEntityInstanceDataLocking(
+ const MemoryID& entityID, int instanceIndex = 0) const;
+
+
+ template <class AronDtoT>
+ std::optional<AronDtoT> getLatestEntityInstanceDataLockingAs(
+ const MemoryID& entityID, int instanceIndex = 0) const
+ {
+ // Keep lock to a minimum.
+ wm::EntityInstanceDataPtr data = nullptr;
+ {
+ std::scoped_lock lock(_mutex);
+ data = getLatestEntityInstanceData(entityID, instanceIndex);
+ }
+ if (data)
+ {
+ AronDtoT aron;
+ aron.fromAron(data);
+ return aron;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+ protected:
+
+ mutable std::mutex _mutex;
+
+ };
+
+
+
+ /// @see base::MemoryBase
+ class Memory :
+ public base::MemoryBase<CoreSegment, Memory>
+ {
+ using Base = base::MemoryBase<CoreSegment, Memory>;
+
+ public:
+
+ using Base::MemoryBase;
+
+
+ /**
+ * @brief Perform the commit, locking the core segments.
+ *
+ * Groups the commits by core segment, and updates each core segment
+ * in a batch, locking the core segment.
+ */
+ std::vector<Base::UpdateResult> updateLocking(const Commit& commit);
+
+ /**
+ * @brief Update the memory, locking the updated core segment.
+ */
+ Base::UpdateResult updateLocking(const EntityUpdate& update);
+
+ };
+
+}
+
diff --git a/source/RobotAPI/libraries/armem/test/ArMemForEachTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemForEachTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cc34f7e2d7cb73d7a3e3715064508c3b73cab351
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/test/ArMemForEachTest.cpp
@@ -0,0 +1,187 @@
+/*
+ * 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::ArmarXObjects::armem
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::armem
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include <RobotAPI/libraries/armem/core/Commit.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+
+#include <iostream>
+#include <set>
+
+
+namespace armem = armarx::armem;
+
+
+template <class ValueT>
+static
+std::vector<ValueT> toVector(const std::set<ValueT>& set)
+{
+ return {set.begin(), set.end()};
+}
+
+
+
+BOOST_AUTO_TEST_CASE(test_forEach)
+{
+ using namespace armarx::armem;
+
+ const MemoryID mid("memory");
+ const std::set<MemoryID> emptySet; // For comparison.
+
+ wm::Memory memory(mid);
+ std::set<MemoryID> cids, pids, eids, sids, iids;
+
+ for (size_t c = 0; c <= 2; ++c)
+ {
+ const MemoryID cid = mid.withCoreSegmentName("core_" + std::to_string(c));
+ cids.insert(cid);
+ wm::CoreSegment& coreSeg = memory.addCoreSegment(cid.coreSegmentName);
+ for (size_t p = 0; p <= c; ++p)
+ {
+ const MemoryID pid = cid.withProviderSegmentName("prov_" + std::to_string(p));
+ pids.insert(pid);
+ for (size_t e = 0; e <= p; ++e)
+ {
+ const MemoryID eid = pid.withEntityName("entity_" + std::to_string(e));
+ eids.insert(eid);
+ for (size_t s = 0; s <= e; ++s)
+ {
+ const MemoryID sid = eid.withTimestamp(Time::microSeconds(int(s)));
+ sids.insert(sid);
+
+ EntityUpdate update;
+ update.entityID = eid;
+ update.timeCreated = sid.timestamp;
+ for (size_t i = 0; i <= s; ++i)
+ {
+ const MemoryID iid = sid.withInstanceIndex(int(i));
+ iids.insert(iid);
+ update.instancesData.emplace_back();
+ }
+ auto r = memory.update(update);
+
+ BOOST_CHECK(coreSeg.hasProviderSegment(pid.providerSegmentName));
+ }
+ }
+
+ BOOST_CHECK_EQUAL(coreSeg.size(), p + 1);
+ }
+ BOOST_CHECK_EQUAL(memory.size(), c + 1);
+ }
+
+ BOOST_CHECK_GT(iids.size(), 0);
+ BOOST_CHECK_GT(sids.size(), 0);
+ BOOST_CHECK_GT(eids.size(), 0);
+ BOOST_CHECK_GT(pids.size(), 0);
+ BOOST_CHECK_GT(cids.size(), 0);
+
+ BOOST_TEST_MESSAGE("Memory: \n" << memory.dump());
+
+ memory.forEachInstance([&](const wm::EntityInstance & i) -> bool
+ {
+ BOOST_TEST_CONTEXT(wm::EntityInstance::getLevelName() << " " << i.id())
+ {
+ BOOST_TEST_INFO(toVector(iids));
+ BOOST_CHECK_EQUAL(iids.count(i.id()), 1);
+ iids.erase(i.id());
+ }
+ return true;
+ });
+ BOOST_TEST_CONTEXT(toVector(iids))
+ {
+ BOOST_CHECK_EQUAL(iids.size(), 0);
+ }
+
+ memory.forEachSnapshot([&](const wm::EntitySnapshot & s) -> bool
+ {
+ BOOST_TEST_CONTEXT(wm::EntitySnapshot::getLevelName() << " " << s.id())
+ {
+ BOOST_TEST_INFO(toVector(sids));
+ BOOST_CHECK_EQUAL(sids.count(s.id()), 1);
+ sids.erase(s.id());
+ }
+ return true;
+ });
+ BOOST_TEST_INFO(toVector(sids));
+ BOOST_CHECK_EQUAL(sids.size(), 0);
+
+ memory.forEachEntity([&](const wm::Entity & e) -> bool
+ {
+ BOOST_TEST_CONTEXT(wm::Entity::getLevelName() << " " << e.id())
+ {
+ BOOST_TEST_INFO(toVector(eids));
+ BOOST_CHECK_EQUAL(eids.count(e.id()), 1);
+ eids.erase(e.id());
+ }
+ return true;
+ });
+ BOOST_TEST_INFO(toVector(eids));
+ BOOST_CHECK_EQUAL(eids.size(), 0);
+
+ memory.forEachProviderSegment([&](const wm::ProviderSegment & p) // -> bool
+ {
+ BOOST_TEST_CONTEXT(wm::ProviderSegment::getLevelName() << " " << p.id())
+ {
+ BOOST_TEST_INFO(toVector(pids));
+ BOOST_CHECK_EQUAL(pids.count(p.id()), 1);
+ pids.erase(p.id());
+ }
+ return true;
+ });
+ BOOST_TEST_INFO(toVector(pids));
+ BOOST_CHECK_EQUAL(pids.size(), 0);
+
+ memory.forEachCoreSegment([&](const wm::CoreSegment & c) // -> bool
+ {
+ BOOST_TEST_CONTEXT(wm::CoreSegment::getLevelName() << " " << c.id())
+ {
+ BOOST_TEST_INFO(toVector(cids));
+ BOOST_CHECK_EQUAL(cids.count(c.id()), 1);
+ cids.erase(c.id());
+ }
+ return true;
+ });
+ BOOST_TEST_INFO(toVector(cids));
+ BOOST_CHECK_EQUAL(cids.size(), 0);
+
+}
+
+
+BOOST_AUTO_TEST_CASE(test_forEach_non_bool_func)
+{
+ // Check whether this compiles + runs without break.
+
+ armem::wm::Entity entity;
+ entity.addSnapshot(armem::Time::microSeconds(500));
+ entity.addSnapshot(armem::Time::microSeconds(1500));
+
+ int i = 0;
+ entity.forEachSnapshot([&i](const armem::wm::EntitySnapshot&) -> void
+ {
+ ++i;
+ });
+ BOOST_CHECK_EQUAL(i, 2);
+}
diff --git a/source/RobotAPI/libraries/armem/test/ArMemIceConversionsTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemIceConversionsTest.cpp
index a835f39bef74f745b22c4ee167e4755a031fec50..9fd00a4cf944f50cbd1c0d7bd06b9b56d812c161 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemIceConversionsTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemIceConversionsTest.cpp
@@ -25,7 +25,7 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include "../core/workingmemory/ice_conversions.h"
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <iostream>
@@ -59,12 +59,12 @@ BOOST_AUTO_TEST_CASE(test_entity)
BOOST_CHECK_EQUAL(ice->id.providerSegmentName, entity.id().providerSegmentName);
BOOST_CHECK_EQUAL(ice->id.entityName, entity.id().entityName);
- BOOST_CHECK_EQUAL(ice->history.size(), entity.history().size());
+ BOOST_CHECK_EQUAL(ice->history.size(), entity.size());
armem::wm::Entity entityOut;
armem::fromIce(ice, entityOut);
- BOOST_CHECK_EQUAL(entityOut.history().size(), entity.history().size());
+ BOOST_CHECK_EQUAL(entityOut.size(), entity.size());
std::vector<armem::Time> timestamps = entityOut.getTimestamps();
BOOST_CHECK_EQUAL_COLLECTIONS(timestamps.begin(), timestamps.end(), expectedTimestamps.begin(), expectedTimestamps.end());
diff --git a/source/RobotAPI/libraries/armem/test/ArMemLTMTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemLTMTest.cpp
index 24c0a22203aae81bfc52802a9f48fbb9755c9c5f..3ac3d50411854022aa31b781ba011e9ac8d91c75 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemLTMTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemLTMTest.cpp
@@ -25,8 +25,8 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include "../core/workingmemory/Memory.h"
-#include "../core/error.h"
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
@@ -122,7 +122,7 @@ namespace ArMemLTMTest
update.instancesData = q;
update.timeCreated = armem::Time::now();
BOOST_CHECK_NO_THROW(providerSegment.update(update));
- BOOST_CHECK_EQUAL(providerSegment.entities().size(), 1);
+ BOOST_CHECK_EQUAL(providerSegment.size(), 1);
}
BOOST_CHECK(providerSegment.hasEntity("TestEntity"));
diff --git a/source/RobotAPI/libraries/armem/test/ArMemMemoryIDTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemMemoryIDTest.cpp
index aad3359b0d57cfed79d463cc27dcced44550ef00..72f09fb225aa22b50ed55a9482e7f973acbf0fd2 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemMemoryIDTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemMemoryIDTest.cpp
@@ -25,8 +25,8 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include "../core/MemoryID.h"
-#include "../core/error.h"
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/error.h>
#include <iostream>
diff --git a/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp
index 984c1f8e508ec631527f53d5fdff8ea32081ad6e..376f2ec680f92e64a5c72b496543749a303e8cc1 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp
@@ -26,7 +26,8 @@
#include <RobotAPI/Test.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
#include <RobotAPI/libraries/armem/core/diskmemory/Memory.h>
#include <RobotAPI/libraries/armem/core/error.h>
@@ -34,6 +35,7 @@
#include <iostream>
#include <SimoxUtility/meta/type_name.h>
#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
namespace armem = armarx::armem;
@@ -87,11 +89,11 @@ namespace ArMemMemoryTest
using MemoryItem::MemoryItem;
using MemoryItem::operator=;
- std::string getKeyString() const override
+ std::string getKeyString() const
{
return "";
}
- std::string getLevelName() const override
+ std::string getLevelName() const
{
return "";
}
@@ -154,24 +156,29 @@ namespace ArMemMemoryTest
using MemoryContainerBase::MemoryContainerBase;
using MemoryContainerBase::operator=;
- std::string getKeyString() const override
+ std::string getKeyString() const
{
return "";
}
- std::string getLevelName() const override
+ std::string getLevelName() const
{
return "";
}
+
+ void fill()
+ {
+ _container = std::vector<int> { -1, 2, -3 };
+ }
};
struct MemoryContainerCtorOpTestFixture
{
- const armem::MemoryID id {"A/B/C/123/1"};
- const armem::MemoryID moved {"////1"}; // int is not moved
+ const armem::MemoryID id {"M/C/P/E/123/1"};
+ const armem::MemoryID moved {"////123/1"}; // Time and int are just copied, not moved
TestMemoryContainer cont {id};
MemoryContainerCtorOpTestFixture()
{
- cont.container() = std::vector<int> { -1, 2, -3 };
+ cont.fill();
BOOST_CHECK_EQUAL(cont.id(), id);
BOOST_CHECK_EQUAL(cont.size(), 3);
}
@@ -244,17 +251,6 @@ BOOST_AUTO_TEST_CASE(test_key_ctors)
}
-template <class ...Args>
-auto add_element(std::vector<Args...>& vector)
-{
- return vector.emplace_back();
-}
-template <class ...Args>
-auto add_element(std::map<Args...>& map)
-{
- return map.emplace();
-}
-
template <class T>
struct CustomChecks
@@ -370,14 +366,17 @@ struct CustomChecks<armem::d_ltm::Memory>
struct CopyMoveCtorsOpsTestBase
{
- const armem::MemoryID id {"A/B/C/123000"}; // int index would not be moved
- const armem::MemoryID idMoved;
+ const armem::MemoryID id {"M", "C", "P", "E", armem::Time::microSeconds(123000), 1};
+ //const armem::MemoryID idMoved {"", "", "", "", armem::Time::microSeconds(123000), 1};
+ armem::MemoryID idMoved = id;
std::string typeName;
CopyMoveCtorsOpsTestBase(const std::string& typeName) :
typeName(typeName)
{
+ armem::MemoryID copy = std::move(idMoved);
+ (void) copy;
}
virtual ~CopyMoveCtorsOpsTestBase()
{
@@ -459,21 +458,25 @@ struct InstanceCopyMoveCtorsOpsTest : public CopyMoveCtorsOpsTestBase
}
void testMoveCtor() override
{
+ T copy { in };
T out { std::move(in) };
BOOST_CHECK_EQUAL(in.id(), idMoved);
BOOST_CHECK_EQUAL(out.id(), id);
+ CustomChecks<T>::checkEqual(out, copy);
CustomChecks<T>::checkMoved(in);
}
void testMoveOp() override
{
+ T copy { in };
T out;
out = std::move(in);
BOOST_CHECK_EQUAL(in.id(), idMoved);
BOOST_CHECK_EQUAL(out.id(), id);
+ CustomChecks<T>::checkEqual(out, copy);
CustomChecks<T>::checkMoved(in);
}
};
@@ -492,7 +495,19 @@ struct CopyMoveCtorsOpsTest : public CopyMoveCtorsOpsTestBase
void reset() override
{
in = T {id};
- add_element(in.container());
+ if constexpr(std::is_same_v<T, armem::wm::Memory>
+ || std::is_same_v<T, armem::ltm::Memory>
+ || std::is_same_v<T, armem::d_ltm::Memory>)
+ {
+ in._addMissingCoreSegmentDuringUpdate = true;
+ }
+ {
+ armem::EntityUpdate update;
+ update.entityID = armem::MemoryID("M", "C", "P", "E", armem::Time::microSeconds(123000), 0);
+ update.timeCreated = update.entityID.timestamp;
+ update.instancesData.emplace_back();
+ in.update(update);
+ }
BOOST_CHECK_EQUAL(in.id(), id);
BOOST_CHECK_EQUAL(in.size(), 1);
@@ -652,17 +667,17 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
update.timeCreated = armem::Time::milliSeconds(1000);
BOOST_CHECK_NO_THROW(providerSegment.update(update));
- BOOST_CHECK_EQUAL(providerSegment.entities().size(), 1);
+ BOOST_CHECK_EQUAL(providerSegment.size(), 1);
BOOST_CHECK(providerSegment.hasEntity("image"));
BOOST_CHECK(!providerSegment.hasEntity("other_image"));
armem::wm::Entity& entity = providerSegment.getEntity("image");
BOOST_CHECK_EQUAL(entity.name(), "image");
- BOOST_CHECK_EQUAL(entity.history().size(), 1);
- BOOST_CHECK_EQUAL(entity.history().count(update.timeCreated), 1);
+ BOOST_CHECK_EQUAL(entity.size(), 1);
+ BOOST_CHECK(entity.hasSnapshot(update.timeCreated));
- armem::wm::EntitySnapshot& entitySnapshot = entity.history().at(update.timeCreated);
- BOOST_CHECK_EQUAL(entitySnapshot.instances().size(), update.instancesData.size());
+ armem::wm::EntitySnapshot& entitySnapshot = entity.getSnapshot(update.timeCreated);
+ BOOST_CHECK_EQUAL(entitySnapshot.size(), update.instancesData.size());
// Another update (on memory).
@@ -670,16 +685,16 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
update.instancesData = { std::make_shared<aron::datanavigator::DictNavigator>() };
update.timeCreated = armem::Time::milliSeconds(2000);
memory.update(update);
- BOOST_CHECK_EQUAL(entity.history().size(), 2);
- BOOST_CHECK_EQUAL(entity.history().count(update.timeCreated), 1);
- BOOST_CHECK_EQUAL(entity.history().at(update.timeCreated).instances().size(), update.instancesData.size());
+ BOOST_CHECK_EQUAL(entity.size(), 2);
+ BOOST_CHECK(entity.hasSnapshot(update.timeCreated));
+ BOOST_CHECK_EQUAL(entity.getSnapshot(update.timeCreated).size(), update.instancesData.size());
// A third update (on entity).
update.instancesData = { std::make_shared<aron::datanavigator::DictNavigator>() };
update.timeCreated = armem::Time::milliSeconds(3000);
entity.update(update);
- BOOST_CHECK_EQUAL(entity.history().size(), 3);
+ BOOST_CHECK_EQUAL(entity.size(), 3);
}
@@ -687,7 +702,7 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
BOOST_AUTO_TEST_CASE(test_history_size_in_entity)
{
- armem::wm::Entity entity("entity");
+ armem::server::wm::Entity entity("entity");
armem::EntityUpdate update;
update.entityID.entityName = entity.name();
@@ -699,38 +714,38 @@ BOOST_AUTO_TEST_CASE(test_history_size_in_entity)
entity.update(update);
update.timeCreated = armem::Time::milliSeconds(3000);
entity.update(update);
- BOOST_CHECK_EQUAL(entity.history().size(), 3);
+ BOOST_CHECK_EQUAL(entity.size(), 3);
// Now with maximum history size.
entity.setMaxHistorySize(2);
- BOOST_CHECK_EQUAL(entity.history().size(), 2);
- BOOST_CHECK_EQUAL(entity.history().count(armem::Time::milliSeconds(1000)), 0);
- BOOST_CHECK_EQUAL(entity.history().count(armem::Time::milliSeconds(2000)), 1);
- BOOST_CHECK_EQUAL(entity.history().count(armem::Time::milliSeconds(3000)), 1);
+ BOOST_CHECK_EQUAL(entity.size(), 2);
+ BOOST_CHECK(not entity.hasSnapshot(armem::Time::milliSeconds(1000)));
+ BOOST_CHECK(entity.hasSnapshot(armem::Time::milliSeconds(2000)));
+ BOOST_CHECK(entity.hasSnapshot(armem::Time::milliSeconds(3000)));
update.timeCreated = armem::Time::milliSeconds(4000);
entity.update(update);
- BOOST_CHECK_EQUAL(entity.history().size(), 2);
- BOOST_CHECK_EQUAL(entity.history().count(armem::Time::milliSeconds(2000)), 0);
- BOOST_CHECK_EQUAL(entity.history().count(armem::Time::milliSeconds(3000)), 1);
- BOOST_CHECK_EQUAL(entity.history().count(armem::Time::milliSeconds(4000)), 1);
+ BOOST_CHECK_EQUAL(entity.size(), 2);
+ BOOST_CHECK(not entity.hasSnapshot(armem::Time::milliSeconds(2000)));
+ BOOST_CHECK(entity.hasSnapshot(armem::Time::milliSeconds(3000)));
+ BOOST_CHECK(entity.hasSnapshot(armem::Time::milliSeconds(4000)));
// Disable maximum history size.
entity.setMaxHistorySize(-1);
update.timeCreated = armem::Time::milliSeconds(5000);
entity.update(update);
- BOOST_CHECK_EQUAL(entity.history().size(), 3);
- BOOST_CHECK_EQUAL(entity.history().count(armem::Time::milliSeconds(3000)), 1);
- BOOST_CHECK_EQUAL(entity.history().count(armem::Time::milliSeconds(4000)), 1);
- BOOST_CHECK_EQUAL(entity.history().count(armem::Time::milliSeconds(5000)), 1);
+ BOOST_CHECK_EQUAL(entity.size(), 3);
+ BOOST_CHECK(entity.hasSnapshot(armem::Time::milliSeconds(3000)));
+ BOOST_CHECK(entity.hasSnapshot(armem::Time::milliSeconds(4000)));
+ BOOST_CHECK(entity.hasSnapshot(armem::Time::milliSeconds(5000)));
}
BOOST_AUTO_TEST_CASE(test_history_size_in_provider_segment)
{
- armem::wm::ProviderSegment providerSegment("SomeRGBImageProvider");
+ armem::server::wm::ProviderSegment providerSegment("SomeRGBImageProvider");
armem::EntityUpdate update;
update.entityID.providerSegmentName = providerSegment.name();
@@ -753,22 +768,22 @@ BOOST_AUTO_TEST_CASE(test_history_size_in_provider_segment)
update.timeCreated = armem::Time::milliSeconds(4000);
providerSegment.update(update);
- BOOST_CHECK_EQUAL(providerSegment.getEntity("A").history().size(), 3);
- BOOST_CHECK_EQUAL(providerSegment.getEntity("B").history().size(), 4);
+ BOOST_CHECK_EQUAL(providerSegment.getEntity("A").size(), 3);
+ BOOST_CHECK_EQUAL(providerSegment.getEntity("B").size(), 4);
// Employ maximum history size.
providerSegment.setMaxHistorySize(3);
- BOOST_CHECK_EQUAL(providerSegment.getEntity("A").history().size(), 3);
- BOOST_CHECK_EQUAL(providerSegment.getEntity("B").history().size(), 3);
+ BOOST_CHECK_EQUAL(providerSegment.getEntity("A").size(), 3);
+ BOOST_CHECK_EQUAL(providerSegment.getEntity("B").size(), 3);
providerSegment.setMaxHistorySize(2);
- BOOST_CHECK_EQUAL(providerSegment.getEntity("A").history().size(), 2);
- BOOST_CHECK_EQUAL(providerSegment.getEntity("B").history().size(), 2);
+ BOOST_CHECK_EQUAL(providerSegment.getEntity("A").size(), 2);
+ BOOST_CHECK_EQUAL(providerSegment.getEntity("B").size(), 2);
providerSegment.setMaxHistorySize(3);
- BOOST_CHECK_EQUAL(providerSegment.getEntity("A").history().size(), 2);
- BOOST_CHECK_EQUAL(providerSegment.getEntity("B").history().size(), 2);
+ BOOST_CHECK_EQUAL(providerSegment.getEntity("A").size(), 2);
+ BOOST_CHECK_EQUAL(providerSegment.getEntity("B").size(), 2);
// Add new entity.
providerSegment.setMaxHistorySize(2);
@@ -784,7 +799,7 @@ BOOST_AUTO_TEST_CASE(test_history_size_in_provider_segment)
// Check correctly inherited history size.
BOOST_CHECK_EQUAL(providerSegment.getEntity("C").getMaxHistorySize(), 2);
// Check actual history size.
- BOOST_CHECK_EQUAL(providerSegment.getEntity("C").history().size(), 2);
+ BOOST_CHECK_EQUAL(providerSegment.getEntity("C").size(), 2);
// Remove maximum.
providerSegment.setMaxHistorySize(-1);
@@ -795,58 +810,6 @@ BOOST_AUTO_TEST_CASE(test_history_size_in_provider_segment)
update.entityID.entityName = name;
update.timeCreated = armem::Time::milliSeconds(5000);
providerSegment.update(update);
- BOOST_CHECK_EQUAL(providerSegment.getEntity(name).history().size(), 3);
+ BOOST_CHECK_EQUAL(providerSegment.getEntity(name).size(), 3);
}
}
-
-
-
-template <class T>
-struct CopyTest
-{
- CopyTest()
- {
- }
-
- void test()
- {
- // At least check whether they can be called.
- T t;
- T t2 = t.copy();
-
- if constexpr(!std::is_base_of_v<armem::base::EntityInstanceBase<T>, T>)
- {
- t2 = t.copyEmpty();
- }
- }
-};
-
-
-BOOST_AUTO_TEST_CASE(test_copy)
-{
- {
- CopyTest<armem::wm::EntityInstance>().test();
- CopyTest<armem::wm::EntitySnapshot>().test();
- CopyTest<armem::wm::Entity>().test();
- CopyTest<armem::wm::ProviderSegment>().test();
- CopyTest<armem::wm::CoreSegment>().test();
- CopyTest<armem::wm::Memory>().test();
- }
- {
- CopyTest<armem::ltm::EntityInstance>().test();
- CopyTest<armem::ltm::EntitySnapshot>().test();
- CopyTest<armem::ltm::Entity>().test();
- CopyTest<armem::ltm::ProviderSegment>().test();
- CopyTest<armem::ltm::CoreSegment>().test();
- CopyTest<armem::ltm::Memory>().test();
- }
- {
- CopyTest<armem::d_ltm::EntityInstance>().test();
- CopyTest<armem::d_ltm::EntitySnapshot>().test();
- CopyTest<armem::d_ltm::Entity>().test();
- CopyTest<armem::d_ltm::ProviderSegment>().test();
- CopyTest<armem::d_ltm::CoreSegment>().test();
- CopyTest<armem::d_ltm::Memory>().test();
- }
-}
-
diff --git a/source/RobotAPI/libraries/armem/test/ArMemQueryBuilderTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemQueryBuilderTest.cpp
index 535735b09eeb61d41cdf0dbaf658cebcdb08f4db..78d3077c082e41f32bf6dfd398e9e7d474e19dc3 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemQueryBuilderTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemQueryBuilderTest.cpp
@@ -25,9 +25,10 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include "../client/query/Builder.h"
-#include "../client/query/query_fns.h"
-#include "../core/workingmemory/ice_conversions.h"
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
+
#include <iostream>
diff --git a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
index 4b83fd43261e1d97da45495f5da94279931e606f..656842b3dcd23d49a84a51a04fc02143c14cada2 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
@@ -20,28 +20,26 @@
* GNU General Public License
*/
-#include "ArmarXCore/core/exceptions/LocalException.h"
-#include <RobotAPI/interface/armem/query.h>
-#include <boost/test/tools/old/interface.hpp>
#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::armem
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include "../server/query_proc/workingmemory/EntityQueryProcessor.h"
-#include "../error.h"
-
-
-#include <iostream>
+#include <RobotAPI/interface/armem/query.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.h>
+#include <RobotAPI/libraries/armem/server/query_proc/wm.h>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/string.h>
+#include <iostream>
+
namespace armem = armarx::armem;
namespace aron = armarx::aron;
namespace query = armarx::armem::query::data;
-using EntityQueryProcessor = armarx::armem::wm::query_proc::EntityQueryProcessor;
+using EntityQueryProcessor = armarx::armem::server::query_proc::wm::EntityQueryProcessor;
namespace ArMemQueryTest
@@ -55,8 +53,7 @@ namespace ArMemQueryTest
{
armem::wm::Entity entity;
- armem::wm::query_proc::EntityQueryProcessor processor;
- query::EntityQueryPtr entityQuery;
+ armem::server::query_proc::wm::EntityQueryProcessor processor;
std::vector<armem::wm::Entity> results;
@@ -85,10 +82,10 @@ namespace ArMemQueryTest
template <class QueryT>
void addResults(QueryT query = {})
{
- entityQuery = new QueryT(query);
-
+ // Test whether we get the same result when we pass the concrete type
+ // (static resolution) and when we pass the base type (dynamic resolution).
results.emplace_back(processor.process(query, entity));
- results.emplace_back(processor.process(*entityQuery, entity));
+ results.emplace_back(processor.process(static_cast<query::EntityQuery const&>(query), entity));
}
};
@@ -126,8 +123,8 @@ BOOST_AUTO_TEST_CASE(test_entity_Single_latest)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
BOOST_CHECK_EQUAL(result.size(), 1);
- BOOST_CHECK_EQUAL(result.history().begin()->second.time(), entity.getLatestSnapshot().time());
- BOOST_CHECK_NE(&result.history().begin()->second, &entity.history().rbegin()->second);
+ const armem::wm::EntitySnapshot& first = result.getFirstSnapshotAfterOrAt(armem::Time::microSeconds(0));
+ BOOST_CHECK_EQUAL(first.time(), armem::Time::microSeconds(5000));
}
}
@@ -140,8 +137,8 @@ BOOST_AUTO_TEST_CASE(test_entity_Single_existing)
for (const armem::wm::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
- BOOST_CHECK_EQUAL(result.size(), 1);
- BOOST_CHECK_EQUAL(result.history().begin()->second.time(), armem::Time::microSeconds(3000));
+ BOOST_REQUIRE_EQUAL(result.size(), 1);
+ BOOST_CHECK_EQUAL(result.getFirstSnapshot().time(), armem::Time::microSeconds(3000));
}
}
@@ -168,12 +165,13 @@ BOOST_AUTO_TEST_CASE(test_entity_All)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
BOOST_CHECK_EQUAL(result.size(), entity.size());
- auto jt = entity.history().begin();
- for (auto it = result.history().begin(); it != result.history().end(); ++it, ++jt)
+ for (armem::Time time : entity.getTimestamps())
{
- BOOST_CHECK_EQUAL(it->first, jt->first);
- BOOST_CHECK_EQUAL(it->second.time(), jt->second.time());
- BOOST_CHECK_NE(&it->second, &jt->second);
+ BOOST_REQUIRE(result.hasSnapshot(time));
+ const armem::wm::EntitySnapshot& rs = result.getSnapshot(time);
+ const armem::wm::EntitySnapshot& es = entity.getSnapshot(time);
+ BOOST_CHECK_EQUAL(rs.time(), es.time());
+ BOOST_CHECK_NE(&rs, &es);
}
}
}
@@ -191,7 +189,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_slice)
BOOST_CHECK_EQUAL(result.size(), 2);
- std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ std::vector<armem::Time> times = result.getTimestamps();
std::vector<armem::Time> expected
{
armem::Time::microSeconds(2000), armem::Time::microSeconds(3000)
@@ -212,7 +210,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_exact)
BOOST_CHECK_EQUAL(result.size(), 3);
- std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ std::vector<armem::Time> times = result.getTimestamps();
std::vector<armem::Time> expected
{
armem::Time::microSeconds(2000), armem::Time::microSeconds(3000), armem::Time::microSeconds(4000)
@@ -233,8 +231,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_all)
BOOST_CHECK_EQUAL(result.size(), entity.size());
- std::vector<armem::Time> times = simox::alg::get_keys(result.history());
- std::vector<armem::Time> expected = simox::alg::get_keys(entity.history());
+ const std::vector<armem::Time> times = result.getTimestamps();
+ const std::vector<armem::Time> expected = entity.getTimestamps();
BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
}
}
@@ -266,7 +264,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_from_start)
BOOST_CHECK_EQUAL(result.size(), 2);
- std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ const std::vector<armem::Time> times = result.getTimestamps();
std::vector<armem::Time> expected
{
armem::Time::microSeconds(1000), armem::Time::microSeconds(2000)
@@ -284,10 +282,9 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_to_end)
for (const armem::wm::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
-
BOOST_CHECK_EQUAL(result.size(), 3);
- std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ const std::vector<armem::Time> times = result.getTimestamps();
std::vector<armem::Time> expected
{
armem::Time::microSeconds(3000), armem::Time::microSeconds(4000), armem::Time::microSeconds(5000)
@@ -307,12 +304,11 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_1)
for (const auto& result : results)
{
- std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ const std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE_EQUAL(times.size(), 1);
- BOOST_REQUIRE_EQUAL(times.front(), armem::Time::microSeconds(3000));
+ BOOST_CHECK_EQUAL(times.front(), armem::Time::microSeconds(3000));
}
-
}
BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_2)
@@ -323,7 +319,7 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_2)
for (const auto& result : results)
{
- std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ const std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE_EQUAL(times.size(), 2);
std::vector<armem::Time> expected
@@ -348,7 +344,7 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_before)
for (const auto& result : results)
{
- std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE_EQUAL(times.size(), 1);
BOOST_REQUIRE_EQUAL(times.front(), armem::Time::microSeconds(3000));
@@ -363,7 +359,7 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_at)
for (const auto& result : results)
{
- std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE_EQUAL(times.size(), 1);
BOOST_REQUIRE_EQUAL(times.front(), armem::Time::microSeconds(3000));
@@ -378,7 +374,7 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_lookup_past)
for (const auto& result : results)
{
- std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE(times.empty());
}
}
@@ -398,7 +394,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_no_limit)
for (const auto& result : results)
{
- std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE_EQUAL(times.size(), 2);
std::vector<armem::Time> expected
@@ -424,7 +420,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_600)
for (const auto& result : results)
{
- std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE_EQUAL(times.size(), 2);
std::vector<armem::Time> expected
@@ -450,7 +446,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_too_small)
for (const auto& result : results)
{
- std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE_EQUAL(times.size(), 0);
}
@@ -469,7 +465,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_next)
for (const auto& result : results)
{
- std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE_EQUAL(times.size(), 1);
std::vector<armem::Time> expected
@@ -494,7 +490,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_previous)
for (const auto& result : results)
{
- std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE_EQUAL(times.size(), 1);
std::vector<armem::Time> expected
@@ -519,7 +515,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_perfect_match)
for (const auto& result : results)
{
- std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE_EQUAL(times.size(), 1);
std::vector<armem::Time> expected
@@ -544,7 +540,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_past)
for (const auto& result : results)
{
- std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE(times.empty());
}
}
@@ -562,7 +558,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future)
for (const auto& result : results)
{
- std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE(times.empty());
}
}
@@ -580,7 +576,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future_valid)
for (const auto& result : results)
{
- std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE_EQUAL(times.size(), 1);
std::vector<armem::Time> expected
@@ -601,33 +597,33 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_invalid_timestamp)
BOOST_AUTO_TEST_CASE(test_negative_index_semantics)
{
- BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(0, 0), 0);
- BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(0, 1), 0);
- BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(0, 10), 0);
+ BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(0, 0), 0);
+ BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(0, 1), 0);
+ BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(0, 10), 0);
- BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(1, 1), 0);
- BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(5, 1), 0);
+ BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(1, 1), 0);
+ BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(5, 1), 0);
- BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(1, 5), 1);
- BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(5, 5), 4);
- BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(5, 6), 5);
- BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(5, 10), 5);
+ BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(1, 5), 1);
+ BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(5, 5), 4);
+ BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(5, 6), 5);
+ BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(5, 10), 5);
- BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-1, 0), 0);
- BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-5, 0), 0);
+ BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(-1, 0), 0);
+ BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(-5, 0), 0);
- BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-1, 1), 0);
- BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-5, 1), 0);
+ BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(-1, 1), 0);
+ BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(-5, 1), 0);
- BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-1, 2), 1);
- BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-5, 2), 0);
+ BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(-1, 2), 1);
+ BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(-5, 2), 0);
- BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-1, 5), 4);
- BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-5, 5), 0);
- BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-5, 6), 1);
- BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-5, 10), 5);
- BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-10, 10), 0);
- BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-20, 10), 0);
+ BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(-1, 5), 4);
+ BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(-5, 5), 0);
+ BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(-5, 6), 1);
+ BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(-5, 10), 5);
+ BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(-10, 10), 0);
+ BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(-20, 10), 0);
}
@@ -643,8 +639,8 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_all_default)
BOOST_CHECK_EQUAL(result.size(), entity.size());
- std::vector<armem::Time> times = simox::alg::get_keys(result.history());
- std::vector<armem::Time> expected = simox::alg::get_keys(entity.history());
+ const std::vector<armem::Time> times = result.getTimestamps();
+ const std::vector<armem::Time> expected = entity.getTimestamps();
BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
}
@@ -666,7 +662,7 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_slice)
BOOST_CHECK_EQUAL(result.size(), 3);
- std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ std::vector<armem::Time> times = result.getTimestamps();
std::vector<armem::Time> expected
{
armem::Time::microSeconds(2000), armem::Time::microSeconds(3000), armem::Time::microSeconds(4000)
diff --git a/source/RobotAPI/libraries/armem/test/CMakeLists.txt b/source/RobotAPI/libraries/armem/test/CMakeLists.txt
index e7fa876c119596f5c49352cfd0dca757ab01019c..b10935f95fedd6a1309248007422c53783d2c9e4 100644
--- a/source/RobotAPI/libraries/armem/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/test/CMakeLists.txt
@@ -8,3 +8,4 @@ armarx_add_test(ArMemMemoryIDTest ArMemMemoryIDTest.cpp "${LIBS}")
armarx_add_test(ArMemQueryTest ArMemQueryTest.cpp "${LIBS}")
armarx_add_test(ArMemLTMTest ArMemLTMTest.cpp "${LIBS}")
armarx_add_test(ArMemQueryBuilderTest ArMemQueryBuilderTest.cpp "${LIBS}")
+armarx_add_test(ArMemForEachTest ArMemForEachTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/armem/util/util.h b/source/RobotAPI/libraries/armem/util/util.h
index 35fc1538b78df0b817e7ae0676be68e9675d1f0f..3045d583a6a4144ed996a286257eb5ba2910b9c5 100644
--- a/source/RobotAPI/libraries/armem/util/util.h
+++ b/source/RobotAPI/libraries/armem/util/util.h
@@ -24,12 +24,12 @@
#include <vector>
#include <optional>
-#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h>
+
namespace armarx::armem
{
@@ -52,10 +52,13 @@ namespace armarx::armem
return std::nullopt;
}
+#if 0
+ // item.data() is by definition a DictNavigator
if (item.data()->getDescriptor() != aron::data::Descriptor::eDict)
{
return std::nullopt;
}
+#endif
try
{
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.cpp b/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.cpp
index 5016db50bf5da8dc10d0fac5b5c004b9178ddc40..38e4d8c88719541e7c4fca92964cfb069c22ba5e 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.cpp
@@ -4,6 +4,7 @@
#include <QLineEdit>
#include <QTimer>
#include <QHBoxLayout>
+#include <QFileDialog>
#include <cmath>
@@ -16,32 +17,53 @@ namespace armarx::armem::gui
setSizePolicy(QSizePolicy::Policy::Minimum, QSizePolicy::Policy::Fixed);
auto vlayout = new QVBoxLayout();
- auto hlayout = new QHBoxLayout();
+ auto hlayout1 = new QHBoxLayout();
+ auto hlayout2 = new QHBoxLayout();
+
+ hlayout1->setContentsMargins(10, 2, 10, 2);
+ hlayout2->setContentsMargins(10, 2, 10, 2);
const int margin = 0;
vlayout->setContentsMargins(margin, margin, margin, margin);
_lineEdit = new QLineEdit("/tmp/MemoryExport", this);
_lineEdit->setSizePolicy(QSizePolicy::Ignored, QSizePolicy::Preferred);
- _lineEdit->setMinimumWidth(400);
+ _lineEdit->setMinimumWidth(150);
+
+ _openFileBrowserButton = new QPushButton("Search files", this);
+
+ _storeOnDiskButton = new QPushButton("Store query (Disk)", this);
+ _storeInLTMButton = new QPushButton("Store query (LTM)", this);
- _storeOnDiskButton = new QPushButton("Store current result on local Disk", this);
+ hlayout1->addWidget(_lineEdit);
+ hlayout1->addWidget(_openFileBrowserButton);
- _storeInLTMButton = new QPushButton("Store current result in LTM", this);
+ hlayout2->addWidget(_storeOnDiskButton);
+ hlayout2->addWidget(_storeInLTMButton);
- vlayout->addWidget(_lineEdit);
- hlayout->addWidget(_storeOnDiskButton);
- hlayout->addWidget(_storeInLTMButton);
- vlayout->addItem(hlayout);
+ vlayout->addItem(hlayout1);
+ vlayout->addItem(hlayout2);
this->setLayout(vlayout);
// Private connections.
// Public connections.
+ connect(_openFileBrowserButton, &QPushButton::pressed, this, &This::openFileBrowser);
connect(_storeInLTMButton, &QPushButton::pressed, this, &This::storeInLTM);
connect(_storeOnDiskButton, &QPushButton::pressed, this, &This::storeOnDisk);
}
+ void MemoryControlWidget::openFileBrowser()
+ {
+ QFileDialog dialog;
+ dialog.setFileMode(QFileDialog::DirectoryOnly);
+ //dialog.setOption(QFileDialog::DontUseNativeDialog, true);
+ dialog.setOption(QFileDialog::ShowDirsOnly, false);
+ dialog.exec();
+ QString open = dialog.directory().path();
+ _lineEdit->setText(open);
+ }
+
QLineEdit* MemoryControlWidget::pathInputBox()
{
return _lineEdit;
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.h b/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.h
index 19831694acc7b9251c7d372b8381f755df1bc292..c6b156db313b01180a7f5884959ffc21b0a73f9d 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.h
@@ -4,6 +4,7 @@
class QPushButton;
class QLineEdit;
+class QFileDialog;
namespace armarx::armem::gui
{
@@ -20,11 +21,15 @@ namespace armarx::armem::gui
QLineEdit* pathInputBox();
QString getEnteredPath();
+ QPushButton* openFileBrowserButton();
+
QPushButton* storeInLTMButton();
QPushButton* storeOnDiskButton();
public slots:
+ void openFileBrowser();
+
signals:
void storeInLTM();
@@ -40,6 +45,8 @@ namespace armarx::armem::gui
QLineEdit* _lineEdit;
+ QPushButton* _openFileBrowserButton;
+
QPushButton* _storeInLTMButton;
QPushButton* _storeOnDiskButton;
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
index 5037bad396c0e7a83b7704b4895a25225ae8106f..802de477cdd143051b6e45f8f2c4d995ebfa4e29 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
@@ -1,12 +1,12 @@
#include "MemoryViewer.h"
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem_gui/gui_utils.h>
#include <RobotAPI/libraries/armem/core/diskmemory/Memory.h>
-#include <RobotAPI/libraries/armem/server/query_proc/diskmemory/MemoryQueryProcessor.h>
-#include <RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h>
+#include <RobotAPI/libraries/armem/server/query_proc/diskmemory.h>
+#include <RobotAPI/libraries/armem/server/query_proc/wm.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
@@ -26,6 +26,8 @@
#include <QSettings>
#include <filesystem>
+#include <iomanip>
+
namespace armarx::armem::gui
{
@@ -216,108 +218,82 @@ namespace armarx::armem::gui
void MemoryViewer::updateMemories()
{
- memoryReaders.clear();
- memoryData.clear();
+ int errorCount = 0;
+ memoryData.clear();
memoryReaders = mns.getAllReaders(true);
- bool dataChanged = false;
-
- QString qs = memoryControlWidget->getEnteredPath();
- std::string utf8_text = qs.toUtf8().constData();
+ std::filesystem::path path(memoryControlWidget->getEnteredPath().toStdString());
- std::filesystem::path p(utf8_text);
+ armem::client::QueryInput input = memoryGroup->queryWidget()->queryInput();
// first check if the local file system should be queried
if (memoryGroup->queryWidget()->alsoQueryLocalDisk())
{
- if (std::filesystem::is_directory(p))
+ if (std::filesystem::is_directory(path))
{
- for (const auto& d : std::filesystem::directory_iterator(p))
+ for (const auto& directory : std::filesystem::directory_iterator(path))
{
- if (d.is_directory())
+ if (directory.is_directory())
{
- std::string k = d.path().filename();
+ std::string k = directory.path().filename();
armem::d_ltm::Memory dMem(k);
- dMem.reload(p / k);
+ dMem.reload(path / k);
- armem::client::QueryInput input = memoryGroup->queryWidget()->queryInput();
- input.addQueryTargetToAll(armem::query::data::QueryTarget::Disk);
+ input.addQueryTargetToAll(armem::query::data::QueryTarget::LTM); // We use LTM as query target for the disk
- armem::d_ltm::query_proc::MemoryQueryProcessor d_ltm_processor;
- dMem = d_ltm_processor.process(input.toIce(), dMem, /* execute if: */ { query::data::QueryTarget::Disk });
+ armem::server::query_proc::d_ltm::MemoryQueryProcessor d_ltm_processor;
+ dMem = d_ltm_processor.process(input.toIce(), dMem);
wm::Memory converted = dMem.convert();
memoryData[k] = std::move(converted);
- dataChanged = true;
}
}
}
else
{
- ARMARX_WARNING << "Could not import a memory from '" << utf8_text << "'. Skipping import.";
+ ARMARX_WARNING << "Could not import a memory from '" << path << "'. Skipping import.";
}
}
-
- armem::client::QueryInput input = memoryGroup->queryWidget()->queryInput();
- for (auto& [name, reader] : memoryReaders)
+ else
{
- TIMING_START(MemoryQuery);
+ for (auto& [name, reader] : memoryReaders)
{
- armem::client::QueryResult result = reader.query(input);
- if (result)
+ TIMING_START(MemoryQuery);
{
- if (result.memory.hasData())
+ armem::client::QueryResult result = reader.query(input);
+ if (result.success)
{
- if (const auto& it = memoryData.find(name); it != memoryData.end())
- {
- result.memory.append(it->second);
-
- // requery (e.g. to get only the last n values instead of the last n from disk and the last n from wm)
- armem::wm::query_proc::MemoryQueryProcessor wm_processor;
- result.memory = wm_processor.process(input.toIce(), result.memory, /* execute if: */ { query::data::QueryTarget::WM });
-
- if (!result.memory.hasData())
- {
- ARMARX_ERROR << "A memory which had data before lost data. This indicates that there is something wrong.";
- }
- }
-
- dataChanged = true;
- memoryData[name] = std::move(result.memory);
+ memoryData[name] = result.memory;
}
else
{
- ARMARX_INFO << "The memory " << name << " has no data after querying.";
+ ARMARX_INFO << "A query for memory '" << name << "' produced an error: " << result.errorMessage;
+ errorCount++;
}
}
- else
+ TIMING_END_STREAM(MemoryQuery, ARMARX_VERBOSE);
+
+ if (debugObserver)
{
- ARMARX_WARNING << "A query for memory '" + name + "' produced an error: " << result.errorMessage;
- if (statusLabel)
- {
- statusLabel->setText(QString::fromStdString(result.errorMessage));
- }
+ debugObserver->setDebugDatafield(Logging::tag.tagName, "Memory Query [ms]", new Variant(MemoryQuery.toMilliSecondsDouble()));
}
}
- TIMING_END_STREAM(MemoryQuery, ARMARX_VERBOSE);
-
- if (debugObserver)
- {
- debugObserver->setDebugDatafield(Logging::tag.tagName, "Memory Query [ms]", new Variant(MemoryQuery.toMilliSecondsDouble()));
- }
}
- if (dataChanged)
- {
- emit memoryDataChanged();
- }
- else
+ emit memoryDataChanged();
+
+ // Code to output status label information
+ if (statusLabel and errorCount > 0)
{
- if (statusLabel)
- {
- statusLabel->setText("No query result.");
- }
+ auto now = std::chrono::system_clock::now();
+ auto in_time_t = std::chrono::system_clock::to_time_t(now);
+
+ std::stringstream ss;
+ ss << "Last update: " << std::put_time(std::localtime(&in_time_t), "%Y-%m-%d %X");
+ ss << "\nThe query produced " << errorCount << " errors! Please check log.";
+
+ statusLabel->setText(QString::fromStdString(ss.str()));
}
}
@@ -444,17 +420,10 @@ namespace armarx::armem::gui
std::map<std::string, const armem::wm::Memory*> convMap;
for (auto& [name, data] : memoryData)
{
- /*if (data.has_value())
- {
- convMap[name] = &data.value();
- }*/
convMap[name] = &data;
}
- if (convMap.empty())
- {
- return;
- }
+ // if convMap.empty(), we still need to update to remove existing entries.
TIMING_START(GuiUpdate);
memoryGroup->tree()->update(convMap);
diff --git a/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.cpp b/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.cpp
index a62659cd78d4448bb72913ae432fc22f8f2b6eae..dd51a5e7f055f387e7580196874128c1f4bf2b12 100644
--- a/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.cpp
@@ -28,7 +28,7 @@ namespace armarx::armem::gui
_frequencySpinBox->setValue(frequency);
_frequencySpinBox->setMinimum(0);
_frequencySpinBox->setMaximum(maxFrequency);
- _frequencySpinBox->setSingleStep(1.0);
+ _frequencySpinBox->setSingleStep(0.5);
_frequencySpinBox->setSuffix(" Hz");
diff --git a/source/RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h b/source/RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h
index 37de7699ce2c8d9245ced9e26fa8625d9b62159e..f3ca45cf17b05c72936a5afb8958e4eb5686aaab 100644
--- a/source/RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h
+++ b/source/RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h
@@ -16,10 +16,10 @@ namespace armarx
* A class to efficiently build and maintain sorted items of `QTreeWidget`
* or `QTreeWidgetItem` based on a sorted container matching the intended structure.
*/
- template <class ContainerT>
+ template <class _ElementT>
struct TreeWidgetBuilder
{
- using ElementT = typename ContainerT::value_type;
+ using ElementT = _ElementT;
/// Return < 0 if `element < item`, 0 if `element == item`, and > 0 if `element > item`.
using CompareFn = std::function<int(const ElementT& element, QTreeWidgetItem* item)>;
@@ -30,14 +30,15 @@ namespace armarx
TreeWidgetBuilder() = default;
/// Constructor to automatically derive the template argument.
- TreeWidgetBuilder(const ContainerT&)
+ TreeWidgetBuilder(const ElementT&)
{}
TreeWidgetBuilder(CompareFn compareFn, MakeItemFn makeItemFn, UpdateItemFn updateItemFn = NoUpdate) :
compareFn(compareFn), makeItemFn(makeItemFn), updateItemFn(updateItemFn)
{}
- TreeWidgetBuilder(NameFn nameFn, MakeItemFn makeItemFn, UpdateItemFn updateItemFn = NoUpdate) :
- compareFn(MakeCompareNameFn(nameFn)), makeItemFn(makeItemFn), updateItemFn(updateItemFn)
+ TreeWidgetBuilder(NameFn nameFn, MakeItemFn makeItemFn, UpdateItemFn updateItemFn = NoUpdate,
+ int nameColumn = 0) :
+ compareFn(MakeCompareNameFn(nameFn, nameColumn)), makeItemFn(makeItemFn), updateItemFn(updateItemFn)
{}
@@ -45,9 +46,9 @@ namespace armarx
{
this->compareFn = compareFn;
}
- void setNameFn(NameFn nameFn)
+ void setNameFn(NameFn nameFn, int nameColumn = 0)
{
- compareFn = MakeCompareNameFn(nameFn);
+ compareFn = MakeCompareNameFn(nameFn, nameColumn);
}
void setMakeItemFn(MakeItemFn makeItemFn)
{
@@ -64,8 +65,31 @@ namespace armarx
}
- template <class ParentT>
- void updateTree(ParentT* parent, const ContainerT& elements);
+ /**
+ * @brief Update the tree according to the elements iterated over by `iteratorFn`.
+ *
+ * @param parent
+ * The parent, i.e. the tree widget or a tree widget item.
+ * @param iteratorFn
+ * Function taking a function `bool fn(const ElementT& element)`
+ * and calling it for each element in the underlying container, i.e. like:
+ * void iteratorFn(bool (*elementFn)(const ElementT& element));
+ */
+ template <class ParentT, class IteratorFn>
+ void updateTreeWithIterator(ParentT* parent, IteratorFn&& iteratorFn);
+
+ /**
+ * @brief Update the tree with the iterable container.
+ *
+ * @param parent
+ * The parent, i.e. the tree widget or a tree widget item.
+ * @param iteratorFn
+ * Function taking a function `bool fn(const ElementT& element)`
+ * and calling it for each element in the underlying container, i.e. like:
+ * void iteratorFn(bool (*elementFn)(const ElementT& element));
+ */
+ template <class ParentT, class ContainerT>
+ void updateTreeWithContainer(ParentT* parent, const ContainerT& elements);
/// No update function (default).
@@ -76,7 +100,7 @@ namespace armarx
}
/// Uses the name for comparison.
- static CompareFn MakeCompareNameFn(NameFn nameFn);
+ static CompareFn MakeCompareNameFn(NameFn nameFn, int nameColumn);
private:
@@ -100,7 +124,7 @@ namespace armarx
struct MapTreeWidgetBuilder
{
using MapT = std::map<KeyT, ValueT>;
- using Base = TreeWidgetBuilder<MapT>;
+ using Base = TreeWidgetBuilder<typename MapT::value_type>;
using ElementT = typename Base::ElementT;
using CompareFn = std::function<int(const ElementT& element, QTreeWidgetItem* item)>;
@@ -170,7 +194,7 @@ namespace armarx
template <class ParentT>
void updateTree(ParentT* tree, const MapT& elements)
{
- builder.updateTree(tree, elements);
+ builder.updateTreeWithContainer(tree, elements);
}
@@ -200,7 +224,7 @@ namespace armarx
private:
- TreeWidgetBuilder<MapT> builder;
+ TreeWidgetBuilder<typename MapT::value_type> builder;
};
@@ -273,30 +297,52 @@ namespace armarx
}
- template <class ContainerT>
- auto TreeWidgetBuilder<ContainerT>::MakeCompareNameFn(NameFn nameFn) -> CompareFn
+ template <class ElementT>
+ auto TreeWidgetBuilder<ElementT>::MakeCompareNameFn(NameFn nameFn, int nameColumn) -> CompareFn
{
- return [nameFn](const ElementT & element, QTreeWidgetItem * item)
+ return [nameFn, nameColumn](const ElementT & element, QTreeWidgetItem * item)
{
- return detail::compare(nameFn(element), item->text(0).toStdString());
+ return detail::compare(nameFn(element), item->text(nameColumn).toStdString());
};
}
- template <class ContainerT>
- template <typename ParentT>
- void TreeWidgetBuilder<ContainerT>::updateTree(ParentT* parent, const ContainerT& elements)
+ template <class ElementT>
+ template <class ParentT, class ContainerT>
+ void TreeWidgetBuilder<ElementT>::updateTreeWithContainer(ParentT* parent, const ContainerT& elements)
+ {
+ this->updateTreeWithIterator(parent, [&elements](auto&& elementFn)
+ {
+ for (const auto& element : elements)
+ {
+ if (not elementFn(element))
+ {
+ break;
+ }
+ }
+ });
+ }
+
+
+ template <class ElementT>
+ template <class ParentT, class IteratorFn>
+ void TreeWidgetBuilder<ElementT>::updateTreeWithIterator(ParentT* parent, IteratorFn&& iteratorFn)
{
using api = detail::ParentAPI<ParentT>;
+ ARMARX_CHECK_NOT_NULL(makeItemFn) << "makeItemFn must be set";
+ ARMARX_CHECK_NOT_NULL(updateItemFn) << "updateItemFn must be set";
+ ARMARX_CHECK_NOT_NULL(compareFn) << "compareFn must be set";
+
int currentIndex = 0;
- for (const auto& element : elements)
+ iteratorFn([this, &parent, ¤tIndex](const auto & element)
{
bool inserted = false;
QTreeWidgetItem* item = nullptr;
if (currentIndex >= api::getItemCount(parent))
{
// Add elements to the end of the list.
+ ARMARX_CHECK_NOT_NULL(makeItemFn);
item = makeItemFn(element);
api::insertItem(parent, api::getItemCount(parent), item);
++currentIndex;
@@ -331,11 +377,8 @@ namespace armarx
{
item->setExpanded(true);
}
- if (!cont)
- {
- break;
- }
- }
+ return cont;
+ });
// Remove superfluous items. (currentIndex must point behind the last item)
while (api::getItemCount(parent) > currentIndex)
{
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
index 26f9d3be5fccfdce4fdb17a9ddbe430c2f334433..c331f2b450990614733819a7df47e61e8e20fb60 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
@@ -7,7 +7,7 @@
#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
class QGroupBox;
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceViewList.h b/source/RobotAPI/libraries/armem_gui/instance/InstanceViewList.h
index 5f937f419cd3111daf1146a6640e0ea9348f6d42..8dc24e0dcd9759f3ab89e21e33ff600ad253f082 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceViewList.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceViewList.h
@@ -6,7 +6,7 @@
#include <RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
class QGroupBox;
diff --git a/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.cpp b/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.cpp
index b8f568c79cdd1d84fa803df9ffaee737e24c9ea2..ebb4473bb1941a713526d646b37744f5a3314c1c 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.cpp
@@ -33,7 +33,11 @@ namespace armarx::armem::gui::instance
// Timestamp in human-readable format
if (id.hasTimestamp())
{
- child(4)->setText(valueColumn, QString::fromStdString(toDateTimeMilliSeconds(id.timestamp)));
+ static const char* mu = "\u03BC";
+ std::stringstream ss;
+ ss << toDateTimeMilliSeconds(id.timestamp)
+ << " (" << id.timestamp.toMicroSeconds() << " " << mu << "s)";
+ child(4)->setText(valueColumn, QString::fromStdString(ss.str()));
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
index a2811c0bc5a01ef29fbb9cc41d7b5f47118f5a9a..6793fd79513dd7b2ac7a287f4d9934a97e2d7c91 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
@@ -22,7 +22,7 @@ namespace armarx::armem::gui::instance
return true;
});
- builder.updateTree(parent, data->getAllKeys());
+ builder.updateTreeWithContainer(parent, data->getAllKeys());
}
void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, const aron::datanavigator::ListNavigatorPtr& data)
@@ -36,7 +36,7 @@ namespace armarx::armem::gui::instance
return true;
});
- builder.updateTree(parent, getIndex(children.size()));
+ builder.updateTreeWithContainer(parent, getIndex(children.size()));
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.h
index b26a422ea71c90c2b769217a274eac7887eb8f9d..b225a093d4664d60313b882c1b3de750b48f96e8 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.h
@@ -28,8 +28,8 @@ namespace armarx::armem::gui::instance
protected:
- using DictBuilder = armarx::TreeWidgetBuilder<std::vector<std::string>>;
- using ListBuilder = armarx::TreeWidgetBuilder<std::vector<size_t>>;
+ using DictBuilder = armarx::TreeWidgetBuilder<std::string>;
+ using ListBuilder = armarx::TreeWidgetBuilder<size_t>;
DictBuilder getDictBuilder() const;
ListBuilder getListBuilder() const;
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
index 56d204abf42573540c363c0973f4bf10db357317..4673f22f68c40dc54a8048291db623c79a82442a 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
@@ -40,7 +40,7 @@ namespace armarx::armem::gui::instance
return true;
});
- builder.updateTree(parent, data.getAllKeys());
+ builder.updateTreeWithContainer(parent, data.getAllKeys());
}
}
@@ -76,7 +76,7 @@ namespace armarx::armem::gui::instance
return true;
});
- builder.updateTree(parent, type.getAllKeys());
+ builder.updateTreeWithContainer(parent, type.getAllKeys());
}
@@ -100,7 +100,7 @@ namespace armarx::armem::gui::instance
return true;
});
- builder.updateTree(parent, getIndex(children.size()));
+ builder.updateTreeWithContainer(parent, getIndex(children.size()));
}
}
@@ -126,7 +126,7 @@ namespace armarx::armem::gui::instance
return true;
});
- builder.updateTree(parent, getIndex(data.childrenSize()));
+ builder.updateTreeWithContainer(parent, getIndex(data.childrenSize()));
}
@@ -150,7 +150,7 @@ namespace armarx::armem::gui::instance
return true;
});
- builder.updateTree(parent, getIndex(type.getAcceptedTypes().size()));
+ builder.updateTreeWithContainer(parent, getIndex(type.getAcceptedTypes().size()));
}
diff --git a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
index 1d4a2b908eba38fbb4be2d028030598c220aae44..29c137093ac9c75f0c904c6745c72473c205f1ec 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
@@ -66,13 +66,18 @@ namespace armarx::armem::gui::memory
return true;
});
+ auto nameFn = [](const auto & element)
+ {
+ return element.name();
+ };
workingmemoryCoreSegmentBuilder.setExpand(true);
- workingmemoryCoreSegmentBuilder.setMakeItemFn([this](const std::string & name, const wm::CoreSegment & coreSeg)
+ workingmemoryCoreSegmentBuilder.setNameFn(nameFn, int(Columns::KEY));
+ workingmemoryCoreSegmentBuilder.setMakeItemFn([this](const wm::CoreSegment & coreSeg)
{
- return makeItem(name, coreSeg);
+ return makeItem(coreSeg.name(), coreSeg);
});
- workingmemoryCoreSegmentBuilder.setUpdateItemFn([this](const std::string&, const wm::CoreSegment & coreSeg, QTreeWidgetItem * coreSegItem)
+ workingmemoryCoreSegmentBuilder.setUpdateItemFn([this](const wm::CoreSegment & coreSeg, QTreeWidgetItem * coreSegItem)
{
updateContainerItem(coreSeg, coreSegItem);
updateChildren(coreSeg, coreSegItem);
@@ -80,11 +85,12 @@ namespace armarx::armem::gui::memory
});
workingmemoryProvSegmentBuilder.setExpand(true);
- workingmemoryProvSegmentBuilder.setMakeItemFn([this](const std::string & name, const wm::ProviderSegment & provSeg)
+ workingmemoryProvSegmentBuilder.setNameFn(nameFn, int(Columns::KEY));
+ workingmemoryProvSegmentBuilder.setMakeItemFn([this](const wm::ProviderSegment & provSeg)
{
- return makeItem(name, provSeg);
+ return makeItem(provSeg.name(), provSeg);
});
- workingmemoryProvSegmentBuilder.setUpdateItemFn([this](const std::string&, const wm::ProviderSegment & provSeg, QTreeWidgetItem * provSegItem)
+ workingmemoryProvSegmentBuilder.setUpdateItemFn([this](const wm::ProviderSegment & provSeg, QTreeWidgetItem * provSegItem)
{
updateContainerItem(provSeg, provSegItem);
updateChildren(provSeg, provSegItem);
@@ -92,30 +98,30 @@ namespace armarx::armem::gui::memory
});
// entityBuilder.setExpand(true);
- workingmemoryEntityBuilder.setMakeItemFn([this](const std::string & name, const wm::Entity & entity)
+ workingmemoryEntityBuilder.setNameFn(nameFn, int(Columns::KEY));
+ workingmemoryEntityBuilder.setMakeItemFn([this](const wm::Entity & entity)
{
- return makeItem(name, entity);
+ return makeItem(entity.name(), entity);
});
- workingmemoryEntityBuilder.setUpdateItemFn([this](const std::string&, const wm::Entity & entity, QTreeWidgetItem * entityItem)
+ workingmemoryEntityBuilder.setUpdateItemFn([this](const wm::Entity & entity, QTreeWidgetItem * entityItem)
{
updateContainerItem(entity, entityItem);
updateChildren(entity, entityItem);
return true;
});
- workingmemorySnapshotBuilder.setMakeItemFn([this](const armem::Time & time, const wm::EntitySnapshot & snapshot)
+ workingmemorySnapshotBuilder.setMakeItemFn([this](const wm::EntitySnapshot & snapshot)
{
- QTreeWidgetItem* item = makeItem(toDateTimeMilliSeconds(time), snapshot);
+ QTreeWidgetItem* item = makeItem(toDateTimeMilliSeconds(snapshot.time()), snapshot);
item->setData(int(Columns::KEY), Qt::ItemDataRole::UserRole, QVariant(static_cast<qlonglong>(snapshot.time().toMicroSeconds())));
return item;
});
- workingmemorySnapshotBuilder.setCompareFn([](const auto & pair, QTreeWidgetItem * item)
+ workingmemorySnapshotBuilder.setCompareFn([](const wm::EntitySnapshot & snapshot, QTreeWidgetItem * item)
{
- const wm::EntitySnapshot& snapshot = pair.second;
return armarx::detail::compare(static_cast<qlonglong>(snapshot.time().toMicroSeconds()),
item->data(int(Columns::KEY), Qt::ItemDataRole::UserRole).toLongLong());
});
- workingmemorySnapshotBuilder.setUpdateItemFn([this](const armem::Time&, const wm::EntitySnapshot & snapshot, QTreeWidgetItem * snapshotItem)
+ workingmemorySnapshotBuilder.setUpdateItemFn([this](const wm::EntitySnapshot & snapshot, QTreeWidgetItem * snapshotItem)
{
updateContainerItem(snapshot, snapshotItem);
updateChildren(snapshot, snapshotItem);
@@ -183,27 +189,27 @@ namespace armarx::armem::gui::memory
_selectedID = id;
const std::string levelName = item->data(int(Columns::LEVEL), Qt::UserRole).toString().toStdString();
- if (levelName == wm::Memory().getLevelName())
+ if (levelName == wm::Memory::getLevelName())
{
emit memorySelected(*_selectedID);
}
- else if (levelName == wm::CoreSegment().getLevelName())
+ else if (levelName == wm::CoreSegment::getLevelName())
{
emit coreSegmentSelected(*_selectedID);
}
- else if (levelName == wm::ProviderSegment().getLevelName())
+ else if (levelName == wm::ProviderSegment::getLevelName())
{
emit providerSegmentSelected(*_selectedID);
}
- else if (levelName == wm::Entity().getLevelName())
+ else if (levelName == wm::Entity::getLevelName())
{
emit entitySelected(*_selectedID);
}
- else if (levelName == wm::EntitySnapshot().getLevelName())
+ else if (levelName == wm::EntitySnapshot::getLevelName())
{
emit snapshotSelected(*_selectedID);
}
- else if (levelName == wm::EntityInstance().getLevelName())
+ else if (levelName == wm::EntityInstance::getLevelName())
{
emit instanceSelected(*_selectedID);
}
@@ -213,6 +219,16 @@ namespace armarx::armem::gui::memory
emit selectedItemChanged(*_selectedID);
}
+ template <class ContainerT>
+ static
+ auto makeIteratorFn(const ContainerT& container)
+ {
+ return [&container](auto&& elementFn)
+ {
+ container.forEachChild(elementFn);
+ };
+ }
+
void TreeWidget::updateChildren(const armem::wm::Memory& memory, QTreeWidget* tree)
{
@@ -227,27 +243,27 @@ namespace armarx::armem::gui::memory
void TreeWidget::updateChildren(const armem::wm::Memory& memory, QTreeWidgetItem* memoryItem)
{
- workingmemoryCoreSegmentBuilder.updateTree(memoryItem, memory.coreSegments());
+ workingmemoryCoreSegmentBuilder.updateTreeWithIterator(memoryItem, makeIteratorFn(memory));
}
void TreeWidget::updateChildren(const armem::wm::CoreSegment& coreSeg, QTreeWidgetItem* coreSegItem)
{
- workingmemoryProvSegmentBuilder.updateTree(coreSegItem, coreSeg.providerSegments());
+ workingmemoryProvSegmentBuilder.updateTreeWithIterator(coreSegItem, makeIteratorFn(coreSeg));
}
void TreeWidget::updateChildren(const armem::wm::ProviderSegment& provSeg, QTreeWidgetItem* provSegItem)
{
- workingmemoryEntityBuilder.updateTree(provSegItem, provSeg.entities());
+ workingmemoryEntityBuilder.updateTreeWithIterator(provSegItem, makeIteratorFn(provSeg));
}
void TreeWidget::updateChildren(const armem::wm::Entity& entity, QTreeWidgetItem* entityItem)
{
- workingmemorySnapshotBuilder.updateTree(entityItem, entity.history());
+ workingmemorySnapshotBuilder.updateTreeWithIterator(entityItem, makeIteratorFn(entity));
}
void TreeWidget::updateChildren(const armem::wm::EntitySnapshot& snapshot, QTreeWidgetItem* snapshotItem)
{
- workingmemoryInstanceBuilder.updateTree(snapshotItem, snapshot.instances());
+ workingmemoryInstanceBuilder.updateTreeWithIterator(snapshotItem, makeIteratorFn(snapshot));
}
void TreeWidget::updateChildren(const armem::wm::EntityInstance& data, QTreeWidgetItem* dataItem)
@@ -255,19 +271,25 @@ namespace armarx::armem::gui::memory
(void) data, (void) dataItem;
}
- QTreeWidgetItem* TreeWidget::makeItem(const std::string& key, const armem::base::detail::MemoryItem& memoryItem)
+ template <class MemoryItemT>
+ QTreeWidgetItem* TreeWidget::makeItem(const std::string& key, const MemoryItemT& memoryItem)
{
(void) key;
+ return makeItem(memoryItem.getKeyString(), MemoryItemT::getLevelName(), memoryItem.id());
+ }
+
+ QTreeWidgetItem* TreeWidget::makeItem(const std::string& key, const std::string& levelName, const MemoryID& id)
+ {
QStringList columns;
- columns.insert(int(Columns::KEY), QString::fromStdString(memoryItem.getKeyString()));
+ columns.insert(int(Columns::KEY), QString::fromStdString(key));
columns.insert(int(Columns::SIZE), "");
columns.insert(int(Columns::TYPE), "");
- columns.insert(int(Columns::LEVEL), QString::fromStdString(simox::alg::capitalize_words(memoryItem.getLevelName())));
- columns.insert(int(Columns::ID), QString::fromStdString(memoryItem.id().str()));
+ columns.insert(int(Columns::LEVEL), QString::fromStdString(simox::alg::capitalize_words(levelName)));
+ columns.insert(int(Columns::ID), QString::fromStdString(id.str()));
QTreeWidgetItem* item = new QTreeWidgetItem(columns);
- item->setData(int(Columns::LEVEL), Qt::UserRole, QString::fromStdString(memoryItem.getLevelName()));
- item->setData(int(Columns::ID), Qt::UserRole, QString::fromStdString(memoryItem.id().str()));
+ item->setData(int(Columns::LEVEL), Qt::UserRole, QString::fromStdString(levelName));
+ item->setData(int(Columns::ID), Qt::UserRole, QString::fromStdString(id.str()));
item->setTextAlignment(int(Columns::SIZE), Qt::AlignRight);
return item;
}
diff --git a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h
index 2bcf0fa1b6135534407044a3ce341b86afeab442..72987cda1ef736a9f8943161cb8756a6e56b2cdc 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h
@@ -4,7 +4,7 @@
#include <QTreeWidget>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
@@ -67,7 +67,9 @@ namespace armarx::armem::gui::memory
void updateChildren(const armem::wm::EntityInstance& data, QTreeWidgetItem* parent);
- QTreeWidgetItem* makeItem(const std::string& key, const armem::base::detail::MemoryItem& container);
+ template <class MemoryItemT>
+ QTreeWidgetItem* makeItem(const std::string& key, const MemoryItemT& memoryItem);
+ QTreeWidgetItem* makeItem(const std::string& key, const std::string& levelName, const MemoryID& id);
void updateItemItem(const armem::base::detail::MemoryItem& level, QTreeWidgetItem* item);
template <class... T>
@@ -77,11 +79,11 @@ namespace armarx::armem::gui::memory
private:
MapTreeWidgetBuilder<std::string, const wm::Memory*> workingmemoryBuilder;
- MapTreeWidgetBuilder<std::string, wm::CoreSegment> workingmemoryCoreSegmentBuilder;
- MapTreeWidgetBuilder<std::string, wm::ProviderSegment> workingmemoryProvSegmentBuilder;
- MapTreeWidgetBuilder<std::string, wm::Entity> workingmemoryEntityBuilder;
- MapTreeWidgetBuilder<armem::Time, wm::EntitySnapshot> workingmemorySnapshotBuilder;
- TreeWidgetBuilder<std::vector<wm::EntityInstance>> workingmemoryInstanceBuilder;
+ TreeWidgetBuilder<wm::CoreSegment> workingmemoryCoreSegmentBuilder;
+ TreeWidgetBuilder<wm::ProviderSegment> workingmemoryProvSegmentBuilder;
+ TreeWidgetBuilder<wm::Entity> workingmemoryEntityBuilder;
+ TreeWidgetBuilder<wm::EntitySnapshot> workingmemorySnapshotBuilder;
+ TreeWidgetBuilder<wm::EntityInstance> workingmemoryInstanceBuilder;
std::optional<MemoryID> _selectedID;
/// While this is false, do not handle selection updates.
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
index e41bed15cecbc599c7c97b4266a04e0e9a70e47e..9d5cc9ea6064bb6977892623649518791dcaffec 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
@@ -26,11 +26,23 @@ namespace armarx::armem::gui
{
targets.push_back(query::data::QueryTarget::LTM);
}
- if (_DiskMemoryQueryTargetCheckBox->isChecked())
+ return targets;
+ }
+
+ void SnapshotSelectorWidget::localDiskStateChanged()
+ {
+ if (_LocalDiskMemoryQueryTargetCheckBox->isChecked())
{
- targets.push_back(query::data::QueryTarget::Disk);
+ _WMQueryTargetCheckBox->setChecked(false);
+ _LTMQueryTargetCheckBox->setChecked(false);
+ _WMQueryTargetCheckBox->setEnabled(false);
+ _LTMQueryTargetCheckBox->setEnabled(false);
+ }
+ else
+ {
+ _WMQueryTargetCheckBox->setEnabled(true);
+ _LTMQueryTargetCheckBox->setEnabled(true);
}
- return targets;
}
bool SnapshotSelectorWidget::queryTargetContainsLocalDisk() const
@@ -59,13 +71,13 @@ namespace armarx::armem::gui
// query type select box
auto queryTargetLayout = new QHBoxLayout();
_WMQueryTargetCheckBox = new QCheckBox("WM");
- _LTMQueryTargetCheckBox = new QCheckBox("LTM (MongoDB)");
- _DiskMemoryQueryTargetCheckBox = new QCheckBox("LTM (Disk)");
+ _LTMQueryTargetCheckBox = new QCheckBox("LTM");
_LocalDiskMemoryQueryTargetCheckBox = new QCheckBox("Local Disk");
+ connect(_LocalDiskMemoryQueryTargetCheckBox, &QCheckBox::stateChanged, this, &This::localDiskStateChanged);
+
queryTargetLayout->addWidget(_WMQueryTargetCheckBox);
queryTargetLayout->addWidget(_LTMQueryTargetCheckBox);
- queryTargetLayout->addWidget(_DiskMemoryQueryTargetCheckBox);
queryTargetLayout->addWidget(_LocalDiskMemoryQueryTargetCheckBox);
_WMQueryTargetCheckBox->setChecked(true);
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h
index 6f6422c3cdbc7fd0e1899bce47efbaaa32589062..a0b69eb1b6f362e64880ab400c0bea29bcdc73fc 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h
@@ -45,14 +45,13 @@ namespace armarx::armem::gui
signals:
void queryChanged();
-
private slots:
//void updateSelector();
void hideAllForms();
void showSelectedFormForQuery(QString selected);
-
+ void localDiskStateChanged();
signals:
void queryOutdated();
@@ -68,7 +67,6 @@ namespace armarx::armem::gui
QComboBox* _queryComboBox;
QCheckBox* _WMQueryTargetCheckBox;
QCheckBox* _LTMQueryTargetCheckBox;
- QCheckBox* _DiskMemoryQueryTargetCheckBox;
QCheckBox* _LocalDiskMemoryQueryTargetCheckBox;
/// The forms for the different query types. Hidden when not selected.
std::map<QString, SnapshotForm*> _queryForms;
diff --git a/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp b/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp
index 4f259c7af5a50ff198b139084eb604bfd06fead5..303a319bcd30566b8000e1d10cc6ab3ef812093d 100644
--- a/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp
+++ b/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp
@@ -44,13 +44,13 @@ BOOST_AUTO_TEST_CASE(test_sanitizeTypeName)
BOOST_CHECK_EQUAL(sanitizeTypeName(dict.getName()), "Dict<Float>");
}
{
- ListNavigator dict;
- dict.setAcceptedType(std::make_shared<LongNavigator>());
- BOOST_CHECK_EQUAL(sanitizeTypeName(dict.getName()), "Dict<Long>");
+ ListNavigator list;
+ list.setAcceptedType(std::make_shared<LongNavigator>());
+ BOOST_CHECK_EQUAL(sanitizeTypeName(list.getName()), "List<Long>");
}
{
- ObjectNavigator dict;
- dict.setObjectName("namespace::MyObjectName");
- BOOST_CHECK_EQUAL(sanitizeTypeName(dict.getName()), "namespace::MyObjectName");
+ ObjectNavigator obj;
+ obj.setObjectName("namespace::MyObjectName");
+ BOOST_CHECK_EQUAL(sanitizeTypeName(obj.getName()), "MyObjectName (namespace)");
}
}
diff --git a/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.cpp b/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.cpp
index c9aec94015f490d7c992919b96fdbce33b4f1718..5860b2f48f9659c7ac631e1270425f112a0efcab 100644
--- a/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.cpp
@@ -1,19 +1,26 @@
-// STD / STL
-#include <iostream>
-#include <fstream>
-#include <sstream>
-
// BaseClass
#include "Segment.h"
// ArmarX
#include "MotionConverter.h"
+
#include <RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/application/properties/ProxyPropertyDefinition.h>
+
+// STD / STL
+#include <iostream>
+#include <fstream>
+#include <sstream>
+
namespace armarx::armem::server::motions::mdb
{
- Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
- Base(memoryToIceAdapter, memoryMutex)
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ Base(memoryToIceAdapter)
{
}
@@ -72,13 +79,14 @@ namespace armarx::armem::server::motions::mdb
instance.metadata().timeSent = IceUtil::Time::now();
instance.metadata().timeArrived = IceUtil::Time::now();
instance.metadata().confidence = 1.0;
- instance.setData(op->toAron());
+ instance.data() = op->toAron();
}
else
{
ARMARX_WARNING << "Found an invalid path to a motion file: " << pathToInfoJson;
}
- }IceUtil::Time::now();
+ }
+ IceUtil::Time::now();
loadedMotions += allMotions.size();
}
diff --git a/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.h b/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.h
index 10dd57dee5f3c7c177a980ac29ed6dc45c845905..f0dc4d726f83895876e23ab3bc52af36895df5b3 100644
--- a/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.h
+++ b/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.h
@@ -5,7 +5,7 @@
#include <string>
// BaseClass
-#include <RobotAPI/libraries/armem/server/Segment.h>
+#include <RobotAPI/libraries/armem/server/segment/Segment.h>
// ArmarX
#include <RobotAPI/libraries/armem_motions/aron/MDBReference.aron.generated.h>
@@ -13,16 +13,16 @@
namespace armarx::armem::server::motions::mdb
{
- class Segment : public armem::server::wm::ProviderSegmentBase<armarx::armem::mdb::MDBReference>
+ class Segment : public armem::server::segment::wm::AronTypedProviderSegmentBase<armarx::armem::mdb::MDBReference>
{
- using Base = armem::server::wm::ProviderSegmentBase<armarx::armem::mdb::MDBReference>;
+ using Base = armem::server::segment::wm::AronTypedProviderSegmentBase<armarx::armem::mdb::MDBReference>;
public:
- Segment(armem::server::MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex);
+ Segment(armem::server::MemoryToIceAdapter& iceMemory);
virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
virtual void onInit() override;
- virtual void onConnect() override;
+ virtual void onConnect();
private:
int loadByMotionFinder(const std::string&);
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index 54eb919e1804ce099efe5ade4ea14294a202e0bb..ab26b1f8a38b5e18b16dc49a498c9369d6d918db 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -16,6 +16,7 @@ armarx_add_library(
RobotAPI::Core
RobotAPI::armem
RobotAPI::armem_robot
+
HEADERS
aron_conversions.h
aron_forward_declarations.h
@@ -32,11 +33,6 @@ armarx_add_library(
server/instance/Visu.h
server/instance/ArticulatedObjectVisu.h
- # server/articulated_object_class/Segment.h
- # server/articulated_object_instance/Segment.h
- # server/articulated_object/SegmentAdapter.h
- # server/articulated_object_instance/Visu.h
-
server/attachments/Segment.h
client/articulated_object/Reader.h
@@ -64,12 +60,6 @@ armarx_add_library(
server/instance/Visu.cpp
server/instance/ArticulatedObjectVisu.cpp
- server/articulated_object_class/Segment.cpp
-
- # server/articulated_object_instance/Segment.cpp
- # server/articulated_object/SegmentAdapter.cpp
- # server/articulated_object_instance/Visu.cpp
-
server/attachments/Segment.cpp
client/articulated_object/Reader.cpp
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
index c973cf24801967244589677f9b044be099f5c306..976d5bf9abb8a7d7586a0501568e6c8240c1e45c 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
@@ -16,7 +16,7 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
index d0bfb35708104712d2bebc33b4d7676b999ddac5..27d1cd851dd4500a2d3c6373a2b7c1cd2c5eb3e5 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -13,7 +13,7 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
@@ -242,32 +242,19 @@ namespace armarx::armem::articulated_object
.getCoreSegment(properties.coreInstanceSegmentName);
// clang-format on
- for (const auto& [_, providerSegment] : coreSegment.providerSegments())
+ std::optional<wm::EntityInstance> instance;
+ coreSegment.forEachInstance([&instance](const wm::EntityInstance & i)
{
-
- const auto entities = simox::alg::get_values(providerSegment.entities());
-
- if (entities.empty())
- {
- ARMARX_WARNING << "No entity found";
- continue;
- }
-
- const auto entitySnapshots = simox::alg::get_values(entities.front().history());
-
- if (entitySnapshots.empty())
- {
- ARMARX_WARNING << "No entity snapshots found";
- continue;
- }
-
- // TODO(fabian.reister): check if 0 available
- const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
-
- return robot::convertRobotState(instance);
+ instance = i;
+ });
+ if (instance.has_value())
+ {
+ return robot::convertRobotState(instance.value());
+ }
+ else
+ {
+ return std::nullopt;
}
-
- return std::nullopt;
}
@@ -275,67 +262,43 @@ namespace armarx::armem::articulated_object
Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
{
// clang-format off
- const armem::wm::CoreSegment& coreSegment = memory
- .getCoreSegment(properties.coreClassSegmentName);
+ const armem::wm::CoreSegment& coreSegment = memory.getCoreSegment(properties.coreClassSegmentName);
// clang-format on
- for (const auto& [_, providerSegment] : coreSegment.providerSegments())
+ std::optional<wm::EntityInstance> instance;
+ coreSegment.forEachInstance([&instance](const wm::EntityInstance & i)
{
- const auto entities = simox::alg::get_values(providerSegment.entities());
-
- if (entities.empty())
- {
- ARMARX_WARNING << "No entity found";
- continue;
- }
-
- const auto entitySnapshots = simox::alg::get_values(entities.front().history());
-
- if (entitySnapshots.empty())
- {
- ARMARX_WARNING << "No entity snapshots found";
- continue;
- }
-
- if (entitySnapshots.front().hasInstance(0))
- {
- const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
- return convertRobotDescription(instance);
- }
+ instance = i;
+ });
+ if (instance.has_value())
+ {
+ return convertRobotDescription(instance.value());
+ }
+ else
+ {
+ return std::nullopt;
}
-
- return std::nullopt;
}
+
std::vector<robot::RobotDescription>
Reader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
{
- std::vector<robot::RobotDescription> descriptions;
-
const armem::wm::CoreSegment& coreSegment =
memory.getCoreSegment(properties.coreClassSegmentName);
- for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
+ std::vector<robot::RobotDescription> descriptions;
+ coreSegment.forEachEntity([&descriptions](const wm::Entity & entity)
{
- for (const auto& [name, entity] : providerSegment.entities())
+ if (not entity.empty())
{
- if (entity.empty())
- {
- ARMARX_WARNING << "No entity found";
- continue;
- }
-
- const auto entitySnapshots = simox::alg::get_values(entity.history());
- const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
-
- const auto robotDescription = convertRobotDescription(instance);
-
+ const auto robotDescription = convertRobotDescription(entity.getFirstSnapshot().getInstance(0));
if (robotDescription)
{
descriptions.push_back(*robotDescription);
}
}
- }
+ });
return descriptions;
}
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h
index 2e51a9e4b336bd4550027227d30a0236efa0f217..9c5b48f39864b16637739b022c0d1908de69c9a5 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h
@@ -2,7 +2,7 @@
#include <optional>
-#include "RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h"
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include "RobotAPI/libraries/armem_robot/types.h"
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
index f723f30b2eb2d704dcfae73f47e9351cf659b7ad..938ba098681aca145efbeb131af9c7a6a1b6c054 100644
--- a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
@@ -9,7 +9,7 @@
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
@@ -25,50 +25,42 @@ namespace armarx::armem::attachment
namespace
{
- template<typename AronClass, typename ArmemClass>
+ template <typename AronClass, typename ArmemClass>
auto getAttachments(const armarx::armem::wm::Memory& memory)
{
// using ArmemClass = decltype(fromAron(AronClass()));
using ArmemClassVector = std::vector<ArmemClass>;
ArmemClassVector attachments;
-
- for (const auto&[_, coreSegment] : memory.coreSegments())
+ memory.forEachEntity([&attachments](const wm::Entity & entity)
{
- for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
+ if (entity.empty())
{
- for (const auto& [name, entity] : providerSegment.entities())
+ ARMARX_WARNING << "No entity snapshot found";
+ return true;
+ }
+
+ const armem::wm::EntityInstance& instance = entity.getFirstSnapshot().getInstance(0);
+ try
+ {
+ AronClass aronAttachment;
+ aronAttachment.fromAron(instance.data());
+
+ ArmemClass attachment;
+ fromAron(aronAttachment, attachment);
+
+ if (attachment.active)
{
- if (entity.empty())
- {
- ARMARX_WARNING << "No entity found";
- continue;
- }
-
- const auto entitySnapshots = simox::alg::get_values(entity.history());
- const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
-
- try
- {
- AronClass aronAttachment;
- aronAttachment.fromAron(instance.data());
-
- ArmemClass attachment;
- fromAron(aronAttachment, attachment);
-
- if (attachment.active)
- {
- attachments.push_back(attachment);
- }
-
- }
- catch (const armarx::aron::error::AronException&)
- {
- continue;
- }
+ attachments.push_back(attachment);
}
+
}
- }
+ catch (const armarx::aron::error::AronException&)
+ {
+ return true;
+ }
+ return true;
+ });
return attachments;
}
@@ -131,7 +123,9 @@ namespace armarx::armem::attachment
return {};
}
- return getAttachments<::armarx::armem::arondto::attachment::ObjectAttachment, ::armarx::armem::attachment::ObjectAttachment>(qResult.memory);
+ return getAttachments <
+ ::armarx::armem::arondto::attachment::ObjectAttachment,
+ ::armarx::armem::attachment::ObjectAttachment > (qResult.memory);
}
std::vector<ObjectAttachment> Reader::queryObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
deleted file mode 100644
index 425b5af02da13e58f4a654a0784029f5fbe9a9e0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
+++ /dev/null
@@ -1,185 +0,0 @@
-#include "Segment.h"
-
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-#include <sstream>
-
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include "ArmarXCore/core/logging/Logging.h"
-
-#include "RobotAPI/libraries/aron/common/aron_conversions.h"
-
-#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
-#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/client/query/query_fns.h>
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-
-#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-
-
-namespace armarx::armem::server::obj::articulated_object_class
-{
-
- Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
- iceMemory(memoryToIceAdapter),
- memoryMutex(memoryMutex)
- {
- Logging::setTag("ArticulatedObjectInstanceSegment");
- }
-
- Segment::~Segment() = default;
-
- void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- defs->optional(p.coreClassSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
- defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
-
- defs->optional(p.objectsPackage, prefix + "ObjectsPackage", "Name of the objects package to load from.");
- defs->optional(p.loadFromObjectsPackage, prefix + "LoadFromObjectsPackage",
- "If true, load the objects from the objects package on startup.");
-
- }
-
- void Segment::init()
- {
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
-
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreClassSegmentName, arondto::RobotDescription::toAronType());
- coreSegment->setMaxHistorySize(p.maxHistorySize);
-
- if (p.loadFromObjectsPackage)
- {
- loadByObjectFinder(p.objectsPackage);
- }
- }
-
- void Segment::connect(viz::Client arviz)
- {
- // this->visu = std::make_unique<Visu>(arviz, *this);
- }
-
- void Segment::loadByObjectFinder(const std::string& package)
- {
- ObjectFinder finder(package);
-
- const auto knownArticulatedObjectDescriptions = finder.findAllArticulatedObjectsByDataset(true);
- ARMARX_DEBUG << "Found " << knownArticulatedObjectDescriptions.size() << " articulated objects";
-
- loadObjectsIntoMemory(knownArticulatedObjectDescriptions, package);
- }
-
- void Segment::loadObjectsIntoMemory(const std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>>& datasets, const std::string& package)
- {
- const Time now = TimeUtil::GetTime();
-
- const MemoryID providerID = coreSegment->id().withProviderSegmentName(package);
- coreSegment->addProviderSegment(providerID.providerSegmentName);
-
- // ARMARX_INFO << "Loading up to " << infos.size() << " object classes from '"
- // << objectFinder.getPackageName() << "' ...";
- Commit commit;
-
- for (const auto& [datasetName, descriptions] : datasets)
- {
-
- for (const armem::articulated_object::ArticulatedObjectDescription& desc : descriptions)
- {
- EntityUpdate& update = commit.updates.emplace_back();
- update.entityID = providerID.withEntityName(desc.name);
- update.timeArrived = update.timeCreated = update.timeSent = now;
-
- arondto::RobotDescription aronRobotDescription;
- toAron(aronRobotDescription, desc);
- // TODO toAron(aronRobotDescription.timestamp, now);
-
- update.instancesData = { aronRobotDescription.toAron()};
- }
-
- ARMARX_INFO << "Loaded " << commit.updates.size() << " articulated object classes from '"
- << package << "' in dataset '" << datasetName << "'.";
- }
- iceMemory.commit(commit);
- }
-
-
-
- std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> Segment::getKnownObjectClasses() const
- {
- std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> objects;
-
- for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
- {
- for (const auto& [name, entity] : provSeg.entities())
- {
- for (const auto& snapshot : simox::alg::get_values(entity.history()))
- {
- const auto& entityInstance = snapshot.getInstance(0);
- const auto description = robot::convertRobotDescription(entityInstance);
-
- if (not description)
- {
- ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
- continue;
- }
-
- ARMARX_DEBUG << "Key is " << armem::MemoryID(snapshot.id());
-
- objects.emplace(armem::MemoryID(snapshot.id()), *description);
- }
- }
- }
-
- ARMARX_INFO << deactivateSpam(10) << "Number of known articulated object classes: " << objects.size();
-
- return objects;
- }
-
-
-
- // void Segment::RemoteGui::setup(const Segment& data)
- // {
- // using namespace armarx::RemoteGui::Client;
-
- // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
- // maxHistorySize.setRange(1, 1e6);
- // infiniteHistory.setValue(data.p.maxHistorySize == -1);
- // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
-
- // GridLayout grid;
- // int row = 0;
- // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
- // row++;
- // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
- // row++;
- // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
- // row++;
-
- // group.setLabel("Data");
- // group.addChild(grid);
- // }
-
- // void Segment::RemoteGui::update(Segment& data)
- // {
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
- // || discardSnapshotsWhileAttached.hasValueChanged())
- // {
- // std::scoped_lock lock(data.memoryMutex);
-
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
- // {
- // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- // if (data.coreSegment)
- // {
- // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
- // }
- // }
-
- // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
- // }
- // }
-
-} // namespace armarx::armem::server::obj::articulated_object_class
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h
deleted file mode 100644
index e2a7bbc6fbf2c875e11fb25427f5a7a8809aee4c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h
+++ /dev/null
@@ -1,112 +0,0 @@
-/*
- * 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/>.
- *
- * @author Fabian Reister ( fabian dot reister at kit dot edu )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <string>
-#include <optional>
-#include <mutex>
-#include <unordered_map>
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
-
-// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
-
-#include "RobotAPI/components/ArViz/Client/Client.h"
-
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
-#include "RobotAPI/libraries/armem_objects/types.h"
-
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-
-namespace armarx::armem
-{
- namespace server
- {
- class MemoryToIceAdapter;
- }
-
- namespace wm
- {
- class CoreSegment;
- }
-} // namespace armarx::armem
-
-
-namespace armarx::armem::server::obj::articulated_object_class
-{
- class Visu;
-
- class Segment : public armarx::Logging
- {
- public:
- Segment(server::MemoryToIceAdapter& iceMemory,
- std::mutex& memoryMutex);
-
- virtual ~Segment();
-
- void connect(viz::Client arviz);
-
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
-
- void init();
-
- std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> getKnownObjectClasses() const;
-
-
- private:
- void loadByObjectFinder(const std::string& package);
- void loadObjectsIntoMemory(const std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>>& datasets, const std::string& package);
-
- server::MemoryToIceAdapter& iceMemory;
- wm::CoreSegment* coreSegment = nullptr;
- std::mutex& memoryMutex;
-
- struct Properties
- {
- std::string coreClassSegmentName = "ArticulatedObjectClass";
- int64_t maxHistorySize = -1;
-
- std::string objectsPackage = ObjectFinder::DefaultObjectsPackageName;
- bool loadFromObjectsPackage = true;
- };
- Properties p;
-
- // std::unique_ptr<Visu> visu;
-
- public:
-
- // struct RemoteGui
- // {
- // armarx::RemoteGui::Client::GroupBox group;
-
- // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
- // armarx::RemoteGui::Client::CheckBox infiniteHistory;
- // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
-
- // void setup(const Segment& data);
- // void update(Segment& data);
- // };
-
- };
-
-} // namespace armarx::armem::server::obj::articulated_object_class
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp
deleted file mode 100644
index dba8c91a83e3449fd0dc2a4bdb8ef79632a572e0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp
+++ /dev/null
@@ -1,175 +0,0 @@
-#include "Segment.h"
-
-#include <sstream>
-
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
-#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/client/query/query_fns.h>
-#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
-
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-
-#include <RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h>
-
-#include "Visu.h"
-
-
-namespace armarx::armem::server::obj::articulated_object_instance
-{
-
- Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
- iceMemory(memoryToIceAdapter),
- memoryMutex(memoryMutex)
- {
- Logging::setTag("ArticulatedObjectInstanceSegment");
- }
-
- Segment::~Segment() = default;
-
- void Segment::defineProperties(armarx::PropertyDefinitionsPtr& defs, const std::string& prefix)
- {
- defs->optional(p.coreInstanceSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
- defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
- }
-
- void Segment::init()
- {
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
-
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreInstanceSegmentName, arondto::Robot::toAronType());
- coreSegment->setMaxHistorySize(p.maxHistorySize);
-
- }
-
- void Segment::connect(viz::Client arviz)
- {
- this->visu = std::make_unique<Visu>(arviz, *this);
- visu->init();
- }
-
- void Segment::setArticulatedObjectClassSegment(const articulated_object_class::Segment& segment)
- {
- classSegment = &segment;
- }
-
- ::armarx::armem::articulated_object::ArticulatedObjects Segment::getArticulatedObjects() const
- {
- ARMARX_CHECK_NOT_NULL(classSegment);
- const auto knownObjectClasses = classSegment->getKnownObjectClasses();
-
- ARMARX_DEBUG << "Class segment has " << knownObjectClasses.size() << " known articulated objects";
-
- const auto escape = [](std::string & v)
- {
- v = simox::alg::replace_all(v, "/", "\\/");
- };
-
- const auto resolveDescriptionLink = [&](auto & articulatedObject, const auto & aronDescriptionLink) -> bool
- {
- armem::MemoryID descriptionLink;
- fromAron(aronDescriptionLink, descriptionLink);
-
- escape(descriptionLink.providerSegmentName);
-
- ARMARX_DEBUG << "Lookup key is " << descriptionLink;
-
- // const auto keys = simox::alg::get_keys(knownObjectClasses);
- // ARMARX_DEBUG << "Known keys " << keys;
-
- const auto it = knownObjectClasses.find(descriptionLink);
- if (it == knownObjectClasses.end())
- {
- ARMARX_WARNING << "Unknown object class " << descriptionLink
- << "Known classes are " << simox::alg::get_keys(knownObjectClasses);
- return false;
- }
-
- articulatedObject.description = it->second;
- return true;
- };
-
- ::armarx::armem::articulated_object::ArticulatedObjects objects;
- for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreInstanceSegmentName))
- {
- for (const auto& [_, entity] : provSeg.entities())
- {
- const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
-
- arondto::Robot aronArticulatedObject;
- aronArticulatedObject.fromAron(entityInstance.data());
-
- armem::articulated_object::ArticulatedObject articulatedObject;
- fromAron(aronArticulatedObject, articulatedObject);
-
- // resolve memory link for description
- const arondto::MemoryID& aronDescriptionLink = aronArticulatedObject.description;
- if (not resolveDescriptionLink(articulatedObject, aronDescriptionLink))
- {
- continue;
- }
-
- objects.push_back(articulatedObject);
- }
- }
-
- return objects;
- }
-
-
-
- // void Segment::RemoteGui::setup(const Segment& data)
- // {
- // using namespace armarx::RemoteGui::Client;
-
- // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
- // maxHistorySize.setRange(1, 1e6);
- // infiniteHistory.setValue(data.p.maxHistorySize == -1);
- // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
-
- // GridLayout grid;
- // int row = 0;
- // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
- // row++;
- // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
- // row++;
- // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
- // row++;
-
- // group.setLabel("Data");
- // group.addChild(grid);
- // }
-
- // void Segment::RemoteGui::update(Segment& data)
- // {
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
- // || discardSnapshotsWhileAttached.hasValueChanged())
- // {
- // std::scoped_lock lock(data.memoryMutex);
-
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
- // {
- // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- // if (data.coreSegment)
- // {
- // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
- // }
- // }
-
- // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
- // }
- // }
-
-} // namespace armarx::armem::server::obj::articulated_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.h
deleted file mode 100644
index 2d50be817effddb795e8cd6adf1bda992d177e87..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.h
+++ /dev/null
@@ -1,112 +0,0 @@
-/*
- * 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/>.
- *
- * @author Fabian Reister ( fabian dot reister at kit dot edu )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <cstdint>
-#include <map>
-#include <string>
-#include <optional>
-#include <mutex>
-
-#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
-
-#include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
-
-#include <RobotAPI/interface/core/RobotState.h>
-
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-
-#include "RobotAPI/components/ArViz/Client/Client.h"
-
-#include "RobotAPI/libraries/armem_objects/types.h"
-
-
-namespace armarx::armem::server::obj::articulated_object_class
-{
- class Segment;
-}
-
-namespace armarx::armem::server::obj::articulated_object_instance
-{
- class Visu;
-
- class Segment : public armarx::Logging
- {
- public:
-
- struct CommitStats
- {
- int numUpdated = 0;
- };
-
-
- Segment(server::MemoryToIceAdapter& iceMemory,
- std::mutex& memoryMutex);
-
- virtual ~Segment();
-
- void connect(viz::Client arviz);
-
- void defineProperties(armarx::PropertyDefinitionsPtr& defs, const std::string& prefix = "");
-
- void init();
-
- void setArticulatedObjectClassSegment(const articulated_object_class::Segment& segment);
-
- ::armarx::armem::articulated_object::ArticulatedObjects getArticulatedObjects() const;
-
- private:
-
- server::MemoryToIceAdapter& iceMemory;
- wm::CoreSegment* coreSegment = nullptr;
- std::mutex& memoryMutex;
-
- articulated_object_class::Segment const* classSegment;
-
-
- struct Properties
- {
- std::string coreInstanceSegmentName = "ArticulatedObjectInstance";
- int64_t maxHistorySize = -1;
- };
- Properties p;
-
- std::unique_ptr<Visu> visu;
-
- public:
-
- // struct RemoteGui
- // {
- // armarx::RemoteGui::Client::GroupBox group;
-
- // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
- // armarx::RemoteGui::Client::CheckBox infiniteHistory;
- // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
-
- // void setup(const Segment& data);
- // void update(Segment& data);
- // };
-
- };
-
-} // namespace armarx::armem::server::obj::articulated_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
index f2490c841d0042a133fb1c8cf58740eb2af614cd..2ddcc827d80c0700f59cb5f9ee8638322f4e5633 100644
--- a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
@@ -3,14 +3,15 @@
#include <sstream>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/application/properties/PluginAll.h>
-#include "RobotAPI/libraries/armem/util/util.h"
-#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
@@ -23,9 +24,8 @@
namespace armarx::armem::server::obj::attachments
{
- Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
- iceMemory(memoryToIceAdapter),
- memoryMutex(memoryMutex)
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ iceMemory(memoryToIceAdapter)
{
Logging::setTag("Attachments");
}
@@ -34,7 +34,7 @@ namespace armarx::armem::server::obj::attachments
void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
- defs->optional(p.coreClassSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
+ defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
}
@@ -42,85 +42,42 @@ namespace armarx::armem::server::obj::attachments
{
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreClassSegmentName, arondto::Robot::toAronType());
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, arondto::Robot::toAronType());
coreSegment->setMaxHistorySize(p.maxHistorySize);
}
- void Segment::connect(viz::Client arviz)
+ void Segment::connect()
{
- // this->visu = std::make_unique<Visu>(arviz, *this);
}
- std::vector<armarx::armem::attachment::ObjectAttachment> Segment::getAttachments(const armem::Time& timestamp) const
+ std::vector<armarx::armem::attachment::ObjectAttachment>
+ Segment::getAttachments(const armem::Time& timestamp) const
{
- std::vector<armarx::armem::attachment::ObjectAttachment> attachments;
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ std::scoped_lock(coreSegment->mutex());
- for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
+ std::vector<armarx::armem::attachment::ObjectAttachment> attachments;
+ coreSegment->forEachEntity([this, &attachments](const wm::Entity & entity)
{
- for (const auto& [name, entity] : provSeg.entities())
- {
- const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- const auto aronAttachment = tryCast<armarx::armem::arondto::attachment::ObjectAttachment>(entityInstance);
+ const wm::EntityInstance& entityInstance = entity.getLatestSnapshot().getInstance(0);
- if (not aronAttachment)
- {
- ARMARX_WARNING << "Could not convert entity instance to 'ObjectAttachment'";
- continue;
- }
+ const auto aronAttachment = tryCast<armarx::armem::arondto::attachment::ObjectAttachment>(entityInstance);
+ if (not aronAttachment)
+ {
+ ARMARX_WARNING << "Could not convert entity instance to 'ObjectAttachment'";
+ return true;
+ }
- ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
+ ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
- armarx::armem::attachment::ObjectAttachment attachment;
- fromAron(*aronAttachment, attachment);
+ armarx::armem::attachment::ObjectAttachment attachment;
+ fromAron(*aronAttachment, attachment);
- attachments.push_back(attachment);
- }
- }
+ attachments.push_back(attachment);
+ return true;
+ });
return attachments;
}
-
- // void Segment::RemoteGui::setup(const Segment& data)
- // {
- // using namespace armarx::RemoteGui::Client;
-
- // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
- // maxHistorySize.setRange(1, 1e6);
- // infiniteHistory.setValue(data.p.maxHistorySize == -1);
- // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
-
- // GridLayout grid;
- // int row = 0;
- // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
- // row++;
- // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
- // row++;
- // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
- // row++;
-
- // group.setLabel("Data");
- // group.addChild(grid);
- // }
-
- // void Segment::RemoteGui::update(Segment& data)
- // {
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
- // || discardSnapshotsWhileAttached.hasValueChanged())
- // {
- // std::scoped_lock lock(data.memoryMutex);
-
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
- // {
- // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- // if (data.coreSegment)
- // {
- // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
- // }
- // }
-
- // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
- // }
- // }
-
} // namespace armarx::armem::server::obj::attachments
diff --git a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h
index 3817bc22a51227422848ebbe1e4296aa570fee24..4f8cd20ccd9a0ce456bb664f5f5eb8a25f1981e0 100644
--- a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h
@@ -21,54 +21,31 @@
#pragma once
-#include <string>
-#include <optional>
-#include <mutex>
-#include <unordered_map>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+#include <RobotAPI/libraries/armem/server/forward_declarations.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
-
-// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
-
-#include "RobotAPI/components/ArViz/Client/Client.h"
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
-#include "RobotAPI/libraries/armem/core/Time.h"
-#include "RobotAPI/libraries/armem_objects/types.h"
-
-namespace armarx::armem
-{
- namespace server
- {
- class MemoryToIceAdapter;
- }
-
- namespace wm
- {
- class CoreSegment;
- }
-} // namespace armarx::armem
+#include <string>
namespace armarx::armem::server::obj::attachments
{
- class Visu;
-
class Segment : public armarx::Logging
{
public:
- Segment(server::MemoryToIceAdapter& iceMemory,
- std::mutex& memoryMutex);
+ Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment();
- void connect(viz::Client arviz);
-
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
void init();
+ void connect();
std::vector<armarx::armem::attachment::ObjectAttachment> getAttachments(const armem::Time& timestamp) const;
@@ -77,31 +54,14 @@ namespace armarx::armem::server::obj::attachments
server::MemoryToIceAdapter& iceMemory;
wm::CoreSegment* coreSegment = nullptr;
- std::mutex& memoryMutex;
struct Properties
{
- std::string coreClassSegmentName = "Attachments";
+ std::string coreSegmentName = "Attachments";
int64_t maxHistorySize = -1;
};
Properties p;
- // std::unique_ptr<Visu> visu;
-
- public:
-
- // struct RemoteGui
- // {
- // armarx::RemoteGui::Client::GroupBox group;
-
- // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
- // armarx::RemoteGui::Client::CheckBox infiniteHistory;
- // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
-
- // void setup(const Segment& data);
- // void update(Segment& data);
- // };
-
};
} // namespace armarx::armem::server::obj::attachments
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.cpp b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.cpp
index 08927d1e77d1963234f42ea9ab861ec9a33f86c4..1e9957aff7b0d093d3bd1d6e16f289d6a2a04afd 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.cpp
@@ -1,6 +1,7 @@
#include "FloorVis.h"
-#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
@@ -13,16 +14,19 @@ namespace armarx::armem::server::obj::clazz
{
}
+
void FloorVis::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
properties.define(defs, prefix);
}
+
void FloorVis::setArViz(armarx::viz::Client arviz)
{
this->arviz = arviz;
}
+
void FloorVis::updateFloorObject(const wm::CoreSegment& classCoreSegment)
{
viz::Layer layer = arviz.layer(properties.layerName);
@@ -42,6 +46,7 @@ namespace armarx::armem::server::obj::clazz
arviz.commit(layer);
}
+
armarx::viz::Object FloorVis::makeFloorObject(const wm::Entity& classEntity)
{
const wm::EntityInstance& instance = classEntity.getLatestSnapshot().getInstance(0);
@@ -50,6 +55,7 @@ namespace armarx::armem::server::obj::clazz
return makeFloorObject(classEntity.name(), data);
}
+
armarx::viz::Object FloorVis::makeFloorObject(
const std::string& name,
const arondto::ObjectClass& objectClass)
@@ -61,7 +67,6 @@ namespace armarx::armem::server::obj::clazz
}
-
void FloorVis::Properties::define(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
defs->optional(show, prefix + "Show", "Whether to show the floor.");
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h
index dcce63977438bdd0d646b50aa8e5dd9679354df8..49f8a6ddd71f5c24172ddfbbfd8e312dcea2fd3f 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h
+++ b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h
@@ -10,7 +10,7 @@ namespace armarx
{
using PropertyDefinitionsPtr = IceUtil::Handle<class PropertyDefinitionContainer>;
}
-namespace armarx::armem::wm
+namespace armarx::armem::server::wm
{
class CoreSegment;
class Entity;
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
index 3e6ecfed37e3d75d60b3d509f97520c7627f0fcc..cd10a49c91957e5fa86094597c7903f76fb04c13 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -3,9 +3,11 @@
#include <RobotAPI/libraries/aron/core/Exception.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <SimoxUtility/color/Color.h>
@@ -19,17 +21,21 @@
namespace armarx::armem::server::obj::clazz
{
- Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
- iceMemory(memoryToIceAdapter),
- memoryMutex(memoryMutex)
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ SpecializedSegment(memoryToIceAdapter,
+ arondto::ObjectClass::toAronType(), "Class", -1)
{
- Logging::setTag("ClassSegment");
}
+
+ Segment::~Segment()
+ {
+ }
+
+
void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
- defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the object clazz core segment.");
- defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+ SpecializedSegment::defineProperties(defs, prefix);
defs->optional(p.objectsPackage, prefix + "ObjectsPackage", "Name of the objects package to load from.");
defs->optional(p.loadFromObjectsPackage, prefix + "LoadFromObjectsPackage",
@@ -38,12 +44,10 @@ namespace armarx::armem::server::obj::clazz
floorVis.defineProperties(defs, prefix + "Floor.");
}
+
void Segment::init()
{
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
-
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, arondto::ObjectClass::toAronType());
- coreSegment->setMaxHistorySize(p.maxHistorySize);
+ SpecializedSegment::init();
if (p.loadFromObjectsPackage)
{
@@ -51,6 +55,7 @@ namespace armarx::armem::server::obj::clazz
}
}
+
void Segment::connect(viz::Client arviz)
{
this->arviz = arviz;
@@ -65,12 +70,14 @@ namespace armarx::armem::server::obj::clazz
loadByObjectFinder(ObjectFinder(objectsPackage));
}
+
void Segment::loadByObjectFinder(const ObjectFinder& finder)
{
this->objectFinder = finder;
loadByObjectFinder();
}
+
void Segment::loadByObjectFinder()
{
const Time now = TimeUtil::GetTime();
@@ -79,11 +86,6 @@ namespace armarx::armem::server::obj::clazz
std::vector<ObjectInfo> infos = objectFinder.findAllObjects(checkPaths);
const MemoryID providerID = coreSegment->id().withProviderSegmentName(objectFinder.getPackageName());
- if (not coreSegment->hasProviderSegment(providerID.providerSegmentName))
- {
- coreSegment->addProviderSegment(providerID.providerSegmentName);
- }
-
ARMARX_INFO << "Loading up to " << infos.size() << " object classes from '"
<< objectFinder.getPackageName() << "' ...";
Commit commit;
@@ -91,7 +93,7 @@ namespace armarx::armem::server::obj::clazz
{
info.setLogError(false);
- EntityUpdate& update = commit.updates.emplace_back();
+ EntityUpdate& update = commit.add();
update.entityID = providerID.withEntityName(info.id().str());
update.timeArrived = update.timeCreated = update.timeSent = now;
@@ -103,9 +105,10 @@ namespace armarx::armem::server::obj::clazz
}
ARMARX_INFO << "Loaded " << commit.updates.size() << " object classes from '"
<< objectFinder.getPackageName() << "'.";
- iceMemory.commit(commit);
+ iceMemory.commitLocking(commit);
}
+
void Segment::visualizeClass(const MemoryID& entityID, bool showAABB, bool showOOBB)
{
const Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
@@ -121,31 +124,32 @@ namespace armarx::armem::server::obj::clazz
{
try
{
- const armem::wm::Entity& entity = coreSegment->getEntity(entityID);
- const armem::wm::EntityInstance& instance = entity.getLatestSnapshot().getInstance(0);
-
- arondto::ObjectClass aron;
- aron.fromAron(instance.data());
+ std::optional<arondto::ObjectClass> aron =
+ coreSegment->getLatestEntityInstanceDataLockingAs<arondto::ObjectClass>(entityID, 0);
+ if (not aron.has_value())
+ {
+ return;
+ }
- if (!aron.simoxXmlPath.package.empty())
+ if (not aron->simoxXmlPath.package.empty())
{
layerObject.add(viz::Object(entityID.str())
- .file(aron.simoxXmlPath.package, aron.simoxXmlPath.path)
+ .file(aron->simoxXmlPath.package, aron->simoxXmlPath.path)
.pose(pose));
}
if (showAABB)
{
layerAABB.add(viz::Box("AABB")
- .pose(pose * simox::math::pose(aron.aabb.center))
- .size(aron.aabb.extents)
+ .pose(pose * simox::math::pose(aron->aabb.center))
+ .size(aron->aabb.extents)
.color(simox::Color::cyan(255, 64)));
}
if (showOOBB)
{
layerOOBB.add(viz::Box("OOBB")
- .pose(pose * simox::math::pose(aron.oobb.center, aron.oobb.orientation))
- .size(aron.oobb.extents)
+ .pose(pose * simox::math::pose(aron->oobb.center, aron->oobb.orientation))
+ .size(aron->oobb.extents)
.color(simox::Color::lime(255, 64)));
}
}
@@ -164,17 +168,16 @@ namespace armarx::armem::server::obj::clazz
arviz.commit(layerObject, layerOrigin, layerAABB, layerOOBB);
}
+
arondto::ObjectClass Segment::objectClassFromInfo(const ObjectInfo& info)
{
namespace fs = std::filesystem;
arondto::ObjectClass data;
-
toAron(data.id, info.id());
- auto setPathIfExists = [](
- armarx::arondto::PackagePath & aron,
- const PackageFileLocation & location)
+ auto setPathIfExists = [](armarx::arondto::PackagePath & aron,
+ const PackageFileLocation & location)
{
if (fs::is_regular_file(location.absolutePath))
{
@@ -223,21 +226,23 @@ namespace armarx::armem::server::obj::clazz
group.addChildren({layout, VSpacer()});
}
+
void Segment::RemoteGui::update(Segment& segment)
{
data.update(segment);
visu.update(segment);
}
+
void Segment::RemoteGui::Data::setup(const Segment& segment)
{
using namespace armarx::RemoteGui::Client;
reloadButton.setLabel("Reload");
- maxHistorySize.setValue(std::max(1, int(segment.p.maxHistorySize)));
+ maxHistorySize.setValue(std::max(1, int(segment.properties.maxHistorySize)));
maxHistorySize.setRange(1, 1e6);
- infiniteHistory.setValue(segment.p.maxHistorySize == -1);
+ infiniteHistory.setValue(segment.properties.maxHistorySize == -1);
GridLayout grid;
int row = 0;
@@ -253,43 +258,40 @@ namespace armarx::armem::server::obj::clazz
group.addChild(grid);
}
+
void Segment::RemoteGui::Data::update(Segment& segment)
{
if (reloadButton.wasClicked())
{
- std::scoped_lock lock(segment.memoryMutex);
+ // Core segment mutex will be locked on commit.
segment.loadByObjectFinder();
rebuild = true;
}
if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
{
- std::scoped_lock lock(segment.memoryMutex);
- segment.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ std::scoped_lock lock(segment.mutex());
+ segment.properties.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
if (segment.coreSegment)
{
- segment.coreSegment->setMaxHistorySize(long(segment.p.maxHistorySize));
+ segment.coreSegment->setMaxHistorySize(long(segment.properties.maxHistorySize));
}
}
}
-
void Segment::RemoteGui::Visu::setup(const Segment& segment)
{
using namespace armarx::RemoteGui::Client;
showComboBox = {};
showOptionsIndex.clear();
- for (const auto& [_, prov] : *segment.coreSegment)
+ segment.coreSegment->forEachEntity([this](const wm::Entity & entity)
{
- for (const auto& [_, entity] : prov)
- {
- std::stringstream option;
- option << entity.id().entityName << " (" << entity.id().providerSegmentName << ")";
- showComboBox.addOption(option.str());
- showOptionsIndex.push_back(entity.id());
- }
- }
+ std::stringstream option;
+ option << entity.id().entityName << " (" << entity.id().providerSegmentName << ")";
+ showComboBox.addOption(option.str());
+ showOptionsIndex.push_back(entity.id());
+ });
if (showOptionsIndex.empty())
{
showComboBox.addOption("<none>");
@@ -308,6 +310,7 @@ namespace armarx::armem::server::obj::clazz
group.addChild(grid);
}
+
void Segment::RemoteGui::Visu::update(Segment& segment)
{
if (showButton.wasClicked())
@@ -315,7 +318,6 @@ namespace armarx::armem::server::obj::clazz
const size_t index = static_cast<size_t>(showComboBox.getIndex());
if (/*index >= 0 &&*/ index < showOptionsIndex.size())
{
- std::scoped_lock lock(segment.memoryMutex);
segment.visualizeClass(showOptionsIndex.at(index));
}
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
index 88eaedbefb6529b873dfe6033e6a2857672c0586..3e9717b3e51dc07491e55761664e5cc76add574b 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
@@ -3,36 +3,37 @@
#include <mutex>
#include <string>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-
#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
#include <RobotAPI/libraries/armem_objects/server/class/FloorVis.h>
+namespace armarx::armem::arondto
+{
+ class ObjectClass;
+}
namespace armarx::armem::server::obj::clazz
{
- class Segment : public armarx::Logging
+ class Segment : public segment::SpecializedSegment
{
public:
- Segment(armem::server::MemoryToIceAdapter& iceMemory,
- std::mutex& memoryMutex);
+ Segment(armem::server::MemoryToIceAdapter& iceMemory);
+ virtual ~Segment() override;
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
- void init();
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ void init() override;
void connect(viz::Client arviz);
+
void loadByObjectFinder(const std::string& objectsPackage);
void loadByObjectFinder(const ObjectFinder& finder);
void loadByObjectFinder();
@@ -45,10 +46,6 @@ namespace armarx::armem::server::obj::clazz
private:
- armem::server::MemoryToIceAdapter& iceMemory;
- armem::wm::CoreSegment* coreSegment = nullptr;
- std::mutex& memoryMutex;
-
ObjectFinder objectFinder;
viz::Client arviz;
@@ -57,9 +54,6 @@ namespace armarx::armem::server::obj::clazz
struct Properties
{
- std::string coreSegmentName = "Class";
- long maxHistorySize = -1;
-
std::string objectsPackage = ObjectFinder::DefaultObjectsPackageName;
bool loadFromObjectsPackage = true;
};
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index db706075c9bd5431fefd2b04b7634cf5e8ff6747..d6d224fe0580e4aa73656515a671ba5753e33326 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -1,15 +1,16 @@
#include "Segment.h"
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/SceneSnapshot.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
@@ -24,8 +25,9 @@
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/json.h>
@@ -42,12 +44,10 @@
namespace armarx::armem::server::obj::instance
{
- Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
- iceMemory(memoryToIceAdapter),
- memoryMutex(memoryMutex)
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ SpecializedSegment(memoryToIceAdapter,
+ arondto::ObjectInstance::toAronType(), "Instance", 64)
{
- Logging::setTag("InstanceSegment");
-
oobbCache.setFetchFn([this](const ObjectID & id) -> std::optional<simox::OrientedBoxf>
{
// Try to get OOBB from repository.
@@ -79,10 +79,15 @@ namespace armarx::armem::server::obj::instance
}
+ Segment::~Segment()
+ {
+ }
+
+
void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
- defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
- defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+ SpecializedSegment::defineProperties(defs, prefix);
+
defs->optional(p.discardSnapshotsWhileAttached, prefix + "DiscardSnapshotsWhileAttached",
"If true, no new snapshots are stored while an object is attached to a robot node.\n"
"If false, new snapshots are stored, but the attachment is kept in the new snapshots.");
@@ -102,11 +107,7 @@ namespace armarx::armem::server::obj::instance
void Segment::init()
{
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
-
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, arondto::ObjectInstance::toAronType());
- coreSegment->setMaxHistorySize(p.maxHistorySize);
-
+ SpecializedSegment::init();
if (not p.sceneSnapshotToLoad.empty())
{
@@ -115,14 +116,17 @@ namespace armarx::armem::server::obj::instance
}
}
+
void Segment::connect(viz::Client arviz)
{
+ (void) arviz;
// ARMARX_INFO << "ArticulatedObjectVisu";
// this->visu = std::make_unique<ArticulatedObjectVisu>(arviz, *this);
// visu->init();
}
+
Segment::CommitStats Segment::commitObjectPoses(
const std::string& providerName,
const objpose::data::ProvidedObjectPoseSeq& providedPoses,
@@ -135,11 +139,11 @@ namespace armarx::armem::server::obj::instance
stats.numUpdated = 0;
for (const objpose::data::ProvidedObjectPose& provided : providedPoses)
{
- const IceUtil::Time timestamp = IceUtil::Time::microSeconds(provided.timestampMicroSeconds);
+ const Time timestamp = Time::microSeconds(provided.timestampMicroSeconds);
// Check whether we have an old snapshot for this object.
std::optional<objpose::ObjectPose> previousPose;
- const armem::wm::Entity* entity = findObjectEntity(armarx::fromIce(provided.objectID), providerName);
+ const wm::Entity* entity = findObjectEntity(armarx::fromIce(provided.objectID), providerName);
if (entity)
{
const arondto::ObjectInstance data = getLatestInstanceData(*entity);
@@ -208,9 +212,9 @@ namespace armarx::armem::server::obj::instance
void Segment::commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName)
{
- ARMARX_CHECK_NOT_NULL(coreSegment);
Time now = TimeUtil::GetTime();
+ ARMARX_CHECK_NOT_NULL(coreSegment);
const MemoryID coreSegmentID = coreSegment->id();
Commit commit;
@@ -241,25 +245,29 @@ namespace armarx::armem::server::obj::instance
update.instancesData.push_back(dto.toAron());
}
+ // Commit non-locking.
iceMemory.commit(commit);
}
- wm::CoreSegment& Segment::getCoreSegment()
+ wm::CoreSegment&
+ Segment::getCoreSegment()
{
ARMARX_CHECK_NOT_NULL(coreSegment);
return *coreSegment;
}
- const wm::CoreSegment& Segment::getCoreSegment() const
+ const wm::CoreSegment&
+ Segment::getCoreSegment() const
{
ARMARX_CHECK_NOT_NULL(coreSegment);
return *coreSegment;
}
- objpose::ObjectPoseMap Segment::getObjectPoses(IceUtil::Time now)
+ objpose::ObjectPoseMap
+ Segment::getObjectPoses(IceUtil::Time now)
{
ObjectPoseMap objectPoses = getLatestObjectPoses();
updateObjectPoses(objectPoses, now);
@@ -268,7 +276,8 @@ namespace armarx::armem::server::obj::instance
- objpose::ObjectPoseMap Segment::getObjectPosesByProvider(
+ objpose::ObjectPoseMap
+ Segment::getObjectPosesByProvider(
const std::string& providerName,
IceUtil::Time now)
{
@@ -279,27 +288,31 @@ namespace armarx::armem::server::obj::instance
}
- armem::wm::Entity* Segment::findObjectEntity(const ObjectID& objectID, const std::string& providerName)
+ wm::Entity*
+ Segment::findObjectEntity(const ObjectID& objectID, const std::string& providerName)
{
ARMARX_CHECK_NOT_NULL(coreSegment);
armem::MemoryID entityID = armem::MemoryID().withEntityName(objectID.str());
if (providerName.empty())
{
- for (auto& [_, prov] : *coreSegment)
+ wm::Entity* result = nullptr;
+ coreSegment->forEachProviderSegment([&result, &entityID](wm::ProviderSegment & prov)
{
if (prov.hasEntity(entityID.entityName))
{
- return &prov.getEntity(entityID);
+ result = &prov.getEntity(entityID);
+ return false;
}
- }
- return nullptr;
+ return true;
+ });
+ return result;
}
else
{
entityID.providerSegmentName = providerName;
if (coreSegment->hasProviderSegment(providerName))
{
- armem::wm::ProviderSegment& prov = coreSegment->getProviderSegment(providerName);
+ wm::ProviderSegment& prov = coreSegment->getProviderSegment(providerName);
return prov.hasEntity(entityID.entityName) ? &prov.getEntity(entityID) : nullptr;
}
else
@@ -382,14 +395,16 @@ namespace armarx::armem::server::obj::instance
}
- objpose::ObjectPoseMap Segment::getLatestObjectPoses() const
+ objpose::ObjectPoseMap
+ Segment::getLatestObjectPoses() const
{
ARMARX_CHECK_NOT_NULL(coreSegment);
return getLatestObjectPoses(*coreSegment);
}
- objpose::ObjectPoseMap Segment::getLatestObjectPoses(const armem::wm::CoreSegment& coreSeg)
+ objpose::ObjectPoseMap
+ Segment::getLatestObjectPoses(const wm::CoreSegment& coreSeg)
{
ObjectPoseMap result;
getLatestObjectPoses(coreSeg, result);
@@ -397,7 +412,8 @@ namespace armarx::armem::server::obj::instance
}
- objpose::ObjectPoseMap Segment::getLatestObjectPoses(const armem::wm::ProviderSegment& provSeg)
+ objpose::ObjectPoseMap
+ Segment::getLatestObjectPoses(const wm::ProviderSegment& provSeg)
{
ObjectPoseMap result;
getLatestObjectPoses(provSeg, result);
@@ -405,7 +421,8 @@ namespace armarx::armem::server::obj::instance
}
- objpose::ObjectPose Segment::getLatestObjectPose(const armem::wm::Entity& entity)
+ objpose::ObjectPose
+ Segment::getLatestObjectPose(const wm::Entity& entity)
{
ObjectPose result;
getLatestObjectPose(entity, result);
@@ -413,18 +430,18 @@ namespace armarx::armem::server::obj::instance
}
- void Segment::getLatestObjectPoses(const armem::wm::CoreSegment& coreSeg, ObjectPoseMap& out)
+ void Segment::getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseMap& out)
{
- for (const auto& [_, provSegment] : coreSeg)
+ coreSeg.forEachProviderSegment([&out](const wm::ProviderSegment & provSegment)
{
getLatestObjectPoses(provSegment, out);
- }
+ });
}
- void Segment::getLatestObjectPoses(const armem::wm::ProviderSegment& provSegment, ObjectPoseMap& out)
+ void Segment::getLatestObjectPoses(const wm::ProviderSegment& provSegment, ObjectPoseMap& out)
{
- for (const auto& [_, entity] : provSegment)
+ provSegment.forEachEntity([&out](const wm::Entity & entity)
{
if (!entity.empty())
{
@@ -440,29 +457,29 @@ namespace armarx::armem::server::obj::instance
}
}
}
- }
+ });
}
- void Segment::getLatestObjectPose(const armem::wm::Entity& entity, ObjectPose& out)
+ void Segment::getLatestObjectPose(const wm::Entity& entity, ObjectPose& out)
{
- for (const armem::wm::EntityInstance& instance : entity.getLatestSnapshot())
+ entity.getLatestSnapshot().forEachInstance([&out](const wm::EntityInstance & instance)
{
arondto::ObjectInstance dto;
dto.fromAron(instance.data());
fromAron(dto, out);
- }
+ });
}
- arondto::ObjectInstance Segment::getLatestInstanceData(const armem::wm::Entity& entity)
+ arondto::ObjectInstance Segment::getLatestInstanceData(const wm::Entity& entity)
{
ARMARX_CHECK_GREATER_EQUAL(entity.size(), 1);
- const armem::wm::EntitySnapshot& snapshot = entity.getLatestSnapshot();
+ const wm::EntitySnapshot& snapshot = entity.getLatestSnapshot();
ARMARX_CHECK_EQUAL(snapshot.size(), 1);
- const armem::wm::EntityInstance& instance = snapshot.getInstance(0);
+ const wm::EntityInstance& instance = snapshot.getInstance(0);
arondto::ObjectInstance data;
data.fromAron(instance.data());
@@ -471,7 +488,8 @@ namespace armarx::armem::server::obj::instance
}
- ::armarx::armem::articulated_object::ArticulatedObjects Segment::getArticulatedObjects()
+ ::armarx::armem::articulated_object::ArticulatedObjects
+ Segment::getArticulatedObjects()
{
objpose::ObjectPoseMap objectPoses = getObjectPoses(IceUtil::Time::now());
@@ -584,7 +602,7 @@ namespace armarx::armem::server::obj::instance
// Find object pose (provider name can be empty).
- armem::wm::Entity* objectEntity = this->findObjectEntity(objectID, input.providerName);
+ wm::Entity* objectEntity = this->findObjectEntity(objectID, input.providerName);
if (!objectEntity || objectEntity->empty())
{
ARMARX_WARNING << "Tried to attach object " << objectID << " to node '" << frameName
@@ -645,7 +663,8 @@ namespace armarx::armem::server::obj::instance
}
- objpose::DetachObjectFromRobotNodeOutput Segment::detachObjectFromRobotNode(
+ objpose::DetachObjectFromRobotNodeOutput
+ Segment::detachObjectFromRobotNode(
const objpose::DetachObjectFromRobotNodeInput& input)
{
const armem::Time now = armem::Time::now();
@@ -657,7 +676,7 @@ namespace armarx::armem::server::obj::instance
{
// Remove from latest pose (if it was cached).
// Find object pose (provider name can be empty).
- armem::wm::Entity* entity = this->findObjectEntity(objectID, input.providerName);
+ wm::Entity* entity = this->findObjectEntity(objectID, input.providerName);
if (entity)
{
const arondto::ObjectInstance data = getLatestInstanceData(*entity);
@@ -668,7 +687,6 @@ namespace armarx::armem::server::obj::instance
// Store non-attached pose in new snapshot.
storeDetachedSnapshot(*entity, data, now, input.commitAttachedPose);
}
-
if (providerName.empty())
{
providerName = data.pose.providerName;
@@ -693,48 +711,26 @@ namespace armarx::armem::server::obj::instance
}
- struct DetachVisitor : public armem::wm::Visitor
- {
- Segment& owner;
- armem::Time now;
- bool commitAttachedPose;
-
- int numDetached = 0;
-
- DetachVisitor(Segment& owner, armem::Time now, bool commitAttachedPose) :
- owner(owner), now(now), commitAttachedPose(commitAttachedPose)
- {
- }
-
- virtual bool visitEnter(armem::wm::Entity& entity) override;
- };
-
-
- bool DetachVisitor::visitEnter(armem::wm::Entity& entity)
- {
- const arondto::ObjectInstance data = owner.getLatestInstanceData(entity);
- if (data.pose.attachmentValid)
- {
- numDetached++;
- // Store non-attached pose in new snapshot.
- owner.storeDetachedSnapshot(entity, data, now, commitAttachedPose);
- }
- return false; // Stop descending.
- }
-
-
- objpose::DetachAllObjectsFromRobotNodesOutput Segment::detachAllObjectsFromRobotNodes(
+ objpose::DetachAllObjectsFromRobotNodesOutput
+ Segment::detachAllObjectsFromRobotNodes(
const objpose::DetachAllObjectsFromRobotNodesInput& input)
{
ARMARX_CHECK_NOT_NULL(coreSegment);
const armem::Time now = armem::Time::now();
- DetachVisitor visitor(*this, now, input.commitAttachedPose);
- visitor.applyTo(*coreSegment);
-
objpose::DetachAllObjectsFromRobotNodesOutput output;
- output.numDetached = visitor.numDetached;
+ output.numDetached = 0;
+ coreSegment->forEachEntity([this, now, &input, &output](wm::Entity & entity)
+ {
+ const arondto::ObjectInstance data = this->getLatestInstanceData(entity);
+ if (data.pose.attachmentValid)
+ {
+ ++output.numDetached;
+ // Store non-attached pose in new snapshot.
+ this->storeDetachedSnapshot(entity, data, now, input.commitAttachedPose);
+ }
+ });
ARMARX_INFO << "Detached all objects (" << output.numDetached << ") from robot nodes.";
@@ -743,9 +739,9 @@ namespace armarx::armem::server::obj::instance
void Segment::storeDetachedSnapshot(
- armem::wm::Entity& entity,
+ wm::Entity& entity,
const arondto::ObjectInstance& data,
- armem::Time now,
+ Time now,
bool commitAttachedPose)
{
armem::EntityUpdate update;
@@ -777,21 +773,29 @@ namespace armarx::armem::server::obj::instance
}
- std::optional<wm::EntityInstance> Segment::findClassInstance(const ObjectID& objectID) const
+ std::optional<wm::EntityInstance>
+ Segment::findClassInstance(const ObjectID& objectID) const
{
const ObjectID classID = { objectID.dataset(), objectID.className() };
try
{
- for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment("Class"))
+ std::optional<wm::EntityInstance> result;
+ iceMemory.workingMemory->getCoreSegment("Class").forEachProviderSegment(
+ [&classID, &result](const wm::ProviderSegment & provSeg)
{
if (provSeg.hasEntity(classID.str()))
{
- return provSeg.getEntity(classID.str()).getLatestSnapshot().getInstance(0);
+ result = provSeg.getEntity(classID.str()).getLatestSnapshot().getInstance(0);
+ return false;
}
- }
+ return true;
+ });
- ARMARX_WARNING << "No provider segment for classID " << classID.str() << " found";
- return std::nullopt;
+ if (not result.has_value())
+ {
+ ARMARX_WARNING << "No provider segment for classID " << classID.str() << " found";
+ }
+ return result;
}
catch (const armem::error::ArMemError& e)
{
@@ -820,7 +824,8 @@ namespace armarx::armem::server::obj::instance
}
- std::optional<armem::obj::SceneSnapshot> Segment::loadScene(const std::string& filename)
+ std::optional<armem::obj::SceneSnapshot>
+ Segment::loadScene(const std::string& filename)
{
if (const std::optional<std::filesystem::path> path = resolveSceneFilename(filename))
{
@@ -833,7 +838,8 @@ namespace armarx::armem::server::obj::instance
}
- std::optional<armem::obj::SceneSnapshot> Segment::loadScene(const std::filesystem::path& path)
+ std::optional<armem::obj::SceneSnapshot>
+ Segment::loadScene(const std::filesystem::path& path)
{
ARMARX_INFO << "Loading scene snapshot from: \n" << path;
nlohmann::json j;
@@ -855,7 +861,8 @@ namespace armarx::armem::server::obj::instance
const std::string Segment::timestampPlaceholder = "%TIMESTAMP";
- std::optional<std::filesystem::path> Segment::resolveSceneFilename(const std::string& _filename)
+ std::optional<std::filesystem::path>
+ Segment::resolveSceneFilename(const std::string& _filename)
{
std::string filename = _filename;
@@ -890,9 +897,7 @@ namespace armarx::armem::server::obj::instance
armem::obj::SceneSnapshot Segment::getSceneSnapshot() const
{
armem::obj::SceneSnapshot scene;
-
- wm::FunctionalVisitor visitor;
- visitor.entityFn = [&scene](wm::Entity & entity)
+ coreSegment->forEachEntity([&scene](wm::Entity & entity)
{
try
{
@@ -912,10 +917,8 @@ namespace armarx::armem::server::obj::instance
{
ARMARX_WARNING_S << e.what();
}
- return false;
- };
+ });
- visitor.applyTo(*coreSegment);
return scene;
}
@@ -973,7 +976,6 @@ namespace armarx::armem::server::obj::instance
if (lockMemory)
{
- std::scoped_lock lock(memoryMutex);
commitSceneSnapshot(snapshot.value(), filename.string());
}
else
@@ -989,9 +991,9 @@ namespace armarx::armem::server::obj::instance
{
using namespace armarx::RemoteGui::Client;
- maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
+ maxHistorySize.setValue(std::max(1, int(data.properties.maxHistorySize)));
maxHistorySize.setRange(1, 1e6);
- infiniteHistory.setValue(data.p.maxHistorySize == -1);
+ infiniteHistory.setValue(data.properties.maxHistorySize == -1);
discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
storeLoadLine.setValue("Scene_" + timestampPlaceholder);
@@ -1026,39 +1028,39 @@ namespace armarx::armem::server::obj::instance
}
- void Segment::RemoteGui::update(Segment& data)
+ void Segment::RemoteGui::update(Segment& segment)
{
if (loadButton.wasClicked())
{
bool lockMemory = true;
- data.commitSceneSnapshotFromFilename(storeLoadLine.getValue(), lockMemory);
+ segment.commitSceneSnapshotFromFilename(storeLoadLine.getValue(), lockMemory);
}
if (storeButton.wasClicked())
{
armem::obj::SceneSnapshot scene;
{
- std::scoped_lock lock(data.memoryMutex);
- scene = data.getSceneSnapshot();
+ std::scoped_lock lock(segment.mutex());
+ scene = segment.getSceneSnapshot();
}
- data.storeScene(storeLoadLine.getValue(), scene);
+ segment.storeScene(storeLoadLine.getValue(), scene);
}
if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
|| discardSnapshotsWhileAttached.hasValueChanged())
{
- std::scoped_lock lock(data.memoryMutex);
+ std::scoped_lock lock(segment.mutex());
if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
{
- data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- if (data.coreSegment)
+ segment.properties.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ if (segment.coreSegment)
{
- data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+ segment.coreSegment->setMaxHistorySize(long(segment.properties.maxHistorySize));
}
}
- data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+ segment.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
}
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index 3ec6a2f22f41569945cc0c0ed3f5fd0a4021241e..27ffbc746af05451b5fdd54f15a3afc4c7822e08 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -8,34 +8,35 @@
#include <SimoxUtility/caching/CacheMap.h>
#include <SimoxUtility/shapes/OrientedBox.h>
-#include <ArmarXCore/core/logging/Logging.h>
-
-#include "RobotAPI/components/ArViz/Client/Client.h"
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
-
-#include "Decay.h"
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
#include "ArticulatedObjectVisu.h"
+#include "Decay.h"
namespace armarx::armem::obj
{
struct SceneSnapshot;
}
+namespace armarx::armem::arondto
+{
+ class ObjectInstance;
+}
namespace armarx::armem::server::obj::instance
{
- class Segment : public armarx::Logging
+ class Segment : public segment::SpecializedSegment
{
public:
@@ -50,14 +51,12 @@ namespace armarx::armem::server::obj::instance
public:
- Segment(server::MemoryToIceAdapter& iceMemory,
- std::mutex& memoryMutex);
+ Segment(server::MemoryToIceAdapter& iceMemory);
+ virtual ~Segment() override;
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
-
- void init();
-
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ void init() override;
void connect(viz::Client arviz);
@@ -179,16 +178,8 @@ namespace armarx::armem::server::obj::instance
private:
- server::MemoryToIceAdapter& iceMemory;
- wm::CoreSegment* coreSegment = nullptr;
- std::mutex& memoryMutex;
-
-
struct Properties
{
- std::string coreSegmentName = "Instance";
- // -1 would be infinite, but this can let the RAM grow quickly.
- long maxHistorySize = 25;
bool discardSnapshotsWhileAttached = true;
/// Package containing the scene snapshots
@@ -209,6 +200,7 @@ namespace armarx::armem::server::obj::instance
static const std::string timestampPlaceholder;
+
public:
struct RemoteGui
@@ -227,7 +219,9 @@ namespace armarx::armem::server::obj::instance
void update(Segment& data);
};
+
private:
+
std::unique_ptr<ArticulatedObjectVisu> visu;
};
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index e0577877f2ac9e9c7e97e8aaa50d99c438fe4c9f..7cd8b1202ac34600f8bfc6ecc912037b49d8bee6 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -37,17 +37,18 @@
namespace armarx::armem::server::obj::instance
{
- SegmentAdapter::SegmentAdapter(MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex) :
- segment(iceMemory, memoryMutex),
- memoryMutex(memoryMutex)
+ SegmentAdapter::SegmentAdapter(MemoryToIceAdapter& iceMemory) :
+ segment(iceMemory)
{
}
+
std::string SegmentAdapter::getName() const
{
return Logging::tag.tagName;
}
+
void SegmentAdapter::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
calibration.defineProperties(defs, prefix + "calibration.");
@@ -56,6 +57,7 @@ namespace armarx::armem::server::obj::instance
visu.defineProperties(defs, prefix + "visu.");
}
+
void SegmentAdapter::init()
{
segment.setTag(getName());
@@ -66,6 +68,7 @@ namespace armarx::armem::server::obj::instance
segment.init();
}
+
void SegmentAdapter::connect(
RobotStateComponentInterfacePrx robotStateComponent,
VirtualRobot::RobotPtr robot,
@@ -97,6 +100,7 @@ namespace armarx::armem::server::obj::instance
segment.connect(arviz);
}
+
void SegmentAdapter::reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, const Ice::Current&)
{
updateProviderInfo(providerName, info);
@@ -120,7 +124,7 @@ namespace armarx::armem::server::obj::instance
return;
}
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
std::stringstream ss;
for (const auto& id : info.supportedObjects)
{
@@ -161,7 +165,7 @@ namespace armarx::armem::server::obj::instance
}
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
RemoteRobot::synchronizeLocalClone(segment.robot, segment.robotStateComponent);
if (segment.robot->hasRobotNode(calibration.robotNode))
@@ -215,7 +219,7 @@ namespace armarx::armem::server::obj::instance
TIMING_START(tGetObjectPoses);
TIMING_START(tGetObjectPosesLock);
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
TIMING_END_STREAM(tGetObjectPosesLock, ARMARX_VERBOSE);
const IceUtil::Time now = TimeUtil::GetTime();
@@ -240,7 +244,7 @@ namespace armarx::armem::server::obj::instance
TIMING_START(GetObjectPoses);
TIMING_START(GetObjectPosesLock);
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
TIMING_END_STREAM(GetObjectPosesLock, ARMARX_VERBOSE);
const IceUtil::Time now = TimeUtil::GetTime();
@@ -292,7 +296,7 @@ namespace armarx::armem::server::obj::instance
}
else
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
for (const auto& objectID : input.request.objectIDs)
{
bool found = true;
@@ -336,27 +340,31 @@ namespace armarx::armem::server::obj::instance
return output;
}
+
objpose::ProviderInfoMap SegmentAdapter::getAvailableProvidersInfo(const Ice::Current&)
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
return segment.providers;
}
+
Ice::StringSeq SegmentAdapter::getAvailableProviderNames(const Ice::Current&)
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
return simox::alg::get_keys(segment.providers);
}
+
objpose::ProviderInfo SegmentAdapter::getProviderInfo(const std::string& providerName, const Ice::Current&)
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
return segment.getProviderInfo(providerName);
}
+
bool SegmentAdapter::hasProvider(const std::string& providerName, const Ice::Current&)
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
return segment.providers.count(providerName) > 0;
}
@@ -364,28 +372,30 @@ namespace armarx::armem::server::obj::instance
objpose::AttachObjectToRobotNodeOutput SegmentAdapter::attachObjectToRobotNode(
const objpose::AttachObjectToRobotNodeInput& input, const Ice::Current&)
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
return segment.attachObjectToRobotNode(input);
}
+
objpose::DetachObjectFromRobotNodeOutput SegmentAdapter::detachObjectFromRobotNode(
const objpose::DetachObjectFromRobotNodeInput& input, const Ice::Current&)
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
return segment.detachObjectFromRobotNode(input);
}
+
objpose::DetachAllObjectsFromRobotNodesOutput SegmentAdapter::detachAllObjectsFromRobotNodes(
const objpose::DetachAllObjectsFromRobotNodesInput& input, const Ice::Current&)
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
return segment.detachAllObjectsFromRobotNodes(input);
}
objpose::AgentFramesSeq SegmentAdapter::getAttachableFrames(const Ice::Current&)
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
objpose::AgentFramesSeq output;
std::vector<VirtualRobot::RobotPtr> agents = { segment.robot };
@@ -398,6 +408,7 @@ namespace armarx::armem::server::obj::instance
return output;
}
+
objpose::SignalHeadMovementOutput
SegmentAdapter::signalHeadMovement(const objpose::SignalHeadMovementInput& input, const Ice::Current&)
{
@@ -423,7 +434,7 @@ namespace armarx::armem::server::obj::instance
objpose::ObjectPoseMap objectPoses;
visu.minConfidence = -1;
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
const IceUtil::Time now = TimeUtil::GetTime();
@@ -493,6 +504,7 @@ namespace armarx::armem::server::obj::instance
group.addChild(layout);
}
+
void SegmentAdapter::RemoteGui::update(SegmentAdapter& adapter)
{
// Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
@@ -502,7 +514,7 @@ namespace armarx::armem::server::obj::instance
}
this->segment.update(adapter.segment);
{
- std::scoped_lock lock(adapter.memoryMutex);
+ std::scoped_lock lock(adapter.segment.mutex());
this->decay.update(adapter.segment.decay);
}
{
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
index a1e356e53c77608937b0df952798d0427a3cc86b..353f2e0644dc044edfd1f260a76eb0c5d7857a11 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
@@ -55,7 +55,7 @@ namespace armarx::armem::server::obj::instance
{
public:
- SegmentAdapter(MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex);
+ SegmentAdapter(MemoryToIceAdapter& iceMemory);
std::string getName() const;
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
@@ -127,7 +127,6 @@ namespace armarx::armem::server::obj::instance
DebugObserverInterfacePrx debugObserver;
instance::Segment segment;
- std::mutex& memoryMutex;
instance::RobotHeadMovement robotHead;
std::mutex robotHeadMutex;
diff --git a/source/RobotAPI/libraries/armem_objects/types.h b/source/RobotAPI/libraries/armem_objects/types.h
index 976f438e7c39009fd5d37fb6afdf19378da488c4..5ca91ff285e5fc92d1d3523a504379d582dda8b5 100644
--- a/source/RobotAPI/libraries/armem_objects/types.h
+++ b/source/RobotAPI/libraries/armem_objects/types.h
@@ -21,13 +21,11 @@
#pragma once
-#include "RobotAPI/libraries/armem/core/Time.h"
-#include <vector>
-
#include <Eigen/Geometry>
-#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_robot/types.h>
namespace armarx::armem::attachment
@@ -97,4 +95,4 @@ namespace armarx::armem::articulated_object
using ArticulatedObject = armarx::armem::robot::Robot;
using ArticulatedObjects = armarx::armem::robot::Robots;
-} // namespace armarx::armem::articulated_object
\ No newline at end of file
+} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp b/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
index 3bf4171486e35c33a1cca23a666baac2233a8356..6ba61bba4b70f7a4cbbdf8cb2d18b11e4b50bbd6 100644
--- a/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
@@ -4,7 +4,7 @@
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-#include "RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h"
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
diff --git a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
index 498a422fb201bf435cfb8ddbfdc43b71ba1944a0..a2e87f027e868fdc33689eee860577c4083c04d5 100644
--- a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
@@ -6,76 +6,96 @@ armarx_set_target("Library: ${LIB_NAME}")
find_package(Eigen3 QUIET)
armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+
armarx_add_library(
LIBS
# ArmarX
ArmarXCore
ArmarXCoreInterfaces
+ DebugObserverHelper
+ # ArmarXGui
ArmarXGuiComponentPlugins
# This package
RobotAPICore
RobotAPIInterfaces
RobotAPI::armem
RobotAPI::armem_robot
+ aroncommon
+
# System / External
Eigen3::Eigen
- aroncommon
+
HEADERS
- ./common/localization/types.h
- ./common/localization/TransformHelper.h
+ common/localization/types.h
+ common/localization/TransformHelper.h
- ./client/common/RobotReader.h
- ./client/common/VirtualRobotReader.h
+ client/common/RobotReader.h
+ client/common/VirtualRobotReader.h
- ./client/localization/interfaces.h
- ./client/localization/TransformReader.h
- ./client/localization/TransformWriter.h
+ client/localization/interfaces.h
+ client/localization/TransformReader.h
+ client/localization/TransformWriter.h
- ./server/common/Visu.h
+ server/common/Visu.h
- ./server/localization/Segment.h
- # ./server/localization/Visu.h
+ server/localization/Segment.h
- ./server/proprioception/Segment.h
- # ./server/proprioception/Visu.h
+ server/proprioception/Segment.h
+ server/proprioception/aron_conversions.h
+ server/proprioception/RobotStateWriter.h
+ server/proprioception/RobotUnitData.h
+ server/proprioception/RobotUnitReader.h
- ./server/description/Segment.h
+ server/proprioception/converters/Armar6Converter.h
+ server/proprioception/converters/ConverterTools.h
+ server/proprioception/converters/ConverterRegistry.h
+ server/proprioception/converters/ConverterInterface.h
+ server/description/Segment.h
+
+ aron_conversions.h
+ utils.h
- ./aron_conversions.h
- ./utils.h
SOURCES
- ./common/localization/TransformHelper.cpp
+ common/localization/TransformHelper.cpp
- ./client/common/RobotReader.cpp
- ./client/common/VirtualRobotReader.cpp
+ client/common/RobotReader.cpp
+ client/common/VirtualRobotReader.cpp
- ./client/localization/TransformReader.cpp
- ./client/localization/TransformWriter.cpp
+ client/localization/TransformReader.cpp
+ client/localization/TransformWriter.cpp
- ./server/common/Visu.cpp
+ server/common/Visu.cpp
- ./server/localization/Segment.cpp
- # ./server/localization/Visu.cpp
+ server/localization/Segment.cpp
- ./server/proprioception/Segment.cpp
- # ./server/proprioception/Visu.cpp
+ server/proprioception/Segment.cpp
+ server/proprioception/aron_conversions.cpp
+ server/proprioception/RobotStateWriter.cpp
+ server/proprioception/RobotUnitData.cpp
+ server/proprioception/RobotUnitReader.cpp
- ./server/description/Segment.cpp
+ server/proprioception/converters/Armar6Converter.cpp
+ server/proprioception/converters/ConverterTools.cpp
+ server/proprioception/converters/ConverterRegistry.cpp
+ server/proprioception/converters/ConverterInterface.cpp
- ./aron_conversions.cpp
- ./utils.cpp
+ server/description/Segment.cpp
+ aron_conversions.cpp
+ utils.cpp
)
armarx_enable_aron_file_generation_for_target(
TARGET_NAME
"${LIB_NAME}"
+
ARON_FILES
+ aron/JointState.xml
+ aron/Proprioception.xml
aron/TransformHeader.xml
aron/Transform.xml
- aron/JointState.xml
)
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
new file mode 100644
index 0000000000000000000000000000000000000000..99900b7b0766613a555fb060fa19fddf987def7f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
@@ -0,0 +1,277 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!--
+(Armar6) Robot proprioception.
+-->
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<Eigen/Core>" />
+ <Include include="<Eigen/Geometry>" />
+ </CodeIncludes>
+
+ <GenerateTypes>
+
+ <Object name="armarx::armem::prop::arondto::Joints">
+
+ <ObjectChild key="position">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="velocity">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="acceleration">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+
+ <ObjectChild key="relativePosition">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="filteredVelocity">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+
+ <ObjectChild key="currentTarget">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+ <ObjectChild key="positionTarget">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="velocityTarget">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+
+ <ObjectChild key="torque">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="inertiaTorque">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="gravityTorque">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="gravityCompensatedTorque">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="inverseDynamicsTorque">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="torqueTicks">
+ <Dict>
+ <Int />
+ </Dict>
+ </ObjectChild>
+
+
+ <ObjectChild key="orientation">
+ <Dict>
+ <EigenMatrix rows="4" cols="1" type="float" />
+ <!--Orientation /-->
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="angularVelocity">
+ <Dict>
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="linearAcceleration">
+ <Dict>
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </Dict>
+ </ObjectChild>
+
+
+ <ObjectChild key="temperature">
+ <Dict>
+ <Dict>
+ <Float />
+ </Dict>
+ </Dict>
+ </ObjectChild>
+
+
+ <ObjectChild key="motorCurrent">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="maxTargetCurrent">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+
+ <ObjectChild key="sensorBoardUpdateRate">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="I2CUpdateRate">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+
+ <ObjectChild key="JointStatusEmergencyStop">
+ <Dict>
+ <Bool />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="JointStatusEnabled">
+ <Dict>
+ <Bool />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="JointStatusError">
+ <Dict>
+ <Int />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="JointStatusOperation">
+ <Dict>
+ <Int />
+ </Dict>
+ </ObjectChild>
+
+ </Object>
+
+
+ <Object name="armarx::armem::prop::arondto::Platform">
+
+ <ObjectChild key="relativePosition">
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </ObjectChild>
+
+ <!-- Global pose is not part of proprioception. -->
+ <!--ObjectChild key="absolutePosition">
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </ObjectChild-->
+
+ <ObjectChild key="velocity">
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </ObjectChild>
+
+ <ObjectChild key="acceleration">
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </ObjectChild>
+
+ <ObjectChild key="extra">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ </Object>
+
+
+ <Object name="armarx::armem::prop::arondto::ForceTorque">
+
+ <ObjectChild key="force">
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </ObjectChild>
+
+ <ObjectChild key="torque">
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </ObjectChild>
+
+ <ObjectChild key="gravityCompensationForce">
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </ObjectChild>
+
+ <ObjectChild key="gravityCompensationTorque">
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </ObjectChild>
+
+ <ObjectChild key="extra">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ </Object>
+
+
+
+ <Object name="armarx::armem::arondto::Proprioception">
+
+ <ObjectChild key="iterationID">
+ <long />
+ </ObjectChild>
+
+ <ObjectChild key="joints">
+ <armarx::armem::prop::arondto::Joints/>
+ </ObjectChild>
+
+ <ObjectChild key="platform">
+ <armarx::armem::prop::arondto::Platform/>
+ </ObjectChild>
+
+ <ObjectChild key="forceTorque">
+ <Dict>
+ <armarx::armem::prop::arondto::ForceTorque/>
+ </Dict>
+ </ObjectChild>
+
+
+ <ObjectChild key="extraFloats">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="extraLongs">
+ <Dict>
+ <Long />
+ </Dict>
+ </ObjectChild>
+
+ </Object>
+
+
+ </GenerateTypes>
+</AronTypeDefinition>
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 7a500d1391c0f0942795b49af65740f350eecbed..85370141f880a72b26039c0af0c765f929e6a902 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -10,7 +10,7 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
@@ -214,30 +214,31 @@ namespace armarx::armem::robot_state
const armem::wm::ProviderSegment& providerSegment = memory
.getCoreSegment(properties.proprioceptionCoreSegment)
.getProviderSegment(name);
- // clang-format on
- const auto entities = simox::alg::get_values(providerSegment.entities());
// TODO entitiesToRobotState()
- if (entities.empty())
+ if (providerSegment.empty())
{
ARMARX_WARNING << "No entity found";
return std::nullopt;
}
- const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+ // clang-format on
- if (entitySnapshots.empty())
+ const armem::wm::EntityInstance* instance = nullptr;
+ providerSegment.forEachInstance([&instance](const wm::EntityInstance & i)
+ {
+ instance = &i;
+ return false; // break
+ });
+ if (!instance)
{
ARMARX_WARNING << "No entity snapshots found";
return std::nullopt;
}
- // TODO(fabian.reister): check if 0 available
- const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
-
// Here, we access the RobotUnit streaming data stored in the proprioception segment.
- return robot::convertRobotState(instance);
+ return robot::convertRobotState(*instance);
}
// FIXME remove this, use armem/util/util.h
@@ -271,24 +272,19 @@ namespace armarx::armem::robot_state
.getCoreSegment(properties.proprioceptionCoreSegment);
// clang-format on
- for (const auto &[_, providerSegment] : coreSegment.providerSegments())
+ coreSegment.forEachEntity([&jointMap](const wm::Entity & entity)
{
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- for (const auto &[name, entity] : providerSegment.entities())
+ const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
+ if (not jointState)
{
- const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
-
- const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
-
- if (not jointState)
- {
- // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
- continue;
- }
-
- jointMap.emplace(jointState->name, jointState->position);
+ // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+ return;
}
- }
+
+ jointMap.emplace(jointState->name, jointState->position);
+ });
if (jointMap.empty())
{
@@ -307,26 +303,19 @@ namespace armarx::armem::robot_state
.getCoreSegment(properties.descriptionCoreSegment)
.getProviderSegment(name); // TODO(fabian.reister): all
// clang-format on
- const auto entities = simox::alg::get_values(providerSegment.entities());
- if (entities.empty())
+ const armem::wm::EntityInstance* instance = nullptr;
+ providerSegment.forEachInstance([&instance](const wm::EntityInstance & i)
{
- ARMARX_WARNING << "No entity found";
- return std::nullopt;
- }
-
- const auto entitySnapshots = simox::alg::get_values(entities.front().history());
-
- if (entitySnapshots.empty())
+ instance = &i;
+ });
+ if (!instance)
{
ARMARX_WARNING << "No entity snapshots found";
return std::nullopt;
}
- // TODO(fabian.reister): check if 0 available
- const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
-
- return robot::convertRobotDescription(instance);
+ return robot::convertRobotDescription(*instance);
}
} // namespace armarx::armem::robot_state
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 13b5572d382059bcff17ce2bb5bfb04cc819b04b..b3f71e59ac2b33c19ff97fd0c238f5540503b6ee 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -82,7 +82,7 @@ namespace armarx::armem::robot_state
struct Properties
{
- std::string memoryName = "RobotStateMemory";
+ std::string memoryName = "RobotState";
std::string descriptionCoreSegment = "Description";
std::string localizationCoreSegment = "Localization";
std::string proprioceptionCoreSegment = "Proprioception";
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
index 6aa026431e7d9efd0128a9007d23918b291b54bc..32da2d6450e67eb228e7d8099123842a4b8f84d9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
@@ -48,9 +48,9 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h>
#include <RobotAPI/libraries/core/FramedPose.h>
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
index 47660fc439e597f4c8f59bbcff666223c5964e18..608dfa3947153985dc4f528bb0746ab95df706fc 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
@@ -37,9 +37,9 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h>
#include <RobotAPI/libraries/core/FramedPose.h>
@@ -63,22 +63,21 @@ namespace armarx::armem::client::robot_state::localization
{
ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
- def->optional(properties.localizationSegment,
- propertyPrefix + "localizationSegment",
+ def->optional(properties.coreSegmentID.memoryName, propertyPrefix + "MemoryName");
+ def->optional(properties.coreSegmentID.coreSegmentName,
+ propertyPrefix + "LocalizationSegmentName",
"Name of the localization memory core segment to use.");
-
- def->optional(properties.memoryName, propertyPrefix + "Memory");
}
void TransformWriter::connect()
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "TransformWriter: Waiting for memory '" << properties.memoryName
+ ARMARX_IMPORTANT << "TransformWriter: Waiting for memory '" << properties.coreSegmentID.memoryName
<< "' ...";
try
{
- memoryWriter = memoryNameSystem.useWriter(properties.memoryName);
- ARMARX_IMPORTANT << "TransformWriter: Connected to memory '" << properties.memoryName << "'";
+ memoryWriter = memoryNameSystem.useWriter(properties.coreSegmentID);
+ ARMARX_IMPORTANT << "TransformWriter: Connected to memory for '" << properties.coreSegmentID << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -91,50 +90,19 @@ namespace armarx::armem::client::robot_state::localization
{
std::lock_guard g{memoryWriterMutex};
- ARMARX_DEBUG << "Trying to create core segment + provider segment";
-
- const auto providerId = [&]() -> std::optional<armem::MemoryID>
- {
- try
- {
- const auto result =
- memoryWriter.addSegment(properties.localizationSegment, transform.header.agent);
-
- if (not result.success)
- {
- ARMARX_WARNING << "Could not obtain provider id! Reason: " << result.errorMessage;
- return std::nullopt;
- }
-
- return armem::MemoryID(result.segmentID);
- }
- catch (...)
- {
- ARMARX_WARNING << "Could not obtain provider id!";
- return std::nullopt;
- }
- }();
-
- if (not providerId)
- {
- return false;
- }
-
+ const MemoryID providerId = properties.coreSegmentID.withProviderSegmentName(transform.header.agent);
// const auto& timestamp = transform.header.timestamp;
- const auto timestamp = IceUtil::Time::now(); // FIXME remove
-
-
- const auto entityID = providerId->withEntityName(transform.header.parentFrame + "," +
- transform.header.frame).withTimestamp(timestamp);
+ const MemoryID entityID = providerId.withEntityName(
+ transform.header.parentFrame + "," + transform.header.frame);
+ const Time timestamp = Time::now(); // FIXME remove
armem::EntityUpdate update;
- update.entityID = entityID;
+ update.entityID = entityID;
+ update.timeCreated = timestamp;
arondto::Transform aronTransform;
toAron(aronTransform, transform);
-
update.instancesData = {aronTransform.toAron()};
- update.timeCreated = timestamp;
ARMARX_DEBUG << "Committing " << update << " at time " << transform.header.timestamp;
armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
@@ -149,10 +117,4 @@ namespace armarx::armem::client::robot_state::localization
return updateResult.success;
}
- // const std::string& TransformWriter::getPropertyPrefix() const
- // {
- // return propertyPrefix;
- // }
-
-
} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
index 299c79b69222accb8290a7064229a9dfe59e8572..e553b073464e5fcf1444e14a4fef48e2034e808c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
@@ -61,6 +61,7 @@ namespace armarx::armem::client::robot_state::localization
bool commitTransform(const ::armarx::armem::robot_state::Transform& transform) override;
+
private:
armem::client::MemoryNameSystem& memoryNameSystem;
@@ -70,8 +71,7 @@ namespace armarx::armem::client::robot_state::localization
// Properties
struct Properties
{
- std::string memoryName = "RobotState";
- std::string localizationSegment = "Localization";
+ MemoryID coreSegmentID { "RobotState", "Localization" };
} properties;
const std::string propertyPrefix = "mem.robot_state.";
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 a59aa5a4c6cdd3ebe552522c0ffd532e873da82a..18b84d7c665c593369b4bd15dd5505d5c5fa88d1 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -2,104 +2,119 @@
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
-#include <SimoxUtility/color/cmaps.h>
#include <SimoxUtility/math/pose/interpolate.h>
-#include "ArmarXCore/core/exceptions/LocalException.h"
-#include "RobotAPI/libraries/core/FramedPose.h"
+#include <ArmarXCore/core/exceptions/LocalException.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
-#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-#include "RobotAPI/libraries/armem/core/error/ArMemError.h"
-#include "RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h"
-#include "RobotAPI/libraries/armem/core/workingmemory/Memory.h"
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/error/ArMemError.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+
namespace armarx::armem::common::robot_state::localization
{
- TransformChainResult TransformHelper::lookupTransformChain(const armem::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query)
+ template <class ...Args>
+ TransformResult
+ TransformHelper::_lookupTransform(
+ const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
+ const TransformQuery& query)
{
- const std::vector<std::string> tfChain =
- buildTransformChain(localizationCoreSegment, query);
-
+ const std::vector<std::string> tfChain = _buildTransformChain(localizationCoreSegment, query);
if (tfChain.empty())
{
- return {.header = query.header,
- .transforms = std::vector<Eigen::Affine3f>{},
- .status = TransformChainResult::Status::ErrorFrameNotAvailable,
+ return {.transform = {.header = query.header},
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
.errorMessage = "Cannot create tf lookup chain '" +
query.header.parentFrame + " -> " + query.header.frame +
"'"};
}
- const std::vector<Eigen::Affine3f> transforms = obtainTransforms(
+ const std::vector<Eigen::Affine3f> transforms = _obtainTransforms(
localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
-
if (transforms.empty())
{
ARMARX_WARNING << deactivateSpam(1) << "No transform available.";
- return {.header = query.header,
- .transforms = {},
- .status = TransformChainResult::Status::ErrorFrameNotAvailable,
+ return {.transform = {.header = query.header},
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
.errorMessage = "Error in TF loookup: '" + query.header.parentFrame +
" -> " + query.header.frame +
"'. No memory data in time range."};
}
+ const Eigen::Affine3f transform = std::accumulate(transforms.begin(),
+ transforms.end(),
+ Eigen::Affine3f::Identity(),
+ std::multiplies<>());
ARMARX_DEBUG << "Found valid transform";
- return {.header = query.header,
- .transforms = transforms,
- .status = TransformChainResult::Status::Success};
+ return {.transform = {.header = query.header, .transform = transform},
+ .status = TransformResult::Status::Success};
}
- TransformResult TransformHelper::lookupTransform(const armem::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query)
- {
- const std::vector<std::string> tfChain =
- buildTransformChain(localizationCoreSegment, query);
+ template <class ...Args>
+ TransformChainResult
+ TransformHelper::_lookupTransformChain(
+ const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
+ const TransformQuery& query)
+ {
+ const std::vector<std::string> tfChain = _buildTransformChain(localizationCoreSegment, query);
if (tfChain.empty())
{
- return {.transform = {.header = query.header},
- .status = TransformResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Cannot create tf lookup chain '" +
- query.header.parentFrame + " -> " + query.header.frame +
- "'"};
+ return
+ {
+ .header = query.header,
+ .transforms = std::vector<Eigen::Affine3f>{},
+ .status = TransformChainResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Cannot create tf lookup chain '" +
+ query.header.parentFrame + " -> " + query.header.frame +
+ "'"
+ };
}
- const std::vector<Eigen::Affine3f> transforms = obtainTransforms(
+ const std::vector<Eigen::Affine3f> transforms = _obtainTransforms(
localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
-
if (transforms.empty())
{
ARMARX_WARNING << deactivateSpam(1) << "No transform available.";
- return {.transform = {.header = query.header},
- .status = TransformResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Error in TF loookup: '" + query.header.parentFrame +
- " -> " + query.header.frame +
- "'. No memory data in time range."};
+ return
+ {
+ .header = query.header,
+ .transforms = {},
+ .status = TransformChainResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Error in TF loookup: '" + query.header.parentFrame +
+ " -> " + query.header.frame +
+ "'. No memory data in time range."
+ };
}
- const Eigen::Affine3f transform = std::accumulate(transforms.begin(),
- transforms.end(),
- Eigen::Affine3f::Identity(),
- std::multiplies<>());
ARMARX_DEBUG << "Found valid transform";
- return {.transform = {.header = query.header, .transform = transform},
- .status = TransformResult::Status::Success};
+ return
+ {
+ .header = query.header,
+ .transforms = transforms,
+ .status = TransformChainResult::Status::Success
+ };
}
+
+
+ template <class ...Args>
std::vector<std::string>
- TransformHelper::buildTransformChain(const armem::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query)
+ TransformHelper::_buildTransformChain(
+ const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
+ const TransformQuery& query)
{
ARMARX_DEBUG << "Building transform chain";
@@ -123,7 +138,7 @@ namespace armarx::armem::common::robot_state::localization
}
else
{
- ARMARX_WARNING << deactivateSpam(1) << "Cannot perform tf lookup '" << parentFrame << " -> " << frame
+ ARMARX_WARNING << deactivateSpam(10) << "Cannot perform tf lookup '" << parentFrame << " -> " << frame
<< "'";
}
};
@@ -172,8 +187,100 @@ namespace armarx::armem::common::robot_state::localization
return chain;
}
- inline ::armarx::armem::robot_state::Transform
- convertEntityToTransform(const armem::wm::EntityInstance& item)
+
+ template <class ...Args>
+ std::vector<Eigen::Affine3f>
+ TransformHelper::_obtainTransforms(
+ const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
+ const std::vector<std::string>& tfChain,
+ const std::string& agent,
+ const armem::Time& timestamp)
+ {
+ const auto& agentProviderSegment = localizationCoreSegment.getProviderSegment(agent);
+
+ ARMARX_DEBUG << "Provider segments" << localizationCoreSegment.getProviderSegmentNames();
+ ARMARX_DEBUG << "Entities: " << agentProviderSegment.getEntityNames();
+
+ try
+ {
+ std::vector<Eigen::Affine3f> transforms;
+ transforms.reserve(tfChain.size());
+ std::transform(tfChain.begin(),
+ tfChain.end(),
+ std::back_inserter(transforms),
+ [&](const std::string & entityName)
+ {
+ return _obtainTransform(entityName, agentProviderSegment, timestamp);
+ });
+ return transforms;
+ }
+ catch (const armem::error::MissingEntry& missingEntryError)
+ {
+ ARMARX_WARNING << missingEntryError.what();
+ }
+ catch (const ::armarx::exceptions::local::ExpressionException& ex)
+ {
+ ARMARX_WARNING << "local exception: " << ex.what();
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Error: " << GetHandledExceptionString();
+ }
+
+ return {};
+ }
+
+
+ template <class ...Args>
+ Eigen::Affine3f
+ TransformHelper::_obtainTransform(
+ const std::string& entityName,
+ const armem::base::ProviderSegmentBase<Args...>& agentProviderSegment,
+ const armem::Time& timestamp)
+ {
+ ARMARX_DEBUG << "getEntity:" + entityName;
+ const auto& entity = agentProviderSegment.getEntity(entityName);
+
+ ARMARX_DEBUG << "History (size: " << entity.size() << "): " << entity.getTimestamps();
+
+ // if (entity.history.empty())
+ // {
+ // // TODO(fabian.reister): fixme boom
+ // ARMARX_ERROR << "No snapshots received.";
+ // return Eigen::Affine3f::Identity();
+ // }
+
+ std::vector<::armarx::armem::robot_state::Transform> transforms;
+ transforms.push_back(_convertEntityToTransform(entity.getLatestSnapshot().getInstance(0)));
+
+ ARMARX_DEBUG << "obtaining transform";
+ if (transforms.size() > 1)
+ {
+ // TODO(fabian.reister): remove
+ return transforms.front().transform;
+
+ ARMARX_DEBUG << "More than one snapshots received: " << transforms.size();
+ const auto p = _interpolateTransform(transforms, timestamp);
+ ARMARX_DEBUG << "Done interpolating transform";
+ return p;
+ }
+
+ // accept this to fail (will raise armem::error::MissingEntry)
+ if (transforms.empty())
+ {
+ ARMARX_DEBUG << "empty transform";
+
+ throw armem::error::MissingEntry("foo", "bar", "foo2", "bar2", 0);
+ }
+
+ ARMARX_DEBUG << "single transform";
+
+ return transforms.front().transform;
+ }
+
+
+ ::armarx::armem::robot_state::Transform
+ TransformHelper::_convertEntityToTransform(const armem::wm::EntityInstance& item)
{
arondto::Transform aronTransform;
aronTransform.fromAron(item.data());
@@ -211,8 +318,9 @@ namespace armarx::armem::common::robot_state::localization
}
Eigen::Affine3f
- interpolateTransform(const std::vector<::armarx::armem::robot_state::Transform>& queue,
- armem::Time timestamp)
+ TransformHelper::_interpolateTransform(
+ const std::vector<::armarx::armem::robot_state::Transform>& queue,
+ armem::Time timestamp)
{
ARMARX_TRACE;
@@ -255,109 +363,33 @@ namespace armarx::armem::common::robot_state::localization
return simox::math::interpolatePose(posePreIt->transform, poseNextIt->transform, static_cast<float>(t));
}
- std::vector<Eigen::Affine3f>
- TransformHelper::obtainTransforms(const armem::wm::CoreSegment& localizationCoreSegment,
- const std::vector<std::string>& tfChain,
- const std::string& agent,
- const armem::Time& timestamp)
- {
- const auto& agentProviderSegment = localizationCoreSegment.getProviderSegment(agent);
-
- ARMARX_DEBUG << "Provider segments"
- << simox::alg::get_keys(localizationCoreSegment.providerSegments());
-
- ARMARX_DEBUG << "Entities: " << simox::alg::get_keys(agentProviderSegment.entities());
-
- try
- {
- std::vector<Eigen::Affine3f> transforms;
- transforms.reserve(tfChain.size());
- std::transform(tfChain.begin(),
- tfChain.end(),
- std::back_inserter(transforms),
- [&](const std::string & entityName)
- {
- return obtainTransform(
- entityName, agentProviderSegment, timestamp);
- });
- return transforms;
- }
- catch (const armem::error::MissingEntry& missingEntryError)
- {
- ARMARX_WARNING << missingEntryError.what();
- }
- catch (const ::armarx::exceptions::local::ExpressionException& ex)
- {
- ARMARX_WARNING << "local exception: " << ex.what();
- }
- catch (...)
- {
- ARMARX_WARNING << "Error: " << GetHandledExceptionString();
- }
-
-
- return {};
+ TransformResult TransformHelper::lookupTransform(
+ const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query)
+ {
+ return _lookupTransform(localizationCoreSegment, query);
}
-
- Eigen::Affine3f
- TransformHelper::obtainTransform(const std::string& entityName,
- const armem::wm::ProviderSegment& agentProviderSegment,
- const armem::Time& timestamp)
+ TransformResult TransformHelper::lookupTransform(
+ const armem::server::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query)
{
+ return _lookupTransform(localizationCoreSegment, query);
+ }
- ARMARX_DEBUG << "getEntity:" + entityName;
- const auto& entity = agentProviderSegment.getEntity(entityName);
-
- ARMARX_DEBUG << "History (size: " << entity.history().size() << ")"
- << simox::alg::get_keys(entity.history());
-
- // if (entity.history.empty())
- // {
- // // TODO(fabian.reister): fixme boom
- // ARMARX_ERROR << "No snapshots received.";
- // return Eigen::Affine3f::Identity();
- // }
-
- std::vector<::armarx::armem::robot_state::Transform> transforms;
- transforms.reserve(entity.history().size());
-
- // const auto entitySnapshots = simox::alg::get_values(entity.history());
-
- const std::vector<wm::EntitySnapshot> entitySnapshots = {entity.getLatestSnapshot()};
-
- std::transform(
- entitySnapshots.begin(),
- entitySnapshots.end(),
- std::back_inserter(transforms),
- [](const auto & entitySnapshot)
- {
- return convertEntityToTransform(entitySnapshot.getInstance(0));
- });
-
- ARMARX_DEBUG << "obtaining transform";
-
- if (transforms.size() > 1)
- {
- // TODO(fabian.reister): remove
- return transforms.front().transform;
-
- ARMARX_DEBUG << "More than one snapshots received: " << transforms.size();
- const auto p = interpolateTransform(transforms, timestamp);
- ARMARX_DEBUG << "Done interpolating transform";
- return p;
- }
-
- // accept this to fail (will raise armem::error::MissingEntry)
- if (transforms.empty())
- {
- ARMARX_DEBUG << "empty transform";
- throw armem::error::MissingEntry("foo", "bar", "foo2", "bar2", 0);
- }
+ TransformChainResult TransformHelper::lookupTransformChain(
+ const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query)
+ {
+ return _lookupTransformChain(localizationCoreSegment, query);
+ }
+ TransformChainResult TransformHelper::lookupTransformChain(
+ const armem::server::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query)
+ {
+ return _lookupTransformChain(localizationCoreSegment, query);
+ }
- ARMARX_DEBUG << "single transform";
- return transforms.front().transform;
- }
} // namespace armarx::armem::common::robot_state::localization
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 cafbedcbca60f859f6fe158190a5985350ddad65..9de1fbd994f5a17c00987ce70d31525db7d9d1dd 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
@@ -26,44 +26,102 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
-namespace armarx::armem::wm
-{
- class CoreSegment;
- class ProviderSegment;
-} // namespace armarx::armem::wm
namespace armarx::armem::common::robot_state::localization
{
using armarx::armem::common::robot_state::localization::TransformQuery;
using armarx::armem::common::robot_state::localization::TransformResult;
+
+
class TransformHelper
{
public:
- static TransformResult
- lookupTransform(const armem::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query);
- static TransformChainResult
- lookupTransformChain(const armem::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query);
+ static
+ TransformResult
+ lookupTransform(
+ const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query);
+
+ static
+ TransformResult
+ lookupTransform(
+ const armem::server::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query);
+
+
+ static
+ TransformChainResult
+ lookupTransformChain(
+ const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query);
+
+ static
+ TransformChainResult
+ lookupTransformChain(
+ const armem::server::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query);
+
private:
- static std::vector<std::string>
- buildTransformChain(const armem::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query);
-
- static std::vector<Eigen::Affine3f>
- obtainTransforms(const armem::wm::CoreSegment& localizationCoreSegment,
- const std::vector<std::string>& tfChain,
- const std::string& agent,
- const armem::Time& timestamp);
-
- static Eigen::Affine3f
- obtainTransform(const std::string& entityName,
- const armem::wm::ProviderSegment& agentProviderSegment,
- const armem::Time& timestamp);
+
+ template <class ...Args>
+ static
+ TransformResult
+ _lookupTransform(
+ const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
+ const TransformQuery& query);
+
+
+ template <class ...Args>
+ static
+ TransformChainResult
+ _lookupTransformChain(
+ const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
+ const TransformQuery& query);
+
+
+ template <class ...Args>
+ static
+ std::vector<std::string>
+ _buildTransformChain(
+ const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
+ const TransformQuery& query);
+
+
+ template <class ...Args>
+ static
+ std::vector<Eigen::Affine3f>
+ _obtainTransforms(
+ const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
+ const std::vector<std::string>& tfChain,
+ const std::string& agent,
+ const armem::Time& timestamp);
+
+
+ template <class ...Args>
+ static
+ Eigen::Affine3f
+ _obtainTransform(
+ const std::string& entityName,
+ const armem::base::ProviderSegmentBase<Args...>& agentProviderSegment,
+ const armem::Time& timestamp);
+
+
+ static
+ Eigen::Affine3f
+ _interpolateTransform(
+ const std::vector<::armarx::armem::robot_state::Transform>& queue,
+ armem::Time timestamp);
+
+ static
+ ::armarx::armem::robot_state::Transform
+ _convertEntityToTransform(
+ const armem::wm::EntityInstance& item);
+
};
-} // namespace armarx::armem::common::robot_state::localization
\ No newline at end of file
+} // namespace armarx::armem::common::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
index 9762ea0339755892ac79ef62dbfcc88353995b00..d6dcddd7d3fd568216a63657bfe6ae5ca1f10f9c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -1,7 +1,8 @@
#include "Visu.h"
-#include <Eigen/src/Geometry/Transform.h>
#include <algorithm>
+#include <exception>
+#include <string>
#include <Eigen/Geometry>
@@ -10,23 +11,29 @@
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/math/pose.h>
-#include "ArmarXCore/core/logging/Logging.h"
-#include "ArmarXCore/core/time/CycleUtil.h"
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include "RobotAPI/components/ArViz/Client/Elements.h"
-#include "RobotAPI/libraries/armem/core/Time.h"
+#include <RobotAPI/components/ArViz/Client/Elements.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include <exception>
-#include <string>
#include "../description/Segment.h"
#include "../localization/Segment.h"
#include "../proprioception/Segment.h"
+
namespace armarx::armem::server::robot_state
{
+ Visu::Visu(const description::Segment& descriptionSegment, const proprioception::Segment& proprioceptionSegment, const localization::Segment& localizationSegment)
+ : descriptionSegment(descriptionSegment),
+ proprioceptionSegment(proprioceptionSegment),
+ localizationSegment(localizationSegment)
+ {}
+
+
void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
defs->optional(
@@ -34,9 +41,31 @@ namespace armarx::armem::server::robot_state
defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
}
- void Visu::visualizeRobots(viz::Layer& layer, const robot::Robots& robots)
+
+ void Visu::init()
{
+ }
+
+
+ void Visu::connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver)
+ {
+ this->arviz = arviz;
+ if (debugObserver)
+ {
+ bool batchMode = true;
+ this->debugObserver = DebugObserverHelper("RobotStateMemory", debugObserver, batchMode);
+ }
+
+ updateTask = new SimpleRunningTask<>([this]()
+ {
+ this->visualizeRun();
+ });
+ updateTask->start();
+ }
+
+ void Visu::visualizeRobots(viz::Layer& layer, const robot::Robots& robots)
+ {
const auto visualizeRobot = [&](const robot::Robot & robot)
{
const auto xmlPath = robot.description.xml.serialize();
@@ -57,6 +86,7 @@ namespace armarx::armem::server::robot_state
std::for_each(robots.begin(), robots.end(), visualizeRobot);
}
+
void Visu::visualizeFrames(
viz::Layer& layerFrames,
const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames)
@@ -69,79 +99,13 @@ namespace armarx::armem::server::robot_state
for (const auto& frame : robotFrames)
{
Eigen::Affine3f from = pose;
+ Eigen::Affine3f to = pose * frame;
- pose = pose * frame; // to
-
- Eigen::Affine3f to = pose;
-
- const auto arr = viz::Arrow(robotName + std::to_string(++i)).fromTo(from.translation(), to.translation());
- layerFrames.add(arr);
+ layerFrames.add(viz::Arrow(robotName + std::to_string(++i)).fromTo(from.translation(), to.translation()));
}
}
}
- void Visu::init() {}
-
- void Visu::connect(const viz::Client& arviz)
- {
- this->arviz = arviz;
-
- updateTask = new SimpleRunningTask<>([this]()
- {
- this->visualizeRun();
- });
- updateTask->start();
- }
-
- // void Visu::RemoteGui::setup(const Visu& visu)
- // {
- // using namespace armarx::RemoteGui::Client;
-
- // enabled.setValue(visu.enabled);
- // inGlobalFrame.setValue(visu.inGlobalFrame);
- // alpha.setRange(0, 1.0);
- // alpha.setValue(visu.alpha);
- // alphaByConfidence.setValue(visu.alphaByConfidence);
- // oobbs.setValue(visu.oobbs);
- // objectFrames.setValue(visu.objectFrames);
- // {
- // float max = 10000;
- // objectFramesScale.setRange(0, max);
- // objectFramesScale.setDecimals(2);
- // objectFramesScale.setSteps(int(10 * max));
- // objectFramesScale.setValue(visu.objectFramesScale);
- // }
-
- // GridLayout grid;
- // int row = 0;
- // grid.add(Label("Enabled"), {row, 0}).add(enabled, {row, 1});
- // row++;
- // grid.add(Label("Global Frame"), {row, 0}).add(inGlobalFrame, {row, 1});
- // row++;
- // grid.add(Label("Alpha"), {row, 0}).add(alpha, {row, 1}, {1, 3});
- // row++;
- // grid.add(Label("Alpha by Confidence"), {row, 0}).add(alphaByConfidence, {row, 1});
- // row++;
- // grid.add(Label("OOBB"), {row, 0}).add(oobbs, {row, 1});
- // row++;
- // grid.add(Label("Object Frames"), {row, 0}).add(objectFrames, {row, 1});
- // grid.add(Label("Scale:"), {row, 2}).add(objectFramesScale, {row, 3});
- // row++;
-
- // group.setLabel("Visualization");
- // group.addChild(grid);
- // }
-
- // void Visu::RemoteGui::update(Visu& visu)
- // {
- // visu.enabled = enabled.getValue();
- // visu.inGlobalFrame = inGlobalFrame.getValue();
- // visu.alpha = alpha.getValue();
- // visu.alphaByConfidence = alphaByConfidence.getValue();
- // visu.oobbs = oobbs.getValue();
- // visu.objectFrames = objectFrames.getValue();
- // visu.objectFramesScale = objectFramesScale.getValue();
- // }
robot::Robots combine(
const std::unordered_map<std::string, robot::RobotDescription>& robotDescriptions,
@@ -191,34 +155,52 @@ namespace armarx::armem::server::robot_state
return robots;
}
+
void Visu::visualizeRun()
{
-
CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
while (updateTask && not updateTask->isStopped())
{
{
// std::scoped_lock lock(visuMutex);
ARMARX_DEBUG << "Update task";
-
if (p.enabled)
{
- TIMING_START(Visu);
+ TIMING_START(tVisuTotal);
// TODO(fabian.reister): use timestamp
-
- const auto timestamp = IceUtil::Time::now();
+ const Time timestamp = Time::now();
try
{
- const auto robotDescriptions =
- descriptionSegment.getRobotDescriptions(timestamp);
+ // Get data.
+ TIMING_START(tVisuGetData);
+
+ TIMING_START(tRobotDescriptions);
+ const description::Segment::RobotDescriptionMap robotDescriptions =
+ descriptionSegment.getRobotDescriptionsLocking(timestamp);
+ TIMING_END_STREAM(tRobotDescriptions, ARMARX_DEBUG);
+
+ TIMING_START(tGlobalRobotPoseMap);
const auto globalRobotPoseMap =
- localizationSegment.getRobotGlobalPoses(timestamp);
+ localizationSegment.getRobotGlobalPosesLocking(timestamp);
+ TIMING_END_STREAM(tGlobalRobotPoseMap, ARMARX_DEBUG);
+
+ TIMING_START(tRobotFramePoses);
+ const auto frames = localizationSegment.getRobotFramePosesLocking(timestamp);
+ TIMING_END_STREAM(tRobotFramePoses, ARMARX_DEBUG);
+
+ TIMING_START(tRobotJointPositionMap);
const auto robotJointPositionMap =
- proprioceptionSegment.getRobotJointPositions(timestamp);
+ proprioceptionSegment.getRobotJointPositionsLocking(
+ timestamp, debugObserver ? &*debugObserver : nullptr);
+ TIMING_END_STREAM(tRobotJointPositionMap, ARMARX_DEBUG);
+
+ TIMING_END_STREAM(tVisuGetData, ARMARX_DEBUG);
- const auto frames = localizationSegment.getRobotFramePoses(timestamp);
+
+ // Build layers.
+ TIMING_START(tVisuBuildLayers);
// we need all 3 informations:
// - robot description
@@ -228,44 +210,55 @@ namespace armarx::armem::server::robot_state
const robot::Robots robots =
combine(robotDescriptions, globalRobotPoseMap, robotJointPositionMap);
- viz::Layer layer = arviz.layer("Robots");
- ARMARX_DEBUG << "visualizing robots";
+ ARMARX_DEBUG << "Visualize robots ...";
+ viz::Layer layer = arviz.layer("Robots");
visualizeRobots(layer, robots);
- ARMARX_DEBUG << "Committing robots";
-
+ ARMARX_DEBUG << "Visualize frames ...";
viz::Layer layerFrames = arviz.layer("Frames");
-
- ARMARX_DEBUG << "visualizing frames";
visualizeFrames(layerFrames, frames);
- ARMARX_DEBUG << "Committing frames";
-
- arviz.commit({layer, layerFrames});
+ TIMING_END_STREAM(tVisuBuildLayers, ARMARX_DEBUG);
- ARMARX_DEBUG << "Done committing";
- TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
+ // Commit layers.
- // if (debugObserver)
- // {
- // debugObserver->setDebugChannel(getName(),
- // {
- // { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
- // });
- // }
+ ARMARX_DEBUG << "Commit visualization ...";
+ TIMING_START(tVisuCommit);
+ arviz.commit({layer, layerFrames});
+ TIMING_END_STREAM(tVisuCommit, ARMARX_DEBUG);
+
+ TIMING_END_STREAM(tVisuTotal, ARMARX_DEBUG);
+
+ if (debugObserver.has_value())
+ {
+ const std::string p = "Visu | ";
+ debugObserver->setDebugObserverDatafield(p + "t Total (ms)", tVisuTotal.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1 Get Data (ms)", tVisuGetData.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.1 Descriptions (ms)", tRobotDescriptions.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.2 Global Pose (ms)", tGlobalRobotPoseMap.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.3 Frames (ms)", tRobotFramePoses.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.4 Joint Positions (ms)", tRobotJointPositionMap.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 2 Build Layers (ms)", tVisuBuildLayers.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 3 Commit (ms)", tVisuCommit.toMilliSecondsDouble());
+ }
}
catch (const std::exception& ex)
{
- ARMARX_WARNING << ex.what();
+ ARMARX_WARNING << "Caught exception while visualizing robots: \n" << ex.what();
continue;
}
catch (...)
{
- ARMARX_WARNING << "Unknown exception";
+ ARMARX_WARNING << "Caught unknown exception while visualizing robots.";
continue;
}
+
+ if (debugObserver.has_value())
+ {
+ debugObserver->sendDebugObserverBatch();
+ }
}
}
cycle.waitForCycleDuration();
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
index 7e4b3814ce3a2c03f4ea8fb38846d4378c3a18c6..95efae41963c63669e4e4bb11b202ba9ef948ec9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -21,10 +21,11 @@
#pragma once
+#include <optional>
+
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
-
-// #include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
@@ -42,12 +43,10 @@ namespace armarx::armem::server::robot_state
{
class Segment;
}
-
namespace proprioception
{
class Segment;
}
-
namespace description
{
class Segment;
@@ -64,17 +63,13 @@ namespace armarx::armem::server::robot_state
Visu(const description::Segment& descriptionSegment,
const proprioception::Segment& proprioceptionSegment,
- const localization::Segment& localizationSegment)
- : descriptionSegment(descriptionSegment),
- proprioceptionSegment(proprioceptionSegment),
- localizationSegment(localizationSegment)
- {}
+ const localization::Segment& localizationSegment);
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
void init();
+ void connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver = nullptr);
- void connect(const viz::Client& arviz);
protected:
@@ -89,7 +84,9 @@ namespace armarx::armem::server::robot_state
private:
+
viz::Client arviz;
+ std::optional<DebugObserverHelper> debugObserver;
const description::Segment& descriptionSegment;
const proprioception::Segment& proprioceptionSegment;
@@ -105,23 +102,6 @@ namespace armarx::armem::server::robot_state
SimpleRunningTask<>::pointer_type updateTask;
void visualizeRun();
- // struct RemoteGui
- // {
- // armarx::RemoteGui::Client::GroupBox group;
-
- // armarx::RemoteGui::Client::CheckBox enabled;
-
- // armarx::RemoteGui::Client::CheckBox inGlobalFrame;
- // armarx::RemoteGui::Client::FloatSlider alpha;
- // armarx::RemoteGui::Client::CheckBox alphaByConfidence;
- // armarx::RemoteGui::Client::CheckBox oobbs;
- // armarx::RemoteGui::Client::CheckBox objectFrames;
- // armarx::RemoteGui::Client::FloatSpinBox objectFramesScale;
-
- // // void setup(const Visu& visu);
- // // void update(Visu& visu);
- // };
-
};
} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index d50d965608f7cf4ad8541467d75442303a293211..8fd1077e58c9c13ca6eeb3b429aacf569c837950 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -1,79 +1,53 @@
#include "Segment.h"
-#include <IceUtil/Time.h>
-#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
-#include <sstream>
-
-#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
-#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/application/properties/PluginAll.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include "ArmarXCore/core/system/ArmarXDataPath.h"
-#include "ArmarXCore/core/system/cmake/CMakePackageFinder.h"
-#include "RobotAPI/libraries/armem_robot/types.h"
-#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-#include <thread>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+
namespace armarx::armem::server::robot_state::description
{
- Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter,
- std::mutex& memoryMutex) :
- iceMemory(memoryToIceAdapter), memoryMutex(memoryMutex)
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ Base(memoryToIceAdapter, "Description")
{
- Logging::setTag("DescriptionSegment");
}
Segment::~Segment() = default;
- void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- defs->optional(p.coreSegment,
- prefix + "seg.description.CoreSegment",
- "Name of the robot description core segment.");
- defs->optional(p.maxHistorySize,
- prefix + "seg.description.MaxHistorySize",
- "Maximal size of object poses history (-1 for infinite).");
- }
-
- void Segment::init()
- {
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
-
- ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
-
- coreSegment = &iceMemory.workingMemory->addCoreSegment(
- p.coreSegment, arondto::RobotDescription::toAronType());
- coreSegment->setMaxHistorySize(p.maxHistorySize);
- }
- void Segment::connect(const viz::Client& arviz, const RobotUnitInterfacePrx& robotUnitPrx)
+ void Segment::onConnect(const RobotUnitInterfacePrx& robotUnitPrx)
{
- // this->visu = std::make_unique<Visu>(arviz, *this);
robotUnit = robotUnitPrx;
// store the robot description linked to the robot unit in this segment
updateRobotDescription();
}
- void Segment::storeRobotDescription(const robot::RobotDescription& robotDescription)
+
+ void Segment::commitRobotDescription(const robot::RobotDescription& robotDescription)
{
const Time now = TimeUtil::GetTime();
- const MemoryID providerID = coreSegment->id().withProviderSegmentName(robotDescription.name);
- coreSegment->addProviderSegment(providerID.providerSegmentName, arondto::RobotDescription::toAronType());
+ const MemoryID providerID = segment->id().withProviderSegmentName(robotDescription.name);
+ segment->addProviderSegment(providerID.providerSegmentName, arondto::RobotDescription::toAronType());
EntityUpdate update;
update.entityID = providerID.withEntityName("description");
@@ -88,139 +62,70 @@ namespace armarx::armem::server::robot_state::description
Commit commit;
commit.updates.push_back(update);
-
- {
- // std::lock_guard g{memoryMutex};
- iceMemory.commit(commit);
- }
-
+ iceMemory.commitLocking(commit);
}
+
void Segment::updateRobotDescription()
{
ARMARX_CHECK_NOT_NULL(robotUnit);
- auto kinematicUnit = robotUnit->getKinematicUnit();
-
- ARMARX_CHECK_NOT_NULL(kinematicUnit);
+ KinematicUnitInterfacePrx kinematicUnit = robotUnit->getKinematicUnit();
+ if (kinematicUnit)
+ {
+ const std::string robotName = kinematicUnit->getRobotName();
+ const std::string robotFilename = kinematicUnit->getRobotFilename();
- const auto robotName = kinematicUnit->getRobotName();
- const auto robotFilename = kinematicUnit->getRobotFilename();
+ const std::vector<std::string> packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+ const std::string package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
- const auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
- const auto package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
+ ARMARX_INFO << "Robot description '" << robotFilename << "' found in package " << package;
- ARMARX_INFO << "Robot description '" << robotFilename << "' found in package " << package;
+ const robot::RobotDescription robotDescription
+ {
+ .name = kinematicUnit->getRobotName(),
+ .xml = {package, kinematicUnit->getRobotFilename()}
+ }; // FIXME
- const robot::RobotDescription robotDescription
+ commitRobotDescription(robotDescription);
+ }
+ else
{
- .name = kinematicUnit->getRobotName(),
- .xml = {package, kinematicUnit->getRobotFilename()}}; // FIXME
+ ARMARX_WARNING << "Robot unit '" << robotUnit->ice_getIdentity().name << "' does not have a kinematic unit."
+ << "\n Cannot commit robot description.";
+ }
+ }
+
- storeRobotDescription(robotDescription);
+ Segment::RobotDescriptionMap Segment::getRobotDescriptionsLocking(const armem::Time& timestamp) const
+ {
+ std::scoped_lock lock(mutex());
+ return getRobotDescriptions(timestamp);
}
+
Segment::RobotDescriptionMap Segment::getRobotDescriptions(const armem::Time& timestamp) const
{
+ ARMARX_CHECK_NOT_NULL(segment);
+ (void) timestamp; // Unused
RobotDescriptionMap robotDescriptions;
-
+ segment->forEachEntity([this, &robotDescriptions](const wm::Entity & entity)
{
- // std::lock_guard g{memoryMutex};
-
- for (const auto &[_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
+ const wm::EntityInstance& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ const auto description = robot::convertRobotDescription(entityInstance);
+ if (description)
{
- for (const auto &[name, entity] : provSeg.entities())
- {
- const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- const auto description = robot::convertRobotDescription(entityInstance);
-
- if (not description)
- {
- ARMARX_WARNING << "Could not convert entity instance to "
- "'RobotDescription'";
- continue;
- }
-
- ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
-
- robotDescriptions.emplace(description->name, *description);
- }
+ ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
+ robotDescriptions.emplace(description->name, *description);
}
- }
-
- ARMARX_INFO << deactivateSpam(10) << "Number of known robot descriptions: " << robotDescriptions.size();
+ else
+ {
+ ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
+ }
+ });
+ ARMARX_INFO << deactivateSpam(60) << "Number of known robot descriptions: " << robotDescriptions.size();
return robotDescriptions;
}
- // std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> Segment::getKnownObjectClasses() const
- // {
- // std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> objects;
-
- // for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
- // {
- // for (const auto& [name, entity] : provSeg.entities())
- // {
- // const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- // const auto description = articulated_object::convertRobotDescription(entityInstance);
-
- // if (not description)
- // {
- // ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
- // continue;
- // }
-
- // ARMARX_INFO << "Key is " << armem::MemoryID(entity.id());
-
- // objects.emplace(armem::MemoryID(entity.id()), *description);
- // }
- // }
-
- // ARMARX_IMPORTANT << "Number of known articulated object classes: " << objects.size();
-
- // return objects;
- // }
-
- // void Segment::RemoteGui::setup(const Segment& data)
- // {
- // using namespace armarx::RemoteGui::Client;
-
- // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
- // maxHistorySize.setRange(1, 1e6);
- // infiniteHistory.setValue(data.p.maxHistorySize == -1);
- // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
-
- // GridLayout grid;
- // int row = 0;
- // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
- // row++;
- // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
- // row++;
- // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
- // row++;
-
- // group.setLabel("Data");
- // group.addChild(grid);
- // }
-
- // void Segment::RemoteGui::update(Segment& data)
- // {
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
- // || discardSnapshotsWhileAttached.hasValueChanged())
- // {
- // std::scoped_lock lock(data.memoryMutex);
-
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
- // {
- // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- // if (data.coreSegment)
- // {
- // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
- // }
- // }
-
- // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
- // }
- // }
-
} // namespace armarx::armem::server::robot_state::description
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
index 2f603cf4961d90342fbd74770a0ef4d0d181c033..c62821c4afacbf6caa87b5540a4c05c778254f25 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
@@ -21,94 +21,53 @@
#pragma once
-#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+// STD / STL
#include <string>
#include <optional>
#include <mutex>
#include <unordered_map>
-#include <ArmarXCore/core/logging/Logging.h>
-#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
-
-// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
-
-#include "RobotAPI/components/ArViz/Client/Client.h"
+// ArmarX
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
-#include "RobotAPI/libraries/armem_objects/types.h"
-
-namespace armarx::armem
-{
- namespace server
- {
- class MemoryToIceAdapter;
- }
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/server/segment/Segment.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
- namespace wm
- {
- class CoreSegment;
- }
-} // namespace armarx::armem
+// Aron
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
namespace armarx::armem::server::robot_state::description
{
- class Visu;
-
- class Segment : public armarx::Logging
+ class Segment : public segment::wm::AronTypedCoreSegmentBase<arondto::RobotDescription>
{
+ using Base = segment::wm::AronTypedCoreSegmentBase<arondto::RobotDescription>;
+
public:
- Segment(server::MemoryToIceAdapter& iceMemory,
- std::mutex& memoryMutex);
- virtual ~Segment();
+ Segment(server::MemoryToIceAdapter& iceMemory);
+ virtual ~Segment() override;
- void connect(const viz::Client& arviz, const RobotUnitInterfacePrx& robotUnitPrx);
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+ void onConnect(const RobotUnitInterfacePrx& robotUnitPrx);
- void init();
/// mapping "robot name" -> "robot description"
using RobotDescriptionMap = std::unordered_map<std::string, robot::RobotDescription>;
RobotDescriptionMap getRobotDescriptions(const armem::Time& timestamp) const;
+ RobotDescriptionMap getRobotDescriptionsLocking(const armem::Time& timestamp) const;
+
private:
- void storeRobotDescription(const robot::RobotDescription& robotDescription);
+ void commitRobotDescription(const robot::RobotDescription& robotDescription);
void updateRobotDescription();
-
- server::MemoryToIceAdapter& iceMemory;
- wm::CoreSegment* coreSegment = nullptr;
- std::mutex& memoryMutex;
-
RobotUnitInterfacePrx robotUnit;
- struct Properties
- {
- std::string coreSegment = "Description";
- int64_t maxHistorySize = -1;
- };
- Properties p;
-
- // std::unique_ptr<Visu> visu;
-
- public:
-
- // struct RemoteGui
- // {
- // armarx::RemoteGui::Client::GroupBox group;
-
- // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
- // armarx::RemoteGui::Client::CheckBox infiniteHistory;
- // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
-
- // void setup(const Segment& data);
- // void update(Segment& data);
- // };
-
};
} // namespace armarx::armem::server::robot_state::description
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
index e0dcf6c2bcd9d86012d98cf5aded6fee585ed866..4eb19d15583fd12493d5dd2a6312c6154931e15f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -1,7 +1,6 @@
#include "Segment.h"
// STL
-
#include <iterator>
#include <sstream>
@@ -12,6 +11,8 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/application/properties/PluginAll.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/TimeUtil.h>
@@ -25,77 +26,60 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
+
namespace armarx::armem::server::robot_state::localization
{
- Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter,
- std::mutex& memoryMutex) :
- iceMemory(memoryToIceAdapter), memoryMutex(memoryMutex)
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ Base(memoryToIceAdapter, "Localization", 1024)
{
- Logging::setTag("LocalizationSegment");
}
+
Segment::~Segment() = default;
- void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void Segment::onConnect()
{
- defs->optional(p.coreSegment,
- prefix + "seg.localization.CoreSegment",
- "Name of the object instance core segment.");
- defs->optional(p.maxHistorySize,
- prefix + "seg.localization.MaxHistorySize",
- "Maximal size of object poses history (-1 for infinite).");
}
- void Segment::init()
- {
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
-
- ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
-
- coreSegment = &iceMemory.workingMemory->addCoreSegment(
- p.coreSegment, arondto::Transform::toAronType());
- coreSegment->setMaxHistorySize(p.maxHistorySize);
- }
- void Segment::connect(viz::Client arviz)
+ Segment::RobotFramePoseMap
+ Segment::getRobotFramePosesLocking(const armem::Time& timestamp) const
{
- // this->visu = std::make_unique<Visu>(arviz, *this);
+ std::scoped_lock lock(mutex());
+ return getRobotFramePoses(timestamp);
}
- std::unordered_map<std::string, std::vector<Eigen::Affine3f>>
- Segment::getRobotFramePoses(const armem::Time& timestamp) const
+
+ Segment::RobotFramePoseMap
+ Segment::getRobotFramePoses(const armem::Time& timestamp) const
{
using common::robot_state::localization::TransformHelper;
using common::robot_state::localization::TransformQuery;
- std::unordered_map<std::string, std::vector<Eigen::Affine3f>> frames;
-
- const auto& localizationCoreSegment =
- iceMemory.workingMemory->getCoreSegment(p.coreSegment);
- const auto knownRobots = simox::alg::get_keys(localizationCoreSegment);
-
- for (const auto& robotName : knownRobots)
+ RobotFramePoseMap frames;
+ for (const std::string& robotName : segment->getProviderSegmentNames())
{
-
- TransformQuery query{.header{.parentFrame = GlobalFrame,
- .frame = "root", // TODO, FIXME
- .agent = robotName,
- .timestamp = timestamp}};
-
- const auto result = TransformHelper::lookupTransformChain(localizationCoreSegment, query);
-
+ TransformQuery query
+ {
+ .header{
+ .parentFrame = GlobalFrame,
+ .frame = "root", // TODO, FIXME
+ .agent = robotName,
+ .timestamp = timestamp
+ }
+ };
+
+ const auto result = TransformHelper::lookupTransformChain(*segment, query);
if (not result)
{
// TODO
@@ -105,144 +89,92 @@ namespace armarx::armem::server::robot_state::localization
frames.emplace(robotName, result.transforms);
}
- ARMARX_INFO << deactivateSpam(10)
+ ARMARX_INFO << deactivateSpam(60)
<< "Number of known robot pose chains: " << frames.size();
return frames;
}
- std::unordered_map<std::string, Eigen::Affine3f>
- Segment::getRobotGlobalPoses(const armem::Time& timestamp) const
+
+ Segment::RobotPoseMap
+ Segment::getRobotGlobalPosesLocking(const armem::Time& timestamp) const
{
+ std::scoped_lock lock(mutex());
+ return getRobotGlobalPoses(timestamp);
+ }
+
+ Segment::RobotPoseMap
+ Segment::getRobotGlobalPoses(const armem::Time& timestamp) const
+ {
using common::robot_state::localization::TransformHelper;
using common::robot_state::localization::TransformQuery;
- std::unordered_map<std::string, Eigen::Affine3f> robotGlobalPoses;
-
- const auto& localizationCoreSegment =
- iceMemory.workingMemory->getCoreSegment(p.coreSegment);
- const auto knownRobots = simox::alg::get_keys(localizationCoreSegment);
-
- for (const auto& robotName : knownRobots)
+ RobotPoseMap robotGlobalPoses;
+ for (const std::string& robotName : segment->getProviderSegmentNames())
{
-
- TransformQuery query{.header{.parentFrame = GlobalFrame,
- .frame = "root", // TODO, FIXME
- .agent = robotName,
- .timestamp = timestamp}};
-
- const auto result = TransformHelper::lookupTransform(localizationCoreSegment, query);
-
- if (not result)
+ TransformQuery query
+ {
+ .header{
+ .parentFrame = GlobalFrame,
+ .frame = "root", // TODO, FIXME
+ .agent = robotName,
+ .timestamp = timestamp
+ }
+ };
+
+ const auto result = TransformHelper::lookupTransform(*segment, query);
+ if (result)
+ {
+ robotGlobalPoses.emplace(robotName, result.transform.transform);
+ }
+ else
{
// TODO
- continue;
}
-
- robotGlobalPoses.emplace(robotName, result.transform.transform);
}
- ARMARX_INFO << deactivateSpam(10)
+ ARMARX_INFO << deactivateSpam(60)
<< "Number of known robot poses: " << robotGlobalPoses.size();
return robotGlobalPoses;
}
- bool Segment::storeTransform(const armarx::armem::robot_state::Transform& transform)
+
+ bool Segment::commitTransform(const armarx::armem::robot_state::Transform& transform)
{
- const auto& timestamp = transform.header.timestamp;
+ Commit commit;
+ commit.add(makeUpdate(transform));
+
+ const armem::CommitResult result = iceMemory.commit(commit);
+ return result.allSuccess();
+ }
- const MemoryID providerID =
- coreSegment->id().withProviderSegmentName(transform.header.agent);
- if (not coreSegment->hasProviderSegment(providerID.providerSegmentName))
- {
- coreSegment->addProviderSegment(providerID.providerSegmentName, arondto::Transform::toAronType());
- }
+ bool Segment::commitTransformLocking(const armarx::armem::robot_state::Transform& transform)
+ {
Commit commit;
+ commit.add(makeUpdate(transform));
+
+ const armem::CommitResult result = iceMemory.commitLocking(commit);
+ return result.allSuccess();
+ }
- EntityUpdate& update = commit.updates.emplace_back();
- update.entityID =
- providerID.withEntityName(transform.header.parentFrame + "," + transform.header.frame);
+
+ EntityUpdate Segment::makeUpdate(const armarx::armem::robot_state::Transform& transform) const
+ {
+ const armem::Time& timestamp = transform.header.timestamp;
+ const MemoryID providerID = segment->id().withProviderSegmentName(transform.header.agent);
+
+ EntityUpdate update;
+ update.entityID = providerID.withEntityName(transform.header.parentFrame + "," + transform.header.frame);
update.timeArrived = update.timeCreated = update.timeSent = timestamp;
arondto::Transform aronTransform;
toAron(aronTransform, transform);
update.instancesData = {aronTransform.toAron()};
- const armem::CommitResult result = iceMemory.commit(commit);
- return result.allSuccess();
+ return update;
}
- // std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> Segment::getKnownObjectClasses() const
- // {
- // std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> objects;
-
- // for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
- // {
- // for (const auto& [name, entity] : provSeg.entities())
- // {
- // const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- // const auto description = articulated_object::convertRobotDescription(entityInstance);
-
- // if (not description)
- // {
- // ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
- // continue;
- // }
-
- // ARMARX_INFO << "Key is " << armem::MemoryID(entity.id());
-
- // objects.emplace(armem::MemoryID(entity.id()), *description);
- // }
- // }
-
- // ARMARX_IMPORTANT << "Number of known articulated object classes: " << objects.size();
-
- // return objects;
- // }
-
- // void Segment::RemoteGui::setup(const Segment& data)
- // {
- // using namespace armarx::RemoteGui::Client;
-
- // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
- // maxHistorySize.setRange(1, 1e6);
- // infiniteHistory.setValue(data.p.maxHistorySize == -1);
- // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
-
- // GridLayout grid;
- // int row = 0;
- // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
- // row++;
- // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
- // row++;
- // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
- // row++;
-
- // group.setLabel("Data");
- // group.addChild(grid);
- // }
-
- // void Segment::RemoteGui::update(Segment& data)
- // {
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
- // || discardSnapshotsWhileAttached.hasValueChanged())
- // {
- // std::scoped_lock lock(data.memoryMutex);
-
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
- // {
- // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- // if (data.coreSegment)
- // {
- // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
- // }
- // }
-
- // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
- // }
- // }
-
} // namespace armarx::armem::server::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
index 307eaae17babf66aa9d0a1e759cbc348cbb05008..31fffb4207e7bfe40a0d3f6ed23b554069076a6a 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
@@ -21,88 +21,62 @@
#pragma once
+// STD / STL
#include <string>
#include <optional>
-#include <mutex>
#include <unordered_map>
-#include <ArmarXCore/core/logging/Logging.h>
-#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+// Eigen
+#include <Eigen/Geometry>
-// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
-
-#include "RobotAPI/components/ArViz/Client/Client.h"
+// ArmarX
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/server/segment/Segment.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
-#include "RobotAPI/libraries/armem_objects/types.h"
+// Aron
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_state/types.h>
namespace armarx::armem
{
- namespace server
- {
- class MemoryToIceAdapter;
- }
-
- namespace wm
- {
- class CoreSegment;
- }
+ struct EntityUpdate;
} // namespace armarx::armem
-
namespace armarx::armem::server::robot_state::localization
{
- class Visu;
-
- class Segment : public armarx::Logging
+ class Segment : public segment::wm::AronTypedCoreSegmentBase<arondto::Transform>
{
- public:
- Segment(server::MemoryToIceAdapter& iceMemory,
- std::mutex& memoryMutex);
+ using Base = segment::wm::AronTypedCoreSegmentBase<arondto::Transform>;
- virtual ~Segment();
+ public:
- void connect(viz::Client arviz);
+ using RobotPoseMap = std::unordered_map<std::string, Eigen::Affine3f>;
+ using RobotFramePoseMap = std::unordered_map<std::string, std::vector<Eigen::Affine3f>>;
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
- void init();
+ public:
- std::unordered_map<std::string, Eigen::Affine3f> getRobotGlobalPoses(const armem::Time& timestamp) const;
- std::unordered_map<std::string, std::vector<Eigen::Affine3f>> getRobotFramePoses(const armem::Time& timestamp) const;
+ Segment(server::MemoryToIceAdapter& iceMemory);
+ virtual ~Segment() override;
- bool storeTransform(const armarx::armem::robot_state::Transform& transform);
+ void onConnect();
- private:
+ RobotPoseMap getRobotGlobalPoses(const armem::Time& timestamp) const;
+ RobotPoseMap getRobotGlobalPosesLocking(const armem::Time& timestamp) const;
- server::MemoryToIceAdapter& iceMemory;
- wm::CoreSegment* coreSegment = nullptr;
- std::mutex& memoryMutex;
+ RobotFramePoseMap getRobotFramePoses(const armem::Time& timestamp) const;
+ RobotFramePoseMap getRobotFramePosesLocking(const armem::Time& timestamp) const;
- struct Properties
- {
- std::string coreSegment = "Localization";
- int64_t maxHistorySize = -1;
- };
- Properties p;
+ bool commitTransform(const armarx::armem::robot_state::Transform& transform);
+ bool commitTransformLocking(const armarx::armem::robot_state::Transform& transform);
- // std::unique_ptr<Visu> visu;
- public:
-
- // struct RemoteGui
- // {
- // armarx::RemoteGui::Client::GroupBox group;
+ private:
- // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
- // armarx::RemoteGui::Client::CheckBox infiniteHistory;
- // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+ EntityUpdate makeUpdate(const armarx::armem::robot_state::Transform& transform) const;
- // void setup(const Segment& data);
- // void update(Segment& data);
- // };
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fd8ace277ea412ae1ac2553e043ba515040e6ec5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
@@ -0,0 +1,192 @@
+/*
+ * 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::ArmarXObjects::RobotSensorMemory
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "RobotStateWriter.h"
+
+// STL
+#include <chrono>
+
+// Simox
+#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
+
+// ArmarX
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
+
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+
+ void RobotStateWriter::connect(armarx::plugins::DebugObserverComponentPlugin& debugObserver)
+ {
+ // Thread-local copy of debug observer helper.
+ this->debugObserver =
+ DebugObserverHelper(Logging::tag.tagName, debugObserver.getDebugObserver(), true);
+ }
+
+
+ static float toDurationMs(
+ std::chrono::high_resolution_clock::time_point start,
+ std::chrono::high_resolution_clock::time_point end)
+ {
+ auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+ return duration.count() / 1000.f;
+ }
+
+
+ void RobotStateWriter::run(
+ float pollFrequency,
+ std::queue<RobotUnitData>& dataQueue,
+ std::mutex& dataMutex,
+ MemoryToIceAdapter& memory,
+ localization::Segment& localizationSegment)
+ {
+ CycleUtil cycle(static_cast<int>(1000.f / pollFrequency));
+ while (task and not task->isStopped())
+ {
+ size_t queueSize = 0;
+ std::queue<RobotUnitData> batch;
+ {
+ std::lock_guard lock{dataMutex};
+ queueSize = dataQueue.size();
+ if (dataQueue.size() >= properties.memoryBatchSize)
+ {
+ std::swap(batch, dataQueue);
+ }
+ }
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | Queue Size", queueSize);
+ }
+ if (not batch.empty())
+ {
+ Update update = buildUpdate(batch);
+
+ auto start = std::chrono::high_resolution_clock::now();
+ auto endProprioception = start, endLocalization = start;
+
+ armem::CommitResult result;
+ {
+ // Commits lock the core segments.
+
+ result = memory.commitLocking(update.proprioception);
+ endProprioception = std::chrono::high_resolution_clock::now();
+
+ localizationSegment.commitTransformLocking(update.localization);
+ endLocalization = std::chrono::high_resolution_clock::now();
+ }
+ if (not result.allSuccess())
+ {
+ ARMARX_WARNING << "Could not add data to memory. Error message: " << result.allErrorMessages();
+ }
+ if (debugObserver)
+ {
+ auto end = std::chrono::high_resolution_clock::now();
+
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit [ms]", toDurationMs(start, end));
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit Proprioception [ms]", toDurationMs(start, endProprioception));
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit Localization [ms]", toDurationMs(endProprioception, endLocalization));
+ }
+ }
+
+ if (debugObserver)
+ {
+ debugObserver->sendDebugObserverBatch();
+ }
+ cycle.waitForCycleDuration();
+ }
+ }
+
+
+ RobotStateWriter::Update RobotStateWriter::buildUpdate(std::queue<RobotUnitData>& dataQueue)
+ {
+ ARMARX_CHECK_GREATER(dataQueue.size(), 0);
+
+ ARMARX_DEBUG << "RobotStateWriter: Commit batch of " << dataQueue.size() << " timesteps to memory...";
+ auto start = std::chrono::high_resolution_clock::now();
+
+ // Send batch to memory
+ Update update;
+
+ while (dataQueue.size() > 0)
+ {
+ const RobotUnitData& data = dataQueue.front();
+
+ armem::EntityUpdate& up = update.proprioception.add();
+ up.entityID = properties.robotUnitProviderID.withEntityName(properties.robotUnitProviderID.providerSegmentName);
+ up.timeCreated = data.timestamp;
+ up.instancesData = { data.proprioception };
+
+ // odometry pose -> localization segment
+ if (data.proprioception->hasElement("platform"))
+ {
+ ARMARX_DEBUG << "Found odometry data.";
+ prop::arondto::Platform platform;
+ platform.fromAron(aron::datanavigator::DictNavigator::DynamicCastAndCheck(data.proprioception->getElement("platform")));
+ const Eigen::Vector3f& relPose = platform.relativePosition;
+
+ Eigen::Affine3f odometryPose = Eigen::Affine3f::Identity();
+ odometryPose.translation() << relPose.x(), relPose.y(), 0; // TODO set height
+ odometryPose.linear() = simox::math::rpy_to_mat3f(0.F, 0.F, relPose.z());
+
+ armem::robot_state::Transform& transform = update.localization;
+ transform.header.parentFrame = armarx::OdometryFrame;
+ transform.header.frame = "root"; // TODO: robot root node
+ transform.header.agent = properties.robotUnitProviderID.providerSegmentName;
+ transform.header.timestamp = data.timestamp;
+ transform.transform = odometryPose;
+ }
+ // ToDo: Detect that now odometry data was send
+ /*
+ else if (!noOdometryDataLogged)
+ {
+ ARMARX_INFO << "No odometry data! (No group with name '" << properties.platformGroupName << "'.). \n"
+ << "If you are using a mobile platform this should not have happened.";
+ noOdometryDataLogged = true;
+ }
+ */
+
+ dataQueue.pop();
+ }
+
+ auto stop = std::chrono::high_resolution_clock::now();
+ auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
+ ARMARX_DEBUG << "RobotUnitData: The total runtime of sending a batch to the memory is: " << duration;
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | t Build Update [ms]", duration.count() / 1000.f);
+ }
+
+ return update;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
new file mode 100644
index 0000000000000000000000000000000000000000..3f8d33ef8b0c2b18f5515ad130579ec973722877
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
@@ -0,0 +1,97 @@
+/*
+ * 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::ArmarXObjects::RobotSensorMemory
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+#include <optional>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
+
+#include <RobotAPI/libraries/armem/core/Commit.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
+
+#include "RobotUnitData.h"
+
+
+namespace armarx::plugins
+{
+ class DebugObserverComponentPlugin;
+}
+namespace armarx::armem::server
+{
+ class MemoryToIceAdapter;
+}
+namespace armarx::armem::server::robot_state::localization
+{
+ class Segment;
+}
+namespace armarx::armem::server::robot_state::proprioception
+{
+ class RobotStateWriter : public armarx::Logging
+ {
+ public:
+
+ void connect(armarx::plugins::DebugObserverComponentPlugin& debugObserver);
+
+ /// Reads data from `dataQueue` and commits to the memory.
+ void run(float pollFrequency,
+ std::queue<RobotUnitData>& dataQueue, std::mutex& dataMutex,
+ MemoryToIceAdapter& memory,
+ localization::Segment& localizationSegment
+ );
+
+
+ struct Update
+ {
+ armem::Commit proprioception;
+ armem::robot_state::Transform localization;
+ };
+ Update buildUpdate(std::queue<RobotUnitData>& dataQueue);
+
+
+ public:
+
+ struct Properties
+ {
+ unsigned int memoryBatchSize = 50;
+ armem::MemoryID robotUnitProviderID;
+ std::string platformGroupName = "sens.Platform";
+ };
+ Properties properties;
+
+ std::optional<DebugObserverHelper> debugObserver;
+
+
+ armarx::SimpleRunningTask<>::pointer_type task = nullptr;
+
+
+ private:
+
+ bool noOdometryDataLogged = false;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4262bf76e634ff03445a7460cd4fd55a4df861d9
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.cpp
@@ -0,0 +1,9 @@
+#include "RobotUnitData.h"
+
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
new file mode 100644
index 0000000000000000000000000000000000000000..4045742af416c96ad2d101d98f4559d8cef1dce9
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
@@ -0,0 +1,22 @@
+#pragma once
+
+#include <memory>
+
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+
+namespace armarx::aron::datanavigator
+{
+ using DictNavigatorPtr = std::shared_ptr<class DictNavigator>;
+}
+namespace armarx::armem::server::robot_state::proprioception
+{
+
+ struct RobotUnitData
+ {
+ Time timestamp;
+ aron::datanavigator::DictNavigatorPtr proprioception;
+ };
+
+}
+
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f4b79b65ffd31a41ef2f0ea9bc193bbf3e1ec12e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
@@ -0,0 +1,128 @@
+#include "RobotUnitReader.h"
+
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.h>
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/primitive/Long.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/primitive/String.h>
+
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
+#include <istream>
+#include <filesystem>
+#include <fstream>
+
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+
+ RobotUnitReader::RobotUnitReader()
+ {
+ }
+
+
+ void RobotUnitReader::connect(
+ armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
+ armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
+ const std::string& robotTypeName)
+ {
+ {
+ converter = converterRegistry.get(robotTypeName);
+ ARMARX_CHECK_NOT_NULL(converter)
+ << "No converter for robot type '" << robotTypeName << "' available. \n"
+ << "Known are: " << converterRegistry.getKeys();
+
+ config.loggingNames.push_back(properties.sensorPrefix);
+ receiver = robotUnitPlugin.startDataSatreming(config);
+ description = receiver->getDataDescription();
+ }
+ {
+ // Thread-local copy of debug observer helper.
+ debugObserver =
+ DebugObserverHelper(Logging::tag.tagName, debugObserverPlugin.getDebugObserver(), true);
+ }
+ }
+
+
+ void RobotUnitReader::run(
+ float pollFrequency,
+ std::queue<RobotUnitData>& dataQueue,
+ std::mutex& dataMutex)
+ {
+ CycleUtil cycle(static_cast<int>(1000.f / pollFrequency));
+ while (task and not task->isStopped())
+ {
+ if (std::optional<RobotUnitData> commit = fetchAndConvertLatestRobotUnitData())
+ {
+ std::lock_guard g{dataMutex};
+ dataQueue.push(std::move(commit.value()));
+ }
+
+ if (debugObserver)
+ {
+ debugObserver->sendDebugObserverBatch();
+ }
+ cycle.waitForCycleDuration();
+ }
+ }
+
+
+ std::optional<RobotUnitData> RobotUnitReader::fetchAndConvertLatestRobotUnitData()
+ {
+ ARMARX_CHECK_NOT_NULL(converter);
+
+ const std::optional<RobotUnitDataStreaming::TimeStep> data = fetchLatestData();
+ if (not data.has_value())
+ {
+ return std::nullopt;
+ }
+
+ ARMARX_DEBUG << "RobotUnitReader: Converting data current timestep to commit";
+ auto start = std::chrono::high_resolution_clock::now();
+
+ RobotUnitData result;
+ result.proprioception = converter->convert(data.value(), description);
+ result.timestamp = Time::microSeconds(data->timestampUSec);;
+
+ auto stop = std::chrono::high_resolution_clock::now();
+ auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
+ ARMARX_DEBUG << "RobotUnitReader: The total time needed to convert the data to commit s: " << duration;
+
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield("RobotUnitReader | t Read+Group [ms]", duration.count() / 1000.f);
+ }
+
+ return result;
+ }
+
+
+ std::optional<RobotUnitDataStreaming::TimeStep> RobotUnitReader::fetchLatestData()
+ {
+ std::deque<RobotUnitDataStreaming::TimeStep>& data = receiver->getDataBuffer();
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield("RobotUnitReader | Buffer Size", data.size());
+ }
+ if (data.empty())
+ {
+ return std::nullopt;
+ }
+ else
+ {
+ RobotUnitDataStreaming::TimeStep currentTimestep = data.back();
+ data.clear();
+ return currentTimestep;
+ }
+ }
+
+
+}
+
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
new file mode 100644
index 0000000000000000000000000000000000000000..efb7a0d8175de36ebf963fd035d176442df822f2
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
@@ -0,0 +1,84 @@
+#pragma once
+
+#include <queue>
+#include <map>
+#include <memory>
+#include <optional>
+#include <string>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
+
+#include "RobotUnitData.h"
+#include "converters/ConverterRegistry.h"
+#include "converters/ConverterInterface.h"
+
+
+namespace armarx::plugins
+{
+ class RobotUnitComponentPlugin;
+ class DebugObserverComponentPlugin;
+}
+namespace armarx
+{
+ using RobotUnitDataStreamingReceiverPtr = std::shared_ptr<class RobotUnitDataStreamingReceiver>;
+}
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+ class RobotUnitReader : public armarx::Logging
+ {
+ public:
+
+ RobotUnitReader();
+
+
+ void connect(
+ armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
+ armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
+ const std::string& robotTypeName);
+
+
+
+ /// Reads data from `handler` and fills `dataQueue`.
+ void run(float pollFrequency,
+ std::queue<RobotUnitData>& dataQueue,
+ std::mutex& dataMutex);
+
+ std::optional<RobotUnitData> fetchAndConvertLatestRobotUnitData();
+
+
+ private:
+
+ /// Fetch the latest timestep and clear the robot unit buffer.
+ std::optional<RobotUnitDataStreaming::TimeStep> fetchLatestData();
+
+
+ public:
+
+ struct Properties
+ {
+ std::string sensorPrefix = "sens.*";
+ };
+ Properties properties;
+
+ std::optional<DebugObserverHelper> debugObserver;
+
+
+ RobotUnitDataStreaming::Config config;
+ RobotUnitDataStreamingReceiverPtr receiver;
+ RobotUnitDataStreaming::DataStreamingDescription description;
+
+ ConverterRegistry converterRegistry;
+ ConverterInterface* converter = nullptr;
+
+ armarx::SimpleRunningTask<>::pointer_type task = nullptr;
+
+ };
+
+}
+
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
index ac5e5e1c36e66d47251700ec6565a9fb61ce5430..a63610cf6f7727feaebf60ca2139ab09c0d6e6af 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -1,176 +1,172 @@
#include "Segment.h"
-#include <mutex>
-#include <sstream>
-
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
-#include "RobotAPI/libraries/armem_robot_state/types.h"
-#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem/util/util.h>
-#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-
-#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
namespace armarx::armem::server::robot_state::proprioception
{
- Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
- iceMemory(memoryToIceAdapter),
- memoryMutex(memoryMutex)
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ Base(memoryToIceAdapter, "Proprioception", arondto::Proprioception::toAronType(), 1024)
{
- Logging::setTag("ProprioceptionSegment");
}
Segment::~Segment() = default;
- void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- defs->optional(p.coreSegment, prefix + "seg.proprioception.CoreSegment", "Name of the object instance core segment.");
- defs->optional(p.maxHistorySize, prefix + "seg.proprioception.MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
- }
- void Segment::init()
+ void Segment::onConnect(RobotUnitInterfacePrx robotUnitPrx)
{
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+ this->robotUnit = robotUnitPrx;
- ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegment);
- coreSegment->setMaxHistorySize(p.maxHistorySize);
+ std::string providerSegmentName = "Robot";
+ KinematicUnitInterfacePrx kinematicUnit = robotUnit->getKinematicUnit();
+ if (kinematicUnit)
+ {
+ providerSegmentName = kinematicUnit->getRobotName();
+ }
+ else
+ {
+ ARMARX_WARNING << "Robot unit '" << robotUnit->ice_getIdentity().name << "' does not have a kinematic unit."
+ << "\n Falling back to provider segment name '" << providerSegmentName << "'.";
+ }
+ this->robotUnitProviderID = segment->id().withProviderSegmentName(providerSegmentName);
}
- void Segment::connect(viz::Client arviz, RobotUnitInterfacePrx robotUnitPrx)
- {
- robotUnit = robotUnitPrx;
- auto kinematicUnit = robotUnit->getKinematicUnit();
- const auto providerSegmentName = kinematicUnit->getRobotName();
- // TODO what is the purpose?
- auto encoderEntryType = std::make_shared<aron::typenavigator::ObjectNavigator>("RobotUnitEncoderEntry");
- auto encoderNameType = std::make_shared<aron::typenavigator::StringNavigator>();
- auto encoderIterationIDType = std::make_shared<aron::typenavigator::LongNavigator>();
- encoderEntryType->addMemberType("EncoderGroupName", encoderNameType);
- encoderEntryType->addMemberType("IterationId", encoderIterationIDType);
- //auto encoderValueType = std::make_shared<aron::typenavigator::AnyType>();
- //encoderEntryType->addMemberType("value", encoderValueType);
+ Segment::RobotJointPositionMap Segment::getRobotJointPositionsLocking(
+ const armem::Time& timestamp,
+ DebugObserverHelper* debugObserver) const
+ {
+ TIMING_START(tRobotJointPositionsLock);
+ std::scoped_lock lock(mutex());
+ TIMING_END_STREAM(tRobotJointPositionsLock, ARMARX_DEBUG);
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield(dp + "t 0 Lock Core Segment (ms)", tRobotJointPositionsLock.toMilliSecondsDouble());
+ }
+ return getRobotJointPositions(timestamp, debugObserver);
+ }
- ARMARX_INFO << "Adding provider segment " << p.coreSegment << "/" << providerSegmentName;
- armem::data::AddSegmentInput input;
- input.coreSegmentName = p.coreSegment;
- input.providerSegmentName = providerSegmentName;
+ static
+ aron::datanavigator::DictNavigatorPtr
+ getDictElement(const aron::datanavigator::DictNavigator& dict, const std::string& key)
+ {
+ if (dict.hasElement(key))
{
- std::lock_guard g{memoryMutex};
- auto result = iceMemory.addSegments({input})[0];
-
- if (!result.success)
- {
- ARMARX_ERROR << "Could not add segment " << p.coreSegment << "/" << providerSegmentName << ". The error message is: " << result.errorMessage;
- }
+ return aron::datanavigator::DictNavigator::DynamicCastAndCheck(dict.getElement(key));
}
-
- robotUnitProviderID.memoryName = iceMemory.workingMemory->id().memoryName;
- robotUnitProviderID.coreSegmentName = p.coreSegment;
- robotUnitProviderID.providerSegmentName = providerSegmentName;
+ return nullptr;
}
- std::unordered_map<std::string, std::map<std::string, float>> Segment::getRobotJointPositions(const armem::Time& timestamp) const
+
+ Segment::RobotJointPositionMap Segment::getRobotJointPositions(
+ const armem::Time& timestamp,
+ DebugObserverHelper* debugObserver) const
{
- std::unordered_map<std::string, std::map<std::string, float>> jointMap;
+ namespace adn = aron::datanavigator;
+ ARMARX_CHECK_NOT_NULL(segment);
- std::lock_guard g{memoryMutex};
+ RobotJointPositionMap jointMap;
- for (const auto& [robotName, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
+ TIMING_START(tProcessProviders);
+ segment->forEachProviderSegment([&](const wm::ProviderSegment & provSeg)
{
- for (const auto& [name, entity] : provSeg.entities())
+ TIMING_START(tProcessEntities);
+ int i = 0;
+
+ std::map<std::string, float>& robotJointMap = jointMap[provSeg.name()];
+ provSeg.forEachEntity([&](const wm::Entity & entity)
{
- try
+ TIMING_START(tProcessEntity);
+
+ TIMING_START(tGetLatestInstance);
+ if (entity.empty())
+ {
+ return true;
+ }
+ const wm::EntitySnapshot& snapshot = entity.getLatestSnapshot();
+ if (snapshot.empty())
{
- const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- const auto jointState = tryCast<armarx::armem::arondto::JointState>(entityInstance);
+ return true;
+ }
+ const wm::EntityInstance& entityInstance = snapshot.getInstance(0);
+ TIMING_END_COMMENT_STREAM(tGetLatestInstance, "tGetLatestInstance " + std::to_string(i), ARMARX_DEBUG);
- if (not jointState)
+ TIMING_START(tCastAndEmplaceJointState);
+ // Only cast the joint angles while avoiding throwing an exception.
+ adn::DictNavigatorPtr joints = getDictElement(*entityInstance.data(), "joints");
+ if (joints)
+ {
+ adn::DictNavigatorPtr jointPositions = getDictElement(*joints, "position");
+ if (jointPositions)
{
- // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
- continue;
+ for (const auto& [name, value] : jointPositions->getElements())
+ {
+ const float jointPosition = adn::FloatNavigator::DynamicCastAndCheck(value)->getValue();
+ robotJointMap.emplace(name, jointPosition);
+ }
}
- jointMap[robotName].emplace(jointState->name, jointState->position);
-
}
- catch (...) // empty history etc
- {
+ TIMING_END_COMMENT_STREAM(tCastAndEmplaceJointState, "tCastAndEmplaceJointState " + std::to_string(i), ARMARX_DEBUG);
+
+ TIMING_END_COMMENT_STREAM(tProcessEntity, "tProcessEntity " + std::to_string(i), ARMARX_DEBUG);
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1.1.1 GetLatestInstance (ms)", tGetLatestInstance.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1.1.2 CastAndEmplaceJointState (ms)", tCastAndEmplaceJointState.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1.1.T Process Entity (ms)", tProcessEntity.toMilliSecondsDouble());
}
+ ++i;
+
+ return true;
+ });
+
+ TIMING_END_STREAM(tProcessEntities, ARMARX_DEBUG);
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1 Process Entities (ms)", tProcessEntities.toMilliSecondsDouble());
}
- }
+ });
+ TIMING_END_STREAM(tProcessProviders, ARMARX_DEBUG);
- ARMARX_INFO << deactivateSpam(10) << "Number of known robot joint maps: " << jointMap.size();
+ ARMARX_INFO << deactivateSpam(60) << "Number of known robot joint maps: " << jointMap.size();
+
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield(dp + "t 1 Process Providers (ms)", tProcessProviders.toMilliSecondsDouble());
+ }
return jointMap;
}
+
const armem::MemoryID& Segment::getRobotUnitProviderID() const
{
return robotUnitProviderID;
}
-
-
- // void Segment::RemoteGui::setup(const Segment& data)
- // {
- // using namespace armarx::RemoteGui::Client;
-
- // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
- // maxHistorySize.setRange(1, 1e6);
- // infiniteHistory.setValue(data.p.maxHistorySize == -1);
- // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
-
- // GridLayout grid;
- // int row = 0;
- // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
- // row++;
- // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
- // row++;
- // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
- // row++;
-
- // group.setLabel("Data");
- // group.addChild(grid);
- // }
-
- // void Segment::RemoteGui::update(Segment& data)
- // {
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
- // || discardSnapshotsWhileAttached.hasValueChanged())
- // {
- // std::scoped_lock lock(data.memoryMutex);
-
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
- // {
- // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- // if (data.coreSegment)
- // {
- // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
- // }
- // }
-
- // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
- // }
- // }
-
} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
index bc83a946bcadafbc85a15d55cff79c3a18af970f..c1c3e44a7251d45bdccc384ad266d7bf518caaa2 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
@@ -21,89 +21,58 @@
#pragma once
-#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-#include <string>
+// STD / STL
+#include <map>
#include <optional>
-#include <mutex>
+#include <string>
#include <unordered_map>
-#include <ArmarXCore/core/logging/Logging.h>
-#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
-
-// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+// ArmarX
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-#include "RobotAPI/components/ArViz/Client/Client.h"
+// RobotAPI
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/server/segment/Segment.h>
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
-#include "RobotAPI/libraries/armem_objects/types.h"
-namespace armarx::armem
+namespace armarx
{
- namespace server
- {
- class MemoryToIceAdapter;
- }
-
- namespace wm
- {
- class CoreSegment;
- }
-} // namespace armarx::armem
-
-
+ class DebugObserverHelper;
+}
namespace armarx::armem::server::robot_state::proprioception
{
- class Visu;
-
- class Segment : public armarx::Logging
+ class Segment : public segment::wm::CoreSegmentBase
{
+ using Base = segment::wm::CoreSegmentBase;
+
public:
- Segment(server::MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex);
- virtual ~Segment();
+ using RobotJointPositionMap = std::unordered_map<std::string, std::map<std::string, float>>;
- void connect(viz::Client arviz, RobotUnitInterfacePrx robotUnitPrx);
+ public:
+
+ Segment(server::MemoryToIceAdapter& iceMemory);
+ virtual ~Segment() override;
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+ void onConnect(RobotUnitInterfacePrx robotUnitPrx);
- void init();
- std::unordered_map<std::string, std::map<std::string, float>> getRobotJointPositions(const armem::Time& timestamp) const;
+ RobotJointPositionMap getRobotJointPositions(
+ const armem::Time& timestamp, DebugObserverHelper* debugObserver = nullptr) const;
+ RobotJointPositionMap getRobotJointPositionsLocking(
+ const armem::Time& timestamp, DebugObserverHelper* debugObserver = nullptr) const;
const armem::MemoryID& getRobotUnitProviderID() const;
- private:
- server::MemoryToIceAdapter& iceMemory;
- wm::CoreSegment* coreSegment = nullptr;
- std::mutex& memoryMutex;
+ private:
RobotUnitInterfacePrx robotUnit;
-
armem::MemoryID robotUnitProviderID;
- struct Properties
- {
- std::string coreSegment = "Proprioception";
- int64_t maxHistorySize = -1;
- };
- Properties p;
-
- // std::unique_ptr<Visu> visu;
-
- public:
-
- // struct RemoteGui
- // {
- // armarx::RemoteGui::Client::GroupBox group;
-
- // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
- // armarx::RemoteGui::Client::CheckBox infiniteHistory;
- // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
-
- // void setup(const Segment& data);
- // void update(Segment& data);
- // };
+ // Debug Observer prefix
+ const std::string dp = "Proprioception::getRobotJointPositions() | ";
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8caf73057ac54df453f6e3c2d67295dc7e69ab89
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.cpp
@@ -0,0 +1,69 @@
+#include "aron_conversions.h"
+
+#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+
+#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
+
+
+namespace armarx
+{
+ aron::datanavigator::NavigatorPtr RobotUnitDataStreaming::toAron(
+ const TimeStep& timestep,
+ const DataEntry& dataEntry)
+ {
+ switch (dataEntry.type)
+ {
+ case RobotUnitDataStreaming::NodeTypeFloat:
+ {
+ float value = RobotUnitDataStreamingReceiver::GetAs<Ice::Float>(timestep, dataEntry);
+ return std::make_shared<aron::datanavigator::FloatNavigator>(value);
+ }
+ case RobotUnitDataStreaming::NodeTypeBool:
+ {
+ bool value = RobotUnitDataStreamingReceiver::GetAs<bool>(timestep, dataEntry);
+ return std::make_shared<aron::datanavigator::BoolNavigator>(value);
+ }
+ case RobotUnitDataStreaming::NodeTypeByte:
+ {
+ int value = RobotUnitDataStreamingReceiver::GetAs<Ice::Byte>(timestep, dataEntry);
+ return std::make_shared<aron::datanavigator::IntNavigator>(value);
+ }
+ case RobotUnitDataStreaming::NodeTypeShort:
+ {
+ int value = RobotUnitDataStreamingReceiver::GetAs<Ice::Short>(timestep, dataEntry);
+ return std::make_shared<aron::datanavigator::IntNavigator>(value);
+ }
+ case RobotUnitDataStreaming::NodeTypeInt:
+ {
+ int value = RobotUnitDataStreamingReceiver::GetAs<Ice::Int>(timestep, dataEntry);
+ return std::make_shared<aron::datanavigator::IntNavigator>(value);
+ }
+ case RobotUnitDataStreaming::NodeTypeLong:
+ {
+ long value = RobotUnitDataStreamingReceiver::GetAs<Ice::Long>(timestep, dataEntry);
+ return std::make_shared<aron::datanavigator::LongNavigator>(value);
+ }
+ case RobotUnitDataStreaming::NodeTypeDouble:
+ {
+ double value = RobotUnitDataStreamingReceiver::GetAs<Ice::Double>(timestep, dataEntry);
+ return std::make_shared<aron::datanavigator::DoubleNavigator>(value);
+ }
+ default:
+ ARMARX_UNEXPECTED_ENUM_VALUE(RobotUnitDataStreaming::NodeType, dataEntry.type);
+ }
+ }
+
+
+ const simox::meta::EnumNames<RobotUnitDataStreaming::DataEntryType> RobotUnitDataStreaming::DataEntryNames =
+ {
+ { NodeTypeBool, "Bool" },
+ { NodeTypeByte, "Byte" },
+ { NodeTypeShort, "Short" },
+ { NodeTypeInt, "Int" },
+ { NodeTypeLong, "Long" },
+ { NodeTypeFloat, "Float" },
+ { NodeTypeDouble, "Double" },
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..dd2177c9e2f167af3946119f381dcd7ec43d170c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h
@@ -0,0 +1,23 @@
+#pragma once
+
+#include <memory>
+
+#include <SimoxUtility/meta/enum/EnumNames.hpp>
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
+
+namespace armarx::aron::datanavigator
+{
+ using NavigatorPtr = std::shared_ptr<class Navigator>;
+}
+
+namespace armarx::RobotUnitDataStreaming
+{
+ aron::datanavigator::NavigatorPtr toAron(
+ const TimeStep& timestep,
+ const DataEntry& dataEntry);
+
+ extern const simox::meta::EnumNames<DataEntryType> DataEntryNames;
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6c7171a746e0815d2fca648f2b91e0faa31a669e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp
@@ -0,0 +1,209 @@
+#include "Armar6Converter.h"
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/advanced.h>
+
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+
+#include "ConverterTools.h"
+
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+
+ Armar6Converter::Armar6Converter() :
+ tools(std::make_unique<ConverterTools>())
+ {
+ }
+
+
+ Armar6Converter::~Armar6Converter()
+ {
+ }
+
+
+ aron::datanavigator::DictNavigatorPtr
+ Armar6Converter::convert(
+ const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description)
+ {
+ arondto::Proprioception dto;
+ dto.iterationID = data.iterationId;
+
+ for (const auto& [dataEntryName, dataEntry] : description.entries)
+ {
+ process(dto, dataEntryName, {data, dataEntry});
+ }
+ return dto.toAron();
+ }
+
+
+ void Armar6Converter::process(
+ arondto::Proprioception dto,
+ const std::string& entryName,
+ const ConverterValue& value)
+ {
+ const std::vector<std::string> split = simox::alg::split(entryName, ".", false, false);
+ ARMARX_CHECK_GREATER_EQUAL(split.size(), 3);
+ const std::set<size_t> acceptedSizes{3, 4};
+ ARMARX_CHECK_GREATER(acceptedSizes.count(split.size()), 0)
+ << "Data entry name could not be parsed (exected 3 or 4 components between '.'): "
+ << "\n- split: '" << split << "'";
+
+ const std::string& category = split.at(0);
+ const std::string& name = split.at(1);
+ const std::string& field = split.at(2);
+ ARMARX_CHECK_EQUAL(category, "sens") << category << " | " << entryName;
+
+ if (name == "Platform")
+ {
+ // Platform
+ processPlatformEntry(dto.platform, field, value);
+ }
+ else if (simox::alg::starts_with(name, "FT"))
+ {
+ // Force Torque
+ processForceTorqueEntry(dto.forceTorque, split, value);
+ }
+ else
+ {
+ // Joint
+ bool processed = processJointEntry(dto.joints, split, value);
+
+ if (!processed)
+ {
+ // Fallback: Put in extra.
+ const std::vector<std::string> comps{simox::alg::advanced(split.begin(), 1), split.end()};
+ const std::string key = simox::alg::join(comps, ".");
+
+ switch (value.entry.type)
+ {
+ case RobotUnitDataStreaming::NodeTypeFloat:
+ dto.extraFloats[key] = getValueAs<float>(value);
+ break;
+ case RobotUnitDataStreaming::NodeTypeLong:
+ dto.extraLongs[key] = getValueAs<long>(value);
+ break;
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+
+
+ void Armar6Converter::processPlatformEntry(
+ prop::arondto::Platform& dto,
+ const std::string& fieldName,
+ const ConverterValue& value)
+ {
+ if (findByPrefix(fieldName, tools->platformIgnored))
+ {
+ return;
+ }
+ else if (auto getter = findByPrefix(fieldName, tools->platformPoseGetters))
+ {
+ if (Eigen::Vector3f* dst = getter(dto))
+ {
+ if (auto setter = findBySuffix(fieldName, tools->vector3fSetters))
+ {
+ setter(*dst, getValueAs<float>(value));
+ }
+ }
+ }
+ else
+ {
+ // No setter for this field. Put in extra.
+ dto.extra[fieldName] = getValueAs<float>(value);
+ }
+ }
+
+
+ void Armar6Converter::processForceTorqueEntry(
+ std::map<std::string, prop::arondto::ForceTorque>& fts,
+ const std::vector<std::string>& split,
+ const ConverterValue& value)
+ {
+ const std::string& name = split.at(1);
+ std::vector<std::string> splitName = simox::alg::split(name, " ", false, false);
+ ARMARX_CHECK_EQUAL(splitName.size(), 2);
+ ARMARX_CHECK_EQUAL(splitName.at(0), "FT");
+
+ auto it = tools->sidePrefixMap.find(splitName.at(1));
+ ARMARX_CHECK(it != tools->sidePrefixMap.end()) << splitName.at(1);
+
+ const std::string& side = it->second;
+ processForceTorqueEntry(fts[side], split, value);
+ }
+
+
+ void Armar6Converter::processForceTorqueEntry(
+ prop::arondto::ForceTorque& dto,
+ const std::vector<std::string>& split,
+ const ConverterValue& value)
+ {
+ const std::string& fieldName = split.at(2);
+ if (auto getter = findByPrefix(fieldName, tools->ftGetters))
+ {
+ if (Eigen::Vector3f* dst = getter(dto))
+ {
+ if (auto setter = findBySuffix(fieldName, tools->vector3fSetters))
+ {
+ setter(*dst, getValueAs<float>(value));
+ }
+ }
+ }
+ else
+ {
+ // No setter for this field. Put in extra.
+ std::string key = split.size() == 4
+ ? (fieldName + "." + split.at(3))
+ : fieldName;
+ dto.extra[key] = getValueAs<float>(value);
+ }
+ }
+
+
+ bool Armar6Converter::processJointEntry(
+ prop::arondto::Joints& dto,
+ const std::vector<std::string>& split,
+ const ConverterValue& value)
+ {
+ const std::string& jointName = split.at(1);
+ const std::string& fieldName = split.at(2);
+ if (false)
+ {
+ // Only in simulation.
+ if (auto getter = findByPrefix(fieldName, tools->jointGetters))
+ {
+ if (std::map<std::string, float>* map = getter(dto))
+ {
+ (*map)[jointName] = getValueAs<float>(value);
+ }
+ }
+ }
+
+ const std::string tempSuffix = "Temperature";
+ if (simox::alg::ends_with(split.at(2), tempSuffix))
+ {
+ // Handle "dieTemperature" etc
+ const std::string name = split.at(2).substr(0, split.at(2).size() - tempSuffix.size());
+ dto.temperature[split.at(1)][name] = getValueAs<float>(value);
+ return true;
+ }
+ else if (auto it = tools->jointSetters.find(fieldName); it != tools->jointSetters.end())
+ {
+ const ConverterTools::JointSetter& setter = it->second;
+ setter(dto, split, value);
+ return true;
+ }
+ else
+ {
+ ARMARX_DEBUG << "Ignoring unhandled field: '" << simox::alg::join(split, ".") << "'";
+ return false;
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.h
new file mode 100644
index 0000000000000000000000000000000000000000..f7a0da8f3d166fda4b497fd960398ffd861dfc61
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.h
@@ -0,0 +1,69 @@
+#pragma once
+
+#include <map>
+#include <string>
+
+#include <Eigen/Core>
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+
+#include "ConverterInterface.h"
+
+
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+ struct ConverterValue;
+ class ConverterTools;
+
+
+ class Armar6Converter : public ConverterInterface
+ {
+ public:
+
+ Armar6Converter();
+ virtual ~Armar6Converter() override;
+
+
+ aron::datanavigator::DictNavigatorPtr
+ convert(
+ const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description) override;
+
+
+ protected:
+
+ void process(arondto::Proprioception dto, const std::string& entryName, const ConverterValue& value);
+
+
+
+ private:
+
+ void processPlatformEntry(
+ prop::arondto::Platform& dto,
+ const std::string& fieldName,
+ const ConverterValue& value);
+
+ void processForceTorqueEntry(
+ std::map<std::string, prop::arondto::ForceTorque>& fts,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
+
+ void processForceTorqueEntry(
+ prop::arondto::ForceTorque& ft,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
+
+ bool processJointEntry(
+ prop::arondto::Joints& dto,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
+
+
+ private:
+
+ std::unique_ptr<ConverterTools> tools;
+
+ };
+}
+
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterInterface.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterInterface.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8ddf012be951db403448f855966b026efe33ba8f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterInterface.cpp
@@ -0,0 +1,11 @@
+#include "ConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+
+ ConverterInterface::~ConverterInterface()
+ {
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterInterface.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterInterface.h
new file mode 100644
index 0000000000000000000000000000000000000000..1162623895611e89d69740f6bf7d014c325f1427
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterInterface.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include <memory>
+
+
+namespace armarx::RobotUnitDataStreaming
+{
+ struct TimeStep;
+ struct DataStreamingDescription;
+ struct DataEntry;
+}
+namespace armarx::armem::arondto
+{
+ class Proprioception;
+}
+namespace armarx::aron::datanavigator
+{
+ using DictNavigatorPtr = std::shared_ptr<class DictNavigator>;
+}
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+ class ConverterInterface
+ {
+ public:
+
+ virtual ~ConverterInterface();
+
+ virtual
+ aron::datanavigator::DictNavigatorPtr convert(
+ const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description) = 0;
+
+ };
+}
+
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a04ca4cbab6b876b3eb2e8f146ca7858a35f0bc4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp
@@ -0,0 +1,30 @@
+#include "ConverterRegistry.h"
+
+#include "Armar6Converter.h"
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+
+ ConverterRegistry::ConverterRegistry()
+ {
+ add<Armar6Converter>("Armar6");
+ }
+
+
+ ConverterInterface*
+ ConverterRegistry::get(const std::string& key) const
+ {
+ auto it = converters.find(key);
+ return it != converters.end() ? it->second.get() : nullptr;
+ }
+
+
+ std::vector<std::string> ConverterRegistry::getKeys() const
+ {
+ return simox::alg::get_keys(converters);
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.h
new file mode 100644
index 0000000000000000000000000000000000000000..f62f808f6ee305f67aba8ba0bff24e414baaa802
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.h
@@ -0,0 +1,37 @@
+#pragma once
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "ConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+ class ConverterRegistry
+ {
+ public:
+
+ ConverterRegistry();
+
+
+ template <class ConverterT, class ...Args>
+ void add(const std::string& key, Args... args)
+ {
+ converters[key].reset(new ConverterT(args...));
+ }
+
+
+ ConverterInterface* get(const std::string& key) const;
+ std::vector<std::string> getKeys() const;
+
+
+ private:
+
+ std::map<std::string, std::unique_ptr<ConverterInterface>> converters;
+
+ };
+}
+
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterTools.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterTools.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7cc2fc128b55dc9af794804cc569d91b75ba7071
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterTools.cpp
@@ -0,0 +1,179 @@
+#include "ConverterTools.h"
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/advanced.h>
+
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+
+
+namespace armarx::armem::server::robot_state
+{
+
+ std::optional<std::string>
+ proprioception::findByPrefix(const std::string& key, const std::set<std::string>& prefixes)
+ {
+ for (const auto& prefix : prefixes)
+ {
+ if (simox::alg::starts_with(key, prefix))
+ {
+ return prefix;
+ }
+ }
+ return std::nullopt;
+ }
+}
+
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+ ConverterTools::ConverterTools()
+ {
+ {
+ vector3fSetters["X"] = [](Eigen::Vector3f & v, float f)
+ {
+ v.x() = f;
+ };
+ vector3fSetters["Y"] = [](Eigen::Vector3f & v, float f)
+ {
+ v.y() = f;
+ };
+ vector3fSetters["Z"] = [](Eigen::Vector3f & v, float f)
+ {
+ v.z() = f;
+ };
+ vector3fSetters["x"] = vector3fSetters["X"];
+ vector3fSetters["y"] = vector3fSetters["Y"];
+ vector3fSetters["z"] = vector3fSetters["Z"];
+ vector3fSetters["Rotation"] = vector3fSetters["Z"];
+ }
+ {
+ platformPoseGetters["acceleration"] = [](prop::arondto::Platform & p)
+ {
+ return &p.acceleration;
+ };
+ platformPoseGetters["relativePosition"] = [](prop::arondto::Platform & p)
+ {
+ return &p.relativePosition;
+ };
+ platformPoseGetters["velocity"] = [](prop::arondto::Platform & p)
+ {
+ return &p.velocity;
+ };
+ platformIgnored.insert("absolutePosition");
+ }
+ {
+ ftGetters["gravCompF"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.gravityCompensationForce;
+ };
+ ftGetters["gravCompT"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.gravityCompensationTorque;
+ };
+ ftGetters["f"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.force;
+ };
+ ftGetters["t"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.torque;
+ };
+ }
+ {
+ jointGetters["acceleration"] = [](prop::arondto::Joints & j)
+ {
+ return &j.acceleration;
+ };
+ jointGetters["gravityTorque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.gravityTorque;
+ };
+ jointGetters["inertiaTorque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.inertiaTorque;
+ };
+ jointGetters["inverseDynamicsTorque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.inverseDynamicsTorque;
+ };
+ jointGetters["position"] = [](prop::arondto::Joints & j)
+ {
+ return &j.position;
+ };
+ jointGetters["torque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.torque;
+ };
+ jointGetters["velocity"] = [](prop::arondto::Joints & j)
+ {
+ return &j.velocity;
+ };
+ }
+ {
+
+#define ADD_SCALAR_SETTER(container, name, type) \
+ container [ #name ] = []( \
+ prop::arondto::Joints & dto, \
+ const std::vector<std::string>& split, \
+ const ConverterValue & value) \
+ { \
+ dto. name [split.at(1)] = getValueAs< type >(value); \
+ }
+
+ ADD_SCALAR_SETTER(jointSetters, position, float);
+ ADD_SCALAR_SETTER(jointSetters, velocity, float);
+ ADD_SCALAR_SETTER(jointSetters, acceleration, float);
+
+ ADD_SCALAR_SETTER(jointSetters, relativePosition, float);
+ ADD_SCALAR_SETTER(jointSetters, filteredVelocity, float);
+
+ ADD_SCALAR_SETTER(jointSetters, currentTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, positionTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, velocityTarget, float);
+
+ ADD_SCALAR_SETTER(jointSetters, torque, float);
+ ADD_SCALAR_SETTER(jointSetters, inertiaTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, gravityTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, gravityCompensatedTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, inverseDynamicsTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, torqueTicks, int);
+
+ ADD_SCALAR_SETTER(jointSetters, positionTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, currentTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, positionTarget, float);
+
+ // "temperature" handled below
+ // ADD_SCALAR_SETTER(jointSetters, temperature, float);
+
+ ADD_SCALAR_SETTER(jointSetters, motorCurrent, float);
+ ADD_SCALAR_SETTER(jointSetters, maxTargetCurrent, float);
+
+ ADD_SCALAR_SETTER(jointSetters, sensorBoardUpdateRate, float);
+ ADD_SCALAR_SETTER(jointSetters, I2CUpdateRate, float);
+
+ ADD_SCALAR_SETTER(jointSetters, JointStatusEmergencyStop, bool);
+ ADD_SCALAR_SETTER(jointSetters, JointStatusEnabled, bool);
+ ADD_SCALAR_SETTER(jointSetters, JointStatusError, int);
+ ADD_SCALAR_SETTER(jointSetters, JointStatusOperation, int);
+
+
+#define ADD_VECTOR3_SETTER(container, name, type) \
+ container [ #name ] = [this]( \
+ prop::arondto::Joints & dto, \
+ const std::vector<std::string>& split, \
+ const ConverterValue & value) \
+ { \
+ auto& vec = dto. name [split.at(1)]; \
+ auto& setter = this->vector3fSetters.at(split.at(3)); \
+ setter(vec, getValueAs< type >(value)); \
+ }
+
+ ADD_VECTOR3_SETTER(jointSetters, angularVelocity, float);
+ ADD_VECTOR3_SETTER(jointSetters, linearAcceleration, float);
+
+ // ADD_GETTER(jointVectorGetters, orientation, float);
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterTools.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterTools.h
new file mode 100644
index 0000000000000000000000000000000000000000..ff667a1b578c074126e2a2bfb5db9100dafa964d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterTools.h
@@ -0,0 +1,107 @@
+#pragma once
+
+#include <map>
+#include <set>
+#include <string>
+
+#include <Eigen/Core>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+
+#include "ConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+
+ struct ConverterValue
+ {
+ const RobotUnitDataStreaming::TimeStep& data;
+ const RobotUnitDataStreaming::DataEntry& entry;
+ };
+
+
+ template <class T>
+ T
+ getValueAs(const ConverterValue& value)
+ {
+ return RobotUnitDataStreamingReceiver::GetAs<T>(value.data, value.entry);
+ }
+
+
+ /**
+ * @brief Search
+ * @param key
+ * @param prefixes
+ * @return
+ */
+ std::optional<std::string>
+ findByPrefix(const std::string& key, const std::set<std::string>& prefixes);
+
+
+ template <class ValueT>
+ ValueT
+ findByPrefix(const std::string& key, const std::map<std::string, ValueT>& map)
+ {
+ for (const auto& [prefix, value] : map)
+ {
+ if (simox::alg::starts_with(key, prefix))
+ {
+ return value;
+ }
+ }
+ return nullptr;
+ }
+
+
+ template <class ValueT>
+ ValueT
+ findBySuffix(const std::string& key, const std::map<std::string, ValueT>& map)
+ {
+ for (const auto& [suffix, value] : map)
+ {
+ if (simox::alg::ends_with(key, suffix))
+ {
+ return value;
+ }
+ }
+ return nullptr;
+ }
+
+
+
+ class ConverterTools
+ {
+ public:
+
+ ConverterTools();
+
+
+ public:
+
+ std::map<std::string, std::function<void(Eigen::Vector3f&, float)> > vector3fSetters;
+
+ std::map<std::string, std::function<std::map<std::string, float>*(prop::arondto::Joints&)> > jointGetters;
+ std::map<std::string, std::function<std::map<std::string, Eigen::Vector3f>*(prop::arondto::Joints&)> > jointVectorGetters;
+
+ using JointSetter = std::function<void(prop::arondto::Joints& dto, const std::vector<std::string>& split, const ConverterValue& value)>;
+ std::map<std::string, JointSetter> jointSetters;
+
+ std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::Platform&)> > platformPoseGetters;
+ std::set<std::string> platformIgnored;
+
+ std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::ForceTorque&)> > ftGetters;
+
+
+ std::map<std::string, std::string> sidePrefixMap
+ {
+ { "R", "Right" },
+ { "L", "Left" },
+ };
+
+ };
+}
+
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
index 9c46b0c4729dfe6ca038c1f31ef81255f56d4968..4bc22306d7d26603d1edba2c8fc8f8aed40b1a8d 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
@@ -33,22 +33,20 @@
#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
// RobotAPI Armem
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+
#include <RobotAPI/libraries/armem/client/Query.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/selectors.h>
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
-#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
#include <RobotAPI/libraries/armem_vision/types.h>
+
namespace armarx::armem::vision::laser_scans::client
{
@@ -124,11 +122,10 @@ namespace armarx::armem::vision::laser_scans::client
}
std::vector<LaserScanStamped>
- asLaserScans(const std::map<std::string, wm::Entity>& entities)
+ asLaserScans(const wm::ProviderSegment& providerSegment)
{
std::vector<LaserScanStamped> outV;
-
- if (entities.empty())
+ if (providerSegment.empty())
{
ARMARX_WARNING << "No entities!";
}
@@ -153,29 +150,25 @@ namespace armarx::armem::vision::laser_scans::client
};
// loop over all entities and their snapshots
- for (const auto& [s, entity] : entities)
+ providerSegment.forEachEntity([&outV, &convert](const wm::Entity & entity)
{
+ // If we don't need this warning, we could directly iterate over the snapshots.
if (entity.empty())
{
- ARMARX_WARNING << "Empty history for " << s;
+ ARMARX_WARNING << "Empty history for " << entity.id();
}
-
ARMARX_DEBUG << "History size: " << entity.size();
- for (const auto& [ss, entitySnapshot] : entity)
+ entity.forEachInstance([&outV, &convert](const wm::EntityInstance & entityInstance)
{
- for (const auto& entityInstance : entitySnapshot.instances())
+ if (const auto o = tryCast<arondto::LaserScanStamped>(entityInstance))
{
- const auto o =
- tryCast<arondto::LaserScanStamped>(entityInstance);
-
- if (o)
- {
- outV.push_back(convert(*o, entityInstance));
- }
+ outV.push_back(convert(*o, entityInstance));
}
- }
- }
+ return true;
+ });
+ return true;
+ });
return outV;
}
@@ -202,13 +195,16 @@ namespace armarx::armem::vision::laser_scans::client
}
// now create result from memory
- const auto& entities =
- qResult.memory.getCoreSegment(properties.memoryName)
- .getProviderSegment(query.agent)
- .entities();
+ const wm::ProviderSegment& providerSegment =
+ qResult.memory.getCoreSegment(properties.memoryName).getProviderSegment(query.agent);
- const auto laserScans = asLaserScans(entities);
- const auto sensors = simox::alg::get_keys(entities);
+ const auto laserScans = asLaserScans(providerSegment);
+ std::vector<std::string> sensors;
+ providerSegment.forEachEntity([&sensors](const wm::Entity & entity)
+ {
+ sensors.push_back(entity.name());
+ return true;
+ });
return {.laserScans = laserScans,
.sensors = sensors,
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
index 7358e504ffecf3a2a710bd77b1631437e3818a42..861e02c4c9fcabbc40caebba3451b69e81e5016c 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
@@ -1,7 +1,7 @@
#include "Writer.h"
#include <RobotAPI/libraries/armem/core/error.h>
-#include "RobotAPI/libraries/armem_vision/aron_conversions.h"
+#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
index ba7edf547d1d45873609d723b92934b18d7f5c88..717a2ec98e5283515a5454bfa31eb7c00636b01c 100644
--- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
@@ -19,13 +19,13 @@
#include <SimoxUtility/algorithm/get_map_keys_values.h>
// ArmarXCore
-#include "ArmarXCore/core/exceptions/LocalException.h"
+#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/LogSender.h>
#include <ArmarXCore/core/logging/Logging.h>
// RobotAPI Interfaces
-#include "RobotAPI/libraries/aron/converter/eigen/EigenConverter.h"
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
@@ -40,11 +40,7 @@
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/selectors.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
@@ -68,20 +64,23 @@ namespace armarx::armem::vision::occupancy_grid::client
return qb;
}
- OccupancyGrid asOccupancyGrid(const std::map<std::string, wm::Entity>& entities)
+ OccupancyGrid asOccupancyGrid(const wm::ProviderSegment& providerSegment)
{
- ARMARX_CHECK(not entities.empty()) << "No entities";
- ARMARX_CHECK(entities.size() == 1) << "There should be only one entity!";
+ ARMARX_CHECK(not providerSegment.empty()) << "No entities";
+ ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
- const wm::Entity& entity = entities.begin()->second;
- ARMARX_CHECK(not entity.empty()) << "No snapshots";
-
- const auto& entitySnapshot = entity.getLatestSnapshot();
- ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
+ const wm::EntityInstance* entityInstance = nullptr;
+ providerSegment.forEachEntity([&entityInstance](const wm::Entity & entity)
+ {
+ const auto& entitySnapshot = entity.getLatestSnapshot();
+ ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
- const auto& entityInstance = entitySnapshot.instances().front();
+ entityInstance = &entitySnapshot.getInstance(0);
+ return false;
+ });
+ ARMARX_CHECK_NOT_NULL(entityInstance);
- const auto aronDto = tryCast<arondto::OccupancyGrid>(entityInstance);
+ const auto aronDto = tryCast<arondto::OccupancyGrid>(*entityInstance);
ARMARX_CHECK(aronDto) << "Failed casting to OccupancyGrid";
OccupancyGrid occupancyGrid;
@@ -89,7 +88,7 @@ namespace armarx::armem::vision::occupancy_grid::client
// direct access to grid data
const auto ndArrayNavigator = aron::datanavigator::NDArrayNavigator::DynamicCast(
- entityInstance.data()->getElement("grid"));
+ entityInstance->data()->getElement("grid"));
ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
occupancyGrid.grid = aron::converter::AronEigenConverter::ConvertToArray<float>(*ndArrayNavigator);
@@ -118,11 +117,10 @@ namespace armarx::armem::vision::occupancy_grid::client
}
// now create result from memory
- const auto& entities = qResult.memory.getCoreSegment(properties().memoryName)
- .getProviderSegment(query.providerName)
- .entities();
+ const wm::ProviderSegment& providerSegment = qResult.memory.getCoreSegment(properties().memoryName)
+ .getProviderSegment(query.providerName);
- if (entities.empty())
+ if (providerSegment.empty())
{
ARMARX_WARNING << "No entities.";
return {.occupancyGrid = std::nullopt,
@@ -132,7 +130,7 @@ namespace armarx::armem::vision::occupancy_grid::client
try
{
- const auto occupancyGrid = asOccupancyGrid(entities);
+ const auto occupancyGrid = asOccupancyGrid(providerSegment);
return Result{.occupancyGrid = occupancyGrid,
.status = Result::Status::Success};
}
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h
index a43c2c7c1151223358d5b1ab608f824fd3cdf66f..eaf9b2fdfa2021db7f8e1215baedc6aba907dec3 100644
--- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h
@@ -23,9 +23,9 @@
#include <mutex>
-#include "RobotAPI/libraries/armem/client/util/SimpleReaderBase.h"
-#include "RobotAPI/libraries/armem/core/Time.h"
-#include "RobotAPI/libraries/armem_vision/types.h"
+#include <RobotAPI/libraries/armem/client/util/SimpleReaderBase.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
namespace armarx::armem::vision::occupancy_grid::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
index f2930f14f479d9ae68ee222f2643edf327392a43..ce5883afb6eeb72c7ebb9337d0b4980eee7e462e 100644
--- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
@@ -1,8 +1,9 @@
#include "Writer.h"
-#include "RobotAPI/libraries/armem_vision/aron_conversions.h"
+#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
+
namespace armarx::armem::vision::occupancy_grid::client
{
Writer::~Writer() = default;
@@ -67,4 +68,4 @@ namespace armarx::armem::vision::occupancy_grid::client
return SimpleWriterBase::Properties{.memoryName = "Vision",
.coreSegmentName = "OccupancyGrid"};
}
-} // namespace armarx::armem::vision::occupancy_grid::client
\ No newline at end of file
+} // namespace armarx::armem::vision::occupancy_grid::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h
index bf1268444c333d10bb3c2f9efb68da11fe697cc8..b2a900321224d2c4617cab541c7b1cf77f41d1f1 100644
--- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h
@@ -24,8 +24,8 @@
#include <mutex>
-#include "RobotAPI/libraries/armem/client/util/SimpleWriterBase.h"
-#include "RobotAPI/libraries/armem_vision/types.h"
+#include <RobotAPI/libraries/armem/client/util/SimpleWriterBase.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
namespace armarx::armem::vision::occupancy_grid::client
{
diff --git a/source/RobotAPI/libraries/aron/core/CMakeLists.txt b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
index 62548c29719f3942afee49a45e9a3f24a4232634..c5282fb54aba97b583f15f2df1876c0dde3e4ead 100644
--- a/source/RobotAPI/libraries/aron/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
@@ -151,6 +151,7 @@ set(LIB_HEADERS
navigator/data/primitive/Bool.h
navigator/data/AllNavigators.h
navigator/data/NavigatorFactory.h
+ navigator/data/forward_declarations.h
navigator/type/Navigator.h
navigator/type/detail/NavigatorBase.h
@@ -181,6 +182,7 @@ set(LIB_HEADERS
navigator/type/primitive/Time.h
navigator/type/AllNavigators.h
navigator/type/NavigatorFactory.h
+ navigator/type/forward_declarations.h
navigator/visitors/DataVisitor.h
navigator/visitors/TypedDataVisitor.h
diff --git a/source/RobotAPI/libraries/aron/core/Randomizer.h b/source/RobotAPI/libraries/aron/core/Randomizer.h
index bdc05099f108f0abaecb8db68cc58f47b8688a6c..00f355baa9c01e66792a886b027ad4186b808016 100644
--- a/source/RobotAPI/libraries/aron/core/Randomizer.h
+++ b/source/RobotAPI/libraries/aron/core/Randomizer.h
@@ -55,6 +55,7 @@ namespace armarx::aron
{
nextMaybeType = getRandomElement(type::ALL_MAYBE_TYPES);
}
+ (void) nextMaybeType; // Unused
switch (nextType)
{
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h b/source/RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h
new file mode 100644
index 0000000000000000000000000000000000000000..d76939c317aafd2acc4eb797d38d1ba1ff96da26
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h
@@ -0,0 +1,22 @@
+#pragma once
+
+#include <memory>
+
+
+namespace armarx::aron::datanavigator
+{
+ class Navigator;
+ using NavigatorPtr = std::shared_ptr<Navigator>;
+
+ using DictNavigatorPtr = std::shared_ptr<class DictNavigator>;
+ using ListNavigatorPtr = std::shared_ptr<class ListNavigator>;
+ using NDArrayNavigatorPtr = std::shared_ptr<class NDArrayNavigator>;
+
+ using IntNavigatorPtr = std::shared_ptr<class IntNavigator>;
+ using LongNavigatorPtr = std::shared_ptr<class LongNavigator>;
+ using FloatNavigatorPtr = std::shared_ptr<class FloatNavigator>;
+ using DoubleNavigatorPtr = std::shared_ptr<class DoubleNavigator>;
+ using StringNavigatorPtr = std::shared_ptr<class StringNavigator>;
+ using BoolNavigatorPtr = std::shared_ptr<class BoolNavigator>;
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h b/source/RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h
index 51ea5a762037da7b7e4a3808c42d36a468d9c6b1..82702de998bb7e6b0972afd9e96ae027b433eae3 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h
@@ -12,7 +12,6 @@
#include "ndarray/OpenCVMat.h"
#include "ndarray/Orientation.h"
#include "ndarray/PCLPointCloud.h"
-#include "ndarray/PCLPointCloud.h"
#include "ndarray/Pose.h"
#include "ndarray/Position.h"
#include "enum/IntEnum.h"
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h b/source/RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h
new file mode 100644
index 0000000000000000000000000000000000000000..af5d62e7c0decbc296cea942fa1983b0e0cb5924
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include <memory>
+
+
+namespace armarx::aron::typenavigator
+{
+ using NavigatorPtr = std::shared_ptr<class Navigator>;
+
+ using DictNavigatorPtr = std::shared_ptr<class DictNavigator>;
+ using ListNavigatorPtr = std::shared_ptr<class ListNavigator>;
+ using ObjectNavigatorPtr = std::shared_ptr<class ObjectNavigator>;
+ using PoseNavigatorPtr = std::shared_ptr<class PoseNavigator>;
+ using TupleNavigatorPtr = std::shared_ptr<class TupleNavigator>;
+ using NDArrayNavigatorPtr = std::shared_ptr<class NDArrayNavigator>;
+ using EigenMatrixNavigatorPtr = std::shared_ptr<class EigenMatrixNavigator>;
+ using EigenQuaternionNavigatorPtr = std::shared_ptr<class EigenQuaternionNavigator>;
+ using IVTCByteImageNavigatorPtr = std::shared_ptr<class IVTCByteImageNavigator>;
+ using OpenCVMatNavigatorPtr = std::shared_ptr<class OpenCVMatNavigator>;
+ using OrientationNavigatorPtr = std::shared_ptr<class OrientationNavigator>;
+ using PCLPointCloudNavigatorPtr = std::shared_ptr<class PCLPointCloudNavigator>;
+ using PositionNavigatorPtr = std::shared_ptr<class PositionNavigator>;
+ using IntEnumNavigatorPtr = std::shared_ptr<class IntEnumNavigator>;
+
+ using IntNavigatorPtr = std::shared_ptr<class IntNavigator>;
+ using LongNavigatorPtr = std::shared_ptr<class LongNavigator>;
+ using FloatNavigatorPtr = std::shared_ptr<class FloatNavigator>;
+ using DoubleNavigatorPtr = std::shared_ptr<class DoubleNavigator>;
+ using StringNavigatorPtr = std::shared_ptr<class StringNavigator>;
+ using BoolNavigatorPtr = std::shared_ptr<class BoolNavigator>;
+ using TimeNavigatorPtr = std::shared_ptr<class TimeNavigator>;
+
+}
+