diff --git a/scenarios/RobotSkillsMemory/config/SkillsMemory.cfg b/scenarios/RobotSkillsMemory/config/SkillsMemory.cfg
index 57a92b84c82797807a38c8635dbe1fb7d3d3d43d..33ff12f5046dad45169c1661741ff7216055c26d 100644
--- a/scenarios/RobotSkillsMemory/config/SkillsMemory.cfg
+++ b/scenarios/RobotSkillsMemory/config/SkillsMemory.cfg
@@ -151,22 +151,6 @@
# ArmarX.SkillsMemory.ObjectName = ""
-# ArmarX.SkillsMemory.StatechartCoreSegmentName: Name of the core segment for statecharts.
-# Attributes:
-# - Default: Statechart
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SkillsMemory.StatechartCoreSegmentName = Statechart
-
-
-# ArmarX.SkillsMemory.TransitionsProviderSegmentName: Name of the provider segment for statechart transitions.
-# Attributes:
-# - Default: Transitions
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SkillsMemory.TransitionsProviderSegmentName = Transitions
-
-
# ArmarX.SkillsMemory.mem.MemoryName: Name of this memory server.
# Attributes:
# - Default: Skills
diff --git a/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg b/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg
index 5ff571ebdb58c6f447e14154934e7b4cac213a35..b988f3b13d77676d1b3a85206cf37013b3ea36a4 100644
--- a/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg
+++ b/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg
@@ -143,11 +143,19 @@
# ArmarX.SickLaserUnit.ObjectName = ""
-# ArmarX.SickLaserUnit.angleOffset: offset to the scanning angle
+# ArmarX.SickLaserUnit.TopicName: Name of the laserscanner topic to report to.
# Attributes:
-# - Default: 0
+# - Default: LaserScans
# - Case sensitivity: yes
# - Required: no
+# ArmarX.SickLaserUnit.TopicName = LaserScans
+
+
+# ArmarX.SickLaserUnit.angleOffset: No Description
+# Attributes:
+# - Default: 0.0
+# - Case sensitivity: no
+# - Required: no
ArmarX.SickLaserUnit.angleOffset = 0.0
@@ -156,7 +164,45 @@ ArmarX.SickLaserUnit.angleOffset = 0.0
# - Default: LaserScannerFront,192.168.8.133,2112
# - Case sensitivity: yes
# - Required: no
-ArmarX.SickLaserUnit.devices = LaserScannerFront,192.168.8.133,2112
+ArmarX.SickLaserUnit.devices = LaserScannerFront,192.168.8.133,2112;LaserScannerBack,192.168.8.133,2112
+
+
+# ArmarX.SickLaserUnit.heartbeat.TopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SickLaserUnit.heartbeat.TopicName = DebugObserver
+
+
+# ArmarX.SickLaserUnit.heartbeat.maximumCycleTimeErrorMS: TODO: maximumCycleTimeErrorMS
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+# ArmarX.SickLaserUnit.heartbeat.maximumCycleTimeErrorMS = ::_NOT_SET_::
+
+
+# ArmarX.SickLaserUnit.heartbeat.maximumCycleTimeWarningMS: TODO: maximumCycleTimeWarningMS
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+# ArmarX.SickLaserUnit.heartbeat.maximumCycleTimeWarningMS = ::_NOT_SET_::
+
+
+# ArmarX.SickLaserUnit.heartbeatErrorMS: maximum cycle time before heartbeat Error
+# Attributes:
+# - Default: 800
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SickLaserUnit.heartbeatErrorMS = 800
+
+
+# ArmarX.SickLaserUnit.heartbeatWarnMS: maximum cycle time before heartbeat Warning
+# Attributes:
+# - Default: 500
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SickLaserUnit.heartbeatWarnMS = 500
# ArmarX.SickLaserUnit.hostname: No Description
@@ -196,7 +242,7 @@ ArmarX.SickLaserUnit.protocol = ASCII
# - Default: 10
# - Case sensitivity: yes
# - Required: no
-ArmarX.SickLaserUnit.rangeMax = 10.00
+# ArmarX.SickLaserUnit.rangeMax = 10
# ArmarX.SickLaserUnit.rangeMin: minimum Range of the Scanner
@@ -204,20 +250,21 @@ ArmarX.SickLaserUnit.rangeMax = 10.00
# - Default: 0
# - Case sensitivity: yes
# - Required: no
-ArmarX.SickLaserUnit.rangeMin = 0.01
+# ArmarX.SickLaserUnit.rangeMin = 0
# ArmarX.SickLaserUnit.scannerType: Name of the LaserScanner
# Attributes:
+# - Default: sick_tim_5xx
# - Case sensitivity: yes
-# - Required: yes
-ArmarX.SickLaserUnit.scannerType = sick_tim_5xx
+# - Required: no
+# ArmarX.SickLaserUnit.scannerType = sick_tim_5xx
-# ArmarX.SickLaserUnit.timeIncrement: timeIncrement??
+# ArmarX.SickLaserUnit.timeIncrement: No Description
# Attributes:
-# - Default: 0.10000000000000001
-# - Case sensitivity: yes
+# - Default: 0.1
+# - Case sensitivity: no
# - Required: no
ArmarX.SickLaserUnit.timeIncrement = 0.1
@@ -230,22 +277,6 @@ ArmarX.SickLaserUnit.timeIncrement = 0.1
# ArmarX.SickLaserUnit.timelimit = 5
-# ArmarX.SickLaserUnit.topicName: Name of the topic
-# Attributes:
-# - Default: SICKLaserScanner
-# - Case sensitivity: yes
-# - Required: no
-ArmarX.SickLaserUnit.topicName = "myLaserTopic1"
-
-
-# ArmarX.SickLaserUnit.tpc.sub.LaserScannerUnit: Name of the `LaserScannerUnit` topic to subscribe to.
-# Attributes:
-# - Default: SICKLaserScanner
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SickLaserUnit.tpc.sub.LaserScannerUnit = SICKLaserScanner
-
-
# ArmarX.SickLaserUnit.updatePeriod: No Description
# Attributes:
# - Default: 1
diff --git a/source/RobotAPI/applications/RobotControlUI/CMakeLists.txt b/source/RobotAPI/applications/RobotControlUI/CMakeLists.txt
index 8fe3ebb2c6901804392acd2cbb09edd72dc472bf..c32f3a428572868209dc0789f6fbee00f37c1327 100644
--- a/source/RobotAPI/applications/RobotControlUI/CMakeLists.txt
+++ b/source/RobotAPI/applications/RobotControlUI/CMakeLists.txt
@@ -1,7 +1,7 @@
armarx_component_set_name(RobotControlUI)
-set(COMPONENT_LIBS RobotAPIOperations)
+set(COMPONENT_LIBS RobotAPIOperations Simox::SimoxUtility)
set(SOURCES main.cpp
RobotControlUI.cpp
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index e9157a24d9f60b9ba71d70cccd2141b0258665c3..858bff26243d0580d88d158adddb3fc6222e696f 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/source/RobotAPI/components/ArViz/Client/Elements.h
@@ -1,19 +1,9 @@
#pragma once
-#include <RobotAPI/interface/ArViz/Elements.h>
-#include <SimoxUtility/EigenStdVector.h>
-#include <SimoxUtility/math/normal/normal_to_mat4.h>
-#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
-#include <SimoxUtility/math/pose/transform.h>
-#include <SimoxUtility/shapes/OrientedBoxBase.h>
-#include <Eigen/Core>
-#include <Eigen/Geometry>
-#include <string>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include "Color.h"
#include "elements/ElementOps.h"
@@ -22,6 +12,21 @@
#include "elements/Robot.h"
//#include "elements/RobotHand.h" // Not included by default (exposes additional headers).
+#include <RobotAPI/interface/ArViz/Elements.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <SimoxUtility/math/normal/normal_to_mat4.h>
+#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
+#include <SimoxUtility/math/pose/transform.h>
+#include <SimoxUtility/shapes/OrientedBoxBase.h>
+
+#include <Eigen/Geometry>
+#include <Eigen/Core>
+
+#include <ctime>
+#include <string>
+
// The has_member macro causes compile errors if *any* other header uses
// the identifier has_member. Boost.Thread does, so this causes compile
// errors down the line.
@@ -118,7 +123,7 @@ namespace armarx::viz
template<class T>
Box(
const simox::OrientedBoxBase<T>& b,
- const std::string& name = std::to_string(std::chrono::high_resolution_clock::now().time_since_epoch().count())
+ const std::string& name = std::to_string(std::time(0))
)
: Box(name, b)
{}
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Mesh.h b/source/RobotAPI/components/ArViz/Client/elements/Mesh.h
index a6ac8ae67326b3fe43ab5cd855791917b985dbc6..a75475aee537d035edc6f34dd0ea3dbe0370a01e 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Mesh.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/Mesh.h
@@ -2,8 +2,9 @@
#include <functional>
#include <numeric> // for std::accumulate
+#include <vector>
-#include <SimoxUtility/EigenStdVector.h>
+#include <Eigen/Core>
#include <ArmarXCore/util/CPPUtility/Iterator.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
diff --git a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp
index e2436920ed1c5fe526b7abb556d8a8e4e1605e1f..f16ae2ac1a46eac428476fc1987672da280dd733 100644
--- a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp
@@ -1,11 +1,13 @@
-#include <SimoxUtility/EigenStdVector.h>
-
#include "ElementVisualizer.h"
#include <RobotAPI/components/ArViz/IceConversions.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <Inventor/nodes/SoUnits.h>
+#include <Inventor/nodes/SoTransform.h>
+#include <Inventor/nodes/SoMaterial.h>
+
namespace armarx::viz::coin
{
ElementVisualization::ElementVisualization()
diff --git a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h
index 9be0fa5b5c44d5256ae0adb4cc0c6a74d5dfe619..af58bde80e165d9ccded34548f2d57a58bab5f70 100644
--- a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h
+++ b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h
@@ -1,19 +1,20 @@
#pragma once
-#include <Inventor/nodes/SoMaterial.h>
+#include <RobotAPI/interface/ArViz/Elements.h>
+
#include <Inventor/nodes/SoSeparator.h>
-#include <Inventor/nodes/SoUnits.h>
-#include <Inventor/nodes/SoTransform.h>
#include <memory>
-#include <RobotAPI/interface/ArViz/Elements.h>
-
namespace armarx::viz::data
{
class Element;
}
+class SoUnits;
+class SoTransform;
+class SoMaterial;
+
namespace armarx::viz::coin
{
struct ElementVisualization
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationCylindroid.h b/source/RobotAPI/components/ArViz/Coin/VisualizationCylindroid.h
index 06e58d26a4c1119598bd73ce90e61dfa735f12b7..478f99c019e82321a860f7b6a4be18454f97064a 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationCylindroid.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationCylindroid.h
@@ -7,6 +7,8 @@
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
+#include <Inventor/nodes/SoUnits.h>
+
namespace armarx::viz::coin
{
struct VisualizationCylindroid : TypedElementVisualization<SoSeparator>
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp
index a0190abd75a8438a2371c728a580837d68c3eecc..84c7b3a30512d5c0a93339d1f602c524fd613a38 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp
@@ -7,6 +7,7 @@
#include <Inventor/nodes/SoDrawStyle.h>
#include <Inventor/nodes/SoLineSet.h>
#include <Inventor/nodes/SoPointSet.h>
+#include <Inventor/nodes/SoMaterial.h>
namespace armarx::viz::coin
{
@@ -94,4 +95,4 @@ namespace armarx::viz::coin
return true;
}
-} // namespace armarx::viz::coin
\ No newline at end of file
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
index 778c2db5fd47742488efb3ee0f8ce6aaae4fe0e3..c76af807a1c9457e205de2dbf95d5540f89dfeac 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
@@ -1,23 +1,21 @@
#include "Visualizer.h"
+#include "VisualizationRobot.h"
+#include "VisualizationObject.h"
+
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/GetTypeString.h>
-#include <Inventor/sensors/SoTimerSensor.h>
-#include <Inventor/nodes/SoUnits.h>
-#include <QCoreApplication>
-#include <thread>
-
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/Visualization/VisualizationFactory.h>
+#include <Inventor/nodes/SoUnits.h>
#include <Inventor/actions/SoWriteAction.h>
#include <Inventor/actions/SoToVRML2Action.h>
#include <Inventor/VRMLnodes/SoVRMLGroup.h>
-#include <Inventor/nodes/SoRotation.h>
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
-#include <VirtualRobot/Visualization/VisualizationFactory.h>
-#include "VisualizationRobot.h"
+#include <thread>
namespace armarx::viz
{
@@ -93,8 +91,12 @@ namespace armarx::viz
CoinVisualizer::~CoinVisualizer()
{
- // We need to clear the caches while Coin is still initialized
+ }
+
+ void CoinVisualizer::clearCache()
+ {
coin::clearRobotCache();
+ coin::clearObjectCache();
}
@@ -119,12 +121,7 @@ namespace armarx::viz
return;
}
- state = CoinVisualizerState::STOPPING;
- while (state != CoinVisualizerState::STOPPED)
- {
- QCoreApplication::processEvents();
- usleep(1000);
- }
+ state = CoinVisualizerState::STOPPED;
}
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 1494be07dc5615b787f2a50f12a48451b40e045a..195172e9c812c4fef9bd391853748592e113b5d8 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.h
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
@@ -191,6 +191,8 @@ namespace armarx::viz
~CoinVisualizer();
+ void clearCache();
+
void registerVisualizationTypes();
void startAsync(StorageInterfacePrx const& storage);
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
index 487c9c6fc4c732fc1d4fe2f126724cb74c8dd810..70b399165ad3c229d96289676e0da6f2280e91a4 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
@@ -42,15 +42,15 @@
#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>
namespace armarx::articulated_object
{
ArticulatedObjectLocalizerExample::ArticulatedObjectLocalizerExample() :
- articulatedObjectWriter(new ::armarx::armem::articulated_object::ArticulatedObjectWriter(memoryNameSystem)),
- articulatedObjectReader(new ::armarx::armem::articulated_object::ArticulatedObjectReader(memoryNameSystem))
+ articulatedObjectWriter(new ::armarx::armem::articulated_object::ArticulatedObjectWriter(memoryNameSystem())),
+ articulatedObjectReader(new ::armarx::armem::articulated_object::ArticulatedObjectReader(memoryNameSystem()))
{
}
@@ -90,7 +90,8 @@ namespace armarx::articulated_object
start = armem::Time::now();
task = new PeriodicTask<ArticulatedObjectLocalizerExample>(
- this, &ArticulatedObjectLocalizerExample::run, 1000.f / p.updateFrequency);
+ this, &ArticulatedObjectLocalizerExample::run,
+ static_cast<int>(1000.f / p.updateFrequency));
task->start();
}
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
index f85a804e97337e1193ca1f38048beb103e65ce59..e3747fb3f23fb4429ab7a9d028d85efd3f2befaa 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
@@ -13,8 +13,8 @@
#include "RobotAPI/libraries/armem/core/Time.h"
#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/client/plugins.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/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index b59531d4c840f9d7bbfb773f2a5da361bedbcf39..1987708750187cc7a7e212b6c2c105157707b6ff 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -55,8 +55,7 @@
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <boost/algorithm/string/predicate.hpp>
-#include <boost/algorithm/string/replace.hpp>
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/XML/RobotIO.h>
@@ -68,7 +67,7 @@ using namespace VirtualRobot;
#define SELECTION_NAME_PREFIX ("selection_" + std::to_string(reinterpret_cast<std::uintptr_t>(this)))
#define SELECTION_NAME_SPLITTER "____"
-#define SELECTION_NAME(layerName, elementName) boost::replace_all_copy(std::string(SELECTION_NAME_PREFIX + "_" + layerName + SELECTION_NAME_SPLITTER + elementName), " ", "_").c_str()
+#define SELECTION_NAME(layerName, elementName) simox::alg::replace_all(std::string(SELECTION_NAME_PREFIX + "_" + layerName + SELECTION_NAME_SPLITTER + elementName), " ", "_").c_str()
namespace armarx
{
@@ -1162,7 +1161,7 @@ namespace armarx
return;
}
- std::string entryName = boost::replace_all_copy(std::string("__" + d.layerName + "__" + d.name + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + d.layerName + "__" + d.name + "__"), " ", "_");
VirtualRobot::RobotPtr robot = activeRobots[entryName];
std::vector<std::string> nodeNameList;
@@ -1234,7 +1233,7 @@ namespace armarx
auto l = getScopedVisuLock();
auto& layer = requestLayer(d.layerName);
- std::string entryName = boost::replace_all_copy(std::string("__" + d.layerName + "__" + d.name + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + d.layerName + "__" + d.name + "__"), " ", "_");
ARMARX_DEBUG << "Requesting robot " << entryName;
@@ -1563,7 +1562,7 @@ namespace armarx
auto l = getScopedVisuLock();
// process active robots
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + name + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + name + "__"), " ", "_");
ARMARX_DEBUG << "Removing robot " << entryName;
if (activeRobots.find(entryName) != activeRobots.end())
@@ -1686,7 +1685,7 @@ namespace armarx
Eigen::Matrix4f gp = PosePtr::dynamicCast(globalPose)->toEigen();
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + poseName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + poseName + "__"), " ", "_");
CoordData& d = accumulatedUpdateData.coord[entryName];
d.globalPose = gp;
d.layerName = layerName;
@@ -1715,7 +1714,7 @@ namespace armarx
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + poseName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + poseName + "__"), " ", "_");
CoordData& d = accumulatedUpdateData.coord[entryName];
d.layerName = layerName;
d.name = poseName;
@@ -1735,7 +1734,7 @@ namespace armarx
VirtualRobot::VisualizationFactory::Color c(color.r, color.g, color.b, 1 - color.a);
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + lineName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + lineName + "__"), " ", "_");
LineData& d = accumulatedUpdateData.line[entryName];
d.p1 = p1;
d.p2 = p2;
@@ -1756,7 +1755,7 @@ namespace armarx
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + lineName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + lineName + "__"), " ", "_");
LineData& d = accumulatedUpdateData.line[entryName];
d.layerName = layerName;
d.name = lineName;
@@ -1774,7 +1773,7 @@ namespace armarx
ARMARX_DEBUG << VAROUT(layerName) << VAROUT(lineSetName);
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + lineSetName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + lineSetName + "__"), " ", "_");
LineSetData& d = accumulatedUpdateData.lineSet[entryName];
d.lineSet = lineSet;
d.layerName = layerName;
@@ -1792,7 +1791,7 @@ namespace armarx
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + lineSetName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + lineSetName + "__"), " ", "_");
LineSetData& d = accumulatedUpdateData.lineSet[entryName];
d.layerName = layerName;
d.name = lineSetName;
@@ -1811,7 +1810,7 @@ namespace armarx
VirtualRobot::VisualizationFactory::Color c(color.r, color.g, color.b, 1 - color.a);
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + boxName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + boxName + "__"), " ", "_");
BoxData& d = accumulatedUpdateData.box[entryName];
d.width = dimensions->x;
d.height = dimensions->y;
@@ -1833,7 +1832,7 @@ namespace armarx
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + boxName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + boxName + "__"), " ", "_");
BoxData& d = accumulatedUpdateData.box[entryName];
d.layerName = layerName;
d.name = boxName;
@@ -1851,7 +1850,7 @@ namespace armarx
ARMARX_DEBUG << VAROUT(layerName) << VAROUT(textName);
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + textName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + textName + "__"), " ", "_");
TextData& d = accumulatedUpdateData.text[entryName];
d.text = text;
d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
@@ -1872,7 +1871,7 @@ namespace armarx
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + textName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + textName + "__"), " ", "_");
TextData& d = accumulatedUpdateData.text[entryName];
d.layerName = layerName;
d.name = textName;
@@ -1890,7 +1889,7 @@ namespace armarx
ARMARX_DEBUG << VAROUT(layerName) << VAROUT(sphereName);
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + sphereName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + sphereName + "__"), " ", "_");
SphereData& d = accumulatedUpdateData.sphere[entryName];
d.radius = radius;
d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
@@ -1910,7 +1909,7 @@ namespace armarx
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + sphereName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + sphereName + "__"), " ", "_");
SphereData& d = accumulatedUpdateData.sphere[entryName];
d.layerName = layerName;
d.name = sphereName;
@@ -1928,7 +1927,7 @@ namespace armarx
ARMARX_DEBUG << VAROUT(layerName) << VAROUT(cylinderName);
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + cylinderName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + cylinderName + "__"), " ", "_");
CylinderData& d = accumulatedUpdateData.cylinder[entryName];
d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
d.direction = Vector3Ptr::dynamicCast(direction)->toEigen();
@@ -1950,7 +1949,7 @@ namespace armarx
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + cylinderName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + cylinderName + "__"), " ", "_");
CylinderData& d = accumulatedUpdateData.cylinder[entryName];
d.layerName = layerName;
d.name = cylinderName;
@@ -1968,7 +1967,7 @@ namespace armarx
ARMARX_DEBUG << VAROUT(layerName) << VAROUT(pointCloudName);
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
PointCloudData& d = accumulatedUpdateData.pointcloud[entryName];
d.pointCloud = pointCloud;
d.layerName = layerName;
@@ -1986,7 +1985,7 @@ namespace armarx
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
PointCloudData& d = accumulatedUpdateData.pointcloud[entryName];
d.layerName = layerName;
d.name = pointCloudName;
@@ -2003,7 +2002,7 @@ namespace armarx
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + polygonName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + polygonName + "__"), " ", "_");
PolygonData& d = accumulatedUpdateData.polygons[entryName];
std::vector< Eigen::Vector3f > points;
@@ -2034,7 +2033,7 @@ namespace armarx
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + polygonName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + polygonName + "__"), " ", "_");
PolygonData& d = accumulatedUpdateData.polygons[entryName];
d.layerName = layerName;
d.name = polygonName;
@@ -2051,7 +2050,7 @@ namespace armarx
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + triMeshName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + triMeshName + "__"), " ", "_");
TriMeshData& d = accumulatedUpdateData.triMeshes[entryName];
d.triMesh = triMesh;
d.layerName = layerName;
@@ -2069,7 +2068,7 @@ namespace armarx
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + triMeshName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + triMeshName + "__"), " ", "_");
TriMeshData& d = accumulatedUpdateData.triMeshes[entryName];
d.layerName = layerName;
d.name = triMeshName;
@@ -2086,7 +2085,7 @@ namespace armarx
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + arrowName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + arrowName + "__"), " ", "_");
ArrowData& d = accumulatedUpdateData.arrows[entryName];
d.position = Vector3Ptr::dynamicCast(position)->toEigen();
d.direction = Vector3Ptr::dynamicCast(direction)->toEigen();
@@ -2108,7 +2107,7 @@ namespace armarx
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + arrowName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + arrowName + "__"), " ", "_");
ArrowData& d = accumulatedUpdateData.arrows[entryName];
d.layerName = layerName;
d.name = arrowName;
@@ -2124,7 +2123,7 @@ namespace armarx
void DebugDrawerComponent::setRobotVisu(const std::string& layerName, const std::string& robotName, const std::string& robotFile, const std::string& armarxProject, DrawStyle drawStyle, const Ice::Current&)
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
ARMARX_DEBUG << "setting robot visualization for " << entryName;
RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2140,7 +2139,7 @@ namespace armarx
void DebugDrawerComponent::updateRobotPose(const std::string& layerName, const std::string& robotName, const PoseBasePtr& globalPose, const Ice::Current&)
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
ARMARX_DEBUG << "updating robot pose for " << entryName;
RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2157,7 +2156,7 @@ namespace armarx
void DebugDrawerComponent::updateRobotConfig(const std::string& layerName, const std::string& robotName, const std::map<std::string, float>& configuration, const Ice::Current&)
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
ARMARX_DEBUG << "updating robot config for " << entryName;
RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2178,7 +2177,7 @@ namespace armarx
void DebugDrawerComponent::updateRobotColor(const std::string& layerName, const std::string& robotName, const DrawColor& c, const Ice::Current&)
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
ARMARX_DEBUG << "updating robot color for " << entryName;
RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2203,7 +2202,7 @@ namespace armarx
void DebugDrawerComponent::updateRobotNodeColor(const std::string& layerName, const std::string& robotName, const std::string& robotNodeName, const DrawColor& c, const Ice::Current&)
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
ARMARX_DEBUG << "updating robot color for " << entryName;
RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2229,7 +2228,7 @@ namespace armarx
void DebugDrawerComponent::removeRobotVisu(const std::string& layerName, const std::string& robotName, const Ice::Current&)
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
ARMARX_DEBUG << "removing robot visu for " << entryName;
RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2760,7 +2759,7 @@ namespace armarx
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
ColoredPointCloudData& d = accumulatedUpdateData.coloredpointcloud[entryName];
d.pointCloud = pointCloud;
d.layerName = layerName;
@@ -2778,7 +2777,7 @@ namespace armarx
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
ColoredPointCloudData& d = accumulatedUpdateData.coloredpointcloud[entryName];
d.layerName = layerName;
d.name = pointCloudName;
@@ -2876,7 +2875,7 @@ namespace armarx
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
Colored24BitPointCloudData& d = accumulatedUpdateData.colored24Bitpointcloud[entryName];
d.pointCloud = pointCloud;
d.layerName = layerName;
@@ -2894,7 +2893,7 @@ namespace armarx
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
Colored24BitPointCloudData& d = accumulatedUpdateData.colored24Bitpointcloud[entryName];
d.layerName = layerName;
d.name = pointCloudName;
@@ -2911,7 +2910,7 @@ namespace armarx
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + circleName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + circleName + "__"), " ", "_");
CircleData& d = accumulatedUpdateData.circle[entryName];
d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
d.direction = Vector3Ptr::dynamicCast(directionVec)->toEigen();
@@ -2934,7 +2933,7 @@ namespace armarx
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + circleName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + circleName + "__"), " ", "_");
CircleData& d = accumulatedUpdateData.circle[entryName];
d.layerName = layerName;
d.name = circleName;
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
index 5f2d8065a307706ea16789930c12f5165d6044eb..f0820c595d0651933255fc1d207f54c40f6c8aae 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
@@ -32,7 +32,6 @@
#include <shared_mutex>
// Eigen
-#include <SimoxUtility/EigenStdVector.h>
#include <Eigen/Geometry>
// Ice
diff --git a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
index f718a45434ec538fbd5d2541fb9413568f3e79f0..3cebd4612e8c94ddb015480e8079dd5859ed741e 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
@@ -39,6 +39,9 @@ using namespace Ice;
// ArmarX
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/libraries/core/Pose.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
#include <opencv2/core/core_c.h>
using namespace Eigen;
diff --git a/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt b/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt
index b12677c3ff545b9d18e7e11b95d2197c36434dd1..23861609c8852bae3dcfcc013e03ecb13d41c704 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt
+++ b/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt
@@ -3,6 +3,10 @@ armarx_component_set_name("EarlyVisionGraph")
find_package(Eigen3 QUIET)
armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+set(COMPONENT_LIBS
+ Eigen3::Eigen
+)
+
set(SOURCES
./MathTools.cpp
./SphericalGraph.cpp
@@ -26,6 +30,6 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 ")
armarx_add_component("${SOURCES}" "${HEADERS}")
if(Eigen3_FOUND)
- target_include_directories(EarlyVisionGraph SYSTEM PUBLIC ${Eigen3_INCLUDE_DIR})
+ target_include_directories(EarlyVisionGraph SYSTEM PUBLIC ${Eigen3_INCLUDE_DIR} ${Eigen3_INCLUDE_DIRS})
endif()
diff --git a/source/RobotAPI/components/KITHandUnit/KITHandUnit.cpp b/source/RobotAPI/components/KITHandUnit/KITHandUnit.cpp
index b3cb24cfd9d4249aeecbd474f5c040ed488861cd..fb4cb1d94baf63e8013099a443d18890fb5c6352 100644
--- a/source/RobotAPI/components/KITHandUnit/KITHandUnit.cpp
+++ b/source/RobotAPI/components/KITHandUnit/KITHandUnit.cpp
@@ -20,12 +20,12 @@
* GNU General Public License
*/
-#include <boost/algorithm/clamp.hpp>
-
#include "KITHandUnit.h"
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
+#include <algorithm>
+
using namespace KITHand;
namespace armarx
@@ -163,17 +163,19 @@ namespace armarx
{
if (pair.first == "Fingers")
{
- const std::uint64_t pos = boost::algorithm::clamp(
+ const std::uint64_t pos = std::clamp(
static_cast<std::uint64_t>(pair.second * KITHand::ControlOptions::maxPosFingers),
- 0, KITHand::ControlOptions::maxPosFingers);
+ static_cast<std::uint64_t>(0),
+ KITHand::ControlOptions::maxPosFingers);
ARMARX_DEBUG << deactivateSpam(1, std::to_string(pos)) << "set fingers " << pos;
_driver->sendFingersPosition(pos);
}
else if (pair.first == "Thumb")
{
- const std::uint64_t pos = boost::algorithm::clamp(
+ const std::uint64_t pos = std::clamp(
static_cast<std::uint64_t>(pair.second * KITHand::ControlOptions::maxPosThumb),
- 0, KITHand::ControlOptions::maxPosThumb);
+ static_cast<std::uint64_t>(0),
+ KITHand::ControlOptions::maxPosThumb);
ARMARX_DEBUG << deactivateSpam(1, std::to_string(pos)) << "set thumb " << pos;
_driver->sendThumbPosition(pos);
}
diff --git a/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp b/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp
index b68904c7c60a2790bc59d8174b1af6283f55f82e..997be9e96b87d88593d27fd7120f332a21912b05 100644
--- a/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp
+++ b/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp
@@ -20,16 +20,14 @@
* GNU General Public License
*/
-#include <thread>
-#include <regex>
-
-#include <boost/algorithm/clamp.hpp>
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include "KITProstheticHandUnit.h"
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "KITProstheticHandUnit.h"
+#include <algorithm>
+#include <thread>
+#include <regex>
namespace armarx
{
@@ -153,9 +151,9 @@ namespace armarx
{
if (pair.first == "Fingers")
{
- const std::uint64_t pos = boost::algorithm::clamp(
+ const std::uint64_t pos = std::clamp(
static_cast<std::uint64_t>(pair.second * _driver->getMaxPosFingers()),
- 0, _driver->getMaxPosFingers());
+ 0ul, _driver->getMaxPosFingers());
ARMARX_INFO << "set fingers " << pos;
_driver->sendFingerPWM(200, 2999, pos);
// fix until hw driver is fixed to handle multiple commands at the same time
@@ -163,9 +161,9 @@ namespace armarx
}
else if (pair.first == "Thumb")
{
- const std::uint64_t pos = boost::algorithm::clamp(
+ const std::uint64_t pos = std::clamp(
static_cast<std::uint64_t>(pair.second * _driver->getMaxPosThumb()),
- 0, _driver->getMaxPosThumb());
+ 0ul, _driver->getMaxPosThumb());
ARMARX_INFO << "set thumb " << pos;
_driver->sendThumbPWM(200, 2999, pos);
// fix until hw driver is fixed to handle multiple commands at the same time
@@ -199,7 +197,7 @@ namespace armarx
shapeNames->addVariant(currentPreshape);
}
- void KITProstheticHandUnit::setShape(const std::string& shapeName, const Ice::Current& c)
+ void KITProstheticHandUnit::setShape(const std::string& shapeName, const Ice::Current&)
{
if (std::regex_match(shapeName, std::regex{"[gG](0|[1-9][0-9]*)"}))
{
diff --git a/source/RobotAPI/components/armem/MemoryNameSystem/CMakeLists.txt b/source/RobotAPI/components/armem/MemoryNameSystem/CMakeLists.txt
index 75a50c54304d11e5452f1e4d1f8287a65fd0e9f6..fa06c3e77bf057e9ca37e790f908e981db1147af 100644
--- a/source/RobotAPI/components/armem/MemoryNameSystem/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/MemoryNameSystem/CMakeLists.txt
@@ -19,4 +19,6 @@ set(HEADERS
armarx_add_component("${SOURCES}" "${HEADERS}")
#generate the application
-armarx_generate_and_add_component_executable()
+armarx_generate_and_add_component_executable(
+ COMPONENT_NAMESPACE "armarx::armem"
+)
diff --git a/source/RobotAPI/components/armem/MemoryNameSystem/MemoryNameSystem.cpp b/source/RobotAPI/components/armem/MemoryNameSystem/MemoryNameSystem.cpp
index 47131a79a44f25dabc1e3364fcc7d3d044b3e506..0ebd0eac1dad18f7c53374c4a4fcc5ba173bbf14 100644
--- a/source/RobotAPI/components/armem/MemoryNameSystem/MemoryNameSystem.cpp
+++ b/source/RobotAPI/components/armem/MemoryNameSystem/MemoryNameSystem.cpp
@@ -29,16 +29,12 @@
#include <RobotAPI/libraries/armem/core/error.h>
-namespace armarx
+namespace armarx::armem
{
- MemoryNameSystemPropertyDefinitions::MemoryNameSystemPropertyDefinitions(std::string prefix) :
- armarx::ComponentPropertyDefinitions(prefix)
- {
- }
armarx::PropertyDefinitionsPtr MemoryNameSystem::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new MemoryNameSystemPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
return defs;
}
@@ -58,7 +54,7 @@ namespace armarx
void MemoryNameSystem::onConnectComponent()
{
- RemoteGui__createTab();
+ createRemoteGuiTab();
RemoteGui_startRunningTask();
}
@@ -73,19 +69,19 @@ namespace armarx
}
- armem::data::RegisterMemoryResult MemoryNameSystem::registerMemory(
- const armem::data::RegisterMemoryInput& input, const Ice::Current& c)
+ mns::dto::RegisterServerResult MemoryNameSystem::registerServer(
+ const mns::dto::RegisterServerInput& input, const Ice::Current& c)
{
- armem::data::RegisterMemoryResult result = Plugin::registerMemory(input, c);
+ mns::dto::RegisterServerResult result = PluginUser::registerServer(input, c);
tab.rebuild = true;
return result;
}
- armem::data::RemoveMemoryResult MemoryNameSystem::removeMemory(
- const armem::data::RemoveMemoryInput& input, const Ice::Current& c)
+ mns::dto::RemoveServerResult MemoryNameSystem::removeServer(
+ const mns::dto::RemoveServerInput& input, const Ice::Current& c)
{
- armem::data::RemoveMemoryResult result = Plugin::removeMemory(input, c);
+ mns::dto::RemoveServerResult result = PluginUser::removeServer(input, c);
tab.rebuild = true;
return result;
}
@@ -94,12 +90,12 @@ namespace armarx
// REMOTE GUI
- void MemoryNameSystem::RemoteGui__createTab()
+ void MemoryNameSystem::createRemoteGuiTab()
{
using namespace armarx::RemoteGui::Client;
std::scoped_lock lock(mnsMutex);
- GridLayout grid = mns.RemoteGui_buildInfoGrid();
+ GridLayout grid = mns().RemoteGui_buildInfoGrid();
VBoxLayout root = {grid, VSpacer()};
RemoteGui_createTab(getName(), root, &tab);
@@ -110,7 +106,7 @@ namespace armarx
{
if (tab.rebuild.exchange(false))
{
- RemoteGui__createTab();
+ createRemoteGuiTab();
}
}
diff --git a/source/RobotAPI/components/armem/MemoryNameSystem/MemoryNameSystem.h b/source/RobotAPI/components/armem/MemoryNameSystem/MemoryNameSystem.h
index 795aecc66f96423aa29b28660694e09ea50a1426..f0279f4a1103a6b6f5a88f4b31ab5692cc5bda74 100644
--- a/source/RobotAPI/components/armem/MemoryNameSystem/MemoryNameSystem.h
+++ b/source/RobotAPI/components/armem/MemoryNameSystem/MemoryNameSystem.h
@@ -22,33 +22,16 @@
#pragma once
-#include <mutex>
-
-#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/libraries/armem/mns/plugins/PluginUser.h>
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
-
-#include <RobotAPI/libraries/armem/mns/ComponentPlugin.h>
+#include <ArmarXCore/core/Component.h>
-namespace armarx
+namespace armarx::armem
{
- /**
- * @class MemoryNameSystemPropertyDefinitions
- * @brief Property definitions of `MemoryNameSystem`.
- */
- class MemoryNameSystemPropertyDefinitions :
- public armarx::ComponentPropertyDefinitions
- {
- public:
- MemoryNameSystemPropertyDefinitions(std::string prefix);
- };
-
-
-
/**
* @defgroup Component-MemoryNameSystem MemoryNameSystem
* @ingroup RobotAPI-Components
@@ -62,11 +45,9 @@ namespace armarx
*/
class MemoryNameSystem :
virtual public armarx::Component
- , virtual public armem::mns::ComponentPluginUser
+ , virtual public armem::mns::plugins::PluginUser
, virtual public LightweightRemoteGuiComponentPluginUser
- // , virtual public armarx::ArVizComponentPluginUser
{
- using Plugin = ComponentPluginUser;
public:
/// @see armarx::ManagedIceObject::getDefaultName()
@@ -75,20 +56,23 @@ namespace armarx
// mns::MemoryNameSystemInterface interface
public:
- armem::data::RegisterMemoryResult registerMemory(const armem::data::RegisterMemoryInput& input, const Ice::Current&) override;
- armem::data::RemoveMemoryResult removeMemory(const armem::data::RemoveMemoryInput& input, const Ice::Current&) override;
+ mns::dto::RegisterServerResult registerServer(const mns::dto::RegisterServerInput& input, const Ice::Current&) override;
+ mns::dto::RemoveServerResult removeServer(const mns::dto::RemoveServerInput& input, const Ice::Current&) override;
// Others are inherited from ComponentPluginUser
// LightweightRemoteGuiComponentPluginUser interface
public:
- void RemoteGui__createTab();
+ void createRemoteGuiTab();
void RemoteGui_update() override;
protected:
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
/// @see armarx::ManagedIceObject::onInitComponent()
void onInitComponent() override;
@@ -101,17 +85,13 @@ namespace armarx
/// @see armarx::ManagedIceObject::onExitComponent()
void onExitComponent() override;
- /// @see PropertyUser::createPropertyDefinitions()
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
-
private:
struct Properties
{
-
};
- Properties p;
+ Properties properties;
struct RemoteGuiTab : RemoteGui::Client::Tab
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/CMakeLists.txt b/source/RobotAPI/components/armem/client/ExampleMemoryClient/CMakeLists.txt
index d699638cd0eb88c194fd66a9382968012ce40660..96cb0013af5534281083ba693c20ff83770f6155 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/CMakeLists.txt
@@ -3,11 +3,17 @@ armarx_component_set_name("ExampleMemoryClient")
find_package(IVT QUIET)
armarx_build_if(IVT_FOUND "IVT not available")
+find_package(OpenCV QUIET)
+armarx_build_if(OpenCV_FOUND "OpenCV not available")
+
+
set(COMPONENT_LIBS
ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
ArmarXGuiComponentPlugins
RobotAPICore RobotAPIInterfaces armem
+
${IVT_LIBRARIES}
+ ${OpenCV_LIBS}
)
set(SOURCES
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
index 8ae42e00bd5385886696dd6ead35feb927665947..d8b6e4f89038a42fc137a78316cfa3fbd7bc7eb0 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
@@ -22,21 +22,34 @@
#include "ExampleMemoryClient.h"
-#include <random>
-
-#include <SimoxUtility/color/cmaps.h>
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
+#include <RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.aron.generated.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#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/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/operations.h>
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
-#include <RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.aron.generated.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+#include <SimoxUtility/color/cmaps.h>
+#include <SimoxUtility/math/pose/pose.h>
+
+#include <Eigen/Geometry>
+
+#include <random>
+
+
+#define STORE_IMAGES 0
+
+#if STORE_IMAGES
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/imgproc.hpp>
+#endif
namespace armarx
@@ -75,8 +88,8 @@ namespace armarx
ARMARX_IMPORTANT << "Waiting for memory '" << p.usedMemoryName << "' ...";
try
{
- memoryReader = memoryNameSystem.useReader(p.usedMemoryName);
- memoryWriter = memoryNameSystem.useWriter(p.usedMemoryName);
+ memoryReader = memoryNameSystem().useReader(p.usedMemoryName);
+ memoryWriter = memoryNameSystem().useWriter(p.usedMemoryName);
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -92,12 +105,14 @@ namespace armarx
// Subscribe to example_entity updates
// Using a lambda:
- memoryNameSystem.subscribe(exampleEntityID, [&](const armem::MemoryID & exampleEntityID, const std::vector<armem::MemoryID>& snapshotIDs)
+ memoryNameSystem().subscribe(
+ exampleEntityID,
+ [&](const armem::MemoryID & exampleEntityID, const std::vector<armem::MemoryID>& snapshotIDs)
{
ARMARX_INFO << "Entity " << exampleEntityID << " was updated by " << snapshotIDs.size() << " snapshots.";
});
// Using a member function:
- memoryNameSystem.subscribe(exampleEntityID, this, &ExampleMemoryClient::processExampleEntityUpdate);
+ memoryNameSystem().subscribe(exampleEntityID, this, &ExampleMemoryClient::processExampleEntityUpdate);
task = new RunningTask<ExampleMemoryClient>(this, &ExampleMemoryClient::run);
@@ -120,7 +135,6 @@ namespace armarx
{
ARMARX_IMPORTANT << "Running example.";
run_started = IceUtil::Time::now();
- std::srand(std::time(nullptr));
armem::MemoryID snapshotID = commitSingleSnapshot(exampleEntityID);
if (true)
@@ -175,6 +189,9 @@ namespace armarx
armem::MemoryID ExampleMemoryClient::commitSingleSnapshot(const armem::MemoryID& entityID)
{
+ std::default_random_engine gen(std::random_device{}());
+ std::uniform_int_distribution<int> distrib(-20, 20);
+
// Prepare the update with some empty instances.
armem::EntityUpdate update;
update.entityID = entityID;
@@ -190,7 +207,7 @@ namespace armarx
auto sqrt = std::make_shared<aron::datanavigator::DoubleNavigator>(std::sqrt(diff));
auto lin = std::make_shared<aron::datanavigator::LongNavigator>(static_cast<long>(diff * 1000));
- auto rand = std::make_shared<aron::datanavigator::IntNavigator>(std::rand());
+ auto rand = std::make_shared<aron::datanavigator::IntNavigator>(distrib(gen));
dict1->addElement("sin", sin);
dict1->addElement("cos", cos);
@@ -257,40 +274,27 @@ namespace armarx
armem::client::QueryResult qResult = memoryReader.query(builder.buildQueryInput());
ARMARX_INFO << qResult;
-
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)
+ << "Entity " << entityID << " was not found in " << armem::print(memory);
+ 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;
@@ -389,25 +393,59 @@ namespace armarx
{ "three", 3 },
};
+ data.the_position = { 42, 24, 4224 };
+ data.the_orientation = Eigen::AngleAxisf(1.57f, Eigen::Vector3f(1, 1, 1).normalized());
+ data.the_pose = simox::math::pose(data.the_position, data.the_orientation);
+
+ data.the_3x1_vector = { 24, 42, 2442 };
+ data.the_4x4_matrix = 42 * Eigen::Matrix4f::Identity();
+
toAron(data.memoryLink, armem::MemoryID());
+ simox::ColorMap cmap = simox::color::cmaps::plasma();
{
- data.the_ivt_image = std::make_shared<CByteImage>();
- CByteImage& image = *data.the_ivt_image;
- image.Set(20, 10, CByteImage::ImageType::eRGB24);
- simox::ColorMap cmap = simox::color::cmaps::plasma();
- cmap.set_vmax(image.width + image.height);
- for (int row = 0; row < image.height; ++row)
+ cv::Mat& image = data.the_rgb24_image;
+ image.create(10, 20, image.type());
+ cmap.set_vlimits(0, float(image.cols + image.rows));
+ using Pixel = cv::Point3_<uint8_t>;
+ image.forEach<Pixel>([&cmap](Pixel& pixel, const int index[]) -> void
+ {
+ simox::Color color = cmap(float(index[0] + index[1]));
+ pixel.x = color.r;
+ pixel.y = color.g;
+ pixel.z = color.b;
+ });
+
+#if STORE_IMAGES
+ cv::Mat out;
+ cv::cvtColor(image, out, CV_RGB2BGR);
+ cv::imwrite("the_rgb24_image.png", out);
+#endif
+ }
+ {
+ cv::Mat& image = data.the_depth32_image;
+ image.create(20, 10, image.type());
+ image.forEach<float>([&image](float& pixel, const int index[]) -> void
+ {
+ pixel = 100 * float(index[0] + index[1]) / float(image.rows + image.cols);
+ });
+
+#if STORE_IMAGES
+ cmap.set_vlimits(0, 100);
+ cv::Mat rgb(image.rows, image.cols, CV_8UC3);
+ using Pixel = cv::Point3_<uint8_t>;
+ rgb.forEach<Pixel>([&image, &cmap](Pixel& pixel, const int index[]) -> void
{
- for (int col = 0; col < image.width; ++col)
- {
- simox::Color color = cmap(row + col);
- unsigned char* p = &image.pixels[(row * image.width + col) * image.bytesPerPixel];
- p[0] = color.r;
- p[1] = color.g;
- p[2] = color.b;
- }
- }
+ simox::Color color = cmap(image.at<float>(index));
+ pixel.x = color.r;
+ pixel.y = color.g;
+ pixel.z = color.b;
+ });
+
+ cv::Mat out;
+ cv::cvtColor(rgb, out, CV_RGB2BGR);
+ cv::imwrite("the_depth32_image.png", out);
+#endif
}
update.instancesData = { data.toAron() };
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
index 5680e316a6228021932afea5d73abd02fdcda6d8..40eb57625848ba442e7be2abb10ed1c832ee8440 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
@@ -23,18 +23,17 @@
#pragma once
+#include <RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-// ArmarX
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/util/tasks.h>
-#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-
-// RobotAPI
-#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>
namespace armarx
@@ -54,8 +53,8 @@ namespace armarx
*/
class ExampleMemoryClient :
virtual public armarx::Component,
- virtual public armarx::armem::client::ComponentPluginUser,
- virtual public LightweightRemoteGuiComponentPluginUser
+ virtual public armarx::armem::ListeningClientPluginUser,
+ virtual public armarx::LightweightRemoteGuiComponentPluginUser
{
public:
@@ -111,6 +110,8 @@ namespace armarx
};
Properties p;
+ armem::client::Reader memoryReader;
+ armem::client::Writer memoryWriter;
armem::MemoryID exampleProviderID;
armem::MemoryID exampleEntityID;
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
index 6650e573785cce732f0f2bbe7c128aa34fc58626..552f68ac7ea45c24a0041dbedaff43a06c3e2c31 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>
@@ -31,7 +31,7 @@
namespace armarx::robot_state
{
VirtualRobotReaderExampleClient::VirtualRobotReaderExampleClient() :
- virtualRobotReader(this->memoryNameSystem) {}
+ virtualRobotReader(this->memoryNameSystem()) {}
armarx::PropertyDefinitionsPtr VirtualRobotReaderExampleClient::createPropertyDefinitions()
{
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
index 4e1d01f5564e96c7e46ad2db33081835873dd7a6..8111d62c43d3b70546e4d070dd7aec7f55fc717f 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
@@ -32,8 +32,8 @@
// RobotAPI
#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/client/plugins.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/CMakeLists.txt b/source/RobotAPI/components/armem/server/ExampleMemory/CMakeLists.txt
index 99070a2c52f970a6ac9f64992a5a9a113de7f227..cc053e479e74106b7a682d6c2c40549eb486f5d0 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/CMakeLists.txt
@@ -8,7 +8,6 @@ set(COMPONENT_LIBS
ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
ArmarXGuiComponentPlugins
RobotAPICore RobotAPIInterfaces armem
- # RobotAPIComponentPlugins # for ArViz and other plugins
${IVT_LIBRARIES}
)
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
index e007e8f5e3ad47c57f9bb71c75e517d57af1addf..446c0e2294ba03c7b9042d98416fc5a27f1513a7 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
@@ -40,7 +40,7 @@ namespace armarx
defs->topic(debugObserver);
- defs->optional(p.memoryName, "memory.Name", "Name of this memory (server).");
+ workingMemory().name() = "Example";
p.core._defaultSegmentsStr = simox::alg::join(p.core.defaultCoreSegments, ", ");
defs->optional(p.core._defaultSegmentsStr, "core.DefaultSegments",
@@ -60,22 +60,23 @@ namespace armarx
void ExampleMemory::onInitComponent()
{
- workingMemory.name() = p.memoryName;
- longtermMemory.name() = p.memoryName;
-
// Usually, the memory server will specify a number of core segments with a specific aron type.
- workingMemory.addCoreSegment("ExampleData", armem::example::ExampleData::toAronType());
+ workingMemory().addCoreSegment("ExampleData", armem::example::ExampleData::toAronType());
// For illustration purposes, we add more segments (without types).
bool trim = true;
p.core.defaultCoreSegments = simox::alg::split(p.core._defaultSegmentsStr, ",", trim);
p.core._defaultSegmentsStr.clear();
- workingMemory.addCoreSegments(p.core.defaultCoreSegments);
+
+ for (const std::string& name : p.core.defaultCoreSegments)
+ {
+ workingMemory().addCoreSegment(name);
+ }
}
void ExampleMemory::onConnectComponent()
{
- RemoteGui__createTab();
+ createRemoteGuiTab();
RemoteGui_startRunningTask();
}
@@ -94,7 +95,7 @@ namespace armarx
armem::data::AddSegmentsResult ExampleMemory::addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&)
{
// This function is overloaded to trigger the remote gui rebuild.
- armem::data::AddSegmentsResult result = ComponentPluginUser::addSegments(input, p.core.addOnUsage);
+ armem::data::AddSegmentsResult result = ReadWritePluginUser::addSegments(input, p.core.addOnUsage);
tab.rebuild = true;
return result;
}
@@ -103,7 +104,7 @@ namespace armarx
armem::data::CommitResult ExampleMemory::commit(const armem::data::Commit& commit, const Ice::Current&)
{
// This function is overloaded to trigger the remote gui rebuild.
- armem::data::CommitResult result = ComponentPluginUser::commit(commit);
+ armem::data::CommitResult result = ReadWritePluginUser::commit(commit);
tab.rebuild = true;
return result;
}
@@ -117,13 +118,13 @@ namespace armarx
// REMOTE GUI
- void ExampleMemory::RemoteGui__createTab()
+ void ExampleMemory::createRemoteGuiTab()
{
using namespace armarx::RemoteGui::Client;
{
// Core segments are locked by MemoryRemoteGui.
- tab.memoryGroup = armem::server::MemoryRemoteGui().makeGroupBox(workingMemory);
+ tab.memoryGroup = armem::server::MemoryRemoteGui().makeGroupBox(workingMemory());
}
VBoxLayout root = {tab.memoryGroup, VSpacer()};
@@ -135,7 +136,7 @@ namespace armarx
{
if (tab.rebuild.exchange(false))
{
- RemoteGui__createTab();
+ createRemoteGuiTab();
}
}
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
index 37d20608bea3c0c14cda73332693768899d6ce90..a7dda191aa96c14567899a4f9064edf5f69d2bcf 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
@@ -27,9 +27,8 @@
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
+#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
namespace armarx
@@ -47,9 +46,8 @@ namespace armarx
*/
class ExampleMemory :
virtual public armarx::Component
- , virtual public armem::server::ComponentPluginUser
+ , virtual public armem::server::ReadWritePluginUser
, virtual public LightweightRemoteGuiComponentPluginUser
- // , virtual public armarx::ArVizComponentPluginUser
{
public:
@@ -65,7 +63,7 @@ namespace armarx
// LightweightRemoteGuiComponentPluginUser interface
public:
- void RemoteGui__createTab();
+ void createRemoteGuiTab();
void RemoteGui_update() override;
@@ -85,8 +83,6 @@ namespace armarx
struct Properties
{
- std::string memoryName = "Example";
-
struct CoreSegments
{
std::vector<std::string> defaultCoreSegments = { "ExampleModality", "ExampleConcept" };
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml b/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
index edd34d36d5d05c01ce7e40b2f1cbdc9213c2c2f6..6347fef0adf48e84d1104985481b05732db7b11f 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
@@ -1,97 +1,114 @@
-<!--Some fancy comment -->
+<!--
+An example data containing different native ARON types.
+-->
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
- <CodeIncludes>
- <Include include="<Eigen/Core>" />
- <Include include="<Image/ByteImage.h>" />
- <Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" />
- </CodeIncludes>
+ <CodeIncludes>
+ <!-- These should not be necessary in the future: -->
+ <Include include="<Eigen/Core>" /> <!-- For Position, Pose and Matrix. -->
+ <Include include="<Eigen/Geometry>" /> <!-- For Orientation. -->
+ <Include include="<opencv2/core/core.hpp>" /> <!-- For Image. -->
+ <!--Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" /-->
+ </CodeIncludes>
- <AronIncludes>
- <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" />
- </AronIncludes>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" autoinclude="true" />
+ </AronIncludes>
- <GenerateTypes>
- <Object name='armarx::armem::example::ExampleData'>
+ <GenerateTypes>
+ <Object name='armarx::armem::example::ExampleData'>
- <ObjectChild key='the_int'>
- <Int />
- </ObjectChild>
- <ObjectChild key='the_long'>
- <Long />
- </ObjectChild>
- <ObjectChild key='the_float'>
- <Float />
- </ObjectChild>
- <ObjectChild key='the_double'>
- <Double />
- </ObjectChild>
- <ObjectChild key='the_string'>
+ <ObjectChild key='the_int'>
+ <Int />
+ </ObjectChild>
+ <ObjectChild key='the_long'>
+ <Long />
+ </ObjectChild>
+ <ObjectChild key='the_float'>
+ <Float />
+ </ObjectChild>
+ <ObjectChild key='the_double'>
+ <Double />
+ </ObjectChild>
+ <ObjectChild key='the_string'>
<String />
- </ObjectChild>
- <ObjectChild key='the_bool'>
- <Bool />
- </ObjectChild>
+ </ObjectChild>
+ <ObjectChild key='the_bool'>
+ <Bool />
+ </ObjectChild>
- <ObjectChild key='the_eigen_position'>
- <EigenMatrix rows="3" cols="1" type="float" />
- </ObjectChild>
- <ObjectChild key='the_eigen_pose'>
- <EigenMatrix rows="4" cols="4" type="float" />
- </ObjectChild>
- <ObjectChild key='the_ivt_image'>
- <IVTCByteImage type="GrayScale" shared_ptr="true"/>
- </ObjectChild>
+ <ObjectChild key='the_position'>
+ <Position />
+ </ObjectChild>
+ <ObjectChild key='the_orientation'>
+ <Orientation />
+ </ObjectChild>
+ <ObjectChild key='the_pose'>
+ <Pose />
+ </ObjectChild>
- <ObjectChild key='the_float_list'>
- <List>
- <Float />
- </List>
- </ObjectChild>
- <ObjectChild key='the_int_list'>
- <List>
- <Int />
- </List>
- </ObjectChild>
- <ObjectChild key='the_string_list'>
- <List>
- <String />
- </List>
- </ObjectChild>
+ <ObjectChild key='the_3x1_vector'>
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </ObjectChild>
+ <ObjectChild key='the_4x4_matrix'>
+ <EigenMatrix rows="4" cols="4" type="float" />
+ </ObjectChild>
+ <ObjectChild key='the_rgb24_image'>
+ <Image pixelType="rgb24"/>
+ </ObjectChild>
+ <ObjectChild key='the_depth32_image'>
+ <Image pixelType="depth32"/>
+ </ObjectChild>
- <ObjectChild key='the_object_list'>
- <List>
- <Object name='ListClass'>
- <ObjectChild key='element_int'>
- <Int />
- </ObjectChild>
- <ObjectChild key='element_float'>
- <Float />
- </ObjectChild>
- <ObjectChild key='element_string'>
+ <ObjectChild key='the_float_list'>
+ <List>
+ <Float />
+ </List>
+ </ObjectChild>
+ <ObjectChild key='the_int_list'>
+ <List>
+ <Int />
+ </List>
+ </ObjectChild>
+ <ObjectChild key='the_string_list'>
+ <List>
<String />
- </ObjectChild>
- </Object>
- </List>
- </ObjectChild>
+ </List>
+ </ObjectChild>
+
+ <ObjectChild key='the_object_list'>
+ <List>
+ <Object name='InnerClass'>
+ <ObjectChild key='element_int'>
+ <Int />
+ </ObjectChild>
+ <ObjectChild key='element_float'>
+ <Float />
+ </ObjectChild>
+ <ObjectChild key='element_string'>
+ <String />
+ </ObjectChild>
+ </Object>
+ </List>
+ </ObjectChild>
- <ObjectChild key='the_float_dict'>
- <Dict>
- <Float />
- </Dict>
- </ObjectChild>
- <ObjectChild key='the_int_dict'>
- <Dict>
- <Int />
- </Dict>
- </ObjectChild>
+ <ObjectChild key='the_float_dict'>
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+ <ObjectChild key='the_int_dict'>
+ <Dict>
+ <Int />
+ </Dict>
+ </ObjectChild>
- <ObjectChild key="memoryLink">
- <armarx::armem::arondto::MemoryID />
- </ObjectChild>
+ <ObjectChild key="memoryLink">
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
- </Object>
- </GenerateTypes>
+ </Object>
+ </GenerateTypes>
</AronTypeDefinition>
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/GeneralPurposeMemory/GeneralPurposeMemory.cpp b/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.cpp
index 3b17f4fc879dfb49fd1b816f8e4b0e450e3f926f..756ffdcabdaa97085db88e4d186058d24790dfb7 100644
--- a/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.cpp
+++ b/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.cpp
@@ -29,6 +29,7 @@
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+
namespace armarx
{
GeneralPurposeMemory::GeneralPurposeMemory()
@@ -38,6 +39,9 @@ namespace armarx
armarx::PropertyDefinitionsPtr GeneralPurposeMemory::createPropertyDefinitions()
{
armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ workingMemory().name() = "GeneralPurpose";
+
return defs;
}
@@ -50,7 +54,6 @@ namespace armarx
void GeneralPurposeMemory::onInitComponent()
{
- workingMemory.name() = memoryName;
}
@@ -70,17 +73,11 @@ namespace armarx
- // WRITING
armem::data::AddSegmentsResult GeneralPurposeMemory::addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&)
{
- armem::data::AddSegmentsResult result = ComponentPluginUser::addSegments(input, addCoreSegmentOnUsage);
+ // Allowing adding core segments.
+ armem::data::AddSegmentsResult result = ReadWritePluginUser::addSegments(input, addCoreSegmentOnUsage);
return result;
}
-
- armem::data::CommitResult GeneralPurposeMemory::commit(const armem::data::Commit& commit, const Ice::Current&)
- {
- armem::data::CommitResult result = ComponentPluginUser::commit(commit);
- return result;
- }
}
diff --git a/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.h b/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.h
index c91e38d07df966ec61c06653bc7d828358275797..0ee828883381d8e8272b2d49a1fdfa81179e69ed 100644
--- a/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.h
+++ b/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.h
@@ -25,11 +25,10 @@
#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
+#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
namespace armarx
@@ -46,20 +45,28 @@ namespace armarx
* Detailed description of class GeneralPurposeMemory.
*/
class GeneralPurposeMemory :
- virtual public armarx::Component,
- virtual public armem::server::ComponentPluginUser
+ virtual public armarx::Component
+ , virtual public armem::server::ReadWritePluginUser
{
public:
+
GeneralPurposeMemory();
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
+
public:
- armem::data::AddSegmentsResult addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&) override;
- armem::data::CommitResult commit(const armem::data::Commit& commit, const Ice::Current&) override;
+
+ armem::data::AddSegmentsResult
+ addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&) override;
+
protected:
+
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
/// @see armarx::ManagedIceObject::onInitComponent()
void onInitComponent() override;
@@ -72,12 +79,9 @@ namespace armarx
/// @see armarx::ManagedIceObject::onExitComponent()
void onExitComponent() override;
- /// @see PropertyUser::createPropertyDefinitions()
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
-
private:
- std::string memoryName = "GeneralPurposeMemory";
+
bool addCoreSegmentOnUsage = true;
};
diff --git a/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp b/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp
index 350810558ce9d2200e61bda865dd1633da15c128..3555cb2d4ed20b423c43dcc127f35f9983001ad3 100644
--- a/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp
+++ b/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp
@@ -22,12 +22,6 @@
#include "MotionMemory.h"
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <SimoxUtility/algorithm/string.h>
-
-#include <RobotAPI/libraries/armem/core/error.h>
-
namespace armarx
{
@@ -35,16 +29,17 @@ namespace armarx
{
armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ workingMemory().name() = "Motion";
+
const std::string prefix = "mem.";
- defs->optional(memoryName, prefix + "MemoryName", "Name of this memory (server).");
mdbMotions.defineProperties(defs, prefix + "mdbmotions.");
return defs;
}
+
MotionMemory::MotionMemory() :
- mdbMotions(armem::server::ComponentPluginUser::iceMemory)
+ mdbMotions(iceAdapter())
{
-
}
@@ -53,24 +48,26 @@ namespace armarx
return "MotionMemory";
}
+
void MotionMemory::onInitComponent()
{
- workingMemory.name() = memoryName;
- longtermMemory.name() = memoryName;
-
mdbMotions.onInit();
}
+
void MotionMemory::onConnectComponent()
{
mdbMotions.onConnect();
}
+
void MotionMemory::onDisconnectComponent()
{
}
+
void MotionMemory::onExitComponent()
{
}
+
}
diff --git a/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.h b/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.h
index 0218f010631a8f752985dce148c892bed145f5b3..b457d713a58a369b83f8f176d8bd241de95be37f 100644
--- a/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.h
+++ b/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.h
@@ -22,11 +22,12 @@
#pragma once
+#include <RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.h>
+
+#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
#include <ArmarXCore/core/Component.h>
-#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
-#include <RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.h>
namespace armarx
{
@@ -43,7 +44,7 @@ namespace armarx
*/
class MotionMemory :
virtual public armarx::Component
- , virtual public armem::server::ComponentPluginUser
+ , virtual public armem::server::ReadWritePluginUser
{
public:
@@ -52,6 +53,7 @@ namespace armarx
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
+
protected:
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
@@ -63,7 +65,6 @@ namespace armarx
private:
- std::string memoryName = "Motion";
armem::server::motions::mdb::Segment mdbMotions;
// TODO: mdt Segment
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index c624379b89ae336aa561e364706f3780b7fbf0e9..c605df2efece207f0a0b11337dd96c61ca534470 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -22,8 +22,6 @@
#include "ObjectMemory.h"
-#include <mutex>
-
namespace armarx::armem::server::obj
{
@@ -35,7 +33,16 @@ namespace armarx::armem::server::obj
{
armarx::PropertyDefinitionsPtr defs(new ComponentPropertyDefinitions(getConfigIdentifier()));
- // Offer
+ const std::string prefix = "mem.";
+ workingMemory().name() = defaultMemoryName;
+
+ classSegment.defineProperties(defs, prefix + "cls.");
+ instance::SegmentAdapter::defineProperties(defs, prefix + "inst.");
+
+ attachmentSegment.defineProperties(defs, prefix + "attachments.");
+
+
+ // Publish
defs->topic(debugObserver);
// Subscribe
@@ -46,25 +53,14 @@ namespace armarx::armem::server::obj
defs->defineOptionalProperty<std::string>("cmp.KinematicUnitObserverName", "KinematicUnitObserver",
"Name of the kinematic unit observer.");
- const std::string prefix = "mem.";
-
- workingMemory.name() = defaultMemoryName;
- defs->optional(workingMemory.name(), prefix + "MemoryName", "Name of this memory server.");
-
- classSegment.defineProperties(defs, prefix + "cls.");
- instance::SegmentAdapter::defineProperties(defs, prefix + "inst.");
-
- attachmentSegment.defineProperties(defs, prefix + "attachments.");
-
return defs;
}
ObjectMemory::ObjectMemory() :
- server::ComponentPluginUser(),
- instance::SegmentAdapter(server::ComponentPluginUser::iceMemory),
- classSegment(server::ComponentPluginUser::iceMemory),
- attachmentSegment(server::ComponentPluginUser::iceMemory)
+ instance::SegmentAdapter(iceAdapter()),
+ classSegment(iceAdapter()),
+ attachmentSegment(iceAdapter())
{
}
@@ -82,8 +78,6 @@ namespace armarx::armem::server::obj
void ObjectMemory::onInitComponent()
{
- workingMemory.name() = defaultMemoryName;
-
const auto initSegmentWithCatch = [&](const std::string & segmentName, const auto&& fn)
{
try
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index b90406be31ac76a40172d67076f931ebb1a6ba1a..91217c90cc0dcb5be302663a0b472cac3327f3c0 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
@@ -22,23 +22,20 @@
#pragma once
-#include <memory>
-#include <mutex>
-
-#include <VirtualRobot/VirtualRobot.h>
-
-#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+#include <RobotAPI/libraries/armem_objects/server/class/Segment.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h>
+#include <RobotAPI/libraries/armem_objects/server/attachments/Segment.h>
-#include <RobotAPI/interface/armem/server/ObjectMemoryInterface.h>
+#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
-#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
+#include <RobotAPI/interface/armem/server/ObjectMemoryInterface.h>
-#include <RobotAPI/libraries/armem_objects/server/class/Segment.h>
-#include <RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h>
-#include <RobotAPI/libraries/armem_objects/server/attachments/Segment.h>
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+#include <memory>
#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
@@ -62,7 +59,7 @@ namespace armarx::armem::server::obj
virtual public Component
, virtual public armarx::armem::server::ObjectMemoryInterface
- , virtual public armarx::armem::server::ComponentPluginUser
+ , virtual public armarx::armem::server::ReadWritePluginUser
, virtual public armarx::armem::server::obj::instance::SegmentAdapter
, virtual public armarx::RobotStateComponentPluginUser
@@ -79,7 +76,7 @@ namespace armarx::armem::server::obj
public:
ObjectMemory();
- virtual ~ObjectMemory();
+ virtual ~ObjectMemory() override;
/// @see armarx::ManagedIceObject::getDefaultName()
@@ -123,6 +120,6 @@ namespace armarx::armem::server::obj
};
-} // namespace armarx::armem::server::obj
+}
#undef ICE_CURRENT_ARG
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
index a25161ff2cdf35173921720f68ad5bcec16cba30..f49cb33c231a20857aa30458b4f7f5f14c05acdc 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
@@ -15,18 +15,10 @@ set(COMPONENT_LIBS
)
set(SOURCES
- aron_conversions.cpp
RobotStateMemory.cpp
- RobotStateWriter.cpp
- RobotUnitData.cpp
- RobotUnitReader.cpp
)
set(HEADERS
- aron_conversions.h
RobotStateMemory.h
- RobotStateWriter.h
- RobotUnitData.h
- RobotUnitReader.h
)
armarx_add_component("${SOURCES}" "${HEADERS}")
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index c269bd56e747159919ce7358739e6964f20d6355..27aaa07ddf1c0f0dc819a9a74366036cdadfe11c 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -22,33 +22,26 @@
#include "RobotStateMemory.h"
-// STD
-#include <memory>
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
-// Simox
-#include <SimoxUtility/algorithm/string.h>
+#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
-// ArmarX
#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 <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
-
-#include "aron_conversions.h"
+#include <SimoxUtility/algorithm/string.h>
namespace armarx::armem::server::robot_state
{
- RobotStateMemory::RobotStateMemory()
- : descriptionSegment(server::ComponentPluginUser::iceMemory),
- proprioceptionSegment(server::ComponentPluginUser::iceMemory),
- localizationSegment(server::ComponentPluginUser::iceMemory),
- commonVisu(descriptionSegment, proprioceptionSegment, localizationSegment)
+ RobotStateMemory::RobotStateMemory() :
+ descriptionSegment(iceAdapter()),
+ proprioceptionSegment(iceAdapter()),
+ localizationSegment(iceAdapter()),
+ commonVisu(descriptionSegment, proprioceptionSegment, localizationSegment)
{
addPlugin(debugObserver);
ARMARX_CHECK_NOT_NULL(debugObserver);
@@ -59,20 +52,17 @@ namespace armarx::armem::server::robot_state
robotUnit.writer.setTag(getName());
}
- RobotStateMemory::~RobotStateMemory() = default;
+
+ RobotStateMemory::~RobotStateMemory()
+ {
+ }
armarx::PropertyDefinitionsPtr RobotStateMemory::createPropertyDefinitions()
{
armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
- 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};
+ const std::string robotUnitPrefix = sensorValuePrefix;
defs->optional(robotUnit.reader.properties.sensorPrefix, robotUnitPrefix + "SensorValuePrefix",
"Prefix of all sensor values.");
@@ -83,8 +73,11 @@ namespace armarx::armem::server::robot_state
"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);
- defs->optional(robotUnit.configPath, robotUnitPrefix + "ConfigPath",
- "Specify a configuration file to group the sensor values specifically.");
+
+
+ const std::string prefix = "mem.";
+
+ workingMemory().name() = "RobotState";
descriptionSegment.defineProperties(defs, prefix + "desc.");
proprioceptionSegment.defineProperties(defs, prefix + "prop.");
@@ -108,7 +101,7 @@ namespace armarx::armem::server::robot_state
localizationSegment.onInit();
commonVisu.init();
- robotUnit.pollFrequency = std::clamp(robotUnit.pollFrequency, 1, ROBOT_UNIT_MAXIMUM_FREQUENCY);
+ robotUnit.pollFrequency = std::clamp(robotUnit.pollFrequency, 0.f, ROBOT_UNIT_MAXIMUM_FREQUENCY);
robotUnit.writer.properties.memoryBatchSize = std::max(static_cast<unsigned int>(1), robotUnit.writer.properties.memoryBatchSize);
std::vector<std::string> includePaths;
@@ -127,8 +120,6 @@ namespace armarx::armem::server::robot_state
std::vector<std::string> projectIncludePaths = simox::alg::split(pathsString, ";,");
includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
-
- ArmarXDataPath::getAbsolutePath(robotUnit.configPath, robotUnit.configPath, includePaths);
}
@@ -148,7 +139,8 @@ namespace armarx::armem::server::robot_state
commonVisu.connect(getArvizClient(), debugObserver->getDebugObserver());
- robotUnit.reader.connect(*robotUnit.plugin, *debugObserver);
+ robotUnit.reader.connect(*robotUnit.plugin, *debugObserver,
+ proprioceptionSegment.getRobotUnitProviderID().providerSegmentName);
robotUnit.writer.connect(*debugObserver);
robotUnit.writer.properties.robotUnitProviderID = proprioceptionSegment.getRobotUnitProviderID();
@@ -162,7 +154,7 @@ namespace armarx::armem::server::robot_state
{
robotUnit.writer.run(
robotUnit.pollFrequency, robotUnit.dataQueue, robotUnit.dataMutex,
- iceMemory, localizationSegment
+ iceAdapter(), localizationSegment
);
}, "Robot State Writer");
@@ -189,7 +181,6 @@ namespace armarx::armem::server::robot_state
void RobotStateMemory::startRobotUnitStream()
{
- std::lock_guard lock{startStopMutex};
if (robotUnit.reader.task->isRunning() || robotUnit.writer.task->isRunning())
{
if (robotUnit.reader.task->isRunning() && robotUnit.writer.task->isRunning())
@@ -210,18 +201,6 @@ namespace armarx::armem::server::robot_state
ARMARX_INFO << ss.str();
}
- // parse config
- if (std::filesystem::is_regular_file(robotUnit.configPath))
- {
- ARMARX_INFO << "Found a configuration file at: " << robotUnit.configPath;
- // A simple self-made parser for the config file. Extend it if you need to.
- robotUnit.reader.configSensorMapping = RobotUnitReader::readConfig(robotUnit.configPath);
- }
- else
- {
- ARMARX_INFO << "The config path '" << robotUnit.configPath << "' is not valid.";
- }
-
robotUnit.reader.task->start();
robotUnit.writer.task->start();
}
@@ -229,7 +208,6 @@ namespace armarx::armem::server::robot_state
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 c89a51ae47158d0c646a63f216f0a4ff397ba5d6..74dd4a3fa12fea08186ec14d1b93fdf6872ac7dd 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -22,26 +22,22 @@
#pragma once
-// STD
-#include <atomic>
-#include <optional>
+#include <mutex>
#include <queue>
-// ArmarX
#include <ArmarXCore/core/Component.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
-#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
+#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.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 "RobotUnitData.h"
-#include "RobotUnitReader.h"
-#include "RobotStateWriter.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
@@ -49,7 +45,6 @@ namespace armarx::plugins
class DebugObserverComponentPlugin;
class RobotUnitComponentPlugin;
}
-
namespace armarx::armem::server::robot_state
{
@@ -66,7 +61,7 @@ namespace armarx::armem::server::robot_state
*/
class RobotStateMemory :
virtual public armarx::Component,
- virtual public armem::server::ComponentPluginUser,
+ virtual public armem::server::ReadWritePluginUser,
virtual public armarx::ArVizComponentPluginUser
{
public:
@@ -90,11 +85,6 @@ namespace armarx::armem::server::robot_state
private:
- /// Reads from `robotUnitDataQueue` and commits into memory.
- //void convertRobotUnitStreamingDataToAronTask();
- /// Reads from `robotUnitDataQueue` and commits into memory.
- //void storeConvertedRobotUnitDataInMemoryTask();
-
void startRobotUnitStream();
void stopRobotUnitStream();
@@ -103,7 +93,6 @@ namespace armarx::armem::server::robot_state
armarx::plugins::DebugObserverComponentPlugin* debugObserver = nullptr;
- mutable std::recursive_mutex startStopMutex;
// Core segments
// - description
@@ -122,22 +111,21 @@ namespace armarx::armem::server::robot_state
struct RobotUnit
{
- int pollFrequency = 50;
- std::string configPath = "NO CONFIG SET";
+ float pollFrequency = 50;
armarx::plugins::RobotUnitComponentPlugin* plugin = nullptr;
- RobotUnitReader reader;
- RobotStateWriter writer;
+ proprioception::RobotUnitReader reader;
+ proprioception::RobotStateWriter writer;
// queue
- std::queue<RobotUnitData> dataQueue;
- mutable std::mutex dataMutex;
+ std::queue<proprioception::RobotUnitData> dataQueue;
+ std::mutex dataMutex;
};
RobotUnit robotUnit;
// params
- static constexpr int ROBOT_UNIT_MAXIMUM_FREQUENCY = 100;
+ static constexpr float ROBOT_UNIT_MAXIMUM_FREQUENCY = 100;
static constexpr const char* sensorValuePrefix = "RobotUnit.";
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitData.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitData.h
deleted file mode 100644
index dc43dcddca3eb693fe5200dcc0a256a15be23cac..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitData.h
+++ /dev/null
@@ -1,31 +0,0 @@
-#pragma once
-
-#include <map>
-#include <memory>
-#include <string>
-
-#include <RobotAPI/libraries/armem/core/Time.h>
-
-
-namespace armarx::aron::datanavigator
-{
- using DictNavigatorPtr = std::shared_ptr<class DictNavigator>;
-}
-
-namespace armarx::armem::server::robot_state
-{
-
- struct RobotUnitData
- {
- struct RobotUnitDataGroup
- {
- armem::Time timestamp;
- std::string name;
- aron::datanavigator::DictNavigatorPtr data;
- };
-
- std::map<std::string, RobotUnitDataGroup> groups;
- };
-
-}
-
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.cpp
deleted file mode 100644
index 94dc94f8b1b0763dcf0e05e8dae75c05aa261896..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.cpp
+++ /dev/null
@@ -1,202 +0,0 @@
-#include "RobotUnitReader.h"
-
-#include "aron_conversions.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
-{
-
- std::map<std::string, std::string> RobotUnitReader::readConfig(const std::string& configPath)
- {
- ARMARX_CHECK(std::filesystem::is_regular_file(configPath));
-
- std::map<std::string, std::string> config;
-
- 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_S << "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_S << "Setting sensor value '" << split[i] << "' to group '" << split[split.size() - 1] << "'.";
- config.emplace(split[i], split[split.size() - 1]);
- }
- }
- }
- }
-
- return config;
- }
-
-
- void RobotUnitReader::connect(
- armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
- armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin)
- {
- {
- 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> data = fetchAndGroupLatestRobotUnitData())
- {
- std::lock_guard g{dataMutex};
- dataQueue.push(data.value());
- }
-
- if (debugObserver)
- {
- debugObserver->sendDebugObserverBatch();
- }
- cycle.waitForCycleDuration();
- }
- }
-
-
- std::optional<RobotUnitData> RobotUnitReader::fetchAndGroupLatestRobotUnitData()
- {
- const std::optional<RobotUnitDataStreaming::TimeStep> data = fetchLatestData();
- if (not data.has_value())
- {
- return std::nullopt;
- }
-
- 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] : description.entries)
- {
- std::string name = nameEntry;
- std::string jointOrWhateverName = nameEntry;
-
- std::string groupName = "";
- if (auto it = configSensorMapping.find(name); it != configSensorMapping.end())
- {
- groupName = it->second;
- jointOrWhateverName = it->second; // ???
- }
- 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
- }
-
- auto it = convertedAndGroupedData.groups.find(groupName);
- if (it == convertedAndGroupedData.groups.end())
- {
- namespace adn = aron::datanavigator;
-
- // Generate new dict for the group
- RobotUnitData::RobotUnitDataGroup group;
- group.timestamp = armem::Time::microSeconds(data->timestampUSec);
- group.name = groupName;
- group.data = std::make_shared<adn::DictNavigator>();
-
- group.data->addElement("EncoderGroupName", std::make_shared<adn::StringNavigator>(groupName));
- group.data->addElement("IterationId", std::make_shared<adn::LongNavigator>(data->iterationId));
- group.data->addElement("name", std::make_shared<adn::StringNavigator>(jointOrWhateverName));
-
- auto r = convertedAndGroupedData.groups.emplace(groupName, group);
- it = r.first;
- }
-
- RobotUnitData::RobotUnitDataGroup& group = it->second;
- const std::string escapedName = simox::alg::replace_all(name, ".", "/");
- group.data->addElement(escapedName, RobotUnitDataStreaming::toAron(data.value(), dataEntry));
- }
-
- auto stop = std::chrono::high_resolution_clock::now();
- auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
- ARMARX_DEBUG << "RobotUnitData: The total time needed to convert the data to aron is: " << duration;
-
- if (debugObserver)
- {
- debugObserver->setDebugObserverDatafield("RobotUnitReader | t Read+Group [ms]", duration.count() / 1000.f);
- }
-
- return convertedAndGroupedData;
- }
-
-
- 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/components/armem/server/SkillsMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/SkillsMemory/CMakeLists.txt
index b9d1d4fd44ed2bf58f8eccd64faa9ab0eafc83ef..eee41fad2917ac3689f98d103f1a927a0715d205 100644
--- a/source/RobotAPI/components/armem/server/SkillsMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/SkillsMemory/CMakeLists.txt
@@ -2,10 +2,9 @@ armarx_component_set_name("SkillsMemory")
set(COMPONENT_LIBS
- ArmarXCore ArmarXCoreInterfaces ArmarXCoreObservers # for DebugObserverInterface
- ArmarXGuiComponentPlugins
- RobotAPICore RobotAPIInterfaces armem armem_skills
- # RobotAPIComponentPlugins # for ArViz and other plugins
+ ArmarXCore ArmarXCoreInterfaces
+ RobotAPICore RobotAPIInterfaces
+ armem armem_skills
${IVT_LIBRARIES}
)
diff --git a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
index c0a9fd2f33211d9a130f236b9350836b75b51466..f7741f3d3991896c43f2d3172c515bfec35af0ef 100644
--- a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
+++ b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
@@ -29,32 +29,31 @@
#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/server/MemoryRemoteGui.h>
-
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_skills/aron_conversions.h>
namespace armarx
{
+
SkillsMemory::SkillsMemory()
- = default;
+ {
+ }
+
armarx::PropertyDefinitionsPtr SkillsMemory::createPropertyDefinitions()
{
armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
- // Publish
- defs->topic(debugObserver);
+ workingMemory().name() = "Skills";
+
// Statechart Logging
defs->optional(p.statechartCoreSegmentName, "StatechartCoreSegmentName", "Name of the core segment for statecharts.");
defs->optional(p.statechartTransitionsProviderSegmentName, "TransitionsProviderSegmentName", "Name of the provider segment for statechart transitions.");
defs->optional(p.statechartTransitionsTopicName, "tpc.sub.ProfilerListener", "Name of the ProfilerListenerInterface topics to subscribe.");
- const std::string prefix = "mem.";
- defs->optional(p.memoryName, prefix + "MemoryName", "Name of this memory server.");
-
return defs;
}
@@ -67,12 +66,10 @@ namespace armarx
void SkillsMemory::onInitComponent()
{
- workingMemory.name() = p.memoryName;
-
- {
- armarx::armem::wm::CoreSegment& c = workingMemory.addCoreSegment(p.statechartCoreSegmentName, armarx::armem::arondto::Statechart::Transition::toAronType());
- c.addProviderSegment("Transitions", 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());
}
@@ -100,14 +97,7 @@ namespace armarx
// WRITING
armem::data::AddSegmentsResult SkillsMemory::addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&)
{
- armem::data::AddSegmentsResult result = ComponentPluginUser::addSegments({input}, p.core.addOnUsage);
- return result;
- }
-
-
- armem::data::CommitResult SkillsMemory::commit(const armem::data::Commit& commit, const Ice::Current&)
- {
- armem::data::CommitResult result = ComponentPluginUser::commit(commit);
+ armem::data::AddSegmentsResult result = ReadWritePluginUser::addSegments({input}, p.core.addOnUsage);
return result;
}
@@ -131,6 +121,7 @@ namespace armarx
[this](const std::vector<StatechartListener::Transition>& transitions,
armarx::StatechartListener & source)
{
+ (void) source; // Unused.
this->reportTransitions(transitions);
});
@@ -149,6 +140,7 @@ namespace armarx
return listener;
}
+
void
SkillsMemory::reportTransitions(const std::vector<StatechartListener::Transition>& transitions)
{
@@ -159,7 +151,7 @@ namespace armarx
armem::EntityUpdate update;
update.entityID = armem::MemoryID()
- .withMemoryName(p.memoryName)
+ .withMemoryName(workingMemory().name())
.withCoreSegmentName(p.statechartCoreSegmentName)
.withProviderSegmentName(p.statechartTransitionsProviderSegmentName)
.withEntityName(entityName);
@@ -171,7 +163,7 @@ namespace armarx
try
{
- workingMemory.update(update);
+ workingMemory().update(update);
}
catch (const armem::error::ArMemError& e)
{
@@ -180,6 +172,7 @@ namespace armarx
}
}
+
std::string SkillsMemory::getStatechartName(std::string stateName)
{
const std::string delimiter = "->";
diff --git a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h
index ebf0b02c48f90e31b5f93321343b12a66a2723c9..6eb9114c94a94c851ba6e7437bb74338353b90db 100644
--- a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h
+++ b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h
@@ -24,12 +24,9 @@
#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/interface/observers/ObserverInterface.h>
-#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
#include <RobotAPI/libraries/armem_skills/aron/Statechart.aron.generated.h>
#include <RobotAPI/libraries/armem_skills/StatechartListener.h>
@@ -49,22 +46,24 @@ namespace armarx
*/
class SkillsMemory :
virtual public armarx::Component,
- virtual public armem::server::ComponentPluginUser
- // , virtual public armarx::ArVizComponentPluginUser
+ virtual public armem::server::ReadWritePluginUser
{
public:
+
SkillsMemory();
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
+
// WritingInterface interface
public:
+
armem::data::AddSegmentsResult addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&) override;
- armem::data::CommitResult commit(const armem::data::Commit& commit, const Ice::Current&) override;
protected:
+
/// @see armarx::ManagedIceObject::onInitComponent()
void onInitComponent() override;
@@ -82,12 +81,9 @@ namespace armarx
private:
- DebugObserverInterfacePrx debugObserver;
struct Properties
{
- std::string memoryName = "Skills";
-
// Statechart transition logging
std::string statechartCoreSegmentName = "Statechart";
std::string statechartTransitionsProviderSegmentName = "Transitions";
@@ -110,5 +106,6 @@ namespace armarx
// Gets the statechart name from a state name (takes first two levels of the hierarchy)
static std::string getStatechartName(std::string stateName);
+
};
}
diff --git a/source/RobotAPI/components/armem/server/SubjectMemory/SubjectMemory.h b/source/RobotAPI/components/armem/server/SubjectMemory/SubjectMemory.h
index 2382459ee2fa0ad5bd52300c25cbad0562ed5921..8018df78fc2b92de1353089521c8649aded3b377 100644
--- a/source/RobotAPI/components/armem/server/SubjectMemory/SubjectMemory.h
+++ b/source/RobotAPI/components/armem/server/SubjectMemory/SubjectMemory.h
@@ -24,7 +24,7 @@
#include <ArmarXCore/core/Component.h>
-#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
+#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
#include <RobotAPI/libraries/armem_subjects/server/MotionDatabase/Segment.h>
@@ -43,7 +43,7 @@ namespace armarx
*/
class SubjectMemory :
virtual public armarx::Component
- , virtual public armem::server::ComponentPluginUser
+ , virtual public armem::server::ReadWritePluginUser
{
public:
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.cpp b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
index 3d44b10468505f79ee518d54cf4a59c7b7ea83ae..4a99205ed9f2fd841ce793a5a5794ec4c948b660 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
@@ -36,7 +36,7 @@
#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
-#include <boost/algorithm/string/trim.hpp>
+#include <SimoxUtility/algorithm/string/string_tools.h>
#define RAWFORCE "_rawforcevectors"
#define OFFSETFORCE "_forceswithoffsetvectors"
@@ -85,14 +85,14 @@ namespace armarx
auto sensorRobotNodeSplit = armarx::Split(getProperty<std::string>("SensorRobotNodeMapping").getValue(), ",");
for (auto& elem : sensorRobotNodeSplit)
{
- boost::trim(elem);
+ simox::alg::trim(elem);
auto split = armarx::Split(elem, ":");
if (split.size() >= 2)
{
sensorRobotNodeMapping.emplace(
- boost::trim_copy(split.at(0)),
- std::make_pair(boost::trim_copy(split.at(1)),
- split.size() >= 3 ? boost::trim_copy(split.at(2)) : ""));
+ simox::alg::trim_copy(split.at(0)),
+ std::make_pair(simox::alg::trim_copy(split.at(1)),
+ split.size() >= 3 ? simox::alg::trim_copy(split.at(2)) : ""));
}
}
}
diff --git a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
index 6cfb8340d95092343a8fa14eb1ba0818ad67ae29..2a4d495c80c135afa06ba12be235f4b81754affe 100644
--- a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
@@ -27,7 +27,7 @@
#include <ArmarXCore/core/util/StringHelpers.h>
-#include <boost/algorithm/string.hpp>
+#include <SimoxUtility/algorithm/string/string_tools.h>
namespace armarx
{
@@ -38,10 +38,8 @@ namespace armarx
sensorNamesList = Split(getProperty<std::string>("SensorNames").getValue(), ",");
for (auto& sensor : sensorNamesList)
{
- boost::trim(sensor);
+ simox::alg::trim(sensor);
}
- //std::vector<std::string> sensorNamesList;
- //boost::split(sensorNamesList, sensorNames, boost::is_any_of(","));
for (std::vector<std::string>::iterator s = sensorNamesList.begin(); s != sensorNamesList.end(); s++)
{
diff --git a/source/RobotAPI/components/units/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp
index 8e3037badb78a49b9cd6ef2847460c70995f448b..1bbf0abff5599b23ff0ca01acaa4087bdeac1704 100644
--- a/source/RobotAPI/components/units/HeadIKUnit.cpp
+++ b/source/RobotAPI/components/units/HeadIKUnit.cpp
@@ -31,14 +31,10 @@
#include <VirtualRobot/IK/GazeIK.h>
#include <VirtualRobot/LinkedCoordinate.h>
-#include <boost/algorithm/string/trim.hpp>
+#include <SimoxUtility/algorithm/string/string_tools.h>
-#include <boost/shared_ptr.hpp>
#include <memory>
-using boost::dynamic_pointer_cast;
-using std::dynamic_pointer_cast;
-
namespace armarx
{
@@ -147,7 +143,7 @@ namespace armarx
this->robotNodeSetNames = armarx::Split(robotNodeSetName, ",");
for (auto& setName : robotNodeSetNames)
{
- boost::trim(setName);
+ simox::alg::trim(setName);
}
this->targetPosition->x = targetPosition->x;
this->targetPosition->y = targetPosition->y;
@@ -311,7 +307,7 @@ namespace armarx
auto tcpNode = kinematicChain->getTCP();
VirtualRobot::RobotNodePrismaticPtr virtualPrismaticJoint;
- virtualPrismaticJoint = dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(tcpNode);
+ virtualPrismaticJoint = std::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(tcpNode);
if (!virtualPrismaticJoint)
{
ARMARX_WARNING << deactivateSpam(10, robotNodeSetName) << "Head IK Kinematic Chain TCP is not a prismatic joint! skipping joint set";
diff --git a/source/RobotAPI/components/units/KinematicUnit.cpp b/source/RobotAPI/components/units/KinematicUnit.cpp
index 5df97334511a6d3621242ec38b3840232645a38f..9c046e0c714b556b67f2273ed8327b69f1340ae1 100644
--- a/source/RobotAPI/components/units/KinematicUnit.cpp
+++ b/source/RobotAPI/components/units/KinematicUnit.cpp
@@ -29,13 +29,11 @@
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/VirtualRobotException.h>
-#include <VirtualRobot/RuntimeEnvironment.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <boost/algorithm/string/split.hpp>
-#include <boost/algorithm/string/classification.hpp>
+#include <SimoxUtility/algorithm/string/string_tools.h>
namespace armarx
{
@@ -68,12 +66,9 @@ namespace armarx
if (!project.empty())
{
CMakePackageFinder finder(project);
- Ice::StringSeq projectIncludePaths;
+
auto pathsString = finder.getDataDir();
- boost::split(projectIncludePaths,
- pathsString,
- boost::is_any_of(";,"),
- boost::token_compress_on);
+ Ice::StringSeq projectIncludePaths = simox::alg::split(pathsString, ";,");
includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
std::string robotFile = relativeRobotFile;
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.cpp b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
index 1dacd798cc3feb30b0fb04b283b0d9ebb3eb2ae4..64fcc6bf313220e49c6b79d998213962f04ddc12 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
@@ -38,8 +38,7 @@
#include <VirtualRobot/Nodes/RobotNode.h>
#include <ArmarXCore/observers/checks/ConditionCheckEqualsWithTolerance.h>
-#include <boost/algorithm/string/split.hpp>
-#include <boost/algorithm/string/classification.hpp>
+#include <SimoxUtility/algorithm/string/string_tools.h>
namespace armarx
{
@@ -73,12 +72,9 @@ namespace armarx
if (!project.empty())
{
CMakePackageFinder finder(project);
- Ice::StringSeq projectIncludePaths;
+
auto pathsString = finder.getDataDir();
- boost::split(projectIncludePaths,
- pathsString,
- boost::is_any_of(";,"),
- boost::token_compress_on);
+ Ice::StringSeq projectIncludePaths = simox::alg::split(pathsString, ";,");
includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
index 69dbe9ee5ed0c7e455ccf7859e9b42d69924fde2..643995773e1ec95b58862c4fee5cf753c95a24ca 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
@@ -30,7 +30,6 @@
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/VirtualRobotException.h>
-#include <VirtualRobot/RuntimeEnvironment.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/util/algorithm.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
index e106a9dbb943dabd9bd23a05e9d8320b6e415b82..f779b851b17ecfdeb4316013234b62b7696e8cea 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
@@ -22,11 +22,12 @@
#include "BasicControllers.h"
-#include <boost/algorithm/clamp.hpp>
-
#include <ArmarXCore/core/util/algorithm.h>
#include <ArmarXCore/core/logging/Logging.h>
#include "util/CtrlUtil.h"
+
+#include <algorithm>
+
namespace armarx
{
float velocityControlWithAccelerationBounds(
@@ -40,7 +41,7 @@ namespace armarx
maxV = std::abs(maxV);
acceleration = std::abs(acceleration);
deceleration = std::abs(deceleration);
- targetV = boost::algorithm::clamp(targetV, -maxV, maxV);
+ targetV = std::clamp(targetV, -maxV, maxV);
//we can have 3 cases:
// 1. we directly set v and ignore acc/dec (if |curr - target| <= limit)
@@ -67,7 +68,7 @@ namespace armarx
return targetV;
}
- const float deltaVel = boost::algorithm::clamp(sign(currentV) * usedacc * dt, -maxDeltaV, maxDeltaV);
+ const float deltaVel = std::clamp(sign(currentV) * usedacc * dt, -maxDeltaV, maxDeltaV);
const float nextV = currentV + deltaVel;
return nextV;
}
@@ -123,7 +124,7 @@ namespace armarx
// s = v²/(2a) <=> a = v²/(2s)
const float dec = std::abs(vsquared / 2.f / wayToGo);
const float vel = currentV - sign(currentV) * dec * upperDt;
- nextv = boost::algorithm::clamp(vel, -maxV, maxV);
+ nextv = std::clamp(vel, -maxV, maxV);
if (sign(currentV) != sign(nextv))
{
//stop now
@@ -182,9 +183,9 @@ namespace armarx
sign(posErrorIfBrakingNow) != signV; // we are moving away from the target
const float usedacc = decelerate ? -deceleration : acceleration;
const float maxDeltaV = std::abs(usedacc * dt);
- const float deltaVel = boost::algorithm::clamp(signV * usedacc * dt, -maxDeltaV, maxDeltaV);
+ const float deltaVel = std::clamp(signV * usedacc * dt, -maxDeltaV, maxDeltaV);
- float newTargetVel = boost::algorithm::clamp(currentV + deltaVel, -maxV, maxV);
+ float newTargetVel = std::clamp(currentV + deltaVel, -maxV, maxV);
bool usePID = std::abs(newTargetVelPController) < std::abs(newTargetVel);
// ARMARX_INFO << deactivateSpam(0.01) << VAROUT(usePID) << VAROUT(dt) << VAROUT(decelerate) << VAROUT(usedacc) << VAROUT(maxDeltaV) << VAROUT(deltaVel) << VAROUT(newTargetVel) << VAROUT(newTargetVelPController)
// << VAROUT(currentPosition) << VAROUT(targetPosition);
@@ -241,7 +242,7 @@ namespace armarx
dt, maxDt,
currentV, maxV,
acceleration, deceleration,
- currentPosition, boost::algorithm::clamp(targetPosition, positionLimitLo, positionLimitHi),
+ currentPosition, std::clamp(targetPosition, positionLimitLo, positionLimitHi),
p
);
}
@@ -345,7 +346,7 @@ namespace armarx
return targetV;
}
- const float deltaVel = boost::algorithm::clamp(sign(currentV) * usedacc * useddt, -maxDeltaV, maxDeltaV);
+ const float deltaVel = std::clamp(sign(currentV) * usedacc * useddt, -maxDeltaV, maxDeltaV);
const float nextV = currentV + deltaVel;
return nextV;
}
@@ -487,7 +488,7 @@ namespace armarx
// s = v²/(2a) <=> a = v²/(2s)
const float dec = std::abs(vsquared / 2.f / wayToGo);
const float vel = currentV - sign(currentV) * dec * upperDt;
- nextv = boost::algorithm::clamp(vel, -maxV, maxV);
+ nextv = std::clamp(vel, -maxV, maxV);
// ARMARX_INFO << deactivateSpam(1) << "clamped new Vel: " << VAROUT(nextv);
if (sign(currentV) != sign(nextv))
{
@@ -579,7 +580,7 @@ namespace armarx
const float usedacc = decelerate ? -usedDeceleration : acceleration;
const float deltaVel = signV * usedacc * useddt;
- float newTargetVelRampCtrl = boost::algorithm::clamp(currentV + deltaVel, -maxV, maxV);
+ float newTargetVelRampCtrl = std::clamp(currentV + deltaVel, -maxV, maxV);
bool PIDActive = /*std::abs(newTargetVelPController) < std::abs(newTargetVelRampCtrl)
|| std::abs(newTargetVelPController) < pControlVelLimit ||*/
std::abs(positionError) < pControlPosErrorLimit;
@@ -960,7 +961,7 @@ namespace armarx
sign(posErrorIfBrakingNow) != signV; // we are moving away from the target
const double deltaVel = ctrlutil::v(useddt, 0, newAcceleration, usedJerk);//signV * newAcceleration * useddt;
double newTargetVelRampCtrl = ctrlutil::v(useddt, currentV, currentAcc, usedJerk);//boost::algorithm::clamp(currentV + deltaVel, -maxV, maxV);
- newTargetVelRampCtrl = boost::algorithm::clamp(newTargetVelRampCtrl, -maxV, maxV);
+ newTargetVelRampCtrl = std::clamp(newTargetVelRampCtrl, -maxV, maxV);
bool usePID = std::abs(newTargetVelPController) < std::abs(newTargetVelRampCtrl); //||
// std::abs(newTargetVelPController) < pControlVelLimit &&
//std::abs(positionError) < pControlPosErrorLimit;
@@ -1335,7 +1336,7 @@ namespace armarx
// s = v²/(2a) <=> a = v²/(2s)
const double dec = std::abs(vsquared / 2.0 / wayToGo);
const double vel = currentV - sign(currentV) * dec * upperDt;
- nextv = boost::algorithm::clamp(vel, -maxV, maxV);
+ nextv = std::clamp<double>(vel, -maxV, maxV);
// ARMARX_INFO /*<< deactivateSpam(0.1)*/ << "clamped new Vel: " << VAROUT(nextv) << VAROUT(currentV);
if (sign(currentV) != sign(nextv))
{
diff --git a/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.cpp b/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.cpp
index 1af55624f1e5bd9f261e5ba4a0addf2ba71d31ff..a2cce575f38d3bb3f015fd3decdbcb30d3d816bb 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.cpp
@@ -1,4 +1,5 @@
-#include <boost/algorithm/clamp.hpp>
+
+#include "CartesianImpedanceController.h"
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
@@ -6,7 +7,8 @@
#include <VirtualRobot/MathTools.h>
-#include "CartesianImpedanceController.h"
+#include <algorithm>
+
using namespace armarx;
@@ -112,7 +114,7 @@ const Eigen::VectorXf& CartesianImpedanceController::run(
(I - jacobi.transpose() * jtpinv) * nullspaceTorque;
for (int i = 0; i < targetJointTorques.size(); ++i)
{
- targetJointTorques(i) = boost::algorithm::clamp(targetJointTorques(i), -cfg.torqueLimit, cfg.torqueLimit);
+ targetJointTorques(i) = std::clamp(targetJointTorques(i), -cfg.torqueLimit, cfg.torqueLimit);
}
//write debug data
{
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
index 36c2bbdeb1335ba1523585997bc01897f24f2772..d2d10aee0d547b4341c343d368e9c518de86b501 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
@@ -19,15 +19,11 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#include <boost/algorithm/clamp.hpp>
+
+#include "NJointTaskSpaceImpedanceController.h"
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
-#include <SimoxUtility/math/compare/is_equal.h>
-
-#include <VirtualRobot/MathTools.h>
-
-#include "NJointTaskSpaceImpedanceController.h"
using namespace armarx;
@@ -100,7 +96,7 @@ void NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& /*sensorValu
velocitySensors,
positionSensors
);
- ARMARX_CHECK_EXPRESSION(simox::math::is_equal(targets.size(), jointDesiredTorques.size()));
+ ARMARX_CHECK_EQUAL(targets.size(), static_cast<size_t>(jointDesiredTorques.size()));
for (size_t i = 0; i < targets.size(); ++i)
{
diff --git a/source/RobotAPI/components/units/RobotUnit/PDController.h b/source/RobotAPI/components/units/RobotUnit/PDController.h
index e534efd0ca0426daabf74b7bb04408e676c0a412..87abee65875d8827f49dbd1426ec20965cdb8ad8 100644
--- a/source/RobotAPI/components/units/RobotUnit/PDController.h
+++ b/source/RobotAPI/components/units/RobotUnit/PDController.h
@@ -23,11 +23,6 @@
#include <algorithm>
#include <cmath>
-#include <tuple>
-
-#include <boost/algorithm/clamp.hpp>
-
-#include <ArmarXCore/core/util/algorithm.h>
namespace armarx
{
@@ -310,7 +305,7 @@ namespace armarx
}
if (maxError != 0)
{
- error = boost::algorithm::clamp(error, -maxError, +maxError);
+ error = std::clamp(error, -maxError, +maxError);
}
const FloatingType pOut = Kp * error;
@@ -328,11 +323,11 @@ namespace armarx
if (outRampDelta != 0)
{
- out = boost::algorithm::clamp(out, lastOutValue - outRampDelta, lastOutValue + outRampDelta);
+ out = std::clamp(out, lastOutValue - outRampDelta, lastOutValue + outRampDelta);
}
if (limitOutLo != limitOutHi)
{
- out = boost::algorithm::clamp(out, limitOutLo, limitOutHi);
+ out = std::clamp(out, limitOutLo, limitOutHi);
}
if (lastOutInterpolation != 0)
{
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index da20242655d43b949313fe1d295036e3e43fe282..33bdd5efd8d0671e25191e9b9f6020286eed846d 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -20,9 +20,6 @@
* GNU General Public License
*/
-#include <regex>
-
-#include <boost/iterator/transform_iterator.hpp>
#include <ArmarXCore/util/CPPUtility/trace.h>
#include <ArmarXCore/util/CPPUtility/Iterator.h>
@@ -36,7 +33,8 @@
#include "../util/ControlThreadOutputBuffer.h"
-#include <boost/algorithm/string/predicate.hpp>
+#include <regex>
+
namespace armarx::RobotUnitModule::details
{
struct DoLoggingDurations
@@ -122,25 +120,21 @@ namespace armarx::RobotUnitModule
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
Ice::StringSeq result;
- const auto getName = [](const auto & fieldData)
- {
- return fieldData.name;
- };
for (const auto& data : sensorDeviceValueMetaData)
{
- result.insert(
- result.end(),
- boost::make_transform_iterator(data.fields.begin(), getName),
- boost::make_transform_iterator(data.fields.end(), getName));
+ for (auto& fieldData : data.fields)
+ {
+ result.push_back(fieldData.name);
+ }
}
for (const auto& datas : controlDeviceValueMetaData)
{
for (const auto& data : datas)
{
- result.insert(
- result.end(),
- boost::make_transform_iterator(data.fields.begin(), getName),
- boost::make_transform_iterator(data.fields.end(), getName));
+ for (auto& fieldData : data.fields)
+ {
+ result.push_back(fieldData.name);
+ }
}
}
return result;
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
index 955a336053a4b7d605ea19cb70ba993b00b517a7..b60019683741f988d4eefcf131cd44df44b1be9c 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
@@ -30,9 +30,7 @@
#include "RobotUnitModuleRobotData.h"
-#include <boost/algorithm/string/split.hpp>
-#include <boost/algorithm/string/classification.hpp>
-#include <boost/algorithm/string/trim.hpp>
+#include <SimoxUtility/algorithm/string/string_tools.h>
namespace armarx::RobotUnitModule
{
@@ -112,10 +110,7 @@ namespace armarx::RobotUnitModule
{
CMakePackageFinder finder(robotProjectName);
auto pathsString = finder.getDataDir();
- boost::split(includePaths,
- pathsString,
- boost::is_any_of(";,"),
- boost::token_compress_on);
+ includePaths = simox::alg::split(pathsString, ";,");
ArmarXDataPath::addDataPaths(includePaths);
}
if (!ArmarXDataPath::getAbsolutePath(robotFileName, robotFileName, includePaths))
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
index 1e0ce16e1da5103672893f829883630ec186eb6d..e65e6506a5312346f4409f68a09880c3aa77b939 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
@@ -29,8 +29,7 @@
#include "../NJointControllers/NJointController.h"
-#include <boost/algorithm/string/trim.hpp>
-#include <boost/algorithm/string/classification.hpp>
+#include <SimoxUtility/algorithm/string/string_tools.h>
#define FLOOR_OBJ_STR "FLOOR"
@@ -54,7 +53,7 @@ namespace armarx::RobotUnitModule
{
ARMARX_DEBUG << "---- group: " << group;
// Removing parentheses
- boost::trim_if(group, boost::is_any_of(" \t{}"));
+ simox::alg::trim_if(group, " \t{}");
std::set<std::set<std::string>> setsOfNode;
{
auto splittedRaw = armarx::Split(group, ",", true, true);
@@ -64,7 +63,7 @@ namespace armarx::RobotUnitModule
}
for (auto& subentry : splittedRaw)
{
- boost::trim_if(subentry, boost::is_any_of(" \t{}"));
+ simox::alg::trim_if(subentry, " \t{}");
if (selfCollisionAvoidanceRobot->hasRobotNodeSet(subentry))
{
std::set<std::string> nodes;
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
index ef9e3aed23402c1fc0ed6670a22886d9d770167b..25c81d4ac78aff9057daf6fb9dc318ed753eead6 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
@@ -24,20 +24,18 @@
#include "HeterogenousContinuousContainer.h"
-#include <vector>
+#include "../SensorValues/SensorValueBase.h"
+#include "../ControlTargets/ControlTargetBase.h"
-#include <boost/format.hpp>
+#include "../Devices/SensorDevice.h"
+#include "../Devices/ControlDevice.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/util/StringHelperTemplates.h>
#include <ArmarXCore/core/util/PropagateConst.h>
#include <ArmarXCore/core/util/TripleBuffer.h>
-#include "../SensorValues/SensorValueBase.h"
-#include "../ControlTargets/ControlTargetBase.h"
-
-#include "../Devices/SensorDevice.h"
-#include "../Devices/ControlDevice.h"
+#include <vector>
namespace armarx
{
@@ -52,23 +50,6 @@ namespace armarx::RobotUnitModule
namespace armarx::detail
{
- inline std::string armarx_sprintf_helper(boost::format& f)
- {
- return boost::str(f);
- }
-
- template<class T, class... Args>
- inline std::string armarx_sprintf_helper(boost::format& f, T&& t, Args&& ... args)
- {
- return armarx_sprintf_helper(f % std::forward<T>(t), std::forward<Args>(args)...);
- }
- template<class... Args>
- inline std::string armarx_sprintf(const std::string& formatString, Args&& ... args)
- {
- boost::format f(formatString);
- return ::armarx::detail::armarx_sprintf_helper(f, std::forward<Args>(args)...);
- }
-
struct RtMessageLogEntryBase
{
RtMessageLogEntryBase():
@@ -278,7 +259,6 @@ namespace armarx
}
#define _detail_ARMARX_RT_REDIRECT( ...) ([&]{ \
- ::armarx::detail::armarx_sprintf(__VA_ARGS__); \
ARMARX_INFO << deactivateSpam(1) << "Redirected RT Logging:\n";\
return &::armarx::detail::RtMessageLogEntryNull::Instance;}())
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
index 5d0608286c7bf28b28f7ff2583ec3a111d471758..29fb7883065ee2f18e68dd5c909e17e8f8d8246c 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
@@ -24,6 +24,7 @@
#include "../EigenForwardDeclarations.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/observers/variant/TimedVariant.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp
index 9c434042b75b23ab4393ed3e7c3a65a0658181e5..4f79878eef58dbe407b744780f8b50203ca26a04 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnit.cpp
@@ -31,10 +31,9 @@
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/MathTools.h>
-#include <Eigen/Core>
+#include <SimoxUtility/algorithm/string/string_tools.h>
-#include <boost/algorithm/string/split.hpp>
-#include <boost/algorithm/string/classification.hpp>
+#include <Eigen/Core>
#include <memory>
#include <cfloat>
@@ -101,11 +100,7 @@ namespace armarx
}
else
{
- std::vector<std::string> nodesetNames;
- boost::split(nodesetNames,
- nodesetsString,
- boost::is_any_of(","),
- boost::token_compress_on);
+ std::vector<std::string> nodesetNames = simox::alg::split(nodesetsString, ",");
for (auto& name : nodesetNames)
{
diff --git a/source/RobotAPI/components/units/relays/CMakeLists.txt b/source/RobotAPI/components/units/relays/CMakeLists.txt
index 26d539f34c8e5624a614fa891b8f7a19a04f01fd..4c651ad32bb9612e8295a3c58b6a14e98a948b2d 100644
--- a/source/RobotAPI/components/units/relays/CMakeLists.txt
+++ b/source/RobotAPI/components/units/relays/CMakeLists.txt
@@ -1,16 +1,16 @@
-set(LIB_NAME RobotAPIUnitListenerRelays)
-armarx_set_target("RobotAPI Unit Listener Relays Library: ${LIB_NAME}")
+#set(LIB_NAME RobotAPIUnitListenerRelays)
+#armarx_set_target("RobotAPI Unit Listener Relays Library: ${LIB_NAME}")
-set(LIBS ArmarXCore RobotAPIInterfaces)
+#set(LIBS ArmarXCore RobotAPIInterfaces)
-set(LIB_HEADERS
- KinematicUnitListenerRelay.h
- InertialMeasurementUnitListenerRelay.h
- ForceTorqueUnitListenerRelay.h
- RobotStateListenerRelay.h
-)
+#set(LIB_HEADERS
+# KinematicUnitListenerRelay.h
+# InertialMeasurementUnitListenerRelay.h
+# ForceTorqueUnitListenerRelay.h
+# RobotStateListenerRelay.h
+#)
-set(LIB_FILES syntaxcheck.cpp)
+#set(LIB_FILES syntaxcheck.cpp)
-armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+#armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/components/units/relays/ForceTorqueUnitListenerRelay.h b/source/RobotAPI/components/units/relays/ForceTorqueUnitListenerRelay.h
deleted file mode 100644
index 22497a7116897b4e6ee50e814edad776d29b9861..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/units/relays/ForceTorqueUnitListenerRelay.h
+++ /dev/null
@@ -1,56 +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/>.
- *
- * @package VisionX::ArmarXObjects::ForceTorqueUnitListenerRelay
- * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2016
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#include <ArmarXCore/core/ManagedIceObject.h>
-#include <ArmarXCore/core/util/noop.h>
-#include <RobotAPI/interface/units/ForceTorqueUnit.h>
-
-namespace armarx
-{
- class ForceTorqueUnitListenerRelay :
- virtual public ManagedIceObject,
- virtual public ForceTorqueUnitListener
- {
- public:
- using CallbackFunctionSensorValues = std::function<void(const std::string&, const FramedDirectionBasePtr&, const FramedDirectionBasePtr&)>;
-
- // ForceTorqueUnitListener interface
- void reportSensorValues(const std::string& sensorNodeName, const FramedDirectionBasePtr& forces, const FramedDirectionBasePtr& torques, const Ice::Current& = Ice::emptyCurrent) override
- {
- callbackReportSensorValues(sensorNodeName, forces, torques);
- }
- protected:
- // ManagedIceObject interface
- void onInitComponent() override
- {
- usingTopic(topicName);
- }
- void onConnectComponent() override {}
- std::string getDefaultName() const override
- {
- return "ForceTorqueUnitListenerRelay";
- }
- public:
- std::string topicName;
- CallbackFunctionSensorValues callbackReportSensorValues {noop<const std::string&, const FramedDirectionBasePtr&, const FramedDirectionBasePtr&>};
- };
- using ForceTorqueUnitListenerRelayPtr = IceInternal::Handle<ForceTorqueUnitListenerRelay>;
-}
diff --git a/source/RobotAPI/components/units/relays/InertialMeasurementUnitListenerRelay.h b/source/RobotAPI/components/units/relays/InertialMeasurementUnitListenerRelay.h
deleted file mode 100644
index 8a305eb62f96a8fec2162bb54784ef48b682ed9a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/units/relays/InertialMeasurementUnitListenerRelay.h
+++ /dev/null
@@ -1,56 +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/>.
- *
- * @package VisionX::ArmarXObjects::InertialMeasurementUnitListenerRelay
- * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2016
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#include <ArmarXCore/core/ManagedIceObject.h>
-#include <ArmarXCore/core/util/noop.h>
-#include <RobotAPI/interface/units/InertialMeasurementUnit.h>
-
-namespace armarx
-{
- class InertialMeasurementUnitListenerRelay :
- virtual public ManagedIceObject,
- virtual public InertialMeasurementUnitListener
- {
- public:
- using CallbackFunctionSensorValues = std::function<void(const std::string&, const std::string&, const IMUData&, const TimestampBasePtr&)>;
-
- // InertialMeasurementUnitListener interface
- void reportSensorValues(const std::string& device, const std::string& name, const IMUData& values, const TimestampBasePtr& timestamp, const Ice::Current& = Ice::emptyCurrent) override
- {
- callbackReportSensorValues(device, name, values, timestamp);
- }
- protected:
- // ManagedIceObject interface
- void onInitComponent() override
- {
- usingTopic(topicName);
- }
- void onConnectComponent() override {}
- std::string getDefaultName() const override
- {
- return "InertialMeasurementUnitListenerRelay";
- }
- public:
- std::string topicName;
- CallbackFunctionSensorValues callbackReportSensorValues {noop<const std::string&, const std::string&, const IMUData&, const TimestampBasePtr&>};
- };
- using InertialMeasurementUnitListenerRelayPtr = IceInternal::Handle<InertialMeasurementUnitListenerRelay>;
-}
diff --git a/source/RobotAPI/components/units/relays/KinematicUnitListenerRelay.h b/source/RobotAPI/components/units/relays/KinematicUnitListenerRelay.h
deleted file mode 100644
index c653ba682b2e7b4586336abae7cff92a84b54c2a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/units/relays/KinematicUnitListenerRelay.h
+++ /dev/null
@@ -1,94 +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/>.
- *
- * @package VisionX::ArmarXObjects::KinematicUnitListenerRelay
- * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2016
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#include <ArmarXCore/core/ManagedIceObject.h>
-#include <ArmarXCore/core/util/noop.h>
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
-
-namespace armarx
-{
- class KinematicUnitListenerRelay :
- virtual public ManagedIceObject,
- virtual public KinematicUnitListener
- {
- public:
- using CallbackFunctionModes = std::function<void(const NameControlModeMap&, Ice::Long, bool)>;
- using CallbackFunctionState = std::function<void(const NameStatusMap&, Ice::Long, bool)>;
- using CallbackFunctionValue = std::function<void(const NameValueMap&, Ice::Long, bool)>;
-
- // KinematicUnitListener interface
- void reportControlModeChanged(const NameControlModeMap& map, Ice::Long time, bool changes, const Ice::Current& = Ice::emptyCurrent) override
- {
- callbackReportControlModeChanged(map, time, changes);
- }
- void reportJointAngles(const NameValueMap& map, Ice::Long time, bool changes, const Ice::Current& = Ice::emptyCurrent) override
- {
- callbackReportJointAngles(map, time, changes);
- }
- void reportJointVelocities(const NameValueMap& map, Ice::Long time, bool changes, const Ice::Current& = Ice::emptyCurrent) override
- {
- callbackReportJointVelocities(map, time, changes);
- }
- void reportJointTorques(const NameValueMap& map, Ice::Long time, bool changes, const Ice::Current& = Ice::emptyCurrent) override
- {
- callbackReportJointTorques(map, time, changes);
- }
- void reportJointAccelerations(const NameValueMap& map, Ice::Long time, bool changes, const Ice::Current& = Ice::emptyCurrent) override
- {
- callbackReportJointAccelerations(map, time, changes);
- }
- void reportJointCurrents(const NameValueMap& map, Ice::Long time, bool changes, const Ice::Current& = Ice::emptyCurrent) override
- {
- callbackReportJointCurrents(map, time, changes);
- }
- void reportJointMotorTemperatures(const NameValueMap& map, Ice::Long time, bool changes, const Ice::Current& = Ice::emptyCurrent) override
- {
- callbackReportJointMotorTemperatures(map, time, changes);
- }
- void reportJointStatuses(const NameStatusMap& map, Ice::Long time, bool changes, const Ice::Current& = Ice::emptyCurrent) override
- {
- callbackReportJointStatuses(map, time, changes);
- }
-
- protected:
- // ManagedIceObject interface
- void onInitComponent() override
- {
- usingTopic(topicName);
- }
- void onConnectComponent() override {}
- std::string getDefaultName() const override
- {
- return "KinematicUnitListenerRelay";
- }
- public:
- std::string topicName;
- CallbackFunctionModes callbackReportControlModeChanged {noop<const NameControlModeMap&, Ice::Long, bool>};
- CallbackFunctionValue callbackReportJointAngles {noop<const NameValueMap&, Ice::Long, bool>};
- CallbackFunctionValue callbackReportJointVelocities {noop<const NameValueMap&, Ice::Long, bool>};
- CallbackFunctionValue callbackReportJointTorques {noop<const NameValueMap&, Ice::Long, bool>};
- CallbackFunctionValue callbackReportJointAccelerations {noop<const NameValueMap&, Ice::Long, bool>};
- CallbackFunctionValue callbackReportJointCurrents {noop<const NameValueMap&, Ice::Long, bool>};
- CallbackFunctionValue callbackReportJointMotorTemperatures {noop<const NameValueMap&, Ice::Long, bool>};
- CallbackFunctionState callbackReportJointStatuses {noop<const NameStatusMap&, Ice::Long, bool>};
- };
- using KinematicUnitListenerRelayPtr = IceInternal::Handle<KinematicUnitListenerRelay>;
-}
diff --git a/source/RobotAPI/components/units/relays/RobotStateListenerRelay.h b/source/RobotAPI/components/units/relays/RobotStateListenerRelay.h
deleted file mode 100644
index ab4bf2ac5addfddc1d84662b02bfbe73adf9c6a4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/units/relays/RobotStateListenerRelay.h
+++ /dev/null
@@ -1,61 +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/>.
- *
- * @package VisionX::ArmarXObjects::KinematicUnitListenerRelay
- * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2016
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#include <ArmarXCore/core/ManagedIceObject.h>
-#include <ArmarXCore/core/util/noop.h>
-#include <RobotAPI/interface/core/RobotState.h>
-
-namespace armarx
-{
- class RobotStateListenerRelay :
- virtual public ManagedIceObject,
- virtual public RobotStateListenerInterface
- {
- public:
- using CallbackFunctionValue = std::function<void(const NameValueMap&, Ice::Long, bool)>;
- using CallbackFunctionPose = std::function<void(const FramedPoseBasePtr&, Ice::Long, bool)>;
-
- virtual void reportJointValues(const NameValueMap& map, Ice::Long time, bool changes, const Ice::Current& = Ice::emptyCurrent)
- {
- callbackReportJointValues(map, time, changes);
- }
- void reportGlobalRobotRootPose(const FramedPoseBasePtr& pose, Ice::Long time, bool changes, const Ice::Current& = Ice::emptyCurrent)
- {
- callbackReportGlobalRobotRootPose(pose, time, changes);
- }
- protected:
- // ManagedIceObject interface
- virtual void onInitComponent()
- {
- usingTopic(topicName);
- }
- virtual void onConnectComponent() {}
- virtual std::string getDefaultName() const
- {
- return "RobotStateListener";
- }
- public:
- std::string topicName;
- CallbackFunctionValue callbackReportJointValues {noop<const NameValueMap&, Ice::Long, bool>};
- CallbackFunctionPose callbackReportGlobalRobotRootPose {noop<const FramedPoseBasePtr&, Ice::Long, bool>};
- };
- using RobotStateListenerRelayPtr = IceInternal::Handle<RobotStateListenerRelay>;
-}
diff --git a/source/RobotAPI/components/units/relays/syntaxcheck.cpp b/source/RobotAPI/components/units/relays/syntaxcheck.cpp
deleted file mode 100644
index 82b6a31b758ca9668e4219e346f9ae3be5e4a477..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/units/relays/syntaxcheck.cpp
+++ /dev/null
@@ -1,3 +0,0 @@
-#include "KinematicUnitListenerRelay.h"
-#include "InertialMeasurementUnitListenerRelay.h"
-#include "ForceTorqueUnitListenerRelay.h"
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
index af108e0e0839fe4b8d1d6c912163e67e0ffeaec9..52af2c8dd81679509f7aaf965caa91e3b49349eb 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
@@ -24,10 +24,11 @@
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-#include <boost/algorithm/string/replace.hpp>
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <HokuyoLaserScannerDriver/urg_utils.h>
+
using namespace armarx;
@@ -277,7 +278,7 @@ void HokuyoLaserScanDevice::run()
durations["update_ms"] = new Variant((time_update - time_measure).toMilliSecondsDouble());
durations["topic_sensor_ms"] = new Variant((time_topicSensor - time_update).toMilliSecondsDouble());
durations["topic_health_ms"] = new Variant((time_topicHeartbeat - time_topicSensor).toMilliSecondsDouble());
- debugObserver->setDebugChannel("LaserScannerDuration_" + boost::replace_all_copy(ip, ".", "_"), durations);
+ debugObserver->setDebugChannel("LaserScannerDuration_" + simox::alg::replace_all(ip, ".", "_"), durations);
if (duration.toSecondsDouble() > 0.1)
{
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
index 04fa0b34bf676e53beae6ba6cdaf185598d72447..8fdadffeb177990068d890b26551b88d5712cc6f 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
@@ -25,10 +25,6 @@
// Include headers you only need in function definitions in the .cpp.
-// #include <Eigen/Core>
-
-// #include <SimoxUtility/color/Color.h>
-
namespace armarx
{
@@ -53,12 +49,12 @@ namespace armarx
case RunState::scannerRun:
if (result == sick_scan::ExitSuccess) // OK -> loop again
{
+ scanData.clear();
result = scanner->loopOnce(scanData, scanTime, scanInfo, false);
if (scanTopic)
{
TimestampVariantPtr scanT(new TimestampVariant(scanTime));
- scanTopic->reportSensorValues(ip, scannerName, scanData, scanT);
- scanData.clear();
+ scanTopic->reportSensorValues(scannerName, scannerName, scanData, scanT);
//trigger heartbeat
scannerHeartbeat->heartbeat(scannerName);
}
@@ -85,12 +81,12 @@ namespace armarx
void SickLaserScanDevice::initScanner()
{
- ARMARX_INFO_S << "Start initialising scanner [Ip: " << this->ip
- << "] [Port: " << this->port << "]";
+ ARMARX_INFO_S << "Start initialising scanner " << scannerName
+ << " [Ip: " << this->ip << "] [Port: " << this->port << "]";
// attempt to connect/reconnect
if (this->scanner)
{
- ARMARX_WARNING_S << "Scanner already initized.";
+ ARMARX_WARNING_S << "Scanner already initialized - reinit.";
this->scanner.reset(); // disconnect scanner
}
this->scanner = std::make_unique<SickScanAdapter>(this->ip, this->port, this->timelimit, this->parser.get(), 'A');
@@ -108,7 +104,6 @@ namespace armarx
}
else
{
- ARMARX_INFO_S << "Reinitializing scanner.";
this->runState = RunState::scannerInit; // If there was an error, try to restart scanner
}
}
@@ -131,12 +126,13 @@ namespace armarx
def->optional(properties.timelimit, "timelimit", "timelimit for communication");
def->optional(properties.rangeMin, "rangeMin", "minimum Range of the Scanner");
def->optional(properties.rangeMax, "rangeMax", "maximum Range of the Scanner");
+ def->optional(properties.heartbeatWarnMS, "heartbeatWarnMS", "maximum cycle time before heartbeat Warning");
+ def->optional(properties.heartbeatErrorMS, "heartbeatErrorMS", "maximum cycle time before heartbeat Error");
return def;
}
SickLaserUnit::SickLaserUnit()
{
- //heartbeat = addPlugin<plugins::HeartbeatComponentPlugin>("SickLaserUnit");
addPlugin(heartbeat);
ARMARX_CHECK_NOT_NULL(heartbeat);
}
@@ -145,6 +141,7 @@ namespace armarx
{
ARMARX_INFO << "On init";
// Topics and properties defined above are automagically registered.
+ //offeringTopic(properties.topicName);
// Keep debug observer data until calling `sendDebugObserverBatch()`.
// (Requies the armarx::DebugObserverComponentPluginUser.)
@@ -163,18 +160,8 @@ namespace armarx
continue;
}
SickLaserScanDevice& device = scanDevices.emplace_back();
- device.scanTopic = topic;
device.scannerName = deviceInfo[0];
- if (deviceInfo[1] != "")
- {
- device.ip = deviceInfo[1];
- }
- else
- {
- ARMARX_FATAL << "TCP is not switched on. Probably hostname or port not set.";
- return;
-
- }
+ device.ip = deviceInfo[1];
device.port = deviceInfo[2];
device.timelimit = properties.timelimit;
//scanInfo
@@ -193,8 +180,10 @@ namespace armarx
ARMARX_ERROR_S << "Could not create parser. Wrong Scanner name.";
return;
}
+ armarx::RobotHealthHeartbeatArgs heartbeatArgs =
+ armarx::RobotHealthHeartbeatArgs(properties.heartbeatWarnMS, properties.heartbeatErrorMS, "No LaserScan data available");
device.scannerHeartbeat = heartbeat;
- device.scannerHeartbeat->configureHeartbeatChannel(device.scannerName, armarx::RobotHealthHeartbeatArgs(100, 200, "No updates available"));
+ device.scannerHeartbeat->configureHeartbeatChannel(device.scannerName, heartbeatArgs);
}
}
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
index 662260de57aea4188e45bc6052061a9d01d19598..8fa16e4bf6961f13e1cd8acdf64b9acea8e40332 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
@@ -27,7 +27,6 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
-//#include <RobotAPI/interface/components/RobotHealthInterface.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h>
#include <vector>
@@ -55,7 +54,7 @@ namespace armarx
struct SickLaserScanDevice
{
- std::string scannerName = "LaserScannerFront";
+ std::string scannerName;
//communication parameters
std::string ip;
std::string port;
@@ -125,18 +124,20 @@ namespace armarx
/// Properties shown in the Scenario GUI.
struct Properties
{
- std::string topicName = "SICKLaserScanner";
+ std::string topicName = "LaserScans";
//scanner parameters
std::string devices = "LaserScannerFront,192.168.8.133,2112";
std::string scannerType = "sick_tim_5xx";
int timelimit = 5;
double rangeMin = 0.0;
double rangeMax = 10.0;
+ int heartbeatWarnMS = 500;
+ int heartbeatErrorMS = 800;
};
Properties properties;
std::vector<SickLaserScanDevice> scanDevices;
LaserScannerUnitListenerPrx topic;
- plugins::HeartbeatComponentPlugin* heartbeat;
+ plugins::HeartbeatComponentPlugin* heartbeat = NULL;
};
} // namespace armarx
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
index 715265cb484cabb48629d08b770ece7171a567d6..649d40ccf31ae409e140f651c40010c7fd22ad8b 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
@@ -88,15 +88,15 @@
//std::vector<unsigned char> receivedData(65536);
//static long receivedDataLen = 0;
-static int getDiagnosticErrorCode()
-{
-#ifdef _MSC_VER
-#undef ERROR
- return (2);
-#else
- return (diagnostic_msgs::DiagnosticStatus::ERROR);
-#endif
-}
+//static int getDiagnosticErrorCode()
+//{
+//#ifdef _MSC_VER
+//#undef ERROR
+// return (2);
+//#else
+// return (diagnostic_msgs::DiagnosticStatus::ERROR);
+//#endif
+//}
namespace armarx
{
@@ -154,9 +154,8 @@ namespace armarx
int actual_length = 0;
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);
+ //always use ASCII
+ int result = getDatagramIceTime(scanTime, receiveBuffer, &actual_length, &packetsInLoop);
//ros::Duration dur = recvTimeStampPush - recvTimeStamp;
if (result != 0)
{
@@ -168,19 +167,13 @@ namespace armarx
return sick_scan::ExitSuccess;
} // return success to continue looping
- //convert ros::Time recvTimeStamp to IceUtil
- ros::Time correctedStamp = recvTimeStamp + ros::Duration(config_.time_offset);
- uint64_t recvMsec = correctedStamp.toNSec() / 1000;
- scanTime = IceUtil::Time::microSeconds(recvMsec);
-
//datagrams are enclosed in <STX> (0x02), <ETX> (0x03) pairs
char* buffer_pos = (char*) receiveBuffer;
- char* dstart, *dend;
+ char* dstart = NULL;
+ char* dend = NULL;
bool dataToProcess = true;
std::vector<float> vang_vec;
vang_vec.clear();
- dstart = NULL;
- dend = NULL;
while (dataToProcess)
{
@@ -309,7 +302,7 @@ namespace armarx
}
}
- //ros::Time start_time = ros::Time::now(); // will be adjusted in the end
+ //IceUtil::Time start_time = IceUtil::Time::now(); // will be adjusted in the end
// <STX> (\x02)
// 0: Type of command (SN)
@@ -668,7 +661,7 @@ namespace armarx
* hereingekommen sind.
*/
- void SickScanAdapter::processFrame(ros::Time timeStamp, SopasEventMessage& frame)
+ void SickScanAdapter::processFrame(IceUtil::Time timeStamp, SopasEventMessage& frame)
{
if (getProtocolType() == CoLa_A)
@@ -697,11 +690,8 @@ namespace armarx
void SickScanAdapter::readCallbackFunction(UINT8* buffer, UINT32& numOfBytes)
{
- ros::Time rcvTimeStamp = ros::Time::now(); // stamp received datagram
+ IceUtil::Time rcvTimeStamp = IceUtil::Time::now(); // stamp received datagram
bool beVerboseHere = false;
- printInfoMessage(
- "SickScanCommonNw::readCallbackFunction(): Called with " + toString(numOfBytes) + " available bytes.",
- beVerboseHere);
ScopedLock lock(&m_receiveDataMutex); // Mutex for access to the input buffer
UINT32 remainingSpace = sizeof(m_receiveBuffer) - m_numberOfBytesInReceiveBuffer;
@@ -949,10 +939,14 @@ namespace armarx
return sick_scan::ExitSuccess;
}
-
- int SickScanAdapter::get_datagram(ros::Time& recvTimeStamp, unsigned char* receiveBuffer, int bufferSize,
- int* actual_length,
+ int SickScanAdapter::get_datagram(ros::Time& recvTimeStamp, unsigned char* receiveBuffer, int bufferSize, int* actual_length,
bool isBinaryProtocol, int* numberOfRemainingFifoEntries)
+ {
+ return sick_scan::ExitError;
+ }
+
+ int SickScanAdapter::getDatagramIceTime(IceUtil::Time& recvTimeStamp, unsigned char* receiveBuffer,
+ int* actual_length, int* numberOfRemainingFifoEntries)
{
if (NULL != numberOfRemainingFifoEntries)
{
@@ -988,61 +982,6 @@ namespace armarx
memcpy(receiveBuffer, &(dataBuffer[0]), size);
*actual_length = size;
-#if 0
- static int cnt = 0;
- char szFileName[255];
- sprintf(szFileName, "/tmp/dg%06d.bin", cnt++);
-
- FILE* fout;
-
- fout = fopen(szFileName, "wb");
- if (fout != NULL)
- {
- fwrite(receiveBuffer, size, 1, fout);
- fclose(fout);
- }
-#endif
- return sick_scan::ExitSuccess;
-
- if (!socket_.is_open())
- {
- ROS_ERROR("get_datagram: socket not open");
- diagnostics_.broadcast(getDiagnosticErrorCode(), "get_datagram: socket not open.");
- return sick_scan::ExitError;
- }
-
- /*
- * Write a SOPAS variable read request to the device.
- */
- std::vector<unsigned char> reply;
-
- // Wait at most 5000ms for a new scan
- size_t timeout = 30000;
- bool exception_occured = false;
-
- char* buffer = reinterpret_cast<char*>(receiveBuffer);
-
- if (readWithTimeout(timeout, buffer, bufferSize, actual_length, &exception_occured, isBinaryProtocol) !=
- sick_scan::ExitSuccess)
- {
- ROS_ERROR_THROTTLE(1.0, "get_datagram: no data available for read after %zu ms", timeout);
- diagnostics_.broadcast(getDiagnosticErrorCode(), "get_datagram: no data available for read after timeout.");
-
- // Attempt to reconnect when the connection was terminated
- if (!socket_.is_open())
- {
-#ifdef _MSC_VER
- Sleep(1000);
-#else
- sleep(1);
-#endif
- ARMARX_INFO << "Failure - attempting to reconnect";
- return init();
- }
-
- return exception_occured ? sick_scan::ExitError : sick_scan::ExitSuccess; // keep on trying
- }
-
return sick_scan::ExitSuccess;
}
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
index 1707952d9583de18bd6de004f986e216e9c13781..6fe671d13f2974a6fba85835dfb09cf35282858a 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
@@ -57,14 +57,14 @@ namespace armarx
class DatagramWithTimeStamp
{
public:
- DatagramWithTimeStamp(ros::Time timeStamp_, std::vector<unsigned char> datagram_)
+ DatagramWithTimeStamp(IceUtil::Time timeStamp_, std::vector<unsigned char> datagram_)
{
timeStamp = timeStamp_;
datagram = datagram_;
}
// private:
- ros::Time timeStamp;
+ IceUtil::Time timeStamp;
std::vector<unsigned char> datagram;
};
@@ -97,7 +97,7 @@ namespace armarx
SopasEventMessage findFrameInReceiveBuffer();
- void processFrame(ros::Time timeStamp, SopasEventMessage& frame);
+ void processFrame(IceUtil::Time timeStamp, SopasEventMessage& frame);
// Queue<std::vector<unsigned char> > recvQueue;
Queue<DatagramWithTimeStamp> recvQueue;
@@ -129,9 +129,10 @@ namespace armarx
* \param [out] actual_length the actual amount of data written
* \param [in] isBinaryProtocol true=binary False=ASCII
*/
- virtual int
- get_datagram(ros::Time& recvTimeStamp, unsigned char* receiveBuffer, int bufferSize, int* actual_length,
- bool isBinaryProtocol, int* numberOfRemainingFifoEntries);
+ virtual int get_datagram(ros::Time& recvTimeStamp, unsigned char* receiveBuffer, int bufferSize, int* actual_length,
+ bool isBinaryProtocol, int* numberOfRemainingFifoEntries);
+
+ int getDatagramIceTime(IceUtil::Time& recvTimeStamp, unsigned char* receiveBuffer, int* actual_length, int* numberOfRemainingFifoEntries);
// Helpers for boost asio
int readWithTimeout(size_t timeout_ms, char* buffer, int buffer_size, int* bytes_read = 0,
@@ -143,7 +144,6 @@ namespace armarx
private:
-
// Response buffer
UINT32 m_numberOfBytesInResponseBuffer; ///< Number of bytes in buffer
UINT8 m_responseBuffer[1024]; ///< Receive buffer for everything except scan data and eval case data.
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp
index 899c86434cea6757fb0cbb899203131452dea07d..128043174d57794f4df610b2dd9d4658898e4d65 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp
@@ -21,8 +21,10 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
+
#include "WeissHapticSensor.h"
#include "TransmissionException.h"
+
#include <boost/regex.hpp>
#include <boost/format.hpp>
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.h b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.h
index 25191aab19608735c499bacfe3f9d7ae0fb1e401..169d1cf3690bcc2c53e091ab5343836dc93916cf 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.h
+++ b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.h
@@ -23,17 +23,19 @@
*/
#pragma once
-#include <ArmarXCore/core/services/tasks/RunningTask.h>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
#include "SerialInterface.h"
#include "TactileSensor.h"
+#include "TextWriter.h"
+
#include <RobotAPI/interface/units/HapticUnit.h>
+#include <RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h>
+
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <ArmarXCore/util/variants/eigen3/Eigen3VariantObjectFactories.h>
-//#include <ArmarXCore/util/variants/eigen3/Eigen3LibRegistry.h>
-#include "TextWriter.h"
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-#include <RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h>
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <mutex>
diff --git a/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidget.ui b/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidget.ui
index 819331b5f27f58c036bb867e8a17dda8a0c8b977..58bead75ff15f27b039c50e712c1406f83e2fa32 100644
--- a/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidget.ui
+++ b/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidget.ui
@@ -34,7 +34,7 @@
</layout>
</item>
<item>
- <layout class="QHBoxLayout" name="ltmControlWidgetLayout">
+ <layout class="QHBoxLayout" name="diskControlWidgetLayout">
<property name="spacing">
<number>6</number>
</property>
diff --git a/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.cpp b/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.cpp
index 83d1d8743a3b6ed0ebfeca5c704c96334b22108a..ff79f501c35016154272c659db18233caf39a9d6 100644
--- a/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.cpp
@@ -43,7 +43,6 @@ namespace armarx
viewer = std::make_unique<MemoryViewer>(
widget.updateWidgetLayout,
- widget.ltmControlWidgetLayout,
widget.memoryGroupBox,
widget.treesLayout,
@@ -51,9 +50,11 @@ namespace armarx
widget.instanceGroupBox,
widget.treesLayout,
+ widget.diskControlWidgetLayout,
+
widget.statusLabel
);
- viewer->setLogTag(getName());
+ viewer->setLogTag("ArMem Memory Viewer");
armarx::gui::useSplitter(widget.treesLayout);
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
index e210b778f25dd9ce8f83bfd299ff3ec4c75497e2..c3aad744b56229d4ae2f01b716e092abfa5f76f2 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
@@ -21,22 +21,15 @@
*/
#include "ArVizWidgetController.h"
-#include <RobotAPI/components/ArViz/Coin/VisualizationObject.h>
-#include <RobotAPI/components/ArViz/Coin/VisualizationRobot.h>
-#include <string>
+#include <ArmarXCore/observers/variant/Variant.h>
-#include <ArmarXCore/core/ArmarXManager.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <QLineEdit>
#include <QMessageBox>
#include <QTimer>
-
-#include <ArmarXCore/observers/variant/Variant.h>
-
-
#include <QFileDialog>
-#include <boost/algorithm/string/predicate.hpp>
#define ENABLE_INTROSPECTION 1
@@ -154,8 +147,7 @@ namespace armarx
void ArVizWidgetController::onExitComponent()
{
- armarx::viz::coin::clearObjectCache();
- armarx::viz::coin::clearRobotCache();
+ visualizer.clearCache();
}
void ArVizWidgetController::onConnectComponent()
@@ -176,8 +168,6 @@ namespace armarx
void ArVizWidgetController::onDisconnectComponent()
{
- visualizer.stop();
-
// Changes to UI elements are only allowed in the GUI thread
emit disconnectGui();
}
@@ -195,6 +185,7 @@ namespace armarx
void ArVizWidgetController::onDisconnectGui()
{
timingObserverTimer->stop();
+ visualizer.stop();
changeMode(ArVizWidgetMode::NotConnected);
}
@@ -1066,7 +1057,7 @@ namespace armarx
{
return;
}
- if (!boost::algorithm::ends_with(s, ".wrl"))
+ if (!simox::alg::ends_with(s, ".wrl"))
{
s += ".wrl";
}
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
index 4a493dbd19a4c4cab6e05aa2a7ab5376936a3ebe..2dd295afccc97080bea8e7d4a7dc222324fb71d5 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
@@ -33,8 +33,6 @@
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <Inventor/nodes/SoSeparator.h>
-
#include "LayerInfoTree.h"
diff --git a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp
index f4a9f5673d6d782a774fe27333f449fee5cb57b6..9c437484cb636ac16705c5a16d5303501a828361 100644
--- a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp
@@ -23,9 +23,9 @@
#include <ArmarXCore/core/ArmarXManager.h>
-#include <QTimer>
+#include <SimoxUtility/algorithm/string/string_tools.h>
-#include <boost/algorithm/string/join.hpp>
+#include <QTimer>
#include <string>
@@ -282,7 +282,7 @@ namespace armarx
{
std::stringstream itemText;
itemText << layer.layerName
- << " (" << boost::algorithm::join(annotations, ", ") << ")";
+ << " (" << simox::alg::join(annotations, ", ") << ")";
return { itemText.str().c_str() };
}
}
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp
index f224f04b1ec1de9895a94315e2b1bdd24595d7cc..f7693cd8a3fbe75738276da724fcf0ce549856d0 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp
@@ -54,8 +54,8 @@ KinematicUnitConfigDialog::KinematicUnitConfigDialog(QWidget* parent) :
proxyFinder->setSearchMask("*KinematicUnit|KinematicUnit*");
ui->gridLayout->addWidget(proxyFinder, 0, 1, 1, 2);
- connect(proxyFinder->getUi()->cbProxyName, SIGNAL(currentIndexChanged(int)), this, SLOT(selectionChanged(int)));
- connect(proxyFinder->getUi()->cbProxyName, SIGNAL(editTextChanged(QString)), this, SLOT(proxyNameChanged(QString)));
+ connect(proxyFinder->getProxyNameComboBox(), SIGNAL(currentIndexChanged(int)), this, SLOT(selectionChanged(int)));
+ connect(proxyFinder->getProxyNameComboBox(), SIGNAL(editTextChanged(QString)), this, SLOT(proxyNameChanged(QString)));
}
KinematicUnitConfigDialog::~KinematicUnitConfigDialog()
@@ -122,7 +122,7 @@ void KinematicUnitConfigDialog::selectionChanged(int nr)
{
return;
}
- std::string kinematicUnitName = proxyFinder->getUi()->cbProxyName->currentText().toStdString();
+ std::string kinematicUnitName = proxyFinder->getSelectedProxyName().toStdString();
updateSubconfig(kinematicUnitName);
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/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
index 27cf1dca60f0d527637c9c3901998caf4324eb54..4a752e88e1b5962479617584c0fb1870a8a25484 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
@@ -25,6 +25,7 @@
#include "NJointControllerClassesWidget.h"
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/util/StringHelpers.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <QGridLayout>
#include <QDir>
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllersWidget.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllersWidget.cpp
index ab2c8e69926f1adc22e1be4aaee3174106a10dda..b5160a688306a3fec1eadada0567cc14e66e552d 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllersWidget.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllersWidget.cpp
@@ -22,6 +22,8 @@
#include "NJointControllersWidget.h"
#include "StyleSheets.h"
+#include <ArmarXCore/core/logging/Logging.h>
+
namespace armarx
{
NJointControllersWidget::NJointControllersWidget(QWidget* parent) :
diff --git a/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice b/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice
index 85a862f4cd815168d6163d2d085426d71b298068..95b6a9fa6b03d8f7d4644d2538ecfd64d5258229 100644
--- a/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice
+++ b/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice
@@ -1,87 +1,116 @@
#pragma once
-#include <RobotAPI/interface/armem/server/MemoryInterface.ice>
+#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.ice>
+#include <RobotAPI/interface/armem/server/WritingMemoryInterface.ice>
module armarx
{
module armem
{
- module data
+ module mns
{
- struct RegisterMemoryInput
- {
- string name;
- server::MemoryInterface* proxy;
-
- bool existOk = true;
- };
- struct RegisterMemoryResult
- {
- bool success;
- string errorMessage;
- };
-
- struct RemoveMemoryInput
- {
- string name;
- bool notExistOk = true;
- };
- struct RemoveMemoryResult
- {
- bool success;
- string errorMessage;
- };
-
- dictionary<string, server::MemoryInterface*> MemoryInterfaces;
- struct GetAllRegisteredMemoriesResult
- {
- bool success;
- string errorMessage;
-
- MemoryInterfaces proxies;
- };
-
- struct ResolveMemoryNameInput
- {
- string name;
- };
- struct ResolveMemoryNameResult
- {
- bool success;
- string errorMessage;
-
- server::MemoryInterface* proxy;
- };
-
- struct WaitForMemoryInput
- {
- /// The memory name.
- string name;
- /// Negative for no timeout.
- long timeoutMilliSeconds = -1;
- };
- struct WaitForMemoryResult
+ module dto
{
- bool success;
- string errorMessage;
-
- server::MemoryInterface* proxy;
+ /**
+ * A memory server can implement the reading and/or writing
+ * memory interface. They should be handled individually.
+ */
+ struct MemoryServerInterfaces
+ {
+ server::ReadingMemoryInterface* reading;
+ server::WritingMemoryInterface* writing;
+ };
+ dictionary<string, MemoryServerInterfaces> MemoryServerInterfacesMap;
+
+
+ /**
+ * @brief Register a memory server.
+ */
+ struct RegisterServerInput
+ {
+ string name;
+ MemoryServerInterfaces server;
+
+ bool existOk = true;
+ };
+ struct RegisterServerResult
+ {
+ bool success;
+ string errorMessage;
+ };
+
+
+ /**
+ * @brief Remove a memory server.
+ */
+ struct RemoveServerInput
+ {
+ string name;
+ bool notExistOk = true;
+ };
+ struct RemoveServerResult
+ {
+ bool success;
+ string errorMessage;
+ };
+
+ /**
+ * @brief Resolve a memory name.
+ */
+ struct ResolveServerInput
+ {
+ string name;
+ };
+ struct ResolveServerResult
+ {
+ bool success;
+ string errorMessage;
+
+ MemoryServerInterfaces server;
+ };
+
+
+ /**
+ * @brief Get all registered entries.
+ */
+ struct GetAllRegisteredServersResult
+ {
+ bool success;
+ string errorMessage;
+
+ MemoryServerInterfacesMap servers;
+ };
+
+
+ /**
+ * @brief Wait until a server is registered.
+ */
+ struct WaitForServerInput
+ {
+ string name;
+ };
+ struct WaitForServerResult
+ {
+ bool success;
+ string errorMessage;
+
+ MemoryServerInterfaces server;
+ };
};
- };
- module mns
- {
interface MemoryNameSystemInterface
{
- data::RegisterMemoryResult registerMemory(data::RegisterMemoryInput input);
- data::RemoveMemoryResult removeMemory(data::RemoveMemoryInput input);
+ dto::RegisterServerResult registerServer(dto::RegisterServerInput input);
+ dto::RemoveServerResult removeServer(dto::RemoveServerInput input);
+
+ dto::GetAllRegisteredServersResult getAllRegisteredServers();
- data::GetAllRegisteredMemoriesResult getAllRegisteredMemories();
+ dto::ResolveServerResult resolveServer(dto::ResolveServerInput input);
- data::ResolveMemoryNameResult resolveMemoryName(data::ResolveMemoryNameInput input);
- data::WaitForMemoryResult waitForMemory(data::WaitForMemoryInput input);
+ ["amd"] // Asynchronous Method Dispatch
+ dto::WaitForServerResult waitForServer(dto::WaitForServerInput input);
};
}
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/armem/server/LoadingMemoryInterface.ice b/source/RobotAPI/interface/armem/server/LoadingMemoryInterface.ice
index bb1e7f221856b4505184fdeb380683c6f8cb917b..68a3ff2ef6fc06114eaff88dcb3e0393002b1160 100644
--- a/source/RobotAPI/interface/armem/server/LoadingMemoryInterface.ice
+++ b/source/RobotAPI/interface/armem/server/LoadingMemoryInterface.ice
@@ -11,7 +11,7 @@ module armarx
{
interface LoadingMemoryInterface
{
- armem::query::data::Result load(armem::query::data::Input query);
+
};
};
};
diff --git a/source/RobotAPI/interface/aron/Aron.ice b/source/RobotAPI/interface/aron/Aron.ice
index dc88e0484490476691ee98e3c3bf338964c02e6d..9c3baf7929680117c4b933960fa00342600ee06b 100644
--- a/source/RobotAPI/interface/aron/Aron.ice
+++ b/source/RobotAPI/interface/aron/Aron.ice
@@ -72,11 +72,25 @@ module armarx
class AronDict extends AronType { AronType acceptedType; }
// Complex Types (serialize to ndarray)
+
+
class AronNDArray extends AronType { AronIntSequence dimensions; int elementSize; string typeName; }
+ // NdArray should look like this:
+ // class AronNDArray extends AronType { int ndim; string type; }
+ // type can be: "uint8", "int8", "uint16", "int16", "uint32", "int32", "float32", "float64"
+
+
+
class AronEigenMatrix extends AronType { int rows; int cols; string typeName; }
class AronEigenQuaternion extends AronType { string typeName; }
+
+ // pixelType can be: "rgb24", "depth32"
+ class AronImage extends AronType { string pixelType; }
+
+ // to be removed:
class AronIVTCByteImage extends AronType { }
- class AronOpenCVMat extends AronType { }
+ class AronOpenCVMat extends AronType { AronIntSequence shape; string typeName; }
+
class AronPCLPointCloud extends AronType { string typeName; }
class AronPosition extends AronType { }
class AronOrientation extends AronType { }
@@ -114,6 +128,10 @@ module armarx
// Further, note the difference between the type's dimensions (e.g. 128x128) and the data's dimensions (128x128x3 for RGB)
class AronNDArray extends AronData { AronIntSequence dimensions; string type; AronByteSequence data; };
+ // NDArray should look like this: (type is be as above and implies the bit-width of an element)
+ // class AronNDArray extends AronData { AronIntSequence shape; string type; AronByteSequence data; };
+
+
// Primitive Data
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
class Aron##upperType extends AronData { lowerType value; };
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/libraries/ArmarXEtherCAT/DeviceContainer.h b/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.h
index 52288ccc8797644cf80038d71f2eacfd97d02cb1..538c1b5cb5c5b47222b97f13d6417706d4b8a746 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.h
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.h
@@ -26,6 +26,11 @@
#include <memory>
#include <VirtualRobot/VirtualRobot.h>
+// These includes are only here because Armar6RT is currently locked (missing include in Slave.cpp)
+// TODO: Remove once Armar6RT includes the needed headers
+#include <boost/core/demangle.hpp>
+#include <boost/optional.hpp>
+
namespace armarx
{
class MultiNodeRapidXMLReader;
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/SlaveIdentifier.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/SlaveIdentifier.cpp
index f295d0ffd6bddd4c05c0d63d4a47d2ff970157b1..8dca1b8e84979538be9fead1a60af32ee7bb0a22 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/SlaveIdentifier.cpp
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/SlaveIdentifier.cpp
@@ -23,6 +23,8 @@
#include "SlaveIdentifier.h"
+#include <ArmarXCore/core/logging/Logging.h>
+
using namespace armarx;
SlaveIdentifier::SlaveIdentifier(const RapidXmlReaderNode& node)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
index 47728cbd7249f4983717f9a1dbb079e79215fadc..93bdb0286401d7a47a153cf3da6eda1e16dae93a 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
@@ -6,6 +6,7 @@
#include <SimoxUtility/shapes/OrientedBox.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
namespace armarx
diff --git a/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp b/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
index 85ca31bec07bbe8af4108a64a24805da44dd19d5..433fc67ec7890cbb7fede64d2edda22533e46eb8 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
@@ -97,8 +97,10 @@ BOOST_AUTO_TEST_CASE(test_find)
BOOST_TEST_CONTEXT("Object: " << object.id() << " at " << objectDir)
{
BOOST_CHECK(fs::is_directory(objectDir));
+#if 0
BOOST_CHECK(fs::is_regular_file(simoxXML)
|| fs::is_regular_file(object.articulatedSimoxXML().absolutePath));
+#endif
}
}
}
diff --git a/source/RobotAPI/libraries/DSControllers/DSJointCarryController.h b/source/RobotAPI/libraries/DSControllers/DSJointCarryController.h
index c2da13cd958557b1462d97c58c3c2b458f14c21f..8f360615d02c3ecb3d2a07192ee8013b8ee73abf 100644
--- a/source/RobotAPI/libraries/DSControllers/DSJointCarryController.h
+++ b/source/RobotAPI/libraries/DSControllers/DSJointCarryController.h
@@ -44,7 +44,7 @@
namespace armarx
{
- typedef boost::shared_ptr<GMRDynamics> JointCarryGMMPtr;
+ using JointCarryGMMPtr = std::shared_ptr<GMRDynamics>;
struct JointCarryGMRParameters
{
@@ -150,7 +150,7 @@ namespace armarx
};
- typedef boost::shared_ptr<JointCarryGMMMotionGen> JointCarryGMMMotionGenPtr;
+ using JointCarryGMMMotionGenPtr = std::shared_ptr<JointCarryGMMMotionGen>;
class DSJointCarryControllerControlData
{
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.h b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.h
index 02fc3f8ab609b6cbcdd6529256e6955e126467a8..7a693af5524f0d7364de46a00746ad0d9e834f17 100644
--- a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.h
+++ b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.h
@@ -56,7 +56,7 @@ namespace armarx
};
- typedef boost::shared_ptr<GMRDynamics> BimanualGMMPtr;
+ using BimanualGMMPtr = std::shared_ptr<class GMRDynamics>;
struct BimanualGMRParameters
{
@@ -291,7 +291,7 @@ namespace armarx
};
- typedef boost::shared_ptr<BimanualGMMMotionGen> BimanualGMMMotionGenPtr;
+ using BimanualGMMMotionGenPtr = std::shared_ptr<BimanualGMMMotionGen>;
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTController.h b/source/RobotAPI/libraries/DSControllers/DSRTController.h
index d60552cef0a5bb6bf563b3d161a93185c0560220..3ac02e59e58a27e44e0dc59a9682286dc8c0f938 100644
--- a/source/RobotAPI/libraries/DSControllers/DSRTController.h
+++ b/source/RobotAPI/libraries/DSControllers/DSRTController.h
@@ -46,7 +46,7 @@ namespace armarx
Eigen::Vector3f tcpDesiredAngularError;
};
- typedef boost::shared_ptr<GMRDynamics> GMMPtr;
+ using GMMPtr = std::shared_ptr<GMRDynamics>;
struct GMRParameters
{
@@ -155,7 +155,7 @@ namespace armarx
Eigen::Vector3f currentDesiredVelocity;
};
- typedef boost::shared_ptr<GMMMotionGen> GMMMotionGenPtr;
+ using GMMMotionGenPtr = std::shared_ptr<GMMMotionGen>;
class DSAdaptor
{
@@ -343,7 +343,234 @@ namespace armarx
}
};
- typedef boost::shared_ptr<DSAdaptor> DSAdaptorPtr;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ using DSAdaptorPtr = std::shared_ptr<DSAdaptor>;
diff --git a/source/RobotAPI/libraries/GraspingUtility/BimanualGraspCandidateHelper.h b/source/RobotAPI/libraries/GraspingUtility/BimanualGraspCandidateHelper.h
index 19a474878ee3c0f8f622122164810958320c455d..1b2e9b10cf81e17240e09af578468a12c13c4b40 100644
--- a/source/RobotAPI/libraries/GraspingUtility/BimanualGraspCandidateHelper.h
+++ b/source/RobotAPI/libraries/GraspingUtility/BimanualGraspCandidateHelper.h
@@ -23,17 +23,18 @@
#pragma once
-#include <boost/shared_ptr.hpp>
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+#include <RobotAPI/libraries/core/Pose.h>
#include <VirtualRobot/Robot.h>
-#include <RobotAPI/libraries/core/Pose.h>
+#include <memory>
+
namespace armarx
{
- typedef boost::shared_ptr<class BimanualGraspCandidateHelper> BimanualGraspCandidateHelperPtr;
+ typedef std::shared_ptr<class BimanualGraspCandidateHelper> BimanualGraspCandidateHelperPtr;
class BimanualGraspCandidateHelper
{
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h
index 050958f91cb13e5ad34f1a87110e759bc5402b1e..0fa9fe1ea3d713e090dda9d3a529050e1b9f82d1 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h
@@ -23,17 +23,17 @@
#pragma once
-#include <boost/shared_ptr.hpp>
-
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+#include <RobotAPI/libraries/core/Pose.h>
#include <VirtualRobot/Robot.h>
-#include <RobotAPI/libraries/core/Pose.h>
+#include <memory>
+
namespace armarx
{
- typedef boost::shared_ptr<class GraspCandidateHelper> GraspCandidateHelperPtr;
+ typedef std::shared_ptr<class GraspCandidateHelper> GraspCandidateHelperPtr;
class GraspCandidateHelper
{
diff --git a/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.ipp b/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.ipp
index 640185fc12c57b7c880307132401d000225910c9..651d4534de98e41bc9972249aafce732a9231a90 100644
--- a/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.ipp
+++ b/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.ipp
@@ -6,8 +6,7 @@
#include <SimoxUtility/meta/enum/adapt_enum.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
-
-#include "box_to_grasp_candidates.ipp"
+#include <ArmarXCore/core/logging/Logging.h>
namespace simox::meta
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 1730ea67e98907735b71c42798cbec2f3e5d0b63..df6f5374d391a6bdf0ec9378df8668017599b4eb 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -1,6 +1,9 @@
#include "NJointTaskSpaceImpedanceDMPController.h"
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
namespace armarx
{
@@ -747,7 +750,7 @@ namespace armarx
void NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKd(const Eigen::VectorXf& kd, const Ice::Current&)
{
- ARMARX_CHECK_EQUAL(kd.size(), targets.size());
+ ARMARX_CHECK_EQUAL((std::size_t)kd.size(), targets.size());
ARMARX_INFO << "set nullspace kd " << VAROUT(kd);
LockGuardType guard(controllerMutex);
ctrlParams.getWriteBuffer().dnull = kd;
@@ -756,7 +759,7 @@ namespace armarx
void NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKp(const Eigen::VectorXf& kp, const Ice::Current&)
{
- ARMARX_CHECK_EQUAL(kp.size(), targets.size());
+ ARMARX_CHECK_EQUAL((std::size_t)kp.size(), targets.size());
ARMARX_INFO << "set linear kp " << VAROUT(kp);
LockGuardType guard(controllerMutex);
ctrlParams.getWriteBuffer().knull = kp;
@@ -767,7 +770,7 @@ namespace armarx
void NJointTaskSpaceImpedanceDMPController::setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues,
const Ice::Current&)
{
- ARMARX_CHECK_EQUAL(jointValues.size(), targets.size());
+ ARMARX_CHECK_EQUAL((std::size_t)jointValues.size(), targets.size());
defaultNullSpaceJointValues.getWriteBuffer() = jointValues;
defaultNullSpaceJointValues.commitWrite();
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/Track.cpp b/source/RobotAPI/libraries/SimpleTrajectory/Track.cpp
index 6f6bd2970e4f0233510753e1ea6c5354e5fbff0a..88258722f663c186ae3aabe393bcf710c11c12c4 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/Track.cpp
+++ b/source/RobotAPI/libraries/SimpleTrajectory/Track.cpp
@@ -7,9 +7,9 @@ namespace armarx::trajectory
template <>
void Track<VariantValue>::checkValueType(const VariantValue& value)
{
- if (!empty() && value.type() != keyframes.front().value.type())
+ if (!empty() && value.index() != keyframes.front().value.index())
{
- throw error::WrongValueTypeInKeyframe(id, value.type(), keyframes.front().value.type());
+ throw error::WrongValueTypeInKeyframe(id, value.index(), keyframes.front().value.index());
}
}
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.cpp b/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.cpp
index 5211a504403ce4dff2feca568998ac4cd88ca555..3dad904583eca19ac645d364ec045851f1045805 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.cpp
+++ b/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.cpp
@@ -82,7 +82,7 @@ namespace armarx
{
return [func](VariantValue value)
{
- func(boost::get<float>(value));
+ func(std::get<float>(value));
};
}
@@ -90,7 +90,7 @@ namespace armarx
{
return [func](VariantValue value)
{
- func(boost::get<Eigen::MatrixXf>(value));
+ func(std::get<Eigen::MatrixXf>(value));
};
}
@@ -98,7 +98,7 @@ namespace armarx
{
return [func](VariantValue value)
{
- func(boost::get<Eigen::Quaternionf>(value));
+ func(std::get<Eigen::Quaternionf>(value));
};
}
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.h b/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.h
index 5600a669fa6bf6afce56ff08d3f2a947912fd139..f79148ab16b1d8ca6a0cc4d2ccc33ed92ba44ffe 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.h
+++ b/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.h
@@ -90,7 +90,7 @@ namespace armarx::trajectory
{
return [&v](VariantValue value)
{
- v = boost::get<ValueT>(value);
+ v = std::get<ValueT>(value);
};
}
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/VariantValue.h b/source/RobotAPI/libraries/SimpleTrajectory/VariantValue.h
index c7e6db669445a95033f304da8d4eb0da0fb9890b..ec4f5086b8944f12ad22ad096ba8cd9253941a2d 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/VariantValue.h
+++ b/source/RobotAPI/libraries/SimpleTrajectory/VariantValue.h
@@ -1,15 +1,16 @@
#pragma once
-#include <boost/variant.hpp>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
-#include <Eigen/Eigen>
+#include <variant>
namespace armarx::trajectory
{
/// Variant for trajectory values.
- using VariantValue = boost::variant<float, Eigen::MatrixXf, Eigen::Quaternionf>;
+ using VariantValue = std::variant<float, Eigen::MatrixXf, Eigen::Quaternionf>;
/// ID of tracks.
using TrackID = std::string;
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/exceptions.cpp b/source/RobotAPI/libraries/SimpleTrajectory/exceptions.cpp
index 0a1bbdbc953456e391c7d8b1f918c9a2863b503a..8af3f984e0005d1e3f6b8ce1271135d0662fb57d 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/exceptions.cpp
+++ b/source/RobotAPI/libraries/SimpleTrajectory/exceptions.cpp
@@ -34,18 +34,18 @@ namespace armarx::trajectory::error
return ss.str();
}
- WrongValueTypeInKeyframe::WrongValueTypeInKeyframe(
- const TrackID& trackID, const std::type_info& type, const std::type_info& expected) :
- TrajectoryException(makeMsg(trackID, type, expected))
- {}
-
- std::string WrongValueTypeInKeyframe::makeMsg(
- const TrackID& id, const std::type_info& type, const std::type_info& expected)
+ static std::string makeMsg(const TrackID& id, int typeIndex, int expectedTypeIndex)
{
std::stringstream ss;
- ss << "Tried to add keyframe with value type '" << type.name() << "' to non-empty track '"
- << id << "' containing values of type '" << expected.name() << "'. \n"
+ ss << "Tried to add keyframe with value type '" << typeIndex << "' to non-empty track '"
+ << id << "' containing values of type '" << expectedTypeIndex << "'. \n"
<< "Only one value type per track is allowed.";
return ss.str();
}
+
+ WrongValueTypeInKeyframe::WrongValueTypeInKeyframe(const TrackID& trackID, int typeIndex, int expectedTypeIndex) :
+ TrajectoryException(makeMsg(trackID, typeIndex, expectedTypeIndex))
+ {}
+
+
}
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/exceptions.h b/source/RobotAPI/libraries/SimpleTrajectory/exceptions.h
index 6235ef3f737fe97359f4da72c25ec6c870a49da2..131c5fea4a33436bb28c7f63188b9caf4a6719d2 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/exceptions.h
+++ b/source/RobotAPI/libraries/SimpleTrajectory/exceptions.h
@@ -38,10 +38,7 @@ namespace armarx::trajectory::error
struct WrongValueTypeInKeyframe : public TrajectoryException
{
- WrongValueTypeInKeyframe(const TrackID& trackID, const std::type_info& type,
- const std::type_info& expected);
- static std::string makeMsg(const TrackID& trackID, const std::type_info& type,
- const std::type_info& expected);
+ WrongValueTypeInKeyframe(const TrackID& trackID, int typeIndex, int expectedTypeIndex);
};
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.cpp b/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.cpp
index c4b41c638d9ae8f3c5100d9f60f1d47743bcb99d..4736420d31260a9e25ad69b23f2e90a793c3535b 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.cpp
+++ b/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.cpp
@@ -7,8 +7,8 @@ namespace armarx::trajectory
template <>
VariantValue interpolate::linear<VariantValue>(float t, const VariantValue& lhs, const VariantValue& rhs)
{
- // dont use boost::get
- return boost::apply_visitor(Linear {t}, lhs, rhs);
+ // dont use std::get
+ return std::visit(Linear {t}, lhs, rhs);
}
}
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.h b/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.h
index 4cf381844ec819550b1a3bc857b28ee713705a5c..20fc87cc935a751fa2ce9a344f114276c8c1685e 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.h
+++ b/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.h
@@ -10,7 +10,7 @@ namespace armarx::trajectory::interpolate
/**
* @brief Linear interpolation visitor: Interpolates between the given values linearly.
*/
- class Linear : public boost::static_visitor<>
+ class Linear
{
public:
@@ -25,22 +25,22 @@ namespace armarx::trajectory::interpolate
Linear(float t) : t(t) {}
template <typename U, typename V>
- float operator()(const U&, const V&) const
+ VariantValue operator()(const U&, const V&) const
{
throw error::InterpolateDifferentTypesError();
}
- float operator()(const float& lhs, const float& rhs) const
+ VariantValue operator()(const float& lhs, const float& rhs) const
{
return (1 - t) * lhs + t * rhs;
}
- Eigen::MatrixXf operator()(const Eigen::MatrixXf& lhs, const Eigen::MatrixXf& rhs) const
+ VariantValue operator()(const Eigen::MatrixXf& lhs, const Eigen::MatrixXf& rhs) const
{
return (1 - t) * lhs + t * rhs;
}
- Eigen::Quaternionf operator()(const Eigen::Quaternionf& lhs, const Eigen::Quaternionf& rhs) const
+ VariantValue operator()(const Eigen::Quaternionf& lhs, const Eigen::Quaternionf& rhs) const
{
return lhs.slerp(t, rhs);
}
@@ -55,7 +55,8 @@ namespace armarx::trajectory::interpolate
template <typename ReturnT>
ReturnT linear(float t, const VariantValue& lhs, const VariantValue& rhs)
{
- return boost::get<ReturnT>(boost::apply_visitor(Linear {t}, lhs, rhs));
+ VariantValue result = std::visit(Linear {t}, lhs, rhs);
+ return std::get<ReturnT>(result);
}
template <>
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/test/TrajectoryTest.cpp b/source/RobotAPI/libraries/SimpleTrajectory/test/TrajectoryTest.cpp
index 6f0553c521f68f8a583736d7b12ac59632cffb46..73afd8c5616497db533115bacbbe6a90ef92f1cf 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/test/TrajectoryTest.cpp
+++ b/source/RobotAPI/libraries/SimpleTrajectory/test/TrajectoryTest.cpp
@@ -109,7 +109,7 @@ BOOST_AUTO_TEST_CASE(testDifferentTypes)
{
for (std::size_t j = 0; j < i; ++j)
{
- BOOST_CHECK_NE(values[i].type(), values[j].type());
+ BOOST_CHECK_NE(values[i].index(), values[j].index());
}
}
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index ef5f227686bf252f3f7941fd3f0d96aed72442d1..c524ecd93da7721ee95cc111beb518cafd848de5 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -17,6 +17,9 @@ set(LIBS
ArmarXCoreInterfaces ArmarXCore
RemoteGui
aron
+
+ # Needed for LTM
+ RobotAPI::aron::converter::json
${LIBMONGOCXX_LIBRARIES}
${LIBBSONCXX_LIBRARIES}
)
@@ -25,65 +28,47 @@ 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/lookup_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/longtermmemory/CoreSegment.cpp
- core/longtermmemory/Entity.cpp
- core/longtermmemory/EntityInstance.cpp
- core/longtermmemory/EntitySnapshot.cpp
- core/longtermmemory/Memory.cpp
- core/longtermmemory/ProviderSegment.cpp
- core/longtermmemory/mongodb/MongoDBConnectionManager.cpp
-
- core/diskmemory/TypeIO.cpp
- core/diskmemory/CoreSegment.cpp
- core/diskmemory/Entity.cpp
- core/diskmemory/EntityInstance.cpp
- core/diskmemory/EntitySnapshot.cpp
- core/diskmemory/Memory.cpp
- core/diskmemory/ProviderSegment.cpp
+ # core/base/MemoryBase.cpp
+ # core/base/ProviderSegmentBase.cpp
+ core/base/ice_conversions.cpp
+
+ core/wm/memory_definitions.cpp
+ core/wm/aron_conversions.cpp
+ core/wm/ice_conversions.cpp
+ core/wm/detail/data_lookup_mixins.cpp
+ core/wm/visitor/Visitor.cpp
+ core/wm/visitor/FunctionalVisitor.cpp
core/error/ArMemError.cpp
core/error/mns.cpp
- client/ComponentPlugin.cpp
+
client/MemoryNameSystem.cpp
- client/MemoryNameSystemComponentPlugin.cpp
client/Reader.cpp
- client/ReaderComponentPlugin.cpp
client/Writer.cpp
- client/WriterComponentPlugin.cpp
+
+ client/plugins/ListeningPluginUser.cpp
+ client/plugins/ListeningPlugin.cpp
+ client/plugins/PluginUser.cpp
+ client/plugins/Plugin.cpp
client/util/MemoryListener.cpp
client/util/SimpleReaderBase.cpp
@@ -93,11 +78,24 @@ set(LIB_FILES
client/query/Builder.cpp
client/query/selectors.cpp
+
server/MemoryToIceAdapter.cpp
- server/ComponentPlugin.cpp
server/MemoryRemoteGui.cpp
server/RemoteGuiAronDataVisitor.cpp
+ server/ltm/LongtermMemoryBase.cpp
+ server/ltm/disk/MemoryManager.cpp
+ server/ltm/mongodb/MemoryManager.cpp
+ server/ltm/mongodb/ConnectionManager.cpp
+
+ server/wm/memory_definitions.cpp
+ server/wm/ice_conversions.cpp
+ server/wm/detail/MaxHistorySize.cpp
+
+ server/plugins/Plugin.cpp
+ server/plugins/ReadOnlyPluginUser.cpp
+ server/plugins/ReadWritePluginUser.cpp
+
server/segment/Segment.cpp
server/segment/SpecializedSegment.cpp
@@ -107,41 +105,32 @@ 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/ltm.cpp
+ server/query_proc/wm.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
mns/MemoryNameSystem.cpp
- mns/ComponentPlugin.cpp
+ mns/Registry.cpp
+ mns/plugins/Plugin.cpp
+ mns/plugins/PluginUser.cpp
util/util.cpp
)
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/json_conversions.h
core/ice_conversions_boost_templates.h
core/ice_conversions_templates.h
@@ -150,10 +139,12 @@ 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/derived.h
+ core/base/detail/iteration_mixins.h
+ core/base/detail/lookup_mixins.h
+ core/base/detail/negative_index_semantics.h
core/base/CoreSegmentBase.h
core/base/EntityBase.h
@@ -161,48 +152,28 @@ 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/longtermmemory/CoreSegment.h
- core/longtermmemory/Entity.h
- core/longtermmemory/EntityInstance.h
- core/longtermmemory/EntitySnapshot.h
- core/longtermmemory/Memory.h
- core/longtermmemory/ProviderSegment.h
- core/longtermmemory/mongodb/MongoDBConnectionManager.h
-
- core/diskmemory/TypeIO.h
- core/diskmemory/CoreSegment.h
- core/diskmemory/Entity.h
- core/diskmemory/EntityInstance.h
- core/diskmemory/EntitySnapshot.h
- core/diskmemory/Memory.h
- core/diskmemory/ProviderSegment.h
+ core/wm.h
+ core/wm/memory_definitions.h
+ core/wm/aron_conversions.h
+ core/wm/ice_conversions.h
+ core/wm/detail/data_lookup_mixins.h
+ core/wm/visitor/Visitor.h
+ core/wm/visitor/FunctionalVisitor.h
- core/ice_conversions_templates.h
- core/ice_conversions.h
client.h
- client/ComponentPlugin.h
+ client/forward_declarations.h
client/MemoryNameSystem.h
- client/MemoryNameSystemComponentPlugin.h
client/Reader.h
- client/ReaderComponentPlugin.h
client/Writer.h
- client/WriterComponentPlugin.h
+
+ client/plugins.h
+ client/plugins/ListeningPluginUser.h
+ client/plugins/ListeningPlugin.h
+ client/plugins/PluginUser.h
+ client/plugins/Plugin.h
client/query.h
client/Query.h
@@ -216,46 +187,51 @@ set(LIB_HEADERS
client/util/SimpleReaderBase.h
client/util/SimpleWriterBase.h
+
server.h
- server/ComponentPlugin.h
+ server/forward_declarations.h
+
server/MemoryToIceAdapter.h
server/MemoryRemoteGui.h
server/RemoteGuiAronDataVisitor.h
+ server/ltm/LongtermMemoryBase.h
+ server/ltm/disk/MemoryManager.h
+ server/ltm/mongodb/MemoryManager.h
+ server/ltm/mongodb/ConnectionManager.h
+
+ server/wm/memory_definitions.h
+ server/wm/ice_conversions.h
+ server/wm/detail/MaxHistorySize.h
+
+ server/plugins.h
+ server/plugins/Plugin.h
+ server/plugins/ReadOnlyPluginUser.h
+ server/plugins/ReadWritePluginUser.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/ltm.h
+ server/query_proc/wm.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
mns.h
mns/MemoryNameSystem.h
- mns/ComponentPlugin.h
+ mns/Registry.h
+ mns/plugins/Plugin.h
+ mns/plugins/PluginUser.h
util/util.h
-
)
armarx_add_library(
diff --git a/source/RobotAPI/libraries/armem/client.h b/source/RobotAPI/libraries/armem/client.h
index 834d62ab45d88d18e4277929c47acd55c5825263..fa61feb250a44f58dbc3c0d8b00a0c7ad67d8d31 100644
--- a/source/RobotAPI/libraries/armem/client.h
+++ b/source/RobotAPI/libraries/armem/client.h
@@ -1,15 +1,12 @@
#pragma once
-#include "client/ComponentPlugin.h"
#include "client/MemoryNameSystem.h"
-#include "client/MemoryNameSystemComponentPlugin.h"
+#include "client/plugins.h"
#include "client/Query.h"
#include "client/query/Builder.h"
#include "client/query/query_fns.h"
#include "client/Reader.h"
-#include "client/ReaderComponentPlugin.h"
#include "client/Writer.h"
-#include "client/WriterComponentPlugin.h"
namespace armarx::armem
@@ -23,8 +20,4 @@ namespace armarx::armem
using ClientQueryBuilder = client::query::Builder;
namespace client_query_fns = client::query_fns;
- using ClientComponentPluginUser = client::ComponentPluginUser;
- using ClientReaderComponentPluginUser = client::ReaderComponentPluginUser;
- using ClientWriterComponentPluginUser = client::WriterComponentPluginUser;
-
}
diff --git a/source/RobotAPI/libraries/armem/client/ComponentPlugin.cpp b/source/RobotAPI/libraries/armem/client/ComponentPlugin.cpp
deleted file mode 100644
index a88f374d6bddede5fd0b05eb28481d524f5f6b0a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/client/ComponentPlugin.cpp
+++ /dev/null
@@ -1,41 +0,0 @@
-#include <RobotAPI/libraries/armem/client/ComponentPlugin.h>
-
-
-// ArmarX
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-// RobotAPI
-#include <RobotAPI/libraries/armem/core/error.h>
-
-
-armarx::armem::client::ComponentPluginUser::ComponentPluginUser()
-{
- // pass
-}
-
-
-armarx::armem::client::ComponentPluginUser::~ComponentPluginUser()
-{
- // pass
-}
-
-
-void
-armarx::armem::client::ComponentPluginUser::setMemoryServer(server::MemoryInterfacePrx memory)
-{
- setReadingMemoryServer(memory);
- setWritingMemoryServer(memory);
-}
-
-
-armarx::armem::data::WaitForMemoryResult
-armarx::armem::client::ComponentPluginUser::useMemoryServer(const std::string& memoryName)
-{
- armem::data::WaitForMemoryResult result;
- result.proxy = memoryNameSystem.useServer(MemoryID().withMemoryName(memoryName));
- if (result.proxy)
- {
- setMemoryServer(result.proxy);
- }
- return result;
-}
diff --git a/source/RobotAPI/libraries/armem/client/ComponentPlugin.h b/source/RobotAPI/libraries/armem/client/ComponentPlugin.h
deleted file mode 100644
index ae3f26b26557c11046d84cde8a07483e4ba9ad49..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/client/ComponentPlugin.h
+++ /dev/null
@@ -1,47 +0,0 @@
-#pragma once
-
-
-// ArmarX
-#include <ArmarXCore/core/ComponentPlugin.h>
-
-// RobotAPI
-#include <RobotAPI/interface/armem/server/MemoryInterface.h>
-#include <RobotAPI/libraries/armem/client/ReaderComponentPlugin.h>
-#include <RobotAPI/libraries/armem/client/WriterComponentPlugin.h>
-
-
-
-namespace armarx::armem::client
-{
-
- /**
- * @brief Utility for connecting a Client via Ice to ArMem.
- */
- class ComponentPluginUser :
- virtual public ReaderComponentPluginUser,
- virtual public WriterComponentPluginUser
- {
-
- public:
-
- ComponentPluginUser();
- ~ComponentPluginUser() override;
-
-
- /**
- * @brief Resolve the given memory name, add it as dependency and update the readers and writers.
- * @param The memory's name.
- * @return The result of `waitForMemory()`.
- * @see `armem::MemoryNameSystemComponentPluginUser::useMemory()`
- */
- virtual armem::data::WaitForMemoryResult useMemoryServer(const std::string& memoryName) override;
- using MemoryNameSystemComponentPluginUser::useMemoryServer;
-
-
- protected:
-
- void setMemoryServer(server::MemoryInterfacePrx memory);
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
index 4395b55fd45e6bdb019ee1285fbc5cc3dac648f9..121b2924802c1a8c7d6ab411fd1c88755ee56621 100644
--- a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
@@ -9,6 +9,8 @@
#include <SimoxUtility/algorithm/string/string_tools.h>
+
+
namespace armarx::armem::client
{
@@ -27,10 +29,10 @@ namespace armarx::armem::client
{
ARMARX_CHECK_NOT_NULL(mns);
- data::GetAllRegisteredMemoriesResult result;
+ mns::dto::GetAllRegisteredServersResult result;
try
{
- result = mns->getAllRegisteredMemories();
+ result = mns->getAllRegisteredServers();
}
catch (const Ice::NotRegisteredException& e)
{
@@ -39,7 +41,40 @@ namespace armarx::armem::client
if (result.success)
{
- this->servers = result.proxies;
+ for (const auto& [name, server] : result.servers)
+ {
+ auto [it, inserted] = servers.try_emplace(name, server);
+ if (inserted)
+ {
+ // OK
+ }
+ else
+ {
+ // Compare ice identities, update if it changed.
+ auto foo = [](auto & oldProxy, const auto & newProxy)
+ {
+ if (oldProxy->ice_getIdentity() != newProxy->ice_getIdentity())
+ {
+ // Replace old proxy by new one.
+ oldProxy = newProxy;
+ }
+ };
+ foo(it->second.reading, server.reading);
+ foo(it->second.writing, server.writing);
+ }
+ }
+ // Remove all entries which are not there anymore.
+ for (auto it = servers.begin(); it != servers.end();)
+ {
+ if (result.servers.count(it->first) == 0)
+ {
+ it = servers.erase(it);
+ }
+ else
+ {
+ ++it;
+ }
+ }
}
else
{
@@ -48,7 +83,8 @@ namespace armarx::armem::client
}
- server::MemoryInterfacePrx MemoryNameSystem::resolveServer(const MemoryID& memoryID)
+ mns::dto::MemoryServerInterfaces
+ MemoryNameSystem::resolveServer(const MemoryID& memoryID)
{
if (auto it = servers.find(memoryID.memoryName); it != servers.end())
{
@@ -69,7 +105,7 @@ namespace armarx::armem::client
}
- server::MemoryInterfacePrx MemoryNameSystem::waitForServer(const MemoryID& memoryID, Time timeout)
+ mns::dto::MemoryServerInterfaces MemoryNameSystem::waitForServer(const MemoryID& memoryID)
{
if (auto it = servers.find(memoryID.memoryName); it != servers.end())
{
@@ -77,15 +113,14 @@ namespace armarx::armem::client
}
else
{
- armem::data::WaitForMemoryInput input;
+ mns::dto::WaitForServerInput input;
input.name = memoryID.memoryName;
- input.timeoutMilliSeconds = timeout.toMilliSeconds();
ARMARX_CHECK_NOT_NULL(mns);
- armem::data::WaitForMemoryResult result = mns->waitForMemory(input);
+ mns::dto::WaitForServerResult result = mns->waitForServer(input);
if (result.success)
{
- return result.proxy;
+ return result.server;
}
else
{
@@ -94,7 +129,7 @@ namespace armarx::armem::client
}
}
- server::MemoryInterfacePrx MemoryNameSystem::useServer(const MemoryID& memoryID)
+ mns::dto::MemoryServerInterfaces MemoryNameSystem::useServer(const MemoryID& memoryID)
{
ARMARX_CHECK_NOT_NULL(component)
<< "Owning component not set when using a memory server. \n"
@@ -106,22 +141,22 @@ namespace armarx::armem::client
}
- server::MemoryInterfacePrx MemoryNameSystem::useServer(const MemoryID& memoryID, ManagedIceObject& component)
+ mns::dto::MemoryServerInterfaces MemoryNameSystem::useServer(const MemoryID& memoryID, ManagedIceObject& component)
{
- server::MemoryInterfacePrx server = waitForServer(memoryID);
+ mns::dto::MemoryServerInterfaces server = waitForServer(memoryID);
// Add dependency.
- component.usingProxy(server->ice_getIdentity().name);
+ component.usingProxy(server.reading->ice_getIdentity().name);
return server;
}
- server::MemoryInterfacePrx MemoryNameSystem::useServer(const std::string& memoryName)
+ mns::dto::MemoryServerInterfaces MemoryNameSystem::useServer(const std::string& memoryName)
{
return useServer(MemoryID().withMemoryName(memoryName));
}
- server::MemoryInterfacePrx MemoryNameSystem::useServer(const std::string& memoryName, ManagedIceObject& component)
+ mns::dto::MemoryServerInterfaces MemoryNameSystem::useServer(const std::string& memoryName, ManagedIceObject& component)
{
return useServer(MemoryID().withMemoryName(memoryName), component);
}
@@ -129,19 +164,19 @@ namespace armarx::armem::client
Reader MemoryNameSystem::getReader(const MemoryID& memoryID)
{
- return Reader(resolveServer(memoryID));
+ return Reader(resolveServer(memoryID).reading);
}
Reader MemoryNameSystem::useReader(const MemoryID& memoryID)
{
- return Reader(useServer(memoryID));
+ return Reader(useServer(memoryID).reading);
}
Reader MemoryNameSystem::useReader(const MemoryID& memoryID, ManagedIceObject& component)
{
- return Reader(useServer(memoryID, component));
+ return Reader(useServer(memoryID, component).reading);
}
@@ -157,56 +192,55 @@ namespace armarx::armem::client
}
+
template <class ClientT>
- std::map<std::string, ClientT> MemoryNameSystem::_getAllClients() const
+ std::map<std::string, ClientT>
+ MemoryNameSystem::_getAllClients(auto&& getProxyFn) const
{
std::map<std::string, ClientT> result;
for (const auto& [name, server] : servers)
{
- result[name] = ClientT(server);
+ if (auto proxy = getProxyFn(server))
+ {
+ result[name] = ClientT(proxy);
+ }
}
return result;
}
- template <class ClientT>
- std::map<std::string, ClientT> MemoryNameSystem::_getAllClients(bool update)
+
+ std::map<std::string, Reader> MemoryNameSystem::getAllReaders(bool update)
{
if (update)
{
this->update();
}
- return const_cast<const MemoryNameSystem&>(*this)._getAllClients<ClientT>();
- }
-
-
- std::map<std::string, Reader> MemoryNameSystem::getAllReaders(bool update)
- {
- return _getAllClients<Reader>(update);
+ return _getAllClients<Reader>(&mns::getReadingInterface);
}
std::map<std::string, Reader> MemoryNameSystem::getAllReaders() const
{
- return _getAllClients<Reader>();
+ return _getAllClients<Reader>(&mns::getReadingInterface);
}
Writer MemoryNameSystem::getWriter(const MemoryID& memoryID)
{
- return Writer(resolveServer(memoryID));
+ return Writer(resolveServer(memoryID).writing);
}
Writer MemoryNameSystem::useWriter(const MemoryID& memoryID)
{
- return Writer(useServer(memoryID));
+ return Writer(useServer(memoryID).writing);
}
Writer MemoryNameSystem::useWriter(const MemoryID& memoryID, ManagedIceObject& component)
{
- return Writer(useServer(memoryID, component));
+ return Writer(useServer(memoryID, component).writing);
}
@@ -224,13 +258,17 @@ namespace armarx::armem::client
std::map<std::string, Writer> MemoryNameSystem::getAllWriters(bool update)
{
- return _getAllClients<Writer>(update);
+ if (update)
+ {
+ this->update();
+ }
+ return _getAllClients<Writer>(&mns::getWritingInterface);
}
std::map<std::string, Writer> MemoryNameSystem::getAllWriters() const
{
- return _getAllClients<Writer>();
+ return _getAllClients<Writer>(&mns::getWritingInterface);
}
@@ -272,11 +310,11 @@ namespace armarx::armem::client
{
if (id.hasInstanceIndex())
{
- result[id] = queryResult.memory.getEntityInstance(id);
+ result[id] = queryResult.memory.getInstance(id);
}
else if (id.hasTimestamp())
{
- result[id] = queryResult.memory.getEntitySnapshot(id).getInstance(0);
+ result[id] = queryResult.memory.getSnapshot(id).getInstance(0);
}
else if (id.hasEntityName())
{
@@ -316,15 +354,15 @@ namespace armarx::armem::client
}
- void MemoryNameSystem::registerServer(const MemoryID& memoryID, server::MemoryInterfacePrx proxy)
+ void MemoryNameSystem::registerServer(const MemoryID& memoryID, mns::dto::MemoryServerInterfaces server)
{
- data::RegisterMemoryInput input;
+ mns::dto::RegisterServerInput input;
input.name = memoryID.memoryName;
- input.proxy = proxy;
- ARMARX_CHECK_NOT_NULL(input.proxy);
+ input.server = server;
+ ARMARX_CHECK(server.reading or server.writing) << VAROUT(server.reading) << " | " << VAROUT(server.writing);
ARMARX_CHECK_NOT_NULL(mns);
- data::RegisterMemoryResult result = mns->registerMemory(input);
+ mns::dto::RegisterServerResult result = mns->registerServer(input);
if (!result.success)
{
throw error::ServerRegistrationOrRemovalFailed("register", memoryID, result.errorMessage);
@@ -334,11 +372,11 @@ namespace armarx::armem::client
void MemoryNameSystem::removeServer(const MemoryID& memoryID)
{
- data::RemoveMemoryInput input;
+ mns::dto::RemoveServerInput input;
input.name = memoryID.memoryName;
ARMARX_CHECK_NOT_NULL(mns);
- data::RemoveMemoryResult result = mns->removeMemory(input);
+ mns::dto::RemoveServerResult result = mns->removeServer(input);
if (!result.success)
{
throw error::ServerRegistrationOrRemovalFailed("remove", memoryID, result.errorMessage);
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
index d981bab0383f0d04fdc221fbbd2cbd062d061c53..7779ca2d8b1a65b07ef446675685695dc243de58 100644
--- a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
@@ -30,6 +30,7 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/client/util/MemoryListener.h>
+#include <RobotAPI/libraries/armem/mns/Registry.h>
namespace armarx
@@ -97,41 +98,6 @@ namespace armarx::armem::client
*/
void update();
- /**
- * @brief Resolve the given memory server for the given memory ID.
- *
- * @param memoryID The memory ID.
- * @return The memory server proxy.
- *
- * @throw `error::CouldNotResolveMemoryServer` If the memory name could not be resolved.
- */
- server::MemoryInterfacePrx resolveServer(const MemoryID& memoryID);
-
- /**
- * @brief Wait for the given memory server.
- *
- * @param memoryID The memory ID.
- * @param timeout How long to wait at maximum. Negative values indicate infinite wait.
- * @return The memory server proxy.
- *
- * @throw `error::CouldNotResolveMemoryServer` If the memory name could not be resolved.
- */
- server::MemoryInterfacePrx waitForServer(const MemoryID& memoryID, Time timeout = Time::milliSeconds(-1));
-
- /**
- * @brief Wait for the given memory server and add a dependency to the memory server.
- *
- * @param memoryID The memory ID.
- * @param component The component that should depend on the memory server.
- * @return The memory server proxy.
- *
- * @throw `error::CouldNotResolveMemoryServer` If the memory name could not be resolved.
- */
- server::MemoryInterfacePrx useServer(const MemoryID& memoryID);
- server::MemoryInterfacePrx useServer(const MemoryID& memoryID, ManagedIceObject& component);
- server::MemoryInterfacePrx useServer(const std::string& memoryName);
- server::MemoryInterfacePrx useServer(const std::string& memoryName, ManagedIceObject& component);
-
// Reader/Writer construction
@@ -225,7 +191,7 @@ namespace armarx::armem::client
*
* @throw `error::ServerRegistrationOrRemovalFailed` If the registration failed.
*/
- void registerServer(const MemoryID& memoryID, server::MemoryInterfacePrx server);
+ void registerServer(const MemoryID& memoryID, mns::dto::MemoryServerInterfaces server);
/**
* @brief Remove a memory server from the MNS.
@@ -247,12 +213,47 @@ namespace armarx::armem::client
}
+ private:
+
+ /**
+ * @brief Resolve the given memory server for the given memory ID.
+ *
+ * @param memoryID The memory ID.
+ * @return The memory server proxy.
+ *
+ * @throw `error::CouldNotResolveMemoryServer` If the memory name could not be resolved.
+ */
+ mns::dto::MemoryServerInterfaces resolveServer(const MemoryID& memoryID);
+
+ /**
+ * @brief Wait for the given memory server.
+ *
+ * @param memoryID The memory ID.
+ * @return The memory server proxy.
+ *
+ * @throw `error::CouldNotResolveMemoryServer` If the memory name could not be resolved.
+ */
+ mns::dto::MemoryServerInterfaces waitForServer(const MemoryID& memoryID);
+
+ /**
+ * @brief Wait for the given memory server and add a dependency to the memory server.
+ *
+ * @param memoryID The memory ID.
+ * @param component The component that should depend on the memory server.
+ * @return The memory server proxy.
+ *
+ * @throw `error::CouldNotResolveMemoryServer` If the memory name could not be resolved.
+ */
+ mns::dto::MemoryServerInterfaces useServer(const MemoryID& memoryID);
+ mns::dto::MemoryServerInterfaces useServer(const MemoryID& memoryID, ManagedIceObject& component);
+ mns::dto::MemoryServerInterfaces useServer(const std::string& memoryName);
+ mns::dto::MemoryServerInterfaces useServer(const std::string& memoryName, ManagedIceObject& component);
+
+
private:
template <class ClientT>
- std::map<std::string, ClientT> _getAllClients(bool update);
- template <class ClientT>
- std::map<std::string, ClientT> _getAllClients() const;
+ std::map<std::string, ClientT> _getAllClients(auto&& proxyFn) const;
/// The MNS proxy.
@@ -262,7 +263,7 @@ namespace armarx::armem::client
ManagedIceObject* component = nullptr;
/// The registered memory servers.
- std::map<std::string, server::MemoryInterfacePrx> servers;
+ std::map<std::string, mns::dto::MemoryServerInterfaces> servers;
};
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.cpp b/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.cpp
deleted file mode 100644
index 60fae2cf9b37e450fcfbaee806af8115f496c678..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.cpp
+++ /dev/null
@@ -1,154 +0,0 @@
-#include "MemoryNameSystemComponentPlugin.h"
-
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
-
-
-
-namespace armarx::armem::client::plugins
-{
-
- MemoryNameSystemComponentPlugin::~MemoryNameSystemComponentPlugin()
- {}
-
-
- void MemoryNameSystemComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
- {
- if (!properties->hasDefinition(makePropertyName(PROPERTY_MNS_NAME_NAME)))
- {
- properties->defineOptionalProperty<std::string>(
- makePropertyName(PROPERTY_MNS_NAME_NAME),
- parent<MemoryNameSystemComponentPluginUser>().memoryNameSystemName,
- "Name of the Memory Name System (MNS) component.");
- }
- if (!properties->hasDefinition(makePropertyName(PROPERTY_MNS_ENABLED_NAME)))
- {
- properties->defineOptionalProperty<bool>(
- makePropertyName(PROPERTY_MNS_ENABLED_NAME),
- parent<MemoryNameSystemComponentPluginUser>().memoryNameSystemEnabled,
- "Whether to use (and depend on) the Memory Name System (MNS)."
- "\nSet to false to use this memory as a stand-alone.");
- }
-
- // Subscribe topics by single servers, use this as a prefix.
- properties->topic<MemoryListenerInterface>("MemoryUpdates");
- }
-
- void MemoryNameSystemComponentPlugin::preOnInitComponent()
- {
- if (isMemoryNameSystemEnabled())
- {
- parent().usingProxy(getMemoryNameSystemName());
- }
- }
-
- void MemoryNameSystemComponentPlugin::preOnConnectComponent()
- {
- if (isMemoryNameSystemEnabled())
- {
- ARMARX_DEBUG << "Creating MemoryNameSystem client with owning component '" << parent().getName() << "'.";
- parent<MemoryNameSystemComponentPluginUser>().memoryNameSystem =
- MemoryNameSystem(getMemoryNameSystem(), &parent());
- }
- }
-
- bool MemoryNameSystemComponentPlugin::isMemoryNameSystemEnabled()
- {
- return parentDerives<Component>() ?
- parent<Component>().getProperty<bool>(makePropertyName(PROPERTY_MNS_ENABLED_NAME)) :
- parent<MemoryNameSystemComponentPluginUser>().memoryNameSystemEnabled;
- }
-
- std::string MemoryNameSystemComponentPlugin::getMemoryNameSystemName()
- {
- return parentDerives<Component>() ?
- parent<Component>().getProperty<std::string>(makePropertyName(PROPERTY_MNS_NAME_NAME)) :
- std::string{parent<MemoryNameSystemComponentPluginUser>().memoryNameSystemName};
- }
-
- mns::MemoryNameSystemInterfacePrx MemoryNameSystemComponentPlugin::getMemoryNameSystem()
- {
- return isMemoryNameSystemEnabled() && parentDerives<Component>()
- ? parent<Component>().getProxy<mns::MemoryNameSystemInterfacePrx>(getMemoryNameSystemName())
- : nullptr;
- }
-
-}
-
-namespace armarx::armem::client
-{
-
- MemoryNameSystemComponentPluginUser::MemoryNameSystemComponentPluginUser()
- {
- addPlugin(plugin);
- }
-
- MemoryNameSystemComponentPluginUser::~MemoryNameSystemComponentPluginUser()
- {
- }
-
- void MemoryNameSystemComponentPluginUser::memoryUpdated(
- const std::vector<data::MemoryID>& updatedSnapshotIDs, const Ice::Current&)
- {
- memoryNameSystem.updated(updatedSnapshotIDs);
- }
-
- armem::data::WaitForMemoryResult MemoryNameSystemComponentPluginUser::useMemoryServer(const MemoryID& id)
- {
- armem::data::WaitForMemoryResult result;
- try
- {
- // Add dependency.
- result.proxy = memoryNameSystem.useServer(id);
- result.success = true;
- }
- catch (const armem::error::CouldNotResolveMemoryServer& e)
- {
- result.success = false;
- result.errorMessage = e.what();
- }
- return result;
- }
-
- armem::data::WaitForMemoryResult MemoryNameSystemComponentPluginUser::useMemoryServer(const std::string& memoryName)
- {
- return useMemoryServer(MemoryID().withMemoryName(memoryName));
- }
-
- armem::data::WaitForMemoryResult MemoryNameSystemComponentPluginUser::waitForMemoryServer(const std::string& memoryName)
- {
- armem::data::WaitForMemoryResult result;
- try
- {
- result.proxy = memoryNameSystem.waitForServer(MemoryID().withMemoryName(memoryName));
- result.success = true;
- }
- catch (const armem::error::CouldNotResolveMemoryServer& e)
- {
- result.success = false;
- result.errorMessage = e.what();
- }
- return result;
- }
-
- armem::data::ResolveMemoryNameResult MemoryNameSystemComponentPluginUser::resolveMemoryServer(const std::string& memoryName)
- {
- armem::data::ResolveMemoryNameResult result;
- try
- {
- result.proxy = memoryNameSystem.resolveServer(MemoryID().withMemoryName(memoryName));
- result.success = true;
- }
- catch (const armem::error::CouldNotResolveMemoryServer& e)
- {
- result.success = false;
- result.errorMessage = e.what();
- }
- return result;
- }
-
-}
-
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h b/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h
deleted file mode 100644
index a6a2434a68a033d3e540729c08e83c90babda847..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h
+++ /dev/null
@@ -1,101 +0,0 @@
-#pragma once
-
-#include <ArmarXCore/core/ComponentPlugin.h>
-
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
-#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
-#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-
-
-namespace armarx::armem::mns
-{
- class MemoryNameSystemComponentPluginUser;
-}
-
-
-namespace armarx::armem::client::plugins
-{
- /**
- * @brief A base plugin offering optional access and dependency
- * to the Memory Name System (MNS).
- */
- class MemoryNameSystemComponentPlugin : public ComponentPlugin
- {
- public:
-
- using ComponentPlugin::ComponentPlugin;
- virtual ~MemoryNameSystemComponentPlugin() override;
-
- void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
-
- void preOnInitComponent() override;
- void preOnConnectComponent() override;
-
- /**
- * @brief Indicate whether the Memory Name System (MNS) is enabled.
- */
- bool isMemoryNameSystemEnabled();
- /**
- * @brief Get the name of the MNS component.
- */
- std::string getMemoryNameSystemName();
-
- /**
- * @brief Get the MNS proxy.
- * @return The MNS proxy when MNS is enabled, nullptr when MNS is disabled.
- */
- mns::MemoryNameSystemInterfacePrx getMemoryNameSystem();
-
- public:
- static constexpr const char* PROPERTY_MNS_ENABLED_NAME = "mns.MemoryNameSystemEnabled";
- static constexpr const char* PROPERTY_MNS_NAME_NAME = "mns.MemoryNameSystemName";
- };
-
-}
-
-
-#include <ArmarXCore/core/ManagedIceObject.h>
-
-namespace armarx::armem::client
-{
-
- class MemoryNameSystemComponentPluginUser :
- virtual public ManagedIceObject,
- virtual public MemoryListenerInterface
- {
- protected:
-
- MemoryNameSystemComponentPluginUser();
- virtual ~MemoryNameSystemComponentPluginUser() override;
-
-
- virtual void memoryUpdated(const std::vector<data::MemoryID>& updatedSnapshotIDs, const Ice::Current& current) override;
-
-
- [[deprecated("Use memoryNameSystem member instead of function inherited by MemoryNameSystemComponentPluginUser.")]]
- armem::data::WaitForMemoryResult waitForMemoryServer(const std::string& memoryName);
- [[deprecated("Use memoryNameSystem member instead of function inherited by MemoryNameSystemComponentPluginUser.")]]
- armem::data::ResolveMemoryNameResult resolveMemoryServer(const std::string& memoryName);
-
- [[deprecated("Use memoryNameSystem member instead of function inherited by MemoryNameSystemComponentPluginUser.")]]
- armem::data::WaitForMemoryResult useMemoryServer(const MemoryID& id);
- [[deprecated("Use memoryNameSystem member instead of function inherited by MemoryNameSystemComponentPluginUser.")]]
- virtual armem::data::WaitForMemoryResult useMemoryServer(const std::string& memoryName);
-
-
- public:
-
- /// Only valid when enabled.
- MemoryNameSystem memoryNameSystem;
-
- bool memoryNameSystemEnabled = true;
- std::string memoryNameSystemName = "MemoryNameSystem";
-
-
- private:
-
- plugins::MemoryNameSystemComponentPlugin* plugin = nullptr;
-
- };
-}
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 c6bb445bfe8ee9a3547ae621aa8c16ec8e0e2be9..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
diff --git a/source/RobotAPI/libraries/armem/client/Reader.cpp b/source/RobotAPI/libraries/armem/client/Reader.cpp
index a589512f13bcdaaa2536f597a54474eb72f96574..2662f86e0d7f36827f9d5cff67f963ac29613de9 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem/client/Reader.cpp
@@ -5,10 +5,8 @@
#include <ArmarXCore/core/logging/Logging.h>
#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"
@@ -117,7 +115,7 @@ namespace armarx::armem::client
{
try
{
- wm::EntitySnapshot& snapshot = result.memory.getEntitySnapshot(snapshotID);
+ wm::EntitySnapshot& snapshot = result.memory.getSnapshot(snapshotID);
return snapshot;
}
catch (const armem::error::ArMemError&)
@@ -162,30 +160,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
deleted file mode 100644
index 3ac9f7438408460c157f7484d313ef0d6a3808a6..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp
+++ /dev/null
@@ -1,60 +0,0 @@
-#include <RobotAPI/libraries/armem/client/ReaderComponentPlugin.h>
-
-
-// ArmarX
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-// RobotAPI
-#include <RobotAPI/libraries/armem/core/error.h>
-
-
-namespace armarx::armem::client::plugins
-{
- ReaderComponentPlugin::ReaderComponentPlugin(ManagedIceObject& parent, std::string pre) :
- ComponentPlugin(parent, pre)
- {
- addPlugin(mnsPlugin, pre);
- addPluginDependency(mnsPlugin);
- }
-
-
- ReaderComponentPlugin::~ReaderComponentPlugin()
- {
- }
-}
-
-
-
-namespace armarx::armem::client
-{
- ReaderComponentPluginUser::ReaderComponentPluginUser()
- {
- addPlugin(plugin);
- }
-
-
- ReaderComponentPluginUser::~ReaderComponentPluginUser()
- {
- }
-
-
- void
- ReaderComponentPluginUser::setReadingMemoryServer(server::ReadingMemoryInterfacePrx memory)
- {
- memoryReader.setReadingMemory(memory);
- }
-
-
- armem::data::WaitForMemoryResult ReaderComponentPluginUser::useMemoryServer(const std::string& 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
deleted file mode 100644
index c0139e609e830e2c07517837f6cc3d48aff434db..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
+++ /dev/null
@@ -1,71 +0,0 @@
-#pragma once
-
-
-// STD/STL
-#include <vector>
-
-// ArmarX
-#include <ArmarXCore/core/ComponentPlugin.h>
-
-// RobotAPI
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
-#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
-
-#include <RobotAPI/libraries/armem/client/Reader.h>
-#include <RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h>
-
-
-namespace armarx::armem::client::plugins
-{
-
- class ReaderComponentPlugin : public ComponentPlugin
- {
-
- public:
-
- ReaderComponentPlugin(ManagedIceObject& parent, std::string pre);
- ~ReaderComponentPlugin() override;
-
-
- private:
-
- plugins::MemoryNameSystemComponentPlugin* mnsPlugin = nullptr;
-
- };
-
-}
-
-
-namespace armarx::armem::client
-{
-
- class ReaderComponentPluginUser :
- virtual public ManagedIceObject,
- virtual public MemoryNameSystemComponentPluginUser
- {
-
- public:
-
- ReaderComponentPluginUser();
- ~ReaderComponentPluginUser() override;
-
- [[deprecated("Use memoryNameSystem member instead of function inherited by ReaderComponentPluginUser.")]]
- virtual armem::data::WaitForMemoryResult useMemoryServer(const std::string& memoryName) override;
- using MemoryNameSystemComponentPluginUser::useMemoryServer;
-
-
- protected:
-
- void setReadingMemoryServer(server::ReadingMemoryInterfacePrx memory);
-
- Reader memoryReader;
-
-
- private:
-
- plugins::ReaderComponentPlugin* plugin = nullptr;
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/client/Writer.cpp b/source/RobotAPI/libraries/armem/client/Writer.cpp
index 2021f3a189367dc9134d60ce1dd597cb8513dfaf..d9f64e4ca4ea068a9beb01807aa1c8fb4f7075be 100644
--- a/source/RobotAPI/libraries/armem/client/Writer.cpp
+++ b/source/RobotAPI/libraries/armem/client/Writer.cpp
@@ -12,17 +12,23 @@ namespace armarx::armem::client
{
}
- data::AddSegmentResult Writer::addSegment(const std::string& coreSegmentName, const std::string& providerSegmentName)
+ data::AddSegmentResult Writer::addSegment(const std::string& coreSegmentName, const std::string& providerSegmentName, bool clearWhenExists)
{
data::AddSegmentInput input;
input.coreSegmentName = coreSegmentName;
input.providerSegmentName = providerSegmentName;
+ input.clearWhenExists = clearWhenExists;
return addSegment(input);
}
- data::AddSegmentResult Writer::addSegment(const std::pair<std::string, std::string>& names)
+ data::AddSegmentResult Writer::addSegment(const MemoryID& providerSegmentID, bool clearWhenExists)
{
- return addSegment(names.first, names.second);
+ return addSegment(providerSegmentID.coreSegmentName, providerSegmentID.providerSegmentName, clearWhenExists);
+ }
+
+ data::AddSegmentResult Writer::addSegment(const std::pair<std::string, std::string>& names, bool clearWhenExists)
+ {
+ return addSegment(names.first, names.second, clearWhenExists);
}
data::AddSegmentResult Writer::addSegment(const data::AddSegmentInput& input)
diff --git a/source/RobotAPI/libraries/armem/client/Writer.h b/source/RobotAPI/libraries/armem/client/Writer.h
index dd953fa368d9a46363677d3bc39bc71f8da2a9ce..c63ce8a6fc11ae221caa17dab95dde1c205e1186 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
@@ -32,8 +32,9 @@ namespace armarx::armem::client
Writer(server::WritingMemoryInterfacePrx memory = nullptr);
- data::AddSegmentResult addSegment(const std::string& coreSegmentName, const std::string& providerSegmentName);
- data::AddSegmentResult addSegment(const std::pair<std::string, std::string>& names);
+ data::AddSegmentResult addSegment(const std::string& coreSegmentName, const std::string& providerSegmentName, bool clearWhenExists = false);
+ data::AddSegmentResult addSegment(const MemoryID& providerSegmentID, bool clearWhenExists = false);
+ data::AddSegmentResult addSegment(const std::pair<std::string, std::string>& names, bool clearWhenExists = false);
data::AddSegmentResult addSegment(const data::AddSegmentInput& input);
data::AddSegmentsResult addSegments(const data::AddSegmentsInput& input);
diff --git a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp
deleted file mode 100644
index d4b95ca3565e8552322e7e7cdfac131335ca06f3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp
+++ /dev/null
@@ -1,59 +0,0 @@
-#include <RobotAPI/libraries/armem/client/WriterComponentPlugin.h>
-
-
-// ArmarX
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-// RobotAPI
-#include <RobotAPI/libraries/armem/core/error.h>
-
-
-namespace armarx::armem::client::plugins
-{
- WriterComponentPlugin::WriterComponentPlugin(ManagedIceObject& parent, std::string pre) :
- ComponentPlugin(parent, pre)
- {
- addPlugin(mnsPlugin, pre);
- addPluginDependency(mnsPlugin);
- }
-
- WriterComponentPlugin::~WriterComponentPlugin()
- {
- }
-}
-
-
-namespace armarx::armem::client
-{
- WriterComponentPluginUser::WriterComponentPluginUser()
- {
- addPlugin(plugin);
- }
-
-
- WriterComponentPluginUser::~WriterComponentPluginUser()
- {
- // pass
- }
-
-
- void
- WriterComponentPluginUser::setWritingMemoryServer(server::WritingMemoryInterfacePrx memory)
- {
- memoryWriter.setWritingMemory(memory);
- }
-
-
- armem::data::WaitForMemoryResult WriterComponentPluginUser::useMemoryServer(const std::string& memoryName)
- {
- armem::data::WaitForMemoryResult result;
- result.proxy = memoryNameSystem.useServer(MemoryID().withMemoryName(memoryName));
- if (result.proxy)
- {
- setWritingMemoryServer(result.proxy);
- }
- return result;
- }
-}
-
diff --git a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
deleted file mode 100644
index 3ae9a66bfab912237934d4bea6580a2e32a2c7fb..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
+++ /dev/null
@@ -1,64 +0,0 @@
-#pragma once
-
-
-// ArmarX
-#include <ArmarXCore/core/ComponentPlugin.h>
-
-// RobotAPI
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
-#include <RobotAPI/interface/armem/server/WritingMemoryInterface.h>
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
-
-#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h>
-
-
-namespace armarx::armem::client::plugins
-{
-
- class WriterComponentPlugin : public ComponentPlugin
- {
-
- public:
-
- WriterComponentPlugin(ManagedIceObject& parent, std::string pre);
- ~WriterComponentPlugin() override;
-
- private:
-
- plugins::MemoryNameSystemComponentPlugin* mnsPlugin = nullptr;
-
- };
-}
-
-
-namespace armarx::armem::client
-{
- class WriterComponentPluginUser :
- virtual public ManagedIceObject,
- virtual public MemoryNameSystemComponentPluginUser
- {
-
- public:
-
- WriterComponentPluginUser();
- ~WriterComponentPluginUser() override;
-
- virtual armem::data::WaitForMemoryResult useMemoryServer(const std::string& memoryName) override;
- using MemoryNameSystemComponentPluginUser::useMemoryServer;
-
-
- protected:
-
- void setWritingMemoryServer(server::WritingMemoryInterfacePrx memory);
-
- Writer memoryWriter;
-
-
- private:
-
- plugins::WriterComponentPlugin* plugin = nullptr;
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/client/forward_declarations.h b/source/RobotAPI/libraries/armem/client/forward_declarations.h
new file mode 100644
index 0000000000000000000000000000000000000000..ab6a8fb90cabb42d6838bcc2f57561aeafe01a30
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/forward_declarations.h
@@ -0,0 +1,38 @@
+#pragma once
+
+// #include <RobotAPI/libraries/armem/core/forward_declarations.h>
+
+
+
+namespace armarx::armem::client::query
+{
+ class Builder;
+}
+
+namespace armarx::armem::client
+{
+ class MemoryNameSystem;
+
+ class Reader;
+ class Writer;
+
+ using QueryBuilder = query::Builder;
+ struct QueryInput;
+ struct QueryResult;
+}
+
+namespace armarx::armem::client::plugins
+{
+ class Plugin;
+ class PluginUser;
+ class ListeningPluginUser;
+}
+
+namespace armarx::armem
+{
+ using ClientPlugin = client::plugins::Plugin;
+ using ClientPluginUser = client::plugins::PluginUser;
+ using ListeningClientPluginUser = client::plugins::ListeningPluginUser;
+}
+
+
diff --git a/source/RobotAPI/libraries/armem/client/plugins.h b/source/RobotAPI/libraries/armem/client/plugins.h
new file mode 100644
index 0000000000000000000000000000000000000000..b91ccec3895f6a817c800278b3b3c0318d038750
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/plugins.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/client/forward_declarations.h>
+
+#include <RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h>
+#include <RobotAPI/libraries/armem/client/plugins/Plugin.h>
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+
+
+namespace armarx::armem::client
+{
+ using ComponentPluginUser = plugins::ListeningPluginUser;
+}
diff --git a/source/RobotAPI/libraries/armem/client/plugins/ListeningPlugin.cpp b/source/RobotAPI/libraries/armem/client/plugins/ListeningPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..463c6443e11ddd0ec92d6c5bb4b1001e4e97dfed
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/plugins/ListeningPlugin.cpp
@@ -0,0 +1,21 @@
+#include "ListeningPlugin.h"
+
+#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+
+namespace armarx::armem::client::plugins
+{
+
+ ListeningPlugin::~ListeningPlugin()
+ {}
+
+
+ void ListeningPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ // Subscribe topics by single servers, use this as a prefix.
+ properties->topic<MemoryListenerInterface>("MemoryUpdates");
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/client/plugins/ListeningPlugin.h b/source/RobotAPI/libraries/armem/client/plugins/ListeningPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..80aac20c61d49ffc67963da3b14be0c984ab5a3a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/plugins/ListeningPlugin.h
@@ -0,0 +1,29 @@
+#pragma once
+
+#include <ArmarXCore/core/ComponentPlugin.h>
+
+
+namespace armarx::armem::client::plugins
+{
+
+ /**
+ * @brief Subscribes the memory updates topic.
+ *
+ * When using this plugin, the component needs to implement the
+ * `MemoryListenerInterface`.
+ *
+ * @see MemoryListenerInterface
+ */
+ class ListeningPlugin :
+ public armarx::ComponentPlugin
+ {
+ public:
+
+ using ComponentPlugin::ComponentPlugin;
+ virtual ~ListeningPlugin() override;
+
+ virtual void postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties) override;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.cpp b/source/RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3a40a9ffe27b1116fe5a78f5d1a3088652818b38
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.cpp
@@ -0,0 +1,29 @@
+#include "ListeningPluginUser.h"
+
+#include "ListeningPlugin.h"
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+
+
+namespace armarx::armem::client::plugins
+{
+
+ ListeningPluginUser::ListeningPluginUser()
+ {
+ addPlugin(listeningPlugin);
+ }
+
+
+ ListeningPluginUser::~ListeningPluginUser()
+ {
+ }
+
+
+ void ListeningPluginUser::memoryUpdated(
+ const std::vector<data::MemoryID>& updatedSnapshotIDs, const Ice::Current&)
+ {
+ memoryNameSystem().updated(updatedSnapshotIDs);
+ }
+
+}
+
diff --git a/source/RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h b/source/RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h
new file mode 100644
index 0000000000000000000000000000000000000000..7314b32c048ddfff685ad2095d440cf0f515194c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h
@@ -0,0 +1,51 @@
+#pragma once
+
+#include "PluginUser.h"
+
+#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
+
+#include <vector>
+
+
+namespace armarx::armem::client::plugins
+{
+ class ListeningPlugin;
+
+
+ /**
+ * @brief A memory name system client which listens to the memory updates
+ * topic (`MemoryListenerInterface`).
+ *
+ * Derive from this plugin user class to receive memory update events.
+ */
+ class ListeningPluginUser :
+ virtual public PluginUser,
+ virtual public MemoryListenerInterface
+ {
+ protected:
+
+ ListeningPluginUser();
+ virtual ~ListeningPluginUser() override;
+
+
+ // MemoryListenerInterface
+ virtual void
+ memoryUpdated(const std::vector<data::MemoryID>& updatedSnapshotIDs,
+ const Ice::Current&) override;
+
+
+ private:
+
+ ListeningPlugin* listeningPlugin = nullptr;
+
+ };
+
+}
+namespace armarx::armem::client
+{
+ using ListeningPluginUser = plugins::ListeningPluginUser;
+}
+namespace armarx::armem
+{
+ using ListeningClientPluginUser = client::ListeningPluginUser;
+}
diff --git a/source/RobotAPI/libraries/armem/client/plugins/Plugin.cpp b/source/RobotAPI/libraries/armem/client/plugins/Plugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..74cf798f6f65d31ee1e4bb2a2ba239c264d9f88b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/plugins/Plugin.cpp
@@ -0,0 +1,100 @@
+#include "Plugin.h"
+
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+
+namespace armarx::armem::client::plugins
+{
+
+ Plugin::Plugin(
+ ManagedIceObject& parent, std::string pre) :
+ ComponentPlugin(parent, pre)
+ {
+ }
+
+
+ Plugin::~Plugin()
+ {}
+
+
+ void Plugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (not properties->hasDefinition(makePropertyName(PROPERTY_MNS_NAME_NAME)))
+ {
+ properties->defineOptionalProperty<std::string>(
+ makePropertyName(PROPERTY_MNS_NAME_NAME),
+ memoryNameSystemName,
+ "Name of the Memory Name System (MNS) component.");
+ }
+ if (not properties->hasDefinition(makePropertyName(PROPERTY_MNS_ENABLED_NAME)))
+ {
+ properties->defineOptionalProperty<bool>(
+ makePropertyName(PROPERTY_MNS_ENABLED_NAME),
+ memoryNameSystemEnabled,
+ "Whether to use (and depend on) the Memory Name System (MNS)."
+ "\nSet to false to use this memory as a stand-alone.");
+ }
+ }
+
+
+ void Plugin::preOnInitComponent()
+ {
+ if (isMemoryNameSystemEnabled())
+ {
+ parent().usingProxy(getMemoryNameSystemName());
+ }
+ }
+
+
+ void Plugin::preOnConnectComponent()
+ {
+ if (isMemoryNameSystemEnabled())
+ {
+ ARMARX_DEBUG << "Creating MemoryNameSystem client with owning component '" << parent().getName() << "'.";
+ memoryNameSystem = MemoryNameSystem(getMemoryNameSystemProxy(), &parent());
+ }
+ }
+
+
+ bool
+ Plugin::isMemoryNameSystemEnabled()
+ {
+ return memoryNameSystemEnabled;
+ }
+
+
+ std::string
+ Plugin::getMemoryNameSystemName()
+ {
+ return memoryNameSystemName;
+ }
+
+
+ mns::MemoryNameSystemInterfacePrx
+ Plugin::getMemoryNameSystemProxy()
+ {
+ return isMemoryNameSystemEnabled() and parentDerives<ManagedIceObject>()
+ ? parent<ManagedIceObject>().getProxy<mns::MemoryNameSystemInterfacePrx>(getMemoryNameSystemName())
+ : nullptr;
+ }
+
+
+ MemoryNameSystem&
+ Plugin::getMemoryNameSystemClient()
+ {
+ return memoryNameSystem;
+ }
+
+
+ const MemoryNameSystem&
+ Plugin::getMemoryNameSystemClient() const
+ {
+ return memoryNameSystem;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/client/plugins/Plugin.h b/source/RobotAPI/libraries/armem/client/plugins/Plugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..eff9134fda523d7b91b6cda2fdd4845dd78238cb
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/plugins/Plugin.h
@@ -0,0 +1,79 @@
+#pragma once
+
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+
+#include <ArmarXCore/core/ComponentPlugin.h>
+
+#include <string>
+
+
+namespace armarx::armem::client::plugins
+{
+
+ /**
+ * @brief A component plugin offering client-side access to to the
+ * working memory system by providing a Memory Name System (MNS) client.
+ *
+ * @see MemoryNameSystem
+ */
+ class Plugin :
+ public armarx::ComponentPlugin
+ {
+ public:
+
+ Plugin(ManagedIceObject& parent, std::string pre);
+ virtual ~Plugin() override;
+
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ void preOnInitComponent() override;
+ void preOnConnectComponent() override;
+
+
+ public:
+
+ /**
+ * @brief Get the MNS client.
+ *
+ * Only valid when enabled.
+ */
+ MemoryNameSystem& getMemoryNameSystemClient();
+ const MemoryNameSystem& getMemoryNameSystemClient() const;
+
+
+ /**
+ * @brief Indicate whether the Memory Name System (MNS) is enabled.
+ */
+ bool isMemoryNameSystemEnabled();
+
+ /**
+ * @brief Get the name of the MNS component.
+ */
+ std::string getMemoryNameSystemName();
+
+ /**
+ * @brief Get the MNS proxy.
+ * @return The MNS proxy when MNS is enabled, nullptr when MNS is disabled.
+ */
+ mns::MemoryNameSystemInterfacePrx getMemoryNameSystemProxy();
+
+
+ protected:
+
+ /// The MNS client.
+ MemoryNameSystem memoryNameSystem;
+
+ bool memoryNameSystemEnabled = true;
+ std::string memoryNameSystemName = "MemoryNameSystem";
+
+
+ protected:
+
+ static constexpr const char* PROPERTY_MNS_ENABLED_NAME = "mns.MemoryNameSystemEnabled";
+ static constexpr const char* PROPERTY_MNS_NAME_NAME = "mns.MemoryNameSystemName";
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/client/plugins/PluginUser.cpp b/source/RobotAPI/libraries/armem/client/plugins/PluginUser.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c4129c63d9c51b011bbeb42f412a5372f244a532
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/plugins/PluginUser.cpp
@@ -0,0 +1,33 @@
+#include "PluginUser.h"
+#include "Plugin.h"
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+
+
+
+namespace armarx::armem::client::plugins
+{
+
+ PluginUser::PluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+
+ PluginUser::~PluginUser()
+ {
+ }
+
+
+ MemoryNameSystem& PluginUser::memoryNameSystem()
+ {
+ return plugin->getMemoryNameSystemClient();
+ }
+
+
+ const MemoryNameSystem& PluginUser::memoryNameSystem() const
+ {
+ return plugin->getMemoryNameSystemClient();
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/client/plugins/PluginUser.h b/source/RobotAPI/libraries/armem/client/plugins/PluginUser.h
new file mode 100644
index 0000000000000000000000000000000000000000..0638084dd61bc6563a6a99797a0e7eef96415204
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/plugins/PluginUser.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/client/forward_declarations.h>
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
+
+// Use this one include in your .cpp:
+// #include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+
+
+namespace armarx::armem::client::plugins
+{
+ class Plugin;
+
+
+ /**
+ * @brief Adds the Memory Name System client component plugin.
+ *
+ * This plugin class does not implement the `MemoryListenerInterface`.
+ * Therefore, it will not receive and process memory udpate events via the
+ * memory updates topic. If you want to receive these updates, use the
+ * `ListeningMemoryNameSystemPluginUser`.
+ *
+ *
+ * @see MemoryNameSystemPlugin
+ * @see ListeningMemoryNameSystemPluginUser
+ */
+ class PluginUser :
+ virtual public armarx::ManagedIceObject
+ {
+ protected:
+
+ PluginUser();
+ virtual ~PluginUser() override;
+
+
+ public:
+
+ MemoryNameSystem& memoryNameSystem();
+ const MemoryNameSystem& memoryNameSystem() const;
+
+
+ private:
+
+ plugins::Plugin* plugin = nullptr;
+
+ };
+
+}
+namespace armarx::armem::client
+{
+ using PluginUser = plugins::PluginUser;
+}
+namespace armarx::armem
+{
+ using ClientPluginUser = client::PluginUser;
+}
diff --git a/source/RobotAPI/libraries/armem/client/query/Builder.cpp b/source/RobotAPI/libraries/armem/client/query/Builder.cpp
index 59ae0b7c082bc25a8b1084f8ac49611df107d132..8637055e5b850494f15c10255b29de94785cec08 100644
--- a/source/RobotAPI/libraries/armem/client/query/Builder.cpp
+++ b/source/RobotAPI/libraries/armem/client/query/Builder.cpp
@@ -45,6 +45,25 @@ namespace armarx::armem::client::query
return _addChild(selector);
}
+
+ void Builder::all()
+ {
+ coreSegments().all()
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().all();
+ }
+
+
+ void Builder::allLatest()
+ {
+ coreSegments().all()
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().latest();
+ }
+
+
void Builder::allInCoreSegment(const MemoryID& coreSegmentID)
{
coreSegments().withName(coreSegmentID.coreSegmentName)
diff --git a/source/RobotAPI/libraries/armem/client/query/Builder.h b/source/RobotAPI/libraries/armem/client/query/Builder.h
index dc6147ef03e7de81518e64bddc9bb486f0062b07..3b52aa4b1945d6a2bef62932403013670843f94a 100644
--- a/source/RobotAPI/libraries/armem/client/query/Builder.h
+++ b/source/RobotAPI/libraries/armem/client/query/Builder.h
@@ -42,6 +42,11 @@ namespace armarx::armem::client::query
// Short hands for common queries
+ /// Get all snapshots from all entities in all segments.
+ void all();
+ /// Get all latest snapshots from entities in all segments.
+ void allLatest();
+
/// Get all snapshots from all entities in all provider segments in a core segment.
void allInCoreSegment(const MemoryID& coreSegmentID);
/// Get latest snapshots from all entities in all provider segments in a core segment.
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/client/util/SimpleWriterBase.cpp b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
index ca399bf8f64a2fd347d17c31587d33639cded032..9e27df2c2ef5d7af53529502489085e4f43c231d 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
@@ -24,7 +24,9 @@ namespace armarx::armem::client::util
def->optional(props.memoryName, prefix + "Memory");
def->optional(props.coreSegmentName, prefix + "CoreSegment");
- def->required(props.providerName,
+ // TODO(fabian.reister): this might also be part of the subclass
+ // if the provider name has to be derived from e.g. the component name
+ def->optional(props.providerName,
prefix + "Provider",
"Name of this provider");
}
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..db5f1b630e84ff276901f3321dd2a3fbbccc0442 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
{
@@ -12,7 +14,7 @@ namespace armarx::armem
{
return os << "Entity update: "
<< "\n- success: \t" << rhs.entityID
- << "\n- timestamp: \t" << rhs.timeCreated
+ << "\n- timestamp: \t" << toDateTimeMilliSeconds(rhs.timeCreated)
<< "\n- #instances: \t" << rhs.instancesData.size()
<< "\n"
;
@@ -23,7 +25,7 @@ namespace armarx::armem
return os << "Entity update result: "
<< "\n- success: \t" << (rhs.success ? "true" : "false")
<< "\n- snapshotID: \t" << rhs.snapshotID
- << "\n- time arrived: \t" << rhs.timeArrived.toDateTime()
+ << "\n- time arrived: \t" << toDateTimeMilliSeconds(rhs.timeArrived)
<< "\n- error message: \t" << rhs.errorMessage
<< "\n"
;
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/MemoryID.cpp b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
index f8d7c832401a4ebf335347c67745c1ce82af4e03..f3b8c662ddef099b18dc09e03a81f21bd7de2bde 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.cpp
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
@@ -245,6 +245,31 @@ namespace armarx::armem
return id;
}
+ MemoryID MemoryID::removeLeafItem() const
+ {
+ if (instanceIndex != -1)
+ {
+ return getEntitySnapshotID();
+ }
+ if (timestamp != Time::microSeconds(-1))
+ {
+ return getEntityID();
+ }
+ if (!entityName.empty())
+ {
+ return getProviderSegmentID();
+ }
+ if (!providerSegmentName.empty())
+ {
+ return getCoreSegmentID();
+ }
+ if (!coreSegmentName.empty())
+ {
+ return getMemoryID();
+ }
+ return {}; // return empty if already empty. Client needs to check (Avoids using optional as additional include)
+ }
+
void MemoryID::setMemoryID(const MemoryID& id)
{
memoryName = id.memoryName;
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID.h b/source/RobotAPI/libraries/armem/core/MemoryID.h
index f9d3d5b58ca4670a8abb4333e417a9a2109843ae..d583e752258d69e8271e30be2a2d091493aa18cc 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.h
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.h
@@ -138,7 +138,11 @@ namespace armarx::armem
MemoryID getEntitySnapshotID() const;
MemoryID getEntityInstanceID() const;
- // Slice setters: Set upper part of the ID.
+ // Slice getter: remove the last set name
+ MemoryID removeLeafItem() const;
+
+
+ // Slice setters: Set part of the ID.
void setMemoryID(const MemoryID& id);
void setCoreSegmentID(const MemoryID& id);
diff --git a/source/RobotAPI/libraries/armem/core/aron_conversions.cpp b/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
index 1f8e33c403eaeb3570d6848616d556772170e929..4eafef35a33f746b5c8b898598ac6273cce7030e 100644
--- a/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
@@ -1,5 +1,8 @@
#include "aron_conversions.h"
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
+
void armarx::armem::fromAron(const arondto::MemoryID& dto, MemoryID& bo)
{
diff --git a/source/RobotAPI/libraries/armem/core/aron_conversions.h b/source/RobotAPI/libraries/armem/core/aron_conversions.h
index 8688c3b3fdf5dcb2ce9684960ab3a98449c8c13a..064aafe8d7bd4ea964aa1e020ca692d8eed45bf2 100644
--- a/source/RobotAPI/libraries/armem/core/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/aron_conversions.h
@@ -1,7 +1,8 @@
#pragma once
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
-#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
+#include "forward_declarations.h"
+
+#include <ostream>
namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
index 5e22ba7036a7d0b94a4a329e3d73e591b29a9543..f4d0b530f43afcfa7f7aeba95389bdb8a5371918 100644
--- a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
@@ -5,8 +5,9 @@
#include "ProviderSegmentBase.h"
#include "detail/AronTyped.h"
-#include "detail/EntityContainerBase.h"
-#include "detail/MaxHistorySize.h"
+#include "detail/MemoryContainerBase.h"
+#include "detail/iteration_mixins.h"
+#include "detail/lookup_mixins.h"
namespace armarx::armem::base
@@ -17,11 +18,16 @@ 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::MemoryContainerBase<std::map<std::string, _ProviderSegmentT>, _Derived>
+ , public detail::AronTyped
+ , public detail::ForEachEntityInstanceMixin<_Derived>
+ , public detail::ForEachEntitySnapshotMixin<_Derived>
+ , public detail::ForEachEntityMixin<_Derived>
+ , public detail::GetFindInstanceMixin<_Derived>
+ , public detail::GetFindSnapshotMixin<_Derived>
+ , public detail::GetFindEntityMixin<_Derived>
{
- using Base = detail::EntityContainerBase<_ProviderSegmentT, typename _ProviderSegmentT::EntityT, _Derived>;
+ using Base = detail::MemoryContainerBase<std::map<std::string, _ProviderSegmentT>, _Derived>;
public:
@@ -33,6 +39,9 @@ namespace armarx::armem::base
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
+ using ChildT = ProviderSegmentT;
+
+
struct UpdateResult
{
armarx::armem::UpdateType coreSegmentUpdateType;
@@ -50,20 +59,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,86 +85,152 @@ namespace armarx::armem::base
CoreSegmentBase& operator=(CoreSegmentBase&& other) = default;
- inline const std::string& name() const
+ // READ ACCESS
+
+ // Get key
+ inline std::string& name()
{
return this->id().coreSegmentName;
}
- inline std::string& name()
+ inline const std::string& name() const
{
- return const_cast<std::string&>(const_cast<const CoreSegmentBase*>(this)->name());
+ return this->id().coreSegmentName;
}
- inline const auto& providerSegments() const
+ // Has child by key
+ bool hasProviderSegment(const std::string& name) const
{
- return this->_container;
+ return this->findProviderSegment(name) != nullptr;
}
- inline auto& providerSegments()
+ // Has child by memory ID
+ bool hasProviderSegment(const MemoryID& providerSegmentID) const
{
- return this->_container;
+ return this->findProviderSegment(providerSegmentID) != nullptr;
}
- bool hasProviderSegment(const std::string& name) const
+ // Find child by key
+ ProviderSegmentT* findProviderSegment(const std::string& name)
+ {
+ return detail::findChildByKey(name, this->_container);
+ }
+ const ProviderSegmentT* findProviderSegment(const std::string& name) const
{
- return this->_container.count(name) > 0;
+ return detail::findChildByKey(name, this->_container);
}
+ // Get child by key
ProviderSegmentT& getProviderSegment(const std::string& name)
{
- return const_cast<ProviderSegmentT&>(const_cast<const CoreSegmentBase*>(this)->getProviderSegment(name));
+ return detail::getChildByKey(name, this->_container, *this);
}
-
const ProviderSegmentT& getProviderSegment(const std::string& name) const
{
- auto it = this->_container.find(name);
- if (it != this->_container.end())
- {
- return it->second;
- }
- else
- {
- throw armem::error::MissingEntry::create<ProviderSegmentT>(name, *this);
- }
+ return detail::getChildByKey(name, this->_container, *this);
}
- using Base::getEntity;
- const EntityT& getEntity(const MemoryID& id) const override
+ // Find child by MemoryID
+ ProviderSegmentT* findProviderSegment(const MemoryID& providerSegmentID)
+ {
+ detail::checkHasProviderSegmentName(providerSegmentID);
+ return this->findProviderSegment(providerSegmentID.providerSegmentName);
+ }
+ const ProviderSegmentT* findProviderSegment(const MemoryID& providerSegmentID) const
{
- this->_checkContainerName(id.coreSegmentName, this->getKeyString());
- return getProviderSegment(id.providerSegmentName).getEntity(id);
+ detail::checkHasProviderSegmentName(providerSegmentID);
+ return this->findProviderSegment(providerSegmentID.providerSegmentName);
}
- const EntityT* findEntity(const MemoryID& id) const override
+ // Get child by MemoryID
+ ProviderSegmentT& getProviderSegment(const MemoryID& providerSegmentID)
{
- this->_checkContainerName(id.coreSegmentName, this->getKeyString());
- if (id.hasProviderSegmentName())
- {
- return getProviderSegment(id.providerSegmentName).findEntity(id);
- }
- else
- {
- for (const auto& [_, providerSegment] : this->_container)
- {
- if (auto entity = providerSegment.findEntity(id))
- {
- return entity;
- }
- }
- return nullptr;
- }
+ detail::checkHasProviderSegmentName(providerSegmentID);
+ return this->getProviderSegment(providerSegmentID.providerSegmentName);
+ }
+ const ProviderSegmentT& getProviderSegment(const MemoryID& providerSegmentID) const
+ {
+ detail::checkHasProviderSegmentName(providerSegmentID);
+ return this->getProviderSegment(providerSegmentID.providerSegmentName);
+ }
+
+ // get/findInstance are provided by GetFindInstanceMixin
+ // get/findSnapshot are provided by GetFindSnapshotMixin
+ // get/findEntity are provided by GetFindEntityMixin
+ using detail::GetFindEntityMixin<DerivedT>::hasEntity;
+ using detail::GetFindEntityMixin<DerivedT>::findEntity;
+
+ // Search all provider segments for the first matching entity.
+
+ bool hasEntity(const std::string& entityName) const
+ {
+ return this->findEntity(entityName) != nullptr;
+ }
+ EntityT* findEntity(const std::string& entityName)
+ {
+ return _findEntity(*this, entityName);
+ }
+ const EntityT* findEntity(const std::string& entityName) const
+ {
+ return _findEntity(*this, entityName);
+ }
+
+
+ // 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.
+
+
+ // Get child keys
+ std::vector<std::string> getProviderSegmentNames() const
+ {
+ return simox::alg::get_keys(this->_container);
+ }
+
+
+ [[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.
*
* Missing entity entries are added before updating.
*/
- virtual UpdateResult update(const EntityUpdate& update)
+ UpdateResult update(const EntityUpdate& update)
{
this->_checkContainerName(update.entityID.coreSegmentName, this->name());
- auto [inserted, provSeg] = addProviderSegmentIfMissing(update.entityID.providerSegmentName);
+ auto [inserted, provSeg] = _addProviderSegmentIfMissing(update.entityID.providerSegmentName);
// Update entry.
@@ -170,48 +246,21 @@ namespace armarx::armem::base
return ret;
}
- std::pair<bool, ProviderSegmentT*> addProviderSegmentIfMissing(const std::string& providerSegmentName)
- {
- ProviderSegmentT* provSeg;
-
- auto it = this->_container.find(providerSegmentName);
- if (it == this->_container.end())
- {
- if (_addMissingProviderSegmentDuringUpdate)
- {
- // Insert into map.
- provSeg = &addProviderSegment(providerSegmentName);
- return {true, provSeg};
- }
- else
- {
- throw error::MissingEntry::create<EntitySnapshotT>(providerSegmentName, *this);
- }
- }
- else
- {
- provSeg = &it->second;
- return {false, provSeg};
- }
- }
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())
- {
- // segment already exists
- it->second.append(s);
- }
- else
+ auto it = this->_container.find(provSeg.name());
+ if (it == this->_container.end())
{
- auto wms = this->_container.emplace(k, this->id().withProviderSegmentName(k));
- wms.first->second.append(s);
+ it = this->_container.emplace(provSeg.name(), this->id().withProviderSegmentName(provSeg.name())).first;
}
- }
+ it->second.append(provSeg);
+ return true;
+ });
}
/**
@@ -223,46 +272,35 @@ 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 this->_derived().addProviderSegment(name, name, type);
}
/// Copy and insert a provider segment.
ProviderSegmentT& addProviderSegment(const ProviderSegmentT& providerSegment)
{
- return addProviderSegment(ProviderSegment(providerSegment));
+ return this->_derived().addProviderSegment(providerSegment.name(), ProviderSegmentT(providerSegment));
}
/// Move and insert a provider segment.
ProviderSegmentT& addProviderSegment(ProviderSegmentT&& providerSegment)
{
- if (hasProviderSegment(providerSegment.name()))
- {
- throw armem::error::ContainerEntryAlreadyExists(
- providerSegment.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;
+ const std::string name = providerSegment.name(); // Copy before move.
+ return this->_derived().addProviderSegment(name, std::move(providerSegment));
}
-
- /**
- * @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
+ /// Insert a provider segment in-place.
+ template <class ...Args>
+ ProviderSegmentT& addProviderSegment(const std::string& name, Args... args)
{
- MaxHistorySize::setMaxHistorySize(maxSize);
- for (auto& [name, seg] : this->_container)
- {
- seg.setMaxHistorySize(maxSize);
- }
+ ChildT& child = this->template _addChild<ChildT>(name, args...);
+ child.id() = this->id().withProviderSegmentName(name);
+ return child;
}
- virtual bool equalsDeep(const CoreSegmentBase& other) const
+
+ // MISC
+
+ bool equalsDeep(const DerivedT& other) const
{
//std::cout << "CoreSegment::equalsDeep" << std::endl;
if (this->size() != other.size())
@@ -283,12 +321,12 @@ 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();
}
@@ -296,17 +334,45 @@ namespace armarx::armem::base
protected:
- virtual void _copySelf(DerivedT& other) const override
+ template <class ParentT>
+ static
+ auto*
+ _findEntity(ParentT&& parent, const std::string& entityName)
{
- Base::_copySelf(other);
- other.aronType() = _aronType;
- other._addMissingProviderSegmentDuringUpdate = _addMissingProviderSegmentDuringUpdate;
+ decltype(parent.findEntity(entityName)) result = nullptr;
+ parent.forEachProviderSegment([&result, &entityName](auto & provSeg)
+ {
+ result = provSeg.findEntity(entityName);
+ return result == nullptr; // Keep going if null, break if not null.
+ });
+ return result;
}
- virtual void _copySelfEmpty(DerivedT& other) const override
+
+
+ std::pair<bool, ProviderSegmentT*>
+ _addProviderSegmentIfMissing(const std::string& providerSegmentName)
{
- Base::_copySelfEmpty(other);
- other.aronType() = _aronType;
- other._addMissingProviderSegmentDuringUpdate = _addMissingProviderSegmentDuringUpdate;
+ ProviderSegmentT* provSeg;
+
+ auto it = this->_container.find(providerSegmentName);
+ if (it == this->_container.end())
+ {
+ if (_addMissingProviderSegmentDuringUpdate)
+ {
+ // Insert into map.
+ provSeg = &addProviderSegment(providerSegmentName);
+ return {true, provSeg};
+ }
+ else
+ {
+ throw error::MissingEntry::create<ProviderSegmentT>(providerSegmentName, *this);
+ }
+ }
+ else
+ {
+ provSeg = &it->second;
+ return {false, provSeg};
+ }
}
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 0775a71f977fde16966550f0506e9fd0f591e9a5..f45018ff955fac99b7cf0824eaff7539eee9ac29 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
@@ -5,14 +5,14 @@
#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/lookup_mixins.h"
+#include "detail/negative_index_semantics.h"
namespace armarx::armem::base
@@ -39,8 +39,9 @@ namespace armarx::armem::base
*/
template <class _EntitySnapshotT, class _Derived>
class EntityBase :
- public detail::MemoryContainerBase<std::map<Time, _EntitySnapshotT>, _Derived>,
- public detail::MaxHistorySize
+ public detail::MemoryContainerBase<std::map<Time, _EntitySnapshotT>, _Derived>
+ , public detail::ForEachEntityInstanceMixin<_Derived>
+ , public detail::GetFindInstanceMixin<_Derived>
{
using Base = detail::MemoryContainerBase<std::map<Time, _EntitySnapshotT>, _Derived>;
@@ -52,6 +53,8 @@ namespace armarx::armem::base
using EntitySnapshotT = _EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
+ using ChildT = EntitySnapshotT;
+
struct UpdateResult
{
armarx::armem::UpdateType entityUpdateType;
@@ -66,11 +69,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,277 +85,422 @@ namespace armarx::armem::base
EntityBase& operator=(EntityBase&& other) = default;
- virtual bool equalsDeep(const EntityBase& other) const
+ // READING
+
+ // Get key
+ inline std::string& name()
{
- //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;
+ return this->id().entityName;
}
-
-
inline const std::string& name() const
{
return this->id().entityName;
}
- inline std::string& name()
+
+
+ // Has child by key
+ /// Indicates whether a history entry for the given time exists.
+ bool hasSnapshot(const Time& time) const
{
- return const_cast<std::string&>(const_cast<const EntityBase*>(this)->name());
+ return this->findSnapshot(time) != nullptr;
+ }
+ // Has child by MemoryID
+ bool hasSnapshot(const MemoryID& snapshotID) const
+ {
+ return this->findSnapshot(snapshotID) != nullptr;
}
- inline const ContainerT& history() const
+ // Find child via key
+ EntitySnapshotT*
+ findSnapshot(const Time& timestamp)
{
- return this->_container;
+ return detail::findChildByKey(timestamp, this->_container);
}
- inline ContainerT& history()
+ const EntitySnapshotT*
+ findSnapshot(const Time& timestamp) const
{
- return const_cast<ContainerT&>(const_cast<const EntityBase*>(this)->history());
+ return detail::findChildByKey(timestamp, this->_container);
}
-
+ // Get child via key
/**
- * @brief Indicates whether a history entry for the given time exists.
+ * @brief Get a snapshot.
+ * @param time The time.
+ * @return The snapshot, if it exists.
+ *
+ * @throws `armem::error::MissingEntry` If there is no such entry.
*/
- virtual bool hasSnapshot(const Time& time)
+ EntitySnapshotT&
+ getSnapshot(const Time& time)
+ {
+ return detail::getChildByKey(time, this->_container, *this, [](const Time & time)
+ {
+ return toDateTimeMilliSeconds(time);
+ });
+ }
+ const EntitySnapshotT&
+ getSnapshot(const Time& time) const
+ {
+ return detail::getChildByKey(time, this->_container, *this, [](const Time & time)
+ {
+ return toDateTimeMilliSeconds(time);
+ });
+ }
+
+ // Find child via MemoryID
+ EntitySnapshotT*
+ findSnapshot(const MemoryID& snapshotID)
{
- return const_cast<const EntityBase*>(this)->hasSnapshot(time);
+ detail::checkHasTimestamp(snapshotID);
+ return this->findSnapshot(snapshotID.timestamp);
+ }
+ const EntitySnapshotT*
+ findSnapshot(const MemoryID& snapshotID) const
+ {
+ detail::checkHasTimestamp(snapshotID);
+ return this->findSnapshot(snapshotID.timestamp);
}
- virtual bool hasSnapshot(const Time& time) const
+ // Get child via MemoryID
+ EntitySnapshotT&
+ getSnapshot(const MemoryID& snapshotID)
+ {
+ detail::checkHasTimestamp(snapshotID);
+ return this->getSnapshot(snapshotID.timestamp);
+ }
+ const EntitySnapshotT&
+ getSnapshot(const MemoryID& snapshotID) const
{
- return this->_container.count(time) > 0;
+ detail::checkHasTimestamp(snapshotID);
+ return this->getSnapshot(snapshotID.timestamp);
+
}
+ // get/findInstance are provided by GetFindInstanceMixin
+
+
+ // More getter/finder for snapshots
+
/**
* @brief Get the latest timestamp.
* @throw `armem::error::EntityHistoryEmpty` If the history is empty.
*/
- Time getLatestTimestamp()
+ Time getLatestTimestamp() const
{
- return const_cast<const EntityBase*>(this)->getLatestTimestamp();
+ return this->getLatestSnapshot().time();
}
-
- Time getLatestTimestamp() const
+ /**
+ * @brief Get the oldest timestamp.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ */
+ Time getFirstTimestamp() const
{
- return getLatestItem().first;
+ return this->getFirstSnapshot().time();
}
/**
- * @brief Get all timestamps in the history.
+ * @brief Return the snapshot with the most recent timestamp.
+ * @return The latest snapshot or nullptr if the entity is empty.
*/
- virtual std::vector<Time> getTimestamps()
+ EntitySnapshotT* findLatestSnapshot()
{
- return const_cast<const EntityBase*>(this)->getTimestamps();
+ return this->empty() ? nullptr : &this->_container.rbegin()->second;
}
-
- virtual std::vector<Time> getTimestamps() const
+ const EntitySnapshotT* findLatestSnapshot() const
{
- return simox::alg::get_keys(this->_container);
+ return this->empty() ? nullptr : &this->_container.rbegin()->second;
}
-
/**
- * @brief Get a snapshot.
- * @param time The time.
- * @return The snapshot, if it exists.
- *
- * @throws `armem::error::MissingEntry` If there is no such entry.
+ * @brief Return the snapshot with the most recent timestamp.
+ * @return The latest snapshot.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
*/
- virtual EntitySnapshotT& getSnapshot(const Time& time)
+ EntitySnapshotT& getLatestSnapshot()
{
- return const_cast<EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getSnapshot(time));
+ return const_cast<EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getLatestSnapshot());
}
-
- virtual const EntitySnapshotT& getSnapshot(const Time& time) const
+ const EntitySnapshotT& getLatestSnapshot() const
{
- auto it = this->_container.find(time);
- if (it != this->_container.end())
+ if (const EntitySnapshotT* snapshot = this->findLatestSnapshot())
{
- return it->second;
+ return *snapshot;
}
else
{
- throw armem::error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ throw armem::error::EntityHistoryEmpty(name(), "when getting the latest snapshot.");
}
}
- EntitySnapshotT& getSnapshot(const MemoryID& id)
+
+ /**
+ * @brief Return the snapshot with the least recent timestamp.
+ * @return The first snapshot or nullptr if the entity is empty.
+ */
+ EntitySnapshotT* findFirstSnapshot()
{
- return const_cast<EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getSnapshot(id));
+ return this->empty() ? nullptr : &this->_container.begin()->second;
}
-
- const EntitySnapshotT& getSnapshot(const MemoryID& id) const
+ const EntitySnapshotT* findFirstSnapshot() const
{
- this->_checkContainerName(id.entityName, this->name());
- return getSnapshot(id.timestamp);
+ return this->empty() ? nullptr : &this->_container.begin()->second;
}
/**
- * @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.
*/
- EntitySnapshotT& getLatestSnapshot()
+ EntitySnapshotT& getFirstSnapshot()
{
- return const_cast<EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getLatestSnapshot());
+ return const_cast<EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getFirstSnapshot());
}
-
- const EntitySnapshotT& getLatestSnapshot() const
+ const EntitySnapshotT& getFirstSnapshot() const
{
- return getLatestItem().second;
+ if (const EntitySnapshotT* snapshot = this->findFirstSnapshot())
+ {
+ return *snapshot;
+ }
+ else
+ {
+ throw armem::error::EntityHistoryEmpty(name(), "when getting the first snapshot.");
+ }
}
+
/**
- * @brief Return the lastest snapshot before or at time.
+ * @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
+ * @return The latest snapshot < time or nullptr if none was found.
*/
- virtual const EntitySnapshotT& getLatestSnapshotBeforeOrAt(const Time& time) const
+ const EntitySnapshotT* findLatestSnapshotBefore(const Time& time) const
{
- // first element equal or greater
- typename ContainerT::const_iterator refIt = this->_container.upper_bound(time);
-
- if (refIt == this->_container.begin())
+ if (this->empty())
{
- if (refIt->first == time)
- {
- return refIt->second;
- }
- throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ return nullptr;
}
- // last element less than
- typename ContainerT::const_iterator refItPrev = std::prev(refIt);
-
- // ... or we return the element before if possible
- if (refItPrev != 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())
{
- return refItPrev->second;
+ return nullptr;
}
- throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ // end(): No element >= time, we can still have one < time (rbegin()) => std::prev(end())
+ // empty has been checked above
+
+ // std::prev of end() is ok
+ typename ContainerT::const_iterator less = std::prev(greaterEq);
+
+ // we return the element before if possible
+ return &less->second;
}
/**
- * @brief Return the lastest snapshot before time.
+ * @brief Return the latest 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
+ * @return The latest snapshot <= time or nullptr if none was found.
+ */
+ const EntitySnapshotT* findLatestSnapshotBeforeOrAt(const Time& time) const
+ {
+ return findLatestSnapshotBefore(time + Time::microSeconds(1));
+ }
+
+ /**
+ * @brief Return first snapshot after or at time.
+ * @param time The time.
+ * @return The first snapshot >= time or nullptr if none was found.
*/
- virtual const EntitySnapshotT& getLatestSnapshotBefore(const Time& time) const
+ const EntitySnapshotT* findFirstSnapshotAfterOrAt(const Time& time) const
{
- // first element equal or greater
- typename ContainerT::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 nullptr;
}
+ return &greaterEq->second;
+ }
- // last element less than
- typename ContainerT::const_iterator refItPrev = std::prev(refIt);
+ /**
+ * @brief Return first snapshot after time.
+ * @param time The time.
+ * @return The first snapshot >= time or nullptr if none was found.
+ */
+ const EntitySnapshotT* findFirstSnapshotAfter(const Time& time) const
+ {
+ return findFirstSnapshotAfter(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);
+ auto* findLatestInstance(int instanceIndex = 0)
+ {
+ auto* snapshot = this->findLatestSnapshot();
+ return snapshot ? snapshot->findInstance(instanceIndex) : nullptr;
+ }
+ const auto* findLatestInstance(int instanceIndex = 0) const
+ {
+ auto* snapshot = this->findLatestSnapshot();
+ return snapshot ? snapshot->findInstance(instanceIndex) : nullptr;
}
+#if 0 // Do not offer this yet.
+ auto* findLatestInstanceData(int instanceIndex = 0)
+ {
+ auto* instance = this->findLatestInstance(instanceIndex);
+ return instance ? &instance->data() : nullptr;
+ }
+ const auto* findLatestInstanceData(int instanceIndex = 0) const
+ {
+ auto* instance = this->findLatestInstance(instanceIndex);
+ return instance ? &instance->data() : nullptr;
+ }
+#endif
+
+
+ // ITERATION
+
/**
- * @brief Return all snapshots before or at time.
+ * @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 (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 ContainerT::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 ContainerT::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 ContainerT::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;
+ }
+ }
}
+ }
+
+
+ // Get child keys
+ /// @brief Get all timestamps in the history.
+ std::vector<Time> getTimestamps() const
+ {
+ return simox::alg::get_keys(this->_container);
+ }
+
- 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.
@@ -371,7 +519,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,108 +528,93 @@ 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())
+ auto it = this->_container.find(snapshot.time());
+ if (it == this->_container.end())
{
- // segment already exists
- // We assume that a snapshot does not change, so ignore
+ 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
- {
- 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)));
- }
- }
- }
-
- /**
- * @brief Add a single snapshot with data.
- * @param snapshot The snapshot.
- * @return The stored snapshot.
- */
- EntitySnapshotT& addSnapshot(const EntitySnapshotT& snapshot)
- {
- return addSnapshot(EntitySnapshotT(snapshot));
+ // else: segment already exists
+ // We assume that a snapshot does not change, so ignore
+ return true;
+ });
}
- EntitySnapshotT& addSnapshot(EntitySnapshotT&& snapshot)
- {
- auto it = this->_container.emplace_hint(this->_container.end(), snapshot.time(), std::move(snapshot));
- it->second.id().setEntityID(this->id());
- return it->second;
- }
+ /// Add a snapshot at the given time.
EntitySnapshotT& addSnapshot(const Time& timestamp)
{
- return addSnapshot(EntitySnapshotT(timestamp));
+ return this->addSnapshot(timestamp, EntitySnapshotT(timestamp));
}
-
- /**
- * @brief Sets the maximum history size.
- *
- * The current history is truncated if necessary.
- */
- void setMaxHistorySize(long maxSize) override
+ /// Copy and insert a snapshot
+ EntitySnapshotT& addSnapshot(const EntitySnapshotT& snapshot)
{
- MaxHistorySize::setMaxHistorySize(maxSize);
- truncate();
+ return this->addSnapshot(snapshot.time(), EntitySnapshotT(snapshot));
}
- std::string getKeyString() const override
+ /// Move and insert a snapshot
+ EntitySnapshotT& addSnapshot(EntitySnapshotT&& snapshot)
{
- return this->id().entityName;
+ Time timestamp = snapshot.time(); // Copy before move.
+ return this->addSnapshot(timestamp, std::move(snapshot));
}
- std::string getLevelName() const override
+
+ /// Insert a snapshot in-place.
+ template <class ...Args>
+ EntitySnapshotT& addSnapshot(const Time& timestamp, Args... args)
{
- return "entity";
+ auto it = this->_container.emplace_hint(this->_container.end(), timestamp, args...);
+ it->second.id() = this->id().withTimestamp(timestamp);
+ return it->second;
}
- protected:
- /// If maximum size is set, ensure `history`'s is not higher.
- std::vector<EntitySnapshotT> truncate()
+ // MISC
+
+ bool equalsDeep(const DerivedT& other) const
{
- std::vector<EntitySnapshotT> removedElements;
- if (_maxHistorySize >= 0)
+ //std::cout << "Entity::equalsDeep" << std::endl;
+ if (this->size() != other.size())
{
- while (this->_container.size() > size_t(_maxHistorySize))
+ return false;
+ }
+ for (const auto& [key, snapshot] : this->_container)
+ {
+ if (not other.hasSnapshot(key))
+ {
+ return false;
+ }
+ if (not snapshot.equalsDeep(other.getSnapshot(key)))
{
- removedElements.push_back(std::move(this->_container.begin()->second));
- this->_container.erase(this->_container.begin());
+ return false;
}
- ARMARX_CHECK_LESS_EQUAL(this->_container.size(), _maxHistorySize);
}
- return removedElements;
+ return true;
}
- /**
- * @brief Return the snapshot with the most recent timestamp.
- * @return The latest snapshot.
- * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
- */
- virtual
- const typename ContainerT::value_type&
- getLatestItem() const
+ std::string getKeyString() const
{
- if (this->_container.empty())
- {
- throw armem::error::EntityHistoryEmpty(name(), "when getting the latest snapshot.");
- }
- return *this->_container.rbegin();
+ return this->id().entityName;
+ }
+ static std::string getLevelName()
+ {
+ return "entity";
}
};
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..8f7b8aafc4cebd74c241bfb1245388a943897e01 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,24 +55,24 @@ 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)
{
}
+ // Key
inline int& index()
{
return id().instanceIndex;
@@ -46,29 +82,37 @@ 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;
+ // Data
- virtual DerivedT copy() const
+ EntityInstanceMetadata& metadata()
{
- DerivedT d;
- this->_copySelf(d);
- return d;
+ return _metadata;
+ }
+ const EntityInstanceMetadata& metadata() const
+ {
+ return _metadata;
}
+ const DataT& data() const
+ {
+ return _data;
+ }
- std::string getLevelName() const override
+ DataT& data()
+ {
+ return _data;
+ }
+
+
+ // Misc
+
+ static std::string getLevelName()
{
return "entity instance";
}
- std::string getKeyString() const override
+ std::string getKeyString() const
{
return std::to_string(index());
}
@@ -76,10 +120,11 @@ namespace armarx::armem::base
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..0d0f091f59847cabd2b9d5f0982d634daaa2723a 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
@@ -1,15 +1,20 @@
#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"
+#include "detail/lookup_mixins.h"
+namespace armarx::armem::base::detail
+{
+ void throwIfNotEqual(const Time& ownTime, const Time& updateTime);
+}
namespace armarx::armem::base
{
/**
@@ -34,11 +39,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,85 +54,62 @@ 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
+ // Get key
inline Time& time()
{
return this->id().timestamp;
}
-
inline const Time& time() const
{
return this->id().timestamp;
}
- inline const std::vector<EntityInstanceT>& instances() const
+ // Has child by key
+ bool hasInstance(int index) const
{
- return this->_container;
+ return this->findInstance(index) != nullptr;
}
- inline std::vector<EntityInstanceT>& instances()
+ // Has child by ID
+ bool hasInstance(const MemoryID& instanceID) const
{
- return const_cast<std::vector<EntityInstanceT>&>(const_cast<const EntitySnapshotBase*>(this)->instances());
+ return this->findInstance(instanceID) != nullptr;
}
- void update(const EntityUpdate& update, std::optional<MemoryID> parentID = std::nullopt)
+ // Find child by key
+ EntityInstanceT* findInstance(int index)
{
- 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);
- }
+ return const_cast<EntityInstanceT*>(const_cast<const EntitySnapshotBase*>(this)->findInstance(index));
}
-
-
- bool hasInstance(int index) const
+ const EntityInstanceT* findInstance(int index) const
{
- size_t si = size_t(index);
- return index >= 0 && si < this->_container.size();
+ const size_t si = static_cast<size_t>(index);
+ return (index >= 0 && si < this->_container.size())
+ ? &this->_container[si]
+ : nullptr;
}
+ // Get child by key
/**
* @brief Get the given instance.
* @param index The instance's index.
* @return The instance.
* @throw `armem::error::MissingEntry` If the given index is invalid.
*/
- EntityInstanceT& getInstance(int index)
+ EntityInstanceT&
+ getInstance(int index)
{
return const_cast<EntityInstanceT&>(const_cast<const EntitySnapshotBase*>(this)->getInstance(index));
}
-
- const EntityInstanceT& getInstance(int index) const
+ const EntityInstanceT&
+ getInstance(int index) const
{
- if (hasInstance(index))
+ if (const EntityInstanceT* instance = findInstance(index))
{
- return this->_container[static_cast<size_t>(index)];
+ return *instance;
}
else
{
@@ -135,6 +117,21 @@ namespace armarx::armem::base
}
}
+ // Find child by MemoryID
+ EntityInstanceT*
+ findInstance(const MemoryID& instanceID)
+ {
+ detail::checkHasInstanceIndex(instanceID);
+ return this->findInstance(instanceID.instanceIndex);
+ }
+ const EntityInstanceT*
+ findInstance(const MemoryID& instanceID) const
+ {
+ detail::checkHasInstanceIndex(instanceID);
+ return this->findInstance(instanceID.instanceIndex);
+ }
+
+ // Get child by MemoryID
/**
* @brief Get the given instance.
* @param index The instance's index.
@@ -142,21 +139,79 @@ namespace armarx::armem::base
* @throw `armem::error::MissingEntry` If the given index is invalid.
* @throw `armem::error::InvalidMemoryID` If memory ID does not have an instance index.
*/
- EntityInstanceT& getInstance(const MemoryID& id)
+ EntityInstanceT&
+ getInstance(const MemoryID& instanceID)
+ {
+ detail::checkHasInstanceIndex(instanceID);
+ return this->getInstance(instanceID.instanceIndex);
+ }
+ const EntityInstanceT&
+ getInstance(const MemoryID& instanceID) const
{
- return const_cast<EntityInstanceT&>(const_cast<const EntitySnapshotBase*>(this)->getInstance(id));
+ detail::checkHasInstanceIndex(instanceID);
+ return this->getInstance(instanceID.instanceIndex);
}
- const EntityInstanceT& getInstance(const MemoryID& id) const
+
+ // ITERATION
+
+ /**
+ * @param func Function like void process(EntityInstanceT& instance)>
+ */
+ template <class InstanceFunctionT>
+ bool forEachInstance(InstanceFunctionT&& func)
{
- if (!id.hasInstanceIndex())
+ 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);
+ }
+
+
+ // Get child keys
+ std::vector<int> getInstanceIndices() const
+ {
+ std::vector<int> indices;
+ indices.reserve(this->size());
+ for (size_t i = 0; i < this->size(); ++i)
{
- throw armem::error::InvalidMemoryID(id, "ID has no instance index.");
+ indices.push_back(static_cast<int>(i));
}
- return getInstance(id.instanceIndex);
+ return indices;
}
+ [[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 +236,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 1bc42b0cc579dbf8abe4f2831608166451ff8fbb..e1729bbcd90cc6062e93b8e62ed8db29e3bb4387 100644
--- a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
@@ -3,10 +3,10 @@
#include <map>
#include <string>
-#include <ArmarXCore/core/logging/Logging.h>
-
#include "CoreSegmentBase.h"
-#include "detail/EntityContainerBase.h"
+#include "detail/MemoryContainerBase.h"
+#include "detail/iteration_mixins.h"
+#include "detail/lookup_mixins.h"
namespace armarx::armem::base
@@ -17,9 +17,17 @@ namespace armarx::armem::base
*/
template <class _CoreSegmentT, class _Derived>
class MemoryBase :
- public detail::EntityContainerBase<_CoreSegmentT, typename _CoreSegmentT::ProviderSegmentT::EntityT, _Derived>
+ public detail::MemoryContainerBase<std::map<std::string, _CoreSegmentT>, _Derived>
+ , public detail::ForEachEntityInstanceMixin<_Derived>
+ , public detail::ForEachEntitySnapshotMixin<_Derived>
+ , public detail::ForEachEntityMixin<_Derived>
+ , public detail::ForEachProviderSegmentMixin<_Derived>
+ , public detail::GetFindInstanceMixin<_Derived>
+ , public detail::GetFindSnapshotMixin<_Derived>
+ , public detail::GetFindEntityMixin<_Derived>
+ , public detail::GetFindProviderSegmentMixin<_Derived>
{
- using Base = detail::EntityContainerBase<_CoreSegmentT, typename _CoreSegmentT::ProviderSegmentT::EntityT, _Derived>;
+ using Base = detail::MemoryContainerBase<std::map<std::string, _CoreSegmentT>, _Derived>;
public:
@@ -32,6 +40,8 @@ namespace armarx::armem::base
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
+ using ChildT = CoreSegmentT;
+
struct UpdateResult
{
@@ -57,11 +67,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,149 +82,158 @@ namespace armarx::armem::base
MemoryBase& operator=(MemoryBase&& other) = default;
- inline const std::string& name() const
+ // READ ACCESS
+
+ // Get key
+ inline std::string& name()
{
return this->id().memoryName;
}
- inline std::string& name()
+ inline const std::string& name() const
{
- return const_cast<std::string&>(const_cast<const MemoryBase*>(this)->name());
+ return this->id().memoryName;
}
- inline auto& coreSegments() const
+ // Has child by key
+ bool hasCoreSegment(const std::string& name) const
{
- return this->_container;
+ return this->findCoreSegment(name) != nullptr;
}
- inline auto& coreSegments()
+ // Has child by MemoryID
+ bool hasCoreSegment(const MemoryID& coreSegmentID) const
{
- return this->_container;
+ return this->findCoreSegment(coreSegmentID) != nullptr;
}
-
- bool hasCoreSegment(const std::string& name) const
+ // Find child by key
+ CoreSegmentT* findCoreSegment(const std::string& name)
{
- return this->_container.count(name) > 0;
+ return detail::findChildByKey(name, this->_container);
+ }
+ const CoreSegmentT* findCoreSegment(const std::string& name) const
+ {
+ return detail::findChildByKey(name, this->_container);
}
+ // Get child by key
CoreSegmentT& getCoreSegment(const std::string& name)
{
- return const_cast<CoreSegmentT&>(const_cast<const MemoryBase*>(this)->getCoreSegment(name));
+ return detail::getChildByKey(name, this->_container, *this);
}
-
const CoreSegmentT& getCoreSegment(const std::string& name) const
{
- auto it = this->_container.find(name);
- if (it != this->_container.end())
- {
- return it->second;
- }
- else
- {
- throw armem::error::MissingEntry::create<CoreSegmentT>(name, *this);
- }
+ return detail::getChildByKey(name, this->_container, *this);
}
-
- bool hasProviderSegment(const MemoryID& providerSegmentID) const
+ // Find child by MemoryID
+ CoreSegmentT* findCoreSegment(const MemoryID& coreSegmentID)
{
- auto it = this->_container.find(providerSegmentID.coreSegmentName);
- if (it != this->_container.end())
- {
- return it->second.hasProviderSegment(providerSegmentID.providerSegmentName);
- }
- else
- {
- return false;
- }
+ detail::checkHasCoreSegmentName(coreSegmentID);
+ return this->findCoreSegment(coreSegmentID.coreSegmentName);
+ }
+ const CoreSegmentT* findCoreSegment(const MemoryID& coreSegmentID) const
+ {
+ detail::checkHasCoreSegmentName(coreSegmentID);
+ return this->findCoreSegment(coreSegmentID.coreSegmentName);
}
- ProviderSegmentT& getProviderSegment(const MemoryID& providerSegmentID)
+ // Get child by MemoryID
+ CoreSegmentT& getCoreSegment(const MemoryID& coreSegmentID)
+ {
+ detail::checkHasCoreSegmentName(coreSegmentID);
+ return this->getCoreSegment(coreSegmentID.coreSegmentName);
+ }
+ const CoreSegmentT& getCoreSegment(const MemoryID& coreSegmentID) const
{
- return getCoreSegment(providerSegmentID.coreSegmentName).getProviderSegment(providerSegmentID.providerSegmentName);
+ detail::checkHasCoreSegmentName(coreSegmentID);
+ return this->getCoreSegment(coreSegmentID.coreSegmentName);
}
- const ProviderSegmentT& getProviderSegment(const MemoryID& providerSegmentID) const
+ // get/findInstance are provided by GetFindInstanceMixin
+ // get/findSnapshot are provided by GetFindSnapshotMixin
+ // get/findEntity are provided by GetFindEntityMixin
+ // get/findProviderSegment are provided by GetFindProviderSegmentMixin
+
+
+
+ // 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 getCoreSegment(providerSegmentID.coreSegmentName).getProviderSegment(providerSegmentID.providerSegmentName);
+ return this->forEachChild(func);
}
+ // forEachProviderSegment() is provided by ForEachProviderSegmentMixin.
+ // forEachEntity() is provided by ForEachEntityMixin.
+ // forEachSnapshot() is provided by ForEachEntitySnapshotMixin.
+ // forEachInstance() is provided by ForEachEntityInstanceMixin.
+
- using Base::getEntity;
- const EntityT& getEntity(const MemoryID& id) const override
+ std::vector<std::string> getCoreSegmentNames() const
{
- this->_checkContainerName(id.memoryName, this->name());
- return getCoreSegment(id.coreSegmentName).getEntity(id);
+ return simox::alg::get_keys(this->_container);
}
- const EntityT* findEntity(const MemoryID& id) const override
+
+ [[deprecated("Direct container access is deprecated. Use forEach*() instead.")]]
+ inline auto& coreSegments() const
{
- this->_checkContainerName(id.memoryName, this->name());
- if (id.hasCoreSegmentName())
- {
- return getCoreSegment(id.coreSegmentName).findEntity(id);
- }
- else
- {
- for (const auto& [_, coreSegment] : this->_container)
- {
- if (auto entity = coreSegment.findEntity(id))
- {
- return entity;
- }
- }
- return nullptr;
- }
+ 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 this->_derived().addCoreSegment(name, name, coreSegmentType);
}
+
/// Copy and insert a core segment.
CoreSegmentT& addCoreSegment(const CoreSegmentT& coreSegment)
{
- return addCoreSegment(CoreSegmentT(coreSegment));
+ return this->_derived().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;
+ const std::string name = coreSegment.name(); // Copy before move.
+ return this->_derived().addCoreSegment(name, std::move(coreSegment));
}
- /**
- * @brief Add multiple core segments.
- * @param The core segment names.
- * @return The core segments. The contained pointers are never null.
- */
- std::vector<CoreSegmentT*> addCoreSegments(const std::vector<std::string>& names)
+ /// Move and insert a core segment.
+ template <class ...Args>
+ CoreSegmentT& addCoreSegment(const std::string& name, Args... args)
{
- std::vector<CoreSegmentT*> segments;
- for (const auto& name : names)
- {
- try
- {
- segments.push_back(&addCoreSegment(name));
- }
- catch (const armem::error::ContainerEntryAlreadyExists& e)
- {
- ARMARX_INFO << e.what() << "\nIgnoring multiple addition.";
- }
- }
- return segments;
+ CoreSegmentT& child = this->template _addChild<ChildT>(name, args...);
+ child.id() = this->id().withCoreSegmentName(name);
+ return child;
}
@@ -223,10 +242,10 @@ namespace armarx::armem::base
* @param commit The commit.
* @return The resulting memory IDs.
*/
- virtual std::vector<UpdateResult> update(const Commit& commit)
+ 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));
}
@@ -238,11 +257,11 @@ namespace armarx::armem::base
* @param update The update.
* @return The resulting entity snapshot's ID.
*/
- virtual UpdateResult update(const EntityUpdate& update)
+ UpdateResult update(const EntityUpdate& update)
{
this->_checkContainerName(update.entityID.memoryName, this->name());
- auto [inserted, coreSeg] = addCoreSegmentIfMissing(update.entityID.coreSegmentName);
+ auto [inserted, coreSeg] = _addCoreSegmentIfMissing(update.entityID.coreSegmentName);
// Update entry.
UpdateResult ret(coreSeg->update(update));
@@ -257,30 +276,6 @@ namespace armarx::armem::base
return ret;
}
- 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<EntitySnapshotT>(coreSegmentName, *this);
- }
- }
- else
- {
- coreSeg = &it->second;
- return {false, coreSeg};
- }
- }
/**
* @brief Merge another memory into this one. Append all data
@@ -288,24 +283,21 @@ namespace armarx::armem::base
*/
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())
- {
- // segment already exists
- it->second.append(s);
- }
- else
+ auto it = this->_container.find(coreSeg.name());
+ if (it == this->_container.end())
{
- auto wms = this->_container.emplace(k, this->id().withCoreSegmentName(k));
- wms.first->second.append(s);
+ it = this->_container.emplace(coreSeg.name(), this->id().withCoreSegmentName(coreSeg.name())).first;
}
- }
+ 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())
@@ -326,50 +318,50 @@ 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
+
+ protected:
+
+ std::pair<bool, CoreSegmentT*> _addCoreSegmentIfMissing(const std::string& coreSegmentName)
{
- std::stringstream ss;
- ss << "Memory: " << this->name() << "\n";
- for (const auto& [ckey, cseg] : this->container())
+ CoreSegmentT* coreSeg = nullptr;
+
+ auto it = this->_container.find(coreSegmentName);
+ if (it == this->_container.end())
{
- ss << " |- Found core seg: " << ckey << "\n";
- for (const auto& [pkey, pseg] : cseg.container())
+ if (_addMissingCoreSegmentDuringUpdate)
{
- ss << " | |- Found prov seg: " << pkey << "\n";
- for (const auto& [ekey, eseg] : pseg.container())
- {
- ss << " | | |- Found entity: " << ekey << "\n";
- }
+ // Insert into map.
+ coreSeg = &addCoreSegment(coreSegmentName);
+ return {true, coreSeg};
}
+ else
+ {
+ throw error::MissingEntry::create<CoreSegmentT>(coreSegmentName, *this);
+ }
+ }
+ else
+ {
+ coreSeg = &it->second;
+ return {false, coreSeg};
}
- return ss.str();
}
- protected:
-
- virtual void _copySelf(DerivedT& other) const override
- {
- Base::_copySelf(other);
- other._addMissingCoreSegmentDuringUpdate = _addMissingCoreSegmentDuringUpdate;
- }
- virtual void _copySelfEmpty(DerivedT& other) const override
- {
- Base::_copySelfEmpty(other);
- other._addMissingCoreSegmentDuringUpdate = _addMissingCoreSegmentDuringUpdate;
- }
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..6a5497444c9e5959ac7d59300e1bc99e0fac2018 100644
--- a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
@@ -5,8 +5,9 @@
#include "EntityBase.h"
#include "detail/AronTyped.h"
-#include "detail/EntityContainerBase.h"
-#include "detail/MaxHistorySize.h"
+#include "detail/MemoryContainerBase.h"
+#include "detail/iteration_mixins.h"
+#include "detail/lookup_mixins.h"
namespace armarx::armem::base
@@ -17,11 +18,14 @@ 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::MemoryContainerBase<std::map<std::string, _EntityT>, _Derived>
+ , public detail::AronTyped
+ , public detail::ForEachEntityInstanceMixin<_Derived>
+ , public detail::ForEachEntitySnapshotMixin<_Derived>
+ , public detail::GetFindInstanceMixin<_Derived>
+ , public detail::GetFindSnapshotMixin<_Derived>
{
- using Base = detail::EntityContainerBase<_EntityT, _EntityT, _Derived>;
+ using Base = detail::MemoryContainerBase<std::map<std::string, _EntityT>, _Derived>;
public:
@@ -32,6 +36,8 @@ namespace armarx::armem::base
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
+ using ChildT = EntityT;
+
struct UpdateResult
{
armarx::armem::UpdateType providerSegmentUpdateType;
@@ -47,21 +53,22 @@ namespace armarx::armem::base
{}
};
+
public:
ProviderSegmentBase()
{
}
- 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,70 +80,125 @@ namespace armarx::armem::base
ProviderSegmentBase& operator=(ProviderSegmentBase&& other) = default;
+ // READ ACCESS
+
+ // Get key
+ inline std::string& name()
+ {
+ return this->id().providerSegmentName;
+ }
inline const std::string& name() const
{
return this->id().providerSegmentName;
}
- inline std::string& name()
+
+
+ // Has child by key
+ bool hasEntity(const std::string& name) const
+ {
+ return this->findEntity(name) != nullptr;
+ }
+ // Has child by ID
+ bool hasEntity(const MemoryID& entityID) const
{
- return const_cast<std::string&>(const_cast<const ProviderSegmentBase*>(this)->name());
+ return this->findEntity(entityID) != nullptr;
}
- inline const auto& entities() const
+ // Find child by key
+ EntityT* findEntity(const std::string& name)
{
- return this->_container;
+ return detail::findChildByKey(name, this->_container);
}
- inline auto& entities()
+ const EntityT* findEntity(const std::string& name) const
{
- return this->_container;
+ return detail::findChildByKey(name, this->_container);
}
+ // Get child by key
+ EntityT& getEntity(const std::string& name)
+ {
+ return detail::getChildByKey(name, this->_container, *this);
+ }
+ const EntityT& getEntity(const std::string& name) const
+ {
+ return detail::getChildByKey(name, this->_container, *this);
+ }
- bool hasEntity(const std::string& name) const
+ // Find child by MemoryID
+ EntityT* findEntity(const MemoryID& entityID)
{
- return this->_container.count(name) > 0;
+ detail::checkHasEntityName(entityID);
+ return this->findEntity(entityID.entityName);
+ }
+ const EntityT* findEntity(const MemoryID& entityID) const
+ {
+ detail::checkHasEntityName(entityID);
+ return this->findEntity(entityID.entityName);
}
- using Base::getEntity;
- const EntityT& getEntity(const MemoryID& id) const override
+ // Get child by MemoryID
+ EntityT& getEntity(const MemoryID& entityID)
+ {
+ detail::checkHasEntityName(entityID);
+ return this->getEntity(entityID.entityName);
+ }
+ const EntityT& getEntity(const MemoryID& entityID) const
{
- this->_checkContainerName(id.providerSegmentName, this->getKeyString());
- return getEntity(id.entityName);
+ detail::checkHasEntityName(entityID);
+ return this->getEntity(entityID.entityName);
}
- EntityT& getEntity(const std::string& name)
+ // get/findInstance are provided by GetFindInstanceMixin
+ // get/findSnapshot are provided by GetFindSnapshotMixin
+
+
+ // 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 const_cast<EntityT&>(const_cast<const ProviderSegmentBase*>(this)->getEntity(name));
+ 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);
}
- const EntityT& getEntity(const std::string& name) const
+ // forEachSnapshot() is provided by ForEachEntitySnapshotMixin
+ // forEachInstance() is provided by ForEachEntityInstanceMixin
+
+
+ // Get child keys
+ std::vector<std::string> getEntityNames() const
{
- auto it = this->_container.find(name);
- if (it != this->_container.end())
- {
- return it->second;
- }
- else
- {
- throw armem::error::MissingEntry::create<EntityT>(name, *this);
- }
+ return simox::alg::get_keys(this->_container);
}
- const EntityT* findEntity(const MemoryID& id) const override
+
+ [[deprecated("Direct container access is deprecated. Use forEach*() instead.")]]
+ inline const ContainerT& entities() const
{
- this->_checkContainerName(id.providerSegmentName, this->getKeyString());
- auto it = this->_container.find(id.entityName);
- if (it != this->_container.end())
- {
- return &it->second;
- }
- else
- {
- return nullptr;
- }
+ 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 +216,6 @@ namespace armarx::armem::base
{
// Add entity entry.
entity = &addEntity(update.entityID.entityName);
- entity->setMaxHistorySize(_maxHistorySize);
updateType = UpdateType::InsertedNew;
}
else
@@ -168,62 +229,54 @@ namespace armarx::armem::base
return ret;
}
- void append(const _Derived& m)
+ void append(const DerivedT& m)
{
- 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 this->_derived().addEntity(name, name);
}
+
/// Copy and insert an entity.
EntityT& addEntity(const EntityT& entity)
{
- return addEntity(EntityT(entity));
+ return this->_derived().addEntity(entity.name(), EntityT(entity));
}
+
/// Move and insert an entity.
EntityT& addEntity(EntityT&& entity)
{
- auto it = this->_container.emplace(entity.name(), std::move(entity)).first;
- it->second.id().setProviderSegmentID(this->id());
- return it->second;
+ const std::string name = entity.name(); // Copy before move.
+ return this->_derived().addEntity(name, std::move(entity));
}
-
- /**
- * @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
+ /// Insert an entity in-place.
+ template <class ...Args>
+ EntityT& addEntity(const std::string& name, Args... args)
{
- MaxHistorySize::setMaxHistorySize(maxSize);
- for (auto& [name, entity] : this->_container)
- {
- entity.setMaxHistorySize(maxSize);
- }
+ ChildT& child = this->template _addChild<ChildT>(name, args...);
+ child.id() = this->id().withEntityName(name);
+ return child;
}
- virtual bool equalsDeep(const ProviderSegmentBase& other) const
+ // MISC
+
+ bool equalsDeep(const DerivedT& other) const
{
- //std::cout << "ProviderSegment::equalsDeep" << std::endl;
if (this->size() != other.size())
{
return false;
@@ -243,30 +296,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.cpp b/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.cpp
deleted file mode 100644
index 05a23bc12fea34e175218498beca73e01b6f1b13..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "EntityContainerBase.h"
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h b/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h
deleted file mode 100644
index 95993d560c6435fd147ddfc7a2a7aa9267d94855..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h
+++ /dev/null
@@ -1,106 +0,0 @@
-#pragma once
-
-#include "../../Commit.h"
-#include "../../error/ArMemError.h"
-
-#include "MemoryContainerBase.h"
-
-#include "../EntityBase.h"
-#include "../EntitySnapshotBase.h"
-
-
-namespace armarx::armem::base::detail
-{
-
- /**
- * @brief A container of entities at some point in the hierarchy.
- *
- * Can be updated by multiple entity updates.
- */
- template <class _ValueT, class _EntityT, class _Derived>
- class EntityContainerBase :
- public MemoryContainerBase<std::map<std::string, _ValueT>, _Derived>
- {
- using Base = MemoryContainerBase<std::map<std::string, _ValueT>, _Derived>;
-
- public:
-
- using DerivedT = _Derived;
- using ValueT = _ValueT;
-
- using EntityT = _EntityT;
- using EntitySnapshotT = typename EntityT::EntitySnapshotT;
- using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
-
- public:
-
- using Base::MemoryContainerBase;
- using Base::operator=;
-
- /**
- * @brief Retrieve an entity.
- * @param id The entity ID.
- * @return The entity.
- * @throw An exception deriving from `armem::error::ArMemError` if the entity is missing.
- */
- EntityT& getEntity(const MemoryID& id)
- {
- return const_cast<EntityT&>(const_cast<const EntityContainerBase*>(this)->getEntity(id));
- }
- virtual const EntityT& getEntity(const MemoryID& id) const = 0;
-
- /**
- * @brief Find an entity.
- *
- * Search for the entity with the given ID and return a pointer to the
- * first match. If `id` is underspecified (e.g. no provider segment name),
- * search all children until the first match is found.
- *
- * If no matching entity is found, return `nullptr`.
- *
- * @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;
-
-
- /**
- * @brief Retrieve an entity snapshot.
- *
- * Uses `getEntity()` to retrieve the respective entity.
- *
- * @param id The snapshot ID.
- * @return The entity snapshot.
- * @throw An exception deriving from `armem::error::ArMemError` if the snapshot is missing.
- */
- EntitySnapshotT& getEntitySnapshot(const MemoryID& id)
- {
- return const_cast<EntitySnapshotT&>(const_cast<const EntityContainerBase*>(this)->getEntitySnapshot(id));
- }
-
- const EntitySnapshotT& getEntitySnapshot(const MemoryID& id) const
- {
- const EntityT& entity = getEntity(id);
-
- if (id.hasTimestamp())
- {
- return entity.getSnapshot(id);
- }
- else
- {
- return entity.getLatestSnapshot();
- }
- }
-
- EntityInstanceT& getEntityInstance(const MemoryID& id)
- {
- return getEntitySnapshot(id).getInstance(id);
- }
-
- const EntityInstanceT& getEntityInstance(const MemoryID& id) const
- {
- return getEntitySnapshot(id).getInstance(id);
- }
- };
-
-}
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 518e3f877f38ea364657c0a5448d48bc6cf36fbf..3e22caa31456c6a2afab30a721d064bb661427f5 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,25 +108,16 @@ namespace armarx::armem::base::detail
return _container;
}
- // Copying
- DerivedT copy() const
+ DerivedT& _derived()
{
- DerivedT d;
- this->_copySelf(d);
- return d;
+ return static_cast<DerivedT&>(*this);
}
-
- /// Make a copy not containing any elements.
- DerivedT copyEmpty() const
+ const DerivedT& _derived() const
{
- DerivedT d;
- this->_copySelfEmpty(d);
- return std::move(d);
+ return static_cast<DerivedT&>(*this);
}
- protected:
-
/**
* @throw `armem::error::ContainerNameMismatch` if `gottenName` does not match `actualName`.
*/
@@ -110,19 +126,22 @@ 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
+ template <class ChildT, class KeyT, class ...ChildArgs>
+ ChildT& _addChild(const KeyT& key, ChildArgs... childArgs)
{
- Base::_copySelf(other);
+ auto [it, inserted] = this->_container.try_emplace(key, childArgs...);
+ if (not inserted)
+ {
+ throw armem::error::ContainerEntryAlreadyExists(
+ ChildT::getLevelName(), key, DerivedT::getLevelName(), this->_derived().name());
+ }
+ return it->second;
}
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/derived.h b/source/RobotAPI/libraries/armem/core/base/detail/derived.h
new file mode 100644
index 0000000000000000000000000000000000000000..721becfa05e34e603f2c1bdb852cea191faf42a5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/detail/derived.h
@@ -0,0 +1,22 @@
+#pragma once
+
+
+namespace armarx::armem::base::detail
+{
+
+ template <class DerivedT, class ThisT>
+ DerivedT&
+ derived(ThisT* t)
+ {
+ return static_cast<DerivedT&>(*t);
+ }
+
+
+ template <class DerivedT, class ThisT>
+ const DerivedT&
+ derived(const ThisT* t)
+ {
+ return static_cast<const DerivedT&>(*t);
+ }
+
+}
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/lookup_mixins.cpp b/source/RobotAPI/libraries/armem/core/base/detail/lookup_mixins.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2b35f0b895cf6e4ef3e53a81a8af3ac0eb31a89d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/detail/lookup_mixins.cpp
@@ -0,0 +1,63 @@
+#include "lookup_mixins.h"
+
+#include <RobotAPI/libraries/armem/core/error/ArMemError.h>
+
+
+namespace armarx::armem::base
+{
+
+ void detail::checkHasInstanceIndex(const MemoryID& instanceID)
+ {
+ if (not instanceID.hasInstanceIndex())
+ {
+ throw armem::error::InvalidMemoryID(instanceID, "Instance ID has no instance index.");
+ }
+ }
+
+
+ void detail::checkHasTimestamp(const MemoryID& snapshotID)
+ {
+ if (not snapshotID.hasTimestamp())
+ {
+ throw armem::error::InvalidMemoryID(snapshotID, "Snapshot ID has no timestamp.");
+ }
+ }
+
+
+ void detail::checkHasEntityName(const MemoryID& entityID)
+ {
+ if (not entityID.hasEntityName())
+ {
+ throw armem::error::InvalidMemoryID(entityID, "Entity ID has no entity name.");
+ }
+ }
+
+
+ void detail::checkHasProviderSegmentName(const MemoryID& providerSegmentID)
+ {
+ if (not providerSegmentID.hasProviderSegmentName())
+ {
+ throw armem::error::InvalidMemoryID(providerSegmentID, "Provider segment ID has no provider segment name.");
+ }
+ }
+
+
+ void detail::checkHasCoreSegmentName(const MemoryID& coreSegmentID)
+ {
+ if (not coreSegmentID.hasCoreSegmentName())
+ {
+ throw armem::error::InvalidMemoryID(coreSegmentID, "Core segment ID has no core segment name.");
+ }
+ }
+
+
+ void detail::checkHasMemoryName(const MemoryID& memoryID)
+ {
+ if (not memoryID.hasMemoryName())
+ {
+ throw armem::error::InvalidMemoryID(memoryID, "Memory ID has no memory name.");
+ }
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/lookup_mixins.h b/source/RobotAPI/libraries/armem/core/base/detail/lookup_mixins.h
new file mode 100644
index 0000000000000000000000000000000000000000..006a5fbcef24dd6c3978d78546cd15dff03a72bc
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/detail/lookup_mixins.h
@@ -0,0 +1,274 @@
+#pragma once
+
+#include "derived.h"
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/error/ArMemError.h>
+
+
+namespace armarx::armem::base::detail
+{
+
+ void checkHasInstanceIndex(const MemoryID& instanceID);
+ void checkHasTimestamp(const MemoryID& snapshotID);
+ void checkHasEntityName(const MemoryID& entityID);
+ void checkHasProviderSegmentName(const MemoryID& providerSegmentID);
+ void checkHasCoreSegmentName(const MemoryID& coreSegmentID);
+ void checkHasMemoryName(const MemoryID& memory);
+
+
+ template <class KeyT, class ContainerT>
+ auto* findChildByKey(const KeyT& key, ContainerT&& container)
+ {
+ auto it = container.find(key);
+ return it != container.end() ? &it->second : nullptr;
+ }
+
+ template <class KeyT, class ContainerT, class ParentT, class KeyStringFn>
+ auto&
+ getChildByKey(
+ const KeyT& key,
+ ContainerT&& container,
+ const ParentT& parent,
+ KeyStringFn&& keyStringFn)
+ {
+ if (auto* child = findChildByKey(key, container))
+ {
+ return *child;
+ }
+ else
+ {
+ throw armem::error::MissingEntry::create<typename ParentT::ChildT>(keyStringFn(key), parent);
+ }
+ }
+ template <class KeyT, class ContainerT, class ParentT>
+ auto&
+ getChildByKey(
+ const KeyT& key,
+ ContainerT&& container,
+ const ParentT& parent)
+ {
+ return getChildByKey(key, container, parent, [](const KeyT & key)
+ {
+ return key;
+ });
+ }
+
+
+ template <class DerivedT>
+ struct GetFindInstanceMixin
+ {
+ // Relies on this->find/getSnapshot()
+
+ bool hasInstance(const MemoryID& instanceID) const
+ {
+ return derived<DerivedT>(this).findInstance(instanceID) != nullptr;
+ }
+
+ /**
+ * @brief Find an entity instance.
+ * @param id The instance ID.
+ * @return The instance or nullptr if it is missing.
+ */
+ auto*
+ findInstance(const MemoryID& instanceID)
+ {
+ auto* snapshot = derived<DerivedT>(this).findSnapshot(instanceID);
+ return snapshot ? snapshot->findInstance(instanceID) : nullptr;
+ }
+ const auto*
+ findInstance(const MemoryID& instanceID) const
+ {
+ const auto* snapshot = derived<DerivedT>(this).findSnapshot(instanceID);
+ return snapshot ? snapshot->findInstance(instanceID) : nullptr;
+ }
+
+ /**
+ * @brief Retrieve an entity instance.
+ * @param id The instance ID.
+ * @return The instance if it is found.
+ * @throw `armem::error::ArMemError` if it is missing.
+ */
+ auto&
+ getInstance(const MemoryID& instanceID)
+ {
+ return derived<DerivedT>(this).getSnapshot(instanceID).getInstance(instanceID);
+ }
+ const auto&
+ getInstance(const MemoryID& instanceID) const
+ {
+ return derived<DerivedT>(this).getSnapshot(instanceID).getInstance(instanceID);
+ }
+ };
+
+
+ template <class DerivedT>
+ struct GetFindSnapshotMixin
+ {
+ // Relies on this->find/getEntity()
+
+ bool hasSnapshot(const MemoryID& snapshotID) const
+ {
+ return derived<DerivedT>(this).findSnapshot(snapshotID) != nullptr;
+ }
+
+ /**
+ * @brief Find an entity snapshot.
+ * @param id The snapshot ID.
+ * @return The snapshot or nullptr if it is missing.
+ */
+ auto*
+ findSnapshot(const MemoryID& snapshotID)
+ {
+ auto* entity = derived<DerivedT>(this).findEntity(snapshotID);
+ return entity ? entity->findSnapshot(snapshotID) : nullptr;
+ }
+ const auto*
+ findSnapshot(const MemoryID& snapshotID) const
+ {
+ auto* entity = derived<DerivedT>(this).findEntity(snapshotID);
+ return entity ? entity->findSnapshot(snapshotID) : nullptr;
+ }
+
+ /**
+ * @brief Retrieve an entity snapshot.
+ * @param id The snapshot ID.
+ * @return The snapshot if it is found.
+ * @throw `armem::error::ArMemError` if it is missing.
+ */
+ auto&
+ getSnapshot(const MemoryID& snapshotID)
+ {
+ return derived<DerivedT>(this).getEntity(snapshotID).getSnapshot(snapshotID);
+ }
+ const auto&
+ getSnapshot(const MemoryID& snapshotID) const
+ {
+ return derived<DerivedT>(this).getEntity(snapshotID).getSnapshot(snapshotID);
+ }
+
+
+ // More elaborate cases
+
+ auto* findLatestSnapshot(const MemoryID& entityID)
+ {
+ auto* entity = derived<DerivedT>(this).findEntity(entityID);
+ return entity ? entity->findLatestSnapshot() : nullptr;
+ }
+ const auto* findLatestSnapshot(const MemoryID& entityID) const
+ {
+ auto* entity = derived<DerivedT>(this).findEntity(entityID);
+ return entity ? entity->findLatestSnapshot() : nullptr;
+ }
+
+ auto* findLatestInstance(const MemoryID& entityID, int instanceIndex = 0)
+ {
+ auto* snapshot = derived<DerivedT>(this).findLatestSnapshot(entityID);
+ return snapshot ? snapshot->findInstance(instanceIndex) : nullptr;
+ }
+ const auto* findLatestInstance(const MemoryID& entityID, int instanceIndex = 0) const
+ {
+ auto* snapshot = derived<DerivedT>(this).findLatestSnapshot(entityID);
+ return snapshot ? snapshot->findInstance(instanceIndex) : nullptr;
+ }
+
+ };
+
+
+
+ template <class DerivedT>
+ struct GetFindEntityMixin
+ {
+ // Relies on this->find/getProviderSegment()
+
+ bool hasEntity(const MemoryID& entityID) const
+ {
+ return derived<DerivedT>(this).findEntity(entityID) != nullptr;
+ }
+
+ /**
+ * @brief Find an entity.
+ * @param id The entity ID.
+ * @return The entity or nullptr if it is missing.
+ */
+ auto*
+ findEntity(const MemoryID& entityID)
+ {
+ auto* provSeg = derived<DerivedT>(this).findProviderSegment(entityID);
+ return provSeg ? provSeg->findEntity(entityID) : nullptr;
+ }
+ const auto*
+ findEntity(const MemoryID& entityID) const
+ {
+ auto* provSeg = derived<DerivedT>(this).findProviderSegment(entityID);
+ return provSeg ? provSeg->findEntity(entityID) : nullptr;
+ }
+
+ /**
+ * @brief Retrieve an entity.
+ * @param id The entity ID.
+ * @return The entity if it is found.
+ * @throw `armem::error::ArMemError` if it is missing.
+ */
+ auto&
+ getEntity(const MemoryID& entityID)
+ {
+ return derived<DerivedT>(this).getProviderSegment(entityID).getEntity(entityID);
+ }
+ const auto&
+ getEntity(const MemoryID& entityID) const
+ {
+ return derived<DerivedT>(this).getProviderSegment(entityID).getEntity(entityID);
+ }
+ };
+
+
+
+ template <class DerivedT>
+ struct GetFindProviderSegmentMixin
+ {
+ // Relies on this->find/getCoreSegment()
+
+ bool hasProviderSegment(const MemoryID& providerSegmentID) const
+ {
+ return derived<DerivedT>(this).findProviderSegment(providerSegmentID) != nullptr;
+ }
+
+ /**
+ * @brief Retrieve a provider segment.
+ * @param id The provider segment ID.
+ * @return The provider segment if it is found or nullptr if it missing.
+ */
+ auto*
+ findProviderSegment(const MemoryID& providerSegmentID)
+ {
+ auto* provSeg = derived<DerivedT>(this).findCoreSegment(providerSegmentID);
+ return provSeg ? provSeg->findProviderSegment(providerSegmentID) : nullptr;
+ }
+ const auto*
+ findProviderSegment(const MemoryID& providerSegmentID) const
+ {
+ auto* provSeg = derived<DerivedT>(this).findCoreSegment(providerSegmentID);
+ return provSeg ? provSeg->findProviderSegment(providerSegmentID) : nullptr;
+ }
+
+ /**
+ * @brief Retrieve a provider segment.
+ * @param id The provider segment ID.
+ * @return The provider segment if it is found.
+ * @throw `armem::error::ArMemError` if it is missing.
+ */
+ auto&
+ getProviderSegment(const MemoryID& providerSegmentID)
+ {
+ return derived<DerivedT>(this).getCoreSegment(providerSegmentID).getProviderSegment(providerSegmentID);
+ }
+ const auto&
+ getProviderSegment(const MemoryID& providerSegmentID) const
+ {
+ return derived<DerivedT>(this).getCoreSegment(providerSegmentID).getProviderSegment(providerSegmentID);
+ }
+
+ };
+
+}
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..182e0c0a47aff1a75b356b9cc0b3930450de75e4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/ice_conversions.h
@@ -0,0 +1,190 @@
+#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);
+
+ 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);
+
+ 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);
+
+ ice.providerSegments.clear();
+ coreSegment.forEachProviderSegment([&ice](const auto & providerSegment)
+ {
+ armem::toIce(ice.providerSegments[providerSegment.name()], providerSegment);
+ });
+ }
+ 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);
+ });
+ }
+ 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
deleted file mode 100644
index b166468d81929da12c8204a6e2df9ab90b994528..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp
+++ /dev/null
@@ -1,105 +0,0 @@
-#include "CoreSegment.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "TypeIO.h"
-
-
-namespace armarx::armem::d_ltm
-{
-
- std::filesystem::path CoreSegment::_fullPath() const
- {
- if (path)
- {
- return _fullPath(*path);
- }
- return std::filesystem::path();
- }
-
- std::filesystem::path CoreSegment::_fullPath(const std::filesystem::path& path) const
- {
- return path / id().memoryName / id().coreSegmentName;
- }
-
- wm::CoreSegment CoreSegment::convert() const
- {
- wm::CoreSegment m(id());
- for (const auto& [_, s] : _container)
- {
- m.addProviderSegment(s.convert(_aronType));
- }
- return m;
- }
-
- void CoreSegment::reload(const std::shared_ptr<std::filesystem::path>& p_ptr)
- {
- if (!p_ptr)
- {
- ARMARX_WARNING << "The entered is NULL.";
- }
- std::filesystem::path p = _fullPath(*p_ptr);
- if (std::filesystem::is_regular_file(p))
- {
- ARMARX_ERROR << "The entered path is leading to a file! Abort due to error.";
- }
-
- _container.clear();
- path = p_ptr;
-
- if (!std::filesystem::exists(p))
- {
- ARMARX_INFO << "The entered path does not exist. Assuming an empty container.";
- }
- else
- {
- for (const auto& d : std::filesystem::directory_iterator(p))
- {
- if (d.is_directory())
- {
- std::string k = d.path().filename();
- auto wms = _container.emplace(k, id().withProviderSegmentName(k));
- wms.first->second.reload(p_ptr);
- }
-
- if (d.is_regular_file())
- {
- if (auto type = TypeIO::readAronType(d.path()))
- {
- _aronType = type;
- }
- }
- }
- }
- }
-
- void CoreSegment::append(const wm::CoreSegment& m)
- {
- std::filesystem::create_directories(_fullPath());
- TypeIO::writeAronType(_aronType, _fullPath());
-
- for (const auto& [k, s] : m)
- {
- if (const auto& it = _container.find(k); it != _container.end())
- {
- it->second.append(s);
- }
- else
- {
- try
- {
- std::filesystem::create_directory(_fullPath() / k);
- }
- catch (...)
- {
- ARMARX_WARNING << GetHandledExceptionString();
- return;
- }
-
- auto wms = _container.emplace(k, id().withProviderSegmentName(k));
- wms.first->second.path = path;
- wms.first->second.append(s);
- }
- }
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.h b/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.h
deleted file mode 100644
index 76bb297e76d268ce8517449686ffae1cdb3ed2cd..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.h
+++ /dev/null
@@ -1,43 +0,0 @@
-#pragma once
-
-#include <filesystem>
-
-#include "../base/CoreSegmentBase.h"
-#include "../workingmemory/CoreSegment.h"
-
-#include "ProviderSegment.h"
-
-
-namespace armarx::armem::d_ltm
-{
-
- /**
- * @brief Data of a core segment containing multiple provider segments.
- */
- class CoreSegment :
- public base::CoreSegmentBase<ProviderSegment, CoreSegment>
- {
- using Base = base::CoreSegmentBase<ProviderSegment, CoreSegment>;
-
- public:
-
- using Base::CoreSegmentBase;
- using Base::operator=;
-
-
- // Conversion
- wm::CoreSegment convert() const;
-
- // Filesystem connection
- void reload(const std::shared_ptr<std::filesystem::path>&);
- void append(const wm::CoreSegment&);
-
- private:
- std::filesystem::path _fullPath() const;
- std::filesystem::path _fullPath(const std::filesystem::path&) const;
-
- public:
- std::shared_ptr<std::filesystem::path> path;
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/Entity.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/Entity.cpp
deleted file mode 100644
index 5d784a097de4ef68ec2df0bdc1adce94fa737809..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/diskmemory/Entity.cpp
+++ /dev/null
@@ -1,82 +0,0 @@
-#include "Entity.h"
-
-namespace armarx::armem::d_ltm
-{
- std::filesystem::path Entity::_fullPath() const
- {
- if (path)
- {
- return _fullPath(*path);
- }
- return std::filesystem::path();
- }
-
- std::filesystem::path Entity::_fullPath(const std::filesystem::path& path) const
- {
- return path / id().memoryName / id().coreSegmentName / id().providerSegmentName / id().entityName;
- }
-
- wm::Entity Entity::convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const
- {
- wm::Entity m(id());
- for (const auto& [_, s] : _container)
- {
- m.addSnapshot(s.convert(expectedStructure));
- }
- return m;
- }
-
- void Entity::reload(const std::shared_ptr<std::filesystem::path>& p_ptr)
- {
- if (!p_ptr)
- {
- ARMARX_WARNING << "The entered is NULL.";
- }
- std::filesystem::path p = _fullPath(*p_ptr);
- if (std::filesystem::is_regular_file(p))
- {
- ARMARX_ERROR << "The entered path is leading to a file! Abort due to error.";
- }
-
- _container.clear();
- path = p_ptr;
-
- if (!std::filesystem::exists(p))
- {
- ARMARX_INFO << "The entered path does not exist. Assuming an empty container.";
- }
- else
- {
- for (const auto& d : std::filesystem::directory_iterator(p))
- {
- if (d.is_directory())
- {
- std::string k = d.path().filename();
- armem::Time t = armem::Time::microSeconds(std::stol(k));
- auto wms = _container.emplace(std::make_pair(t, id().withTimestamp(t)));
- wms.first->second.reload(p_ptr);
- }
- }
- }
- }
-
- void Entity::append(const wm::Entity& m)
- {
- std::filesystem::create_directories(_fullPath());
- for (const auto& [k, s] : m.container())
- {
- if (const auto& it = _container.find(k); 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)));
- wms.first->second.path = path;
- wms.first->second.setTo(s);
- }
- }
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/Entity.h b/source/RobotAPI/libraries/armem/core/diskmemory/Entity.h
deleted file mode 100644
index 35022275570c93378cbe0c767aad1776da594838..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/diskmemory/Entity.h
+++ /dev/null
@@ -1,59 +0,0 @@
-#pragma once
-
-#include <filesystem>
-
-#include "../base/EntityBase.h"
-#include "../workingmemory/Entity.h"
-
-#include "EntitySnapshot.h"
-
-
-
-namespace armarx::armem::d_ltm
-{
- /**
- * @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>
- {
- using Base = base::EntityBase<EntitySnapshot, Entity>;
-
- public:
-
- using Base::EntityBase;
- using Base::operator=;
-
-
- // Conversion
- wm::Entity convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const;
-
- // Filesystem connection
- void reload(const std::shared_ptr<std::filesystem::path>&);
- void append(const wm::Entity&);
-
- private:
- std::filesystem::path _fullPath() const;
- std::filesystem::path _fullPath(const std::filesystem::path&) const;
-
- public:
- std::shared_ptr<std::filesystem::path> path;
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.cpp
deleted file mode 100644
index dd0458b83bbb8ed461ddeab50af42181fda843e4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.cpp
+++ /dev/null
@@ -1,134 +0,0 @@
-#include "EntityInstance.h"
-
-#include <iostream>
-#include <fstream>
-
-#include "../../core/error.h"
-#include "ArmarXCore/core/exceptions/LocalException.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-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();
- }
-
-
-
- void EntityInstance::update(const EntityUpdate& update, int index)
- {
- ARMARX_CHECK_FITS_SIZE(index, update.instancesData.size());
-
- this->index() = index;
- }
-
- EntityInstance EntityInstance::copy() const
- {
- EntityInstance d;
- this->_copySelf(d);
- return d;
- }
-
- void EntityInstance::_copySelf(EntityInstance& other) const
- {
- EntityInstanceBase<EntityInstance>::_copySelf(other);
- other.path = path;
- }
-
- std::filesystem::path EntityInstance::_fullPath() const
- {
- if (path)
- {
- return _fullPath(*path);
- }
- ARMARX_WARNING << "The path of the disk memory instance with id '" << id().str() << "' is not set. This may lead to errors.";
- return std::filesystem::path();
- }
-
- std::filesystem::path EntityInstance::_fullPath(const std::filesystem::path& path) const
- {
- return path / id().memoryName / id().coreSegmentName / id().providerSegmentName / id().entityName / std::to_string(id().timestamp.toMicroSeconds()) / std::to_string(id().instanceIndex);
- }
-
- wm::EntityInstance EntityInstance::convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const
- {
- std::filesystem::path p = _fullPath(); // here we assume that "reload" has been called first
- std::filesystem::path d = p / (std::string(DATA_FILENAME) + ".json");
-
- if (std::filesystem::is_regular_file(d))
- {
- std::ifstream ifs(d);
- std::string file_content((std::istreambuf_iterator<char>(ifs)), (std::istreambuf_iterator<char>()));
-
- nlohmann::json j = nlohmann::json::parse(file_content);
- auto aron = std::make_shared<aron::datanavigator::DictNavigator>();
- to_aron(aron, j, expectedStructure);
- wm::EntityInstance e(id());
- from_aron(aron, e);
- return e;
- }
- else
- {
- throw error::ArMemError("An diskMemory EntityInstance is not leading to a regular file. The path was: " + d.string());
- }
- }
-
- void EntityInstance::reload(const std::shared_ptr<std::filesystem::path>& p_ptr)
- {
- if (!p_ptr)
- {
- ARMARX_WARNING << "The entered path is NULL.";
- }
- std::filesystem::path p = _fullPath(*p_ptr);
- if (!std::filesystem::is_directory(p))
- {
- ARMARX_ERROR << "The entered path is not leading to a file! This is an error since if the folder for an EntityInstance exists there must be a data file in it (containing at least the metadata).";
- }
- else
- {
- path = p_ptr;
- }
- }
-
- void EntityInstance::setTo(const wm::EntityInstance& m)
- {
- std::filesystem::path p = _fullPath();
-
- try
- {
- std::filesystem::create_directories(p);
- }
- catch (...)
- {
- ARMARX_WARNING << GetHandledExceptionString();
- return;
- }
-
- std::filesystem::path d = p / (std::string(DATA_FILENAME) + ".json");
-
- if (std::filesystem::is_regular_file(d))
- {
- std::filesystem::remove(d);
- }
-
- std::ofstream ofs;
- ofs.open(d);
-
- auto aron = std::make_shared<aron::datanavigator::DictNavigator>();
- to_aron(aron, m);
- nlohmann::json j;
- from_aron(aron, j);
-
- ofs << j.dump(2);
- ofs.close();
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.h b/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.h
deleted file mode 100644
index 89b96c011fcccb6bfc5471850185b0cea49f77b0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.h
+++ /dev/null
@@ -1,63 +0,0 @@
-#pragma once
-
-#include <filesystem>
-
-#include "../base/EntityInstanceBase.h"
-#include "../workingmemory/EntityInstance.h"
-
-#include "../workingmemory/entityInstance_conversions.h"
-#include "../workingmemory/json_conversions.h"
-
-
-namespace armarx::armem::d_ltm
-{
- /**
- * @brief Data of a single entity instance.
- */
- class EntityInstance :
- public base::EntityInstanceBase<EntityInstance>
- {
- using Base = base::EntityInstanceBase<EntityInstance>;
-
- 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;
-
- virtual bool equalsDeep(const EntityInstance& other) const override;
-
- virtual EntityInstance copy() const override;
-
- // Conversion
- wm::EntityInstance convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const;
-
- // Filesystem connection
- 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;
- std::filesystem::path _fullPath(const std::filesystem::path&) const;
-
- public:
- std::shared_ptr<std::filesystem::path> path;
-
- private:
- static const constexpr char* DATA_FILENAME = "data";
- };
-}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.cpp
deleted file mode 100644
index ab5b56d2274f78c350818693c269af18f0a4a979..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.cpp
+++ /dev/null
@@ -1,103 +0,0 @@
-#include "EntitySnapshot.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "error.h"
-
-
-namespace armarx::armem::d_ltm
-{
-
- std::filesystem::path EntitySnapshot::_fullPath() const
- {
- if (path)
- {
- return _fullPath(*path);
- }
- return std::filesystem::path();
- }
-
- std::filesystem::path EntitySnapshot::_fullPath(const std::filesystem::path& path) const
- {
- return path / id().memoryName / id().coreSegmentName / id().providerSegmentName
- / id().entityName
- / std::to_string(id().timestamp.toMicroSeconds());
- }
-
- wm::EntitySnapshot EntitySnapshot::convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const
- {
- wm::EntitySnapshot m(id());
- for (const auto& s : _container)
- {
- m.addInstance(s.convert(expectedStructure));
- }
- return m;
- }
-
- void EntitySnapshot::reload(const std::shared_ptr<std::filesystem::path>& p_ptr)
- {
- if (!p_ptr)
- {
- ARMARX_WARNING << "The entered is NULL.";
- }
- std::filesystem::path p = _fullPath(*p_ptr);
- if (!std::filesystem::is_directory(p))
- {
- ARMARX_ERROR << "The entered path is not leading to a directory! Every EntitySnapshot must at least contain one EntityInstance.";
- }
- else
- {
- _container.clear();
- path = p_ptr;
-
- // todo
- for (int i = 0; i < 1000; ++i)
- {
- std::filesystem::path d = p / std::to_string(i);
- if (std::filesystem::is_directory(d))
- {
- auto& wms = _container.emplace_back(id().withInstanceIndex(i));
- wms.reload(p_ptr);
- }
- else
- {
- break;
- }
- }
- }
- }
-
- void EntitySnapshot::setTo(const wm::EntitySnapshot& m)
- {
- try
- {
- std::filesystem::create_directories(_fullPath());
- }
- catch (...)
- {
- ARMARX_WARNING << GetHandledExceptionString();
- return;
- }
-
- // We remove the content here and reset it with new values
- _container.clear();
-
- int i = 0;
- for (const auto& s : m.instances())
- {
- try
- {
- std::filesystem::create_directory(_fullPath() / std::to_string(i));
- }
- catch (...)
- {
- ARMARX_WARNING << GetHandledExceptionString();
- continue;;
- }
-
- auto& wms = _container.emplace_back(id().withInstanceIndex(i++));
- wms.path = path;
- wms.setTo(s);
- }
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.h b/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.h
deleted file mode 100644
index 545a8c0d64641c65e7faf7aa26dea77ee9865e0c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.h
+++ /dev/null
@@ -1,42 +0,0 @@
-#pragma once
-
-#include <filesystem>
-
-#include "../base/EntitySnapshotBase.h"
-#include "../workingmemory/EntitySnapshot.h"
-
-#include "EntityInstance.h"
-
-
-namespace armarx::armem::d_ltm
-{
-
- /**
- * @brief Data of an entity at one point in time.
- */
- class EntitySnapshot :
- public base::EntitySnapshotBase<EntityInstance, EntitySnapshot>
- {
- using Base = base::EntitySnapshotBase<EntityInstance, EntitySnapshot>;
-
- public:
-
- using Base::EntitySnapshotBase;
- using Base::operator=;
-
-
- // Conversion
- void reload(const std::shared_ptr<std::filesystem::path>&);
- wm::EntitySnapshot convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const;
-
- // FS connection
- void setTo(const wm::EntitySnapshot&);
-
- private:
- std::filesystem::path _fullPath() const;
- std::filesystem::path _fullPath(const std::filesystem::path&) const;
-
- public:
- std::shared_ptr<std::filesystem::path> path;
- };
-}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp
deleted file mode 100644
index 0fb6fba92b2ebc116787bffba2d7ed5bc743227b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp
+++ /dev/null
@@ -1,92 +0,0 @@
-#include "Memory.h"
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "error.h"
-
-
-namespace armarx::armem::d_ltm
-{
- std::filesystem::path Memory::_fullPath() const
- {
- if (path)
- {
- return _fullPath(*path);
- }
- return std::filesystem::path();
- }
-
- std::filesystem::path Memory::_fullPath(const std::filesystem::path& path) const
- {
- return path / id().memoryName;
- }
-
- wm::Memory Memory::convert() const
- {
- wm::Memory m(id());
- for (const auto& [k, s] : _container)
- {
- m.addCoreSegment(s.convert());
- }
- return m;
- }
-
- void Memory::reload(const std::filesystem::path& p)
- {
- if (std::filesystem::is_regular_file(p))
- {
- ARMARX_ERROR << "The entered path is leading to a file! Abort due to error.";
- }
-
- _container.clear();
- path = std::make_shared<std::filesystem::path>(p.parent_path());
-
- if (!std::filesystem::exists(p))
- {
- ARMARX_INFO << "The entered path does not exist. Returning empty memory.";
- }
- else
- {
- id() = MemoryID().withMemoryName(p.filename());
-
- for (const auto& d : std::filesystem::directory_iterator(p))
- {
- if (d.is_directory())
- {
- std::string k = d.path().filename();
- auto wms = _container.emplace(k, id().withCoreSegmentName(k));
- wms.first->second.reload(path);
- }
- }
- }
- }
-
- void Memory::append(const wm::Memory& m)
- {
- std::filesystem::create_directories(_fullPath());
- for (const auto& [k, s] : m.container())
- {
- if (const auto& it = _container.find(k); it != _container.end())
- {
- it->second.append(s);
- }
- else
- {
- try
- {
- std::filesystem::create_directory(_fullPath() / k);
- }
- catch (...)
- {
- ARMARX_WARNING << GetHandledExceptionString();
- return;
- }
-
- auto wms = _container.emplace(k, id().withCoreSegmentName(k));
- wms.first->second.path = path;
- wms.first->second.append(s);
- }
- }
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/Memory.h b/source/RobotAPI/libraries/armem/core/diskmemory/Memory.h
deleted file mode 100644
index 7f7872f7a638cc31122f09b00a95755ede4d9606..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/diskmemory/Memory.h
+++ /dev/null
@@ -1,42 +0,0 @@
-#pragma once
-
-#include <filesystem>
-
-#include "../base/MemoryBase.h"
-#include "../workingmemory/Memory.h"
-
-#include "CoreSegment.h"
-
-
-namespace armarx::armem::d_ltm
-{
-
- /**
- * @brief Data of a memory consisting of multiple core segments.
- */
- class Memory :
- public base::MemoryBase<CoreSegment, Memory>
- {
- using Base = base::MemoryBase<CoreSegment, Memory>;
-
- public:
-
- using Base::MemoryBase;
- using Base::operator=;
-
-
- // Conversion
- wm::Memory convert() const;
-
- // Filesystem connection
- void reload(const std::filesystem::path&);
- void append(const wm::Memory&);
-
- private:
- std::filesystem::path _fullPath() const;
- std::filesystem::path _fullPath(const std::filesystem::path&) const;
-
- public:
- std::shared_ptr<std::filesystem::path> path;
- };
-}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp
deleted file mode 100644
index 620e819f6b1fc05d4aeadc285edb087f5ea0e119..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp
+++ /dev/null
@@ -1,123 +0,0 @@
-#include "ProviderSegment.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "TypeIO.h"
-
-
-namespace armarx::armem::d_ltm
-{
- std::filesystem::path ProviderSegment::_fullPath() const
- {
- if (path)
- {
- return _fullPath(*path);
- }
- return std::filesystem::path();
- }
-
- std::filesystem::path ProviderSegment::_fullPath(const std::filesystem::path& path) const
- {
- return path / id().memoryName / id().coreSegmentName / id().providerSegmentName;
- }
-
- wm::ProviderSegment ProviderSegment::convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const
- {
- wm::ProviderSegment m(id());
- for (const auto& [_, s] : _container)
- {
- if (hasAronType())
- {
- m.addEntity(s.convert(_aronType));
- }
- else
- {
- m.addEntity(s.convert(expectedStructure));
- }
- }
- return m;
- }
-
- void ProviderSegment::reload(const std::shared_ptr<std::filesystem::path>& p_ptr)
- {
- if (!p_ptr)
- {
- ARMARX_WARNING << "The entered is NULL.";
- }
- std::filesystem::path p = _fullPath(*p_ptr);
- if (std::filesystem::is_regular_file(p))
- {
- ARMARX_ERROR << "The entered path is leading to a file! Abort due to error.";
- }
-
- _container.clear();
- path = p_ptr;
-
- if (!std::filesystem::exists(p))
- {
- ARMARX_INFO << "The entered path does not exist. Assuming an empty container.";
- }
- else
- {
- for (const auto& d : std::filesystem::directory_iterator(p))
- {
- if (d.is_directory())
- {
- std::string k = d.path().filename();
- auto wms = _container.emplace(k, id().withEntityName(k));
- wms.first->second.reload(p_ptr);
- }
-
- if (d.is_regular_file())
- {
- if (auto type = TypeIO::readAronType(d.path()))
- {
- _aronType = type;
- }
- }
- }
- }
- }
-
- void ProviderSegment::append(const wm::ProviderSegment& m)
- {
-
- try
- {
- std::filesystem::create_directories(_fullPath());
-
- }
- catch (...)
- {
- ARMARX_WARNING << GetHandledExceptionString();
- return;
- }
-
- TypeIO::writeAronType(_aronType, _fullPath());
-
- for (const auto& [k, s] : m.container())
- {
- if (const auto& it = _container.find(k); it != _container.end())
- {
- it->second.append(s);
- }
- else
- {
-
- try
- {
- std::filesystem::create_directory(_fullPath() / k);
- }
- catch (...)
- {
- ARMARX_WARNING << GetHandledExceptionString();
- return;
- }
-
- auto wms = _container.emplace(k, id().withEntityName(k));
- wms.first->second.path = path;
- wms.first->second.append(s);
- }
- }
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.h b/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.h
deleted file mode 100644
index 8ad96ea0630af0d5aba8b7270fe229c235c2a010..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.h
+++ /dev/null
@@ -1,43 +0,0 @@
-#pragma once
-
-#include <filesystem>
-
-#include "../base/ProviderSegmentBase.h"
-#include "../workingmemory/ProviderSegment.h"
-
-#include "Entity.h"
-
-
-namespace armarx::armem::d_ltm
-{
-
- /**
- * @brief Data of a provider segment containing multiple entities.
- */
- class ProviderSegment :
- public base::ProviderSegmentBase<Entity, ProviderSegment>
- {
- using Base = base::ProviderSegmentBase<Entity, ProviderSegment>;
-
- public:
-
- using Base::ProviderSegmentBase;
- using Base::operator=;
-
-
- // Conversion
- wm::ProviderSegment convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const;
-
- // Filesystem connection
- void reload(const std::shared_ptr<std::filesystem::path>&);
- void append(const wm::ProviderSegment&);
-
- private:
- std::filesystem::path _fullPath() const;
- std::filesystem::path _fullPath(const std::filesystem::path&) const;
-
- public:
- std::shared_ptr<std::filesystem::path> path;
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/TypeIO.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/TypeIO.cpp
deleted file mode 100644
index dc7f7a83706c7b9f69cc05a6e81bcf810c10a249..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/diskmemory/TypeIO.cpp
+++ /dev/null
@@ -1,71 +0,0 @@
-#include "TypeIO.h"
-
-#include <iostream>
-#include <fstream>
-#include <filesystem>
-
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
-
-
-namespace armarx::armem::d_ltm
-{
-
- aron::typenavigator::ObjectNavigatorPtr TypeIO::unwrapType(const aron::typenavigator::ObjectNavigatorPtr& type)
- {
- return aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(type->getMemberType(TYPE_WRAPPER_DATA_FIELD));
- }
-
- aron::typenavigator::ObjectNavigatorPtr TypeIO::wrapType(const aron::typenavigator::ObjectNavigatorPtr& type)
- {
- aron::typenavigator::ObjectNavigatorPtr typeWrapped(new aron::typenavigator::ObjectNavigator());
- typeWrapped->setObjectName(type->getObjectName() + "__ltm_type_export");
- typeWrapped->addMemberType(TYPE_WRAPPER_DATA_FIELD, type);
-
- typeWrapped->addMemberType(TYPE_WRAPPER_TIME_STORED_FIELD, std::make_shared<aron::typenavigator::LongNavigator>());
- typeWrapped->addMemberType(TYPE_WRAPPER_TIME_CREATED_FIELD, std::make_shared<aron::typenavigator::LongNavigator>());
- typeWrapped->addMemberType(TYPE_WRAPPER_TIME_SENT_FIELD, std::make_shared<aron::typenavigator::LongNavigator>());
- typeWrapped->addMemberType(TYPE_WRAPPER_TIME_ARRIVED_FIELD, std::make_shared<aron::typenavigator::LongNavigator>());
- typeWrapped->addMemberType(TYPE_WRAPPER_CONFIDENCE_FIELD, std::make_shared<aron::typenavigator::DoubleNavigator>());
-
- return typeWrapped;
- }
-
- aron::typenavigator::ObjectNavigatorPtr TypeIO::readAronType(const std::filesystem::__cxx11::path& filepath)
- {
- if (std::filesystem::is_regular_file(filepath))
- {
- if (filepath.filename() == (std::string(TYPE_FILENAME) + ".json"))
- {
- std::ifstream ifs(filepath);
- std::string file_content((std::istreambuf_iterator<char>(ifs)), (std::istreambuf_iterator<char>()));
-
- aron::typeIO::reader::NlohmannJSONReader typeReader(file_content);
- aron::typeIO::writer::NavigatorWriter navWriter;
- aron::typeIO::Converter::ReadAndConvert(typeReader, navWriter);
- return aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(navWriter.getResult());
- }
- }
- return nullptr;
- }
-
- void TypeIO::writeAronType(const aron::typenavigator::ObjectNavigatorPtr& type, const std::filesystem::__cxx11::path& filepath)
- {
- if (type)
- {
- std::ofstream ofs(filepath);
-
- aron::typeIO::writer::NlohmannJSONWriter typeWriter;
- aron::typeIO::Visitor::VisitAndSetup(typeWriter, type);
- std::string new_file_full_content = typeWriter.getResult().dump(2);
-
- ofs << new_file_full_content;
- }
- }
-
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/TypeIO.h b/source/RobotAPI/libraries/armem/core/diskmemory/TypeIO.h
deleted file mode 100644
index 4311bdcd7034a08b30b7c7f6ae9ee3eab0952fac..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/diskmemory/TypeIO.h
+++ /dev/null
@@ -1,37 +0,0 @@
-#pragma once
-
-#include <filesystem>
-
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
-
-
-namespace armarx::armem::d_ltm
-{
-
- /**
- * @brief An entity container with a specific (Aron) type.
- */
- class TypeIO
- {
- public:
-
- static aron::typenavigator::ObjectNavigatorPtr unwrapType(const aron::typenavigator::ObjectNavigatorPtr& type);
- static aron::typenavigator::ObjectNavigatorPtr wrapType(const aron::typenavigator::ObjectNavigatorPtr& type);
-
- static aron::typenavigator::ObjectNavigatorPtr readAronType(const std::filesystem::path& filepath);
- static void writeAronType(const aron::typenavigator::ObjectNavigatorPtr& type, const std::filesystem::path& filepath);
-
-
- private:
-
- static const constexpr char* TYPE_FILENAME = "type";
- static constexpr const char* TYPE_WRAPPER_DATA_FIELD = "__ARON_DATA";
- static constexpr const char* TYPE_WRAPPER_TIME_STORED_FIELD = "__WRITER_METADATA__TIME_STORED";
- static constexpr const char* TYPE_WRAPPER_TIME_CREATED_FIELD = "__ENTITY_METADATA__TIME_CREATED";
- static constexpr const char* TYPE_WRAPPER_TIME_SENT_FIELD = "__ENTITY_METADATA__TIME_SENT";
- static constexpr const char* TYPE_WRAPPER_TIME_ARRIVED_FIELD = "__ENTITY_METADATA__TIME_ARRIVED";
- static constexpr const char* TYPE_WRAPPER_CONFIDENCE_FIELD = "__ENTITY_METADATA__CONFIDENCE";
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
index 8936fab6fe953bc3b7fee870a97ffee598234ea7..003ef9fb5496b92814ad68cd9904987498460c46 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
@@ -59,7 +59,7 @@ namespace armarx::armem::error
{
std::stringstream ss;
ss << simox::alg::capitalize_words(existingTerm) << " with name '" << existingName << "' "
- << " already exists in " << ownTerm << " '" << ownName << "'.";
+ << "already exists in " << ownTerm << " '" << ownName << "'.";
return ss.str();
}
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..04cfbfae26533af158ad52c6c39fd49c837f1afb
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/forward_declarations.h
@@ -0,0 +1,64 @@
+#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::arondto
+{
+ class MemoryID;
+}
+
+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_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
deleted file mode 100644
index d47dc46cb63c5266edc8b9e8457c1fb4b75a4527..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
+++ /dev/null
@@ -1,85 +0,0 @@
-#include "CoreSegment.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "error.h"
-
-
-namespace armarx::armem::ltm
-{
-
- wm::CoreSegment CoreSegment::convert() const
- {
- wm::CoreSegment m(id());
- for (const auto& [_, s] : _container)
- {
- m.addProviderSegment(s.convert());
- }
- return m;
- }
-
- void CoreSegment::reload()
- {
- _container.clear();
-
- mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
- mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[id().str()];
-
-
- mongocxx::cursor cursor = coll.find({});
- for (auto doc : cursor)
- {
- 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"));
- if (i.coreSegmentName != id().coreSegmentName)
- {
- throw error::InvalidMemoryID(i, "A MemoryID in mongodb was invalid. Found the wrong coreSegment name. Expected " + id().coreSegmentName);
- }
-
- std::string k = i.providerSegmentName;
-
- if (const auto& it = _container.find(k); it != _container.end())
- {
- throw error::ArMemError("Somehow after clearing the (core) container a key k = " + k + " was found. Do you have double entries in mongodb?");
- }
- else
- {
- auto wms = _container.emplace(k, id().withProviderSegmentName(k));
- wms.first->second.dbsettings = dbsettings;
- wms.first->second.reload();
- }
- }
-
- ARMARX_INFO << "After reload has core segment " << id().str() << " size: " << _container.size();
- }
-
- void CoreSegment::append(const wm::CoreSegment& m)
- {
- mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
- mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[id().str()];
-
- for (const auto& [k, s] : m.container())
- {
- if (const auto& it = _container.find(k); it != _container.end())
- {
- it->second.append(s);
- }
- else
- {
- auto builder = bsoncxx::builder::stream::document{};
- bsoncxx::document::value foreign_key = builder
- << "foreign_key" << s.id().withProviderSegmentName(k).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);
- }
- }
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h
deleted file mode 100644
index 3a7225058a265949de50e98a63d1d3c6ce5bf371..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h
+++ /dev/null
@@ -1,51 +0,0 @@
-#pragma once
-
-#include "../base/CoreSegmentBase.h"
-
-#include "ProviderSegment.h"
-
-#include "../workingmemory/CoreSegment.h"
-
-
-namespace armarx::armem::ltm
-{
-
- /**
- * @brief Data of a core segment containing multiple provider segments.
- */
- class CoreSegment :
- public base::CoreSegmentBase<ProviderSegment, CoreSegment>
- {
- using Base = base::CoreSegmentBase<ProviderSegment, CoreSegment>;
-
- public:
-
- using Base::CoreSegmentBase;
- using Base::operator=;
-
-
- // Conversion
- wm::CoreSegment convert() const;
-
- // MongoDB connection
- 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
deleted file mode 100644
index bccb79c9d1d162a6c67a4f80c57b58b04425863e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
+++ /dev/null
@@ -1,150 +0,0 @@
-#include "Entity.h"
-
-
-namespace armarx::armem::ltm
-{
-
- wm::Entity Entity::convert() const
- {
- wm::Entity m(id());
- for (const auto& [_, s] : _container)
- {
- m.addSnapshot(s.convert());
- }
- return m;
- }
-
- void Entity::reload()
- {
- _container.clear();
-
- mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
- mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[id().str()];
-
- mongocxx::cursor cursor = coll.find({});
- 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())
- {
- 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
- {
- auto wms = _container.emplace(std::make_pair(k, id().withTimestamp(k)));
- wms.first->second.dbsettings = dbsettings;
- wms.first->second.reload();
- }
- ++i;
- }
-
- ARMARX_INFO << "After reload has entity " << id().str() << " size: " << _container.size();
- }
-
- void Entity::append(const wm::Entity& m)
- {
- mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
- mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[id().str()];
-
- for (const auto& [k, s] : m.container())
- {
- if (const auto& it = _container.find(k); 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)));
- wms.first->second.dbsettings = dbsettings;
- wms.first->second.setTo(s, k);
- //truncateHistoryToSize();
- }
- }
- }
-
- bool Entity::hasSnapshot(const Time& time) const
- {
- // check cache
- if (Base::hasSnapshot(time))
- {
- return true;
- }
- // check mongodb
- mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
- 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);
- 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"];
- EntitySnapshot snapshot(id);
- snapshot.dbsettings = dbsettings;
-
- for (unsigned int i = 0; i < instances.size(); ++i)
- {
- EntityInstance instance(id.withInstanceIndex(i));
- snapshot.addInstance(instance);
- }
-
- _container.emplace(time, snapshot);
- //truncateHistoryToSize();
- return true;
- }
-
- std::vector<Time> Entity::getTimestamps() const
- {
- // get from cache
- std::vector<Time> ret; // = Base::getTimestamps();
-
- // get missing from mongodb
- mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
- mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[id().str()];
-
- auto cursor = coll.find({});
- for (auto doc : cursor)
- {
- auto json = nlohmann::json::parse(bsoncxx::to_json(doc));
- auto ts = json["timestamp"].get<long>();
- ret.push_back(Time::microSeconds(ts));
- }
- return ret;
- }
-
- const EntitySnapshot& Entity::getSnapshot(const Time& time) const
- {
- if (!hasSnapshot(time))
- {
- throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
- }
-
- // the above command already puts the reference to the cache
- return Base::getSnapshot(time);
- }
-
- auto Entity::getLatestItem() const -> const ContainerT::value_type&
- {
- // Directly query mongodb (cache cant know whether it is the latest or not)
- // TODO
- return Base::getLatestItem();
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h
deleted file mode 100644
index 20377befd61f741a642e139aafe1de3c2cc7f24b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h
+++ /dev/null
@@ -1,83 +0,0 @@
-#pragma once
-
-#include "../base/EntityBase.h"
-
-#include "EntitySnapshot.h"
-
-#include "../workingmemory/Entity.h"
-
-
-namespace armarx::armem::ltm
-{
- /**
- * @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>
- {
- using Base = base::EntityBase<EntitySnapshot, Entity>;
-
- 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;
-
- // MongoDB connection
- 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;
- }
-
- virtual void _copySelfEmpty(Entity& other) const override
- {
- Base::_copySelfEmpty(other);
- other.dbsettings = dbsettings;
- }
-
- protected:
- // virtual overrides for LTM storage
- virtual const ContainerT::value_type& getLatestItem() const override;
-
- 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
deleted file mode 100644
index 5e44714904efe74f920b891b665e13497b749508..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.cpp
+++ /dev/null
@@ -1,47 +0,0 @@
-#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)
- {
- ARMARX_CHECK_FITS_SIZE(index, update.instancesData.size());
-
- this->index() = 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;
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.h b/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.h
deleted file mode 100644
index e95ae277e83b2a927172ad1e663c42a700fef849..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.h
+++ /dev/null
@@ -1,84 +0,0 @@
-#pragma once
-
-#include "../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;
-
- /// 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>
- {
- 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;
- }
-
-
- /**
- * @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;
-
-
- 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
deleted file mode 100644
index aae8a6d3258c94a27e9f931838a44c8d1b6426aa..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
+++ /dev/null
@@ -1,105 +0,0 @@
-#include "EntitySnapshot.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "../workingmemory/entityInstance_conversions.h"
-#include "../workingmemory/json_conversions.h"
-
-#include "error.h"
-
-
-namespace armarx::armem::ltm
-{
-
- wm::EntitySnapshot EntitySnapshot::convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const
- {
- 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());
- }
-
- nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(*res));
- nlohmann::json instances = json["instances"];
-
- if (instances.size() != _container.size())
- {
- throw error::ArMemError("The size of the mongodb entity entry at id " + id().getEntitySnapshotID().str() + " has wrong size. Expected: " + std::to_string(_container.size()) + " but got: " + std::to_string(instances.size()));
- }
-
- wm::EntitySnapshot m(id());
- for (unsigned int i = 0; i < _container.size(); ++i)
- {
- nlohmann::json doc = instances[i++];
-
- auto aron = std::make_shared<aron::datanavigator::DictNavigator>();
- to_aron(aron, doc, expectedStructure);
- wm::EntityInstance e(id());
- from_aron(aron, e);
- m.addInstance(e);
- }
- return m;
- }
-
- void EntitySnapshot::reload()
- {
- _container.clear();
-
- 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());
- }
-
- nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(*res));
- for (unsigned int i = 0; i < json.at("instances").size(); ++i)
- {
- _container.emplace_back(id().withInstanceIndex(i));
- }
- }
-
- void EntitySnapshot::setTo(const wm::EntitySnapshot& m, const armem::Time& t)
- {
- // We remove the contente here and reset it with new values
- _container.clear();
-
- mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
- mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[id().getEntityID().str()];
-
- bsoncxx::builder::stream::document builder{};
- auto in_array = builder
- << "id" << id().getEntitySnapshotID().str()
- << "timestamp" << t.toMicroSeconds()
- << "instances";
- auto array_builder = bsoncxx::builder::basic::array{};
-
- int i = 0;
- for (const auto& s : m.container())
- {
- auto wms = _container.emplace_back(id().withInstanceIndex(i++));
-
- auto aron = std::make_shared<aron::datanavigator::DictNavigator>();
- to_aron(aron, s);
- nlohmann::json j;
- from_aron(aron, j);
-
- auto doc_value = bsoncxx::from_json(j.dump(2));
- array_builder.append(doc_value);
- }
-
- auto after_array = in_array << array_builder;
- bsoncxx::document::value doc = after_array << bsoncxx::builder::stream::finalize;
- coll.insert_one(doc.view());
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h
deleted file mode 100644
index 02361d2e708365303fbce81f56eaa9b49fb07a61..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h
+++ /dev/null
@@ -1,50 +0,0 @@
-#pragma once
-
-#include "../base/EntitySnapshotBase.h"
-
-#include "EntityInstance.h"
-
-#include "../workingmemory/EntitySnapshot.h"
-
-
-namespace armarx::armem::ltm
-{
-
- /**
- * @brief Data of an entity at one point in time.
- */
- class EntitySnapshot :
- public base::EntitySnapshotBase<EntityInstance, EntitySnapshot>
- {
- using Base = base::EntitySnapshotBase<EntityInstance, EntitySnapshot>;
-
- public:
-
- using Base::EntitySnapshotBase;
- using Base::operator=;
-
-
- // Conversion
- wm::EntitySnapshot convert(const aron::typenavigator::NavigatorPtr& = nullptr) const;
-
- // MongoDB connection
- 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
deleted file mode 100644
index 2080879c55f1dd94e5aeeb2f6d78f72ada5c4aaf..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
+++ /dev/null
@@ -1,236 +0,0 @@
-#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"
-
-
-namespace armarx::armem::ltm
-{
-
- Memory::Memory(const Memory& other) :
- Base(other),
- dbsettings(other.dbsettings),
- alwaysTransferSettings(other.alwaysTransferSettings),
- periodicTransferSettings(other.periodicTransferSettings),
- onFullTransferSettings(other.onFullTransferSettings),
- mongoDBMutex()
- {
- // Do not copy _mutex.
- }
-
-
- Memory::Memory(Memory&& other) :
- Base(other),
- dbsettings(other.dbsettings),
- alwaysTransferSettings(other.alwaysTransferSettings),
- 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;
- alwaysTransferSettings = other.alwaysTransferSettings;
- periodicTransferSettings = other.periodicTransferSettings;
- onFullTransferSettings = other.onFullTransferSettings;
-
- // Don't copy _mutex.
- return *this;
- }
-
-
- Memory& Memory::operator=(Memory&& other)
- {
- Base::operator=(other);
- dbsettings = std::move(other.dbsettings);
- alwaysTransferSettings = std::move(other.alwaysTransferSettings);
- 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/\""
- << "\n\n";
- return false;
- }
- 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(alwaysTransferSettings.enabled, prefix + "ltm.30_enableAlwaysTransfer", "Enable transfer whenever new data is committed (This disables the other transfer modes!).");
- defs->optional(periodicTransferSettings.enabled, prefix + "ltm.31_enablePeriodicTransfer", "Enable transfer based on periodic interval.");
- defs->optional(onFullTransferSettings.enabled, prefix + "ltm.32_enableOnFullTransfer", "Enable transfer whenever the wm is full (see maxHistorySize).");
- }
-
- wm::Memory Memory::convert() const
- {
- std::lock_guard l(mongoDBMutex);
- if (!checkConnection())
- {
- wm::Memory m(id());
- return m;
- }
-
- 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());
- }
-
- TIMING_END(LTM_Convert);
- return m;
- }
-
- 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 (const auto& doc : cursor)
- {
- nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(doc));
- ARMARX_INFO << "Memory: Found foreign key: " << json.at("foreign_key");
-
- MemoryID i((std::string) json.at("foreign_key"));
- if (i.memoryName != id().memoryName)
- {
- throw error::InvalidMemoryID(i, "A MemoryID in mongodb was invalid. Found the wrong memory name. Expected " + id().memoryName);
- }
-
- std::string k = i.coreSegmentName;
-
- if (const auto& it = _container.find(k); it != _container.end())
- {
- throw error::ArMemError("Somehow after clearing the (memory) container a key k = " + k + " was found. Do you have double entries in mongodb?");
- }
- else
- {
- auto wms = _container.emplace(k, id().withCoreSegmentName(k));
- wms.first->second.dbsettings = dbsettings;
- 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)
- {
- 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() << "'";
-
- TIMING_START(LTM_Append);
-
- mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
- mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[id().str()];
-
- for (const auto& [k, s] : m.container())
- {
- if (const auto& it = _container.find(k); it != _container.end())
- {
- // TODO check if foreign key exists
- it->second.append(s);
- }
- else
- {
- auto builder = bsoncxx::builder::stream::document{};
- bsoncxx::document::value foreign_key = builder
- << "foreign_key" << s.id().withCoreSegmentName(k).str()
- << bsoncxx::builder::stream::finalize;
- coll.insert_one(foreign_key.view());
-
- auto wms = _container.emplace(k, id().withCoreSegmentName(k));
- wms.first->second.dbsettings = dbsettings;
- wms.first->second.append(s);
- }
- }
-
- 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
deleted file mode 100644
index b24f4a7a662e2b5980d3dec00ecbb39b2c972673..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
+++ /dev/null
@@ -1,105 +0,0 @@
-#pragma once
-
-// BaseClass
-#include "../base/MemoryBase.h"
-
-// CoreSegment
-#include "CoreSegment.h"
-
-// WM
-#include "../workingmemory/Memory.h"
-
-// Properties
-#include <ArmarXCore/core/application/properties/PluginAll.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-
-namespace armarx::armem::ltm
-{
- /**
- * @brief Data of a memory consisting of multiple core segments.
- */
- class Memory :
- public base::MemoryBase<CoreSegment, Memory>
- {
- using Base = base::MemoryBase<CoreSegment, Memory>;
-
- public:
-
- using Base::MemoryBase;
- using Base::operator=;
-
- struct TransferSettings
- {
- bool enabled = false;
- };
-
- struct AlwaysTransferSettings : public TransferSettings
- {
-
- };
-
- struct PeriodicTransferSettings : public TransferSettings
- {
- bool deleteFromWMOnTransfer = false;
- int frequencyHz = 1;
- };
-
- struct OnFullTransferSettings : public TransferSettings
- {
- enum class Mode
- {
- TRANSFER_LATEST,
- TRANSFER_LEAST_USED
- };
-
- Mode mode;
- };
-
-
- 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;
-
- // MongoDB connection
- 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;
-
- AlwaysTransferSettings alwaysTransferSettings;
- 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
deleted file mode 100644
index 5fcf4a1351e34f257e773c2ac32e9d4eb64c5401..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
+++ /dev/null
@@ -1,83 +0,0 @@
-#include "ProviderSegment.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "error.h"
-
-
-namespace armarx::armem::ltm
-{
-
- wm::ProviderSegment ProviderSegment::convert() const
- {
- wm::ProviderSegment m(id());
- for (const auto& [_, s] : _container)
- {
- m.addEntity(s.convert());
- }
- return m;
- }
-
- void ProviderSegment::reload()
- {
- _container.clear();
-
- mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
- mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[id().str()];
-
- mongocxx::cursor cursor = coll.find({});
- for (auto doc : cursor)
- {
- nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(doc));
-
- MemoryID i((std::string) json.at("foreign_key"));
- if (i.providerSegmentName != id().providerSegmentName)
- {
- throw error::InvalidMemoryID(i, "A MemoryID in mongodb was invalid. Found the wrong providerSegment name. Expected " + id().providerSegmentName);
- }
-
- std::string k = i.entityName;
-
- if (const auto& it = _container.find(k); it != _container.end())
- {
- throw error::ArMemError("Somehow after clearing the (provvider) container a key k = " + k + " was found. Do you have double entries in mongodb?");
- }
- else
- {
- auto wms = _container.emplace(k, id().withEntityName(k));
- wms.first->second.dbsettings = dbsettings;
- wms.first->second.reload();
- }
- }
-
- ARMARX_INFO << "After reload has provider segment " << id().str() << " size: " << _container.size();
- }
-
- void ProviderSegment::append(const wm::ProviderSegment& m)
- {
- mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
- mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[id().str()];
-
- for (const auto& [k, s] : m.container())
- {
- if (const auto& it = _container.find(k); it != _container.end())
- {
- it->second.append(s);
- }
- else
- {
- auto builder = bsoncxx::builder::stream::document{};
- bsoncxx::document::value foreign_key = builder
- << "foreign_key" << s.id().withEntityName(k).str()
- << bsoncxx::builder::stream::finalize;
- coll.insert_one(foreign_key.view());
-
- auto wms = _container.emplace(k, id().withEntityName(k));
- wms.first->second.dbsettings = dbsettings;
- wms.first->second.append(s);
- }
- }
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h
deleted file mode 100644
index bb49f232e9fe231e1cd500e3f9739ddaa0d7d83f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h
+++ /dev/null
@@ -1,51 +0,0 @@
-#pragma once
-
-#include "../base/ProviderSegmentBase.h"
-
-#include "Entity.h"
-
-#include "../workingmemory/ProviderSegment.h"
-
-
-namespace armarx::armem::ltm
-{
-
- /**
- * @brief Data of a provider segment containing multiple entities.
- */
- class ProviderSegment :
- public base::ProviderSegmentBase<Entity, ProviderSegment>
- {
- using Base = base::ProviderSegmentBase<Entity, ProviderSegment>;
-
- public:
-
- using Base::ProviderSegmentBase;
- using Base::operator=;
-
-
- // Conversion
- wm::ProviderSegment convert() const;
-
- // MongoDB connection
- 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
deleted file mode 100644
index 6ad44836900e04765790a9a3c2a19881e1288656..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/mongodb/MongoDBConnectionManager.cpp
+++ /dev/null
@@ -1,76 +0,0 @@
-#include "MongoDBConnectionManager.h"
-
-namespace armarx::armem::ltm
-{
- bool MongoDBConnectionManager::initialized = false;
- std::map<std::string, mongocxx::client> MongoDBConnectionManager::Connections = {};
-
-
- void MongoDBConnectionManager::initialize_if()
- {
- if (!initialized)
- {
- initialized = true;
- mongocxx::instance instance{}; // This should be done only once.
- }
- }
-
- mongocxx::client& MongoDBConnectionManager::EstablishConnection(const MongoDBSettings& settings)
- {
- initialize_if();
-
- const auto uri_str = settings.uri();
- const auto& it = Connections.find(uri_str);
- if (it == Connections.end())
- {
- mongocxx::uri uri(uri_str);
- auto con = Connections.emplace(uri_str, mongocxx::client(uri));
- return con.first->second;
- }
- else
- {
- // A connection already exists. We do not need to open another one.
- return it->second;
- }
- }
-
- bool MongoDBConnectionManager::ConnectionIsValid(const MongoDBSettings& settings, bool force)
- {
- initialize_if();
-
- try
- {
- if (!force)
- {
- const auto uri_str = settings.uri();
- const auto& it = Connections.find(uri_str);
- if (it != Connections.end())
- {
- auto admin = it->second["admin"];
- auto result = admin.run_command(bsoncxx::builder::basic::make_document(bsoncxx::builder::basic::kvp("isMaster", 1)));
- return true;
- }
- }
-
- const auto uri_str = settings.uri();
- mongocxx::uri uri(uri_str);
- auto client = mongocxx::client(uri);
- auto admin = client["admin"];
- auto result = admin.run_command(bsoncxx::builder::basic::make_document(bsoncxx::builder::basic::kvp("isMaster", 1)));
- return true;
- }
- catch (const std::exception& xcp)
- {
- return false;
- }
- }
-
- bool MongoDBConnectionManager::ConnectionExists(const MongoDBSettings& settings)
- {
- initialize_if();
-
- const auto uri_str = settings.uri();
- const auto& it = Connections.find(uri_str);
- return it != Connections.end();
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/mongodb/MongoDBConnectionManager.h b/source/RobotAPI/libraries/armem/core/longtermmemory/mongodb/MongoDBConnectionManager.h
deleted file mode 100644
index 0cd3cb5ba8f6c575d6d3dd9357f16884f508914c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/mongodb/MongoDBConnectionManager.h
+++ /dev/null
@@ -1,75 +0,0 @@
-#pragma once
-
-#include <string>
-#include <vector>
-#include <map>
-#include <memory>
-#include <iostream>
-
-#include <bsoncxx/json.hpp>
-#include <mongocxx/client.hpp>
-#include <mongocxx/stdx.hpp>
-#include <mongocxx/uri.hpp>
-#include <mongocxx/instance.hpp>
-#include <bsoncxx/builder/stream/helpers.hpp>
-#include <bsoncxx/builder/stream/document.hpp>
-#include <bsoncxx/builder/stream/array.hpp>
-
-
-namespace armarx::armem::ltm
-{
-
- using bsoncxx::builder::stream::close_array;
- using bsoncxx::builder::stream::close_document;
- using bsoncxx::builder::stream::document;
- using bsoncxx::builder::stream::finalize;
- using bsoncxx::builder::stream::open_array;
- using bsoncxx::builder::stream::open_document;
-
-
- /**
- * @brief Data of a memory consisting of multiple core segments.
- */
- class MongoDBConnectionManager
- {
- public:
- struct MongoDBSettings
- {
- std::string host = "localhost";
- unsigned int port = 25270;
- std::string user = "";
- std::string password = "";
- std::string database = "Test";
-
-
- bool isSet() const
- {
- // we always need a user and a host
- return !host.empty() and port != 0 and !user.empty();
- }
-
- std::string uri() const
- {
- return "mongodb://" + host + ":" + std::to_string(port) + user;
- }
-
- std::string toString() const
- {
- return uri() + ":" + password + "/" + database;
- }
- };
-
- static mongocxx::client& EstablishConnection(const MongoDBSettings& settings);
- static bool ConnectionIsValid(const MongoDBSettings& settings, bool force = false);
- static bool ConnectionExists(const MongoDBSettings& settings);
-
- private:
- static void initialize_if();
-
-
- private:
- static bool initialized;
- static std::map<std::string, mongocxx::client> Connections;
-
- };
-}
diff --git a/source/RobotAPI/libraries/armem/core/operations.cpp b/source/RobotAPI/libraries/armem/core/operations.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..533bc623f3844a06ab7b87d858c00942a637fcbc
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/operations.cpp
@@ -0,0 +1,101 @@
+#include "operations.h"
+
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+#include <SimoxUtility/algorithm/string/string_tools.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;
+ }
+
+
+ template <class DataT>
+ static std::string makeLine(int depth, const DataT& data, std::optional<size_t> size = std::nullopt)
+ {
+ std::stringstream ss;
+ while (depth > 1)
+ {
+ ss << "| ";
+ depth--;
+ }
+ if (depth > 0)
+ {
+ ss << "+- ";
+ }
+ ss << simox::alg::capitalize_words(DataT::getLevelName());
+ ss << " '" << simox::alg::capitalize_words(data.getKeyString()) << "'";
+ if (size.has_value())
+ {
+ ss << " (size " << size.value() << ")";
+ }
+ ss << "\n";
+ return ss.str();
+ }
+
+
+ template <class DataT>
+ static std::string _print(const DataT& data, int maxDepth, int depth)
+ {
+ std::stringstream ss;
+ ss << makeLine(depth, data, data.size());
+ if (maxDepth < 0 || maxDepth > 0)
+ {
+ data.forEachChild([&ss, maxDepth, depth](const auto& instance)
+ {
+ ss << armem::print(instance, maxDepth - 1, depth + 1);
+ });
+ }
+ return ss.str();
+ }
+
+ std::string armem::print(const wm::Memory& data, int maxDepth, int depth)
+ {
+ return _print(data, maxDepth, depth);
+ }
+ std::string armem::print(const wm::CoreSegment& data, int maxDepth, int depth)
+ {
+ return _print(data, maxDepth, depth);
+ }
+ std::string armem::print(const wm::ProviderSegment& data, int maxDepth, int depth)
+ {
+ return _print(data, maxDepth, depth);
+ }
+ std::string armem::print(const wm::Entity& data, int maxDepth, int depth)
+ {
+ return _print(data, maxDepth, depth);
+ }
+ std::string armem::print(const wm::EntitySnapshot& data, int maxDepth, int depth)
+ {
+ return _print(data, maxDepth, depth);
+ }
+
+ std::string armem::print(const wm::EntityInstance& data, int maxDepth, int depth)
+ {
+ (void) maxDepth;
+ std::stringstream ss;
+ ss << makeLine(depth, data);
+ return ss.str();
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/core/operations.h b/source/RobotAPI/libraries/armem/core/operations.h
new file mode 100644
index 0000000000000000000000000000000000000000..775b6dd07216a13dcffafc73f8651f6722b56b09
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/operations.h
@@ -0,0 +1,61 @@
+#pragma once
+
+#include "wm/memory_definitions.h"
+
+#include <string>
+
+
+/*
+ * 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;
+ }
+
+
+ template <class ContainerT>
+ const typename ContainerT::EntityInstanceT*
+ findFirstInstance(const ContainerT& container)
+ {
+ const typename ContainerT::EntityInstanceT* instance = nullptr;
+ container.forEachInstance([&instance](const wm::EntityInstance & i)
+ {
+ instance = &i;
+ return false;
+ });
+ return instance;
+ }
+
+
+ std::string print(const wm::Memory& data, int maxDepth = -1, int depth = 0);
+ std::string print(const wm::CoreSegment& data, int maxDepth = -1, int depth = 0);
+ std::string print(const wm::ProviderSegment& data, int maxDepth = -1, int depth = 0);
+ std::string print(const wm::Entity& data, int maxDepth = -1, int depth = 0);
+ std::string print(const wm::EntitySnapshot& data, int maxDepth = -1, int depth = 0);
+ std::string print(const wm::EntityInstance& data, int maxDepth = -1, int depth = 0);
+
+}
+
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/detail/data_lookup_mixins.cpp b/source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..50fd63bf64dabfedbab4e980f9450563d301e02b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.cpp
@@ -0,0 +1,7 @@
+#include "data_lookup_mixins.h"
+
+
+namespace armarx::armem::base
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.h b/source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.h
new file mode 100644
index 0000000000000000000000000000000000000000..c2d240e2977e1a11df3d8959a4f4200382d8650c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.h
@@ -0,0 +1,100 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+#include <RobotAPI/libraries/armem/core/base/detail/derived.h>
+
+#include <RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h>
+
+#include <optional>
+
+
+namespace armarx::armem::wm::detail
+{
+ using base::detail::derived;
+
+
+ template <class AronDtoT>
+ std::optional<AronDtoT>
+ getInstanceDataAs(aron::datanavigator::DictNavigatorPtr data)
+ {
+ if (data)
+ {
+ AronDtoT aron;
+ aron.fromAron(data);
+ return aron;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+
+ template <class DerivedT>
+ struct FindInstanceDataMixinForSnapshot
+ {
+
+ aron::datanavigator::DictNavigatorPtr
+ findInstanceData(int instanceIndex = 0) const
+ {
+ const auto* instance = derived<DerivedT>(this).findInstance(instanceIndex);
+ return instance ? instance->data() : nullptr;
+ }
+
+
+ template <class AronDtoT>
+ std::optional<AronDtoT>
+ findInstanceDataAs(int instanceIndex = 0) const
+ {
+ return getInstanceDataAs<AronDtoT>(derived<DerivedT>(this).findInstanceData(instanceIndex));
+ }
+
+ };
+
+
+
+ template <class DerivedT>
+ struct FindInstanceDataMixinForEntity
+ {
+
+ aron::datanavigator::DictNavigatorPtr
+ findLatestInstanceData(int instanceIndex = 0) const
+ {
+ const auto* instance = derived<DerivedT>(this).findLatestInstance(instanceIndex);
+ return instance ? instance->data() : nullptr;
+ }
+
+
+ template <class AronDtoT>
+ std::optional<AronDtoT>
+ findLatestInstanceDataAs(int instanceIndex = 0) const
+ {
+ return getInstanceDataAs<AronDtoT>(derived<DerivedT>(this).findLatestInstanceData(instanceIndex));
+ }
+
+ };
+
+
+
+ template <class DerivedT>
+ struct FindInstanceDataMixin
+ {
+
+ aron::datanavigator::DictNavigatorPtr
+ findLatestInstanceData(const MemoryID& entityID, int instanceIndex = 0) const
+ {
+ const auto* instance = derived<DerivedT>(this).findLatestInstance(entityID, instanceIndex);
+ return instance ? instance->data() : nullptr;
+ }
+
+ template <class AronDtoT>
+ std::optional<AronDtoT>
+ findLatestInstanceDataAs(const MemoryID& entityID, int instanceIndex = 0) const
+ {
+ return getInstanceDataAs<AronDtoT>(derived<DerivedT>(this).findLatestInstanceData(entityID, instanceIndex));
+ }
+
+ };
+
+}
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..ba6c9782e0e2663ee9395fde31956d5fdbfc2ad2
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/wm/ice_conversions.cpp
@@ -0,0 +1,63 @@
+#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/wm/memory_definitions.cpp b/source/RobotAPI/libraries/armem/core/wm/memory_definitions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..de5ad02c2e9e4f24304c2ae9b9eb97f0cd3e53ee
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/wm/memory_definitions.cpp
@@ -0,0 +1,45 @@
+#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;
+ }
+
+}
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..b77ea3c6e9eb17b69ce3feb848951012a0ed0304
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/wm/memory_definitions.h
@@ -0,0 +1,116 @@
+#pragma once
+
+#include "detail/data_lookup_mixins.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 <RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h>
+
+
+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 detail::FindInstanceDataMixinForSnapshot<EntitySnapshot>
+ {
+ public:
+
+ using base::EntitySnapshotBase<EntityInstance, EntitySnapshot>::EntitySnapshotBase;
+
+ };
+
+
+ /// @see base::EntityBase
+ class Entity :
+ public base::EntityBase<EntitySnapshot, Entity>
+ , public detail::FindInstanceDataMixinForEntity<Entity>
+ {
+ public:
+
+ using base::EntityBase<EntitySnapshot, Entity>::EntityBase;
+
+ };
+
+
+
+ /// @see base::ProviderSegmentBase
+ class ProviderSegment :
+ public base::ProviderSegmentBase<Entity, ProviderSegment>
+ , public detail::FindInstanceDataMixin<ProviderSegment>
+ {
+ using Base = base::ProviderSegmentBase<Entity, ProviderSegment>;
+
+ public:
+
+ using Base::ProviderSegmentBase;
+
+ };
+
+
+
+ /// @see base::CoreSegmentBase
+ class CoreSegment :
+ public base::CoreSegmentBase<ProviderSegment, CoreSegment>
+ , public detail::FindInstanceDataMixin<CoreSegment>
+ {
+ using Base = base::CoreSegmentBase<ProviderSegment, CoreSegment>;
+
+ public:
+
+ using Base::CoreSegmentBase;
+
+ };
+
+
+
+ /// @see base::MemoryBase
+ class Memory :
+ public base::MemoryBase<CoreSegment, Memory>
+ , public detail::FindInstanceDataMixin<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 100%
rename from source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h
rename to source/RobotAPI/libraries/armem/core/wm/visitor/FunctionalVisitor.h
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..9448f55f8f68385d716f9b8e7da26e890ffb48fd
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/wm/visitor/Visitor.cpp
@@ -0,0 +1,142 @@
+#include "Visitor.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/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 601f079350de8536aff130943d539eafee4e43b3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
+++ /dev/null
@@ -1,136 +0,0 @@
-#include "CoreSegment.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "error.h"
-
-
-namespace armarx::armem::wm
-{
-
- CoreSegment::CoreSegment(const CoreSegment& other) :
- CoreSegment::Base(other),
- _mutex()
- {
- // Do not copy _mutex.
- }
-
-
- CoreSegment::CoreSegment(CoreSegment&& other) : CoreSegment::Base(other)
- {
- // Do not move _mutex.
- }
-
-
- CoreSegment& CoreSegment::operator=(const CoreSegment& other)
- {
- Base::operator=(other);
- // Don't copy _mutex.
- return *this;
- }
-
-
- CoreSegment& CoreSegment::operator=(CoreSegment&& other)
- {
- Base::operator=(other);
- // Don't move _mutex.
- return *this;
- }
-
-
- 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());
- }
- }
-
-
- 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;
- }
- }
-
-
- 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);
- }
-
-}
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 86b9b439e72d4d686b2c52fe5fd39f0a9b6aaf1c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
+++ /dev/null
@@ -1,111 +0,0 @@
-#pragma once
-
-#include <mutex>
-#include <optional>
-
-#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);
- CoreSegment(CoreSegment&& other);
- CoreSegment& operator=(const CoreSegment& other);
- CoreSegment& operator=(CoreSegment&& other);
-
- /**
- * @brief Convert the content of this segmnet into a commit
- * @return The resulting commit
- */
- Commit toCommit() const;
-
- 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;
- }
- }
-
-
- // 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:
-
- virtual void _copySelfWithoutData(CoreSegment& other) const override;
-
- mutable std::mutex _mutex;
-
- };
-
-}
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 d3dc888d0304276bc6aaf1c677e8b33f4a1cae1a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h
+++ /dev/null
@@ -1,107 +0,0 @@
-#pragma once
-
-#include "../base/EntityInstanceBase.h"
-
-#include "detail/CopyWithoutData.h"
-
-
-namespace armarx::armem::wm
-{
-
- using EntityInstanceData = armarx::aron::datanavigator::DictNavigator;
- using EntityInstanceDataPtr = armarx::aron::datanavigator::DictNavigatorPtr;
-
-
- /**
- * @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 EntityInstanceDataPtr 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.
- EntityInstanceDataPtr _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.h b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
deleted file mode 100644
index d52bce5ca30d2a464881e73f4cd4994a29429751..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
+++ /dev/null
@@ -1,56 +0,0 @@
-#pragma once
-
-#include <RobotAPI/libraries/armem/core/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 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);
-
-
- /**
- * @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.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/json_conversions.cpp
deleted file mode 100644
index e7499d8b42edf895bbbdb2f0d06a1fe3ed2ee823..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/json_conversions.cpp
+++ /dev/null
@@ -1,22 +0,0 @@
-#include "json_conversions.h"
-
-#include <RobotAPI/libraries/aron/core/Debug.h>
-
-namespace armarx::armem
-{
-
- void from_aron(const aron::datanavigator::DictNavigatorPtr& aron, nlohmann::json& j)
- {
- aron::dataIO::writer::NlohmannJSONWriter dataWriter;
- aron::dataIO::Visitor::VisitAndSetup(dataWriter, aron);
- j = dataWriter.getResult();
- }
-
- 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;
- aron::dataIO::Converter::ReadAndConvert(dataReader, navWriter, expectedStructure);
- a = aron::datanavigator::DictNavigator::DynamicCastAndCheck(navWriter.getResult());
- }
-}
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/mns/ComponentPlugin.cpp b/source/RobotAPI/libraries/armem/mns/ComponentPlugin.cpp
deleted file mode 100644
index 79e58300a6968dfda3f60d89ff0511f9ec72f267..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/mns/ComponentPlugin.cpp
+++ /dev/null
@@ -1,63 +0,0 @@
-#include "ComponentPlugin.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-
-#include "../error.h"
-
-
-namespace armarx::armem::mns::plugins
-{
-
-}
-
-
-namespace armarx::armem::mns
-{
-
- ComponentPluginUser::ComponentPluginUser()
- {
- addPlugin(plugin);
- }
-
-
- armem::data::RegisterMemoryResult ComponentPluginUser::registerMemory(const armem::data::RegisterMemoryInput& input, const Ice::Current&)
- {
- std::scoped_lock lock(mnsMutex);
- armem::data::RegisterMemoryResult result = mns.registerMemory(input);
- return result;
- }
-
-
- armem::data::RemoveMemoryResult ComponentPluginUser::removeMemory(const armem::data::RemoveMemoryInput& input, const Ice::Current&)
- {
- std::scoped_lock lock(mnsMutex);
- armem::data::RemoveMemoryResult result = mns.removeMemory(input);
- return result;
- }
-
-
- armem::data::GetAllRegisteredMemoriesResult
- ComponentPluginUser::getAllRegisteredMemories(const Ice::Current&)
- {
- std::scoped_lock lock(mnsMutex);
- armem::data::GetAllRegisteredMemoriesResult result = mns.getAllRegisteredMemories();
- return result;
- }
-
-
- armem::data::ResolveMemoryNameResult ComponentPluginUser::resolveMemoryName(const armem::data::ResolveMemoryNameInput& input, const Ice::Current&)
- {
- std::scoped_lock lock(mnsMutex);
- armem::data::ResolveMemoryNameResult result = mns.resolveMemoryName(input);
- return result;
- }
-
-
- armem::data::WaitForMemoryResult ComponentPluginUser::waitForMemory(const armem::data::WaitForMemoryInput& input, const Ice::Current&)
- {
- // No lock - this call blocks internally.
- return mns.waitForMemory(input);
- }
-
-}
diff --git a/source/RobotAPI/libraries/armem/mns/ComponentPlugin.h b/source/RobotAPI/libraries/armem/mns/ComponentPlugin.h
deleted file mode 100644
index 1347c197e6d4f560b5773abfed30b92e8f8a7604..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/mns/ComponentPlugin.h
+++ /dev/null
@@ -1,62 +0,0 @@
-#pragma once
-
-#include <mutex>
-
-#include <ArmarXCore/core/Component.h>
-
-#include <RobotAPI/interface/armem/server/MemoryInterface.h>
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
-
-#include "MemoryNameSystem.h"
-
-
-namespace armarx::armem::mns::plugins
-{
-
- class ComponentPlugin : public armarx::ComponentPlugin
- {
- public:
- using armarx::ComponentPlugin::ComponentPlugin;
- };
-
-}
-
-
-namespace armarx::armem::mns
-{
-
- /**
- * @brief Utility for connecting a Memory to Ice.
- */
- class ComponentPluginUser :
- virtual public ManagedIceObject
- , virtual public mns::MemoryNameSystemInterface
- {
- public:
-
- ComponentPluginUser();
-
- // mns::MemoryNameSystemInterface interface
- public:
- armem::data::RegisterMemoryResult registerMemory(const armem::data::RegisterMemoryInput& input, const Ice::Current& = Ice::emptyCurrent) override;
- armem::data::RemoveMemoryResult removeMemory(const armem::data::RemoveMemoryInput& input, const Ice::Current& = Ice::emptyCurrent) override;
- armem::data::GetAllRegisteredMemoriesResult getAllRegisteredMemories(const Ice::Current&) override;
- armem::data::ResolveMemoryNameResult resolveMemoryName(const armem::data::ResolveMemoryNameInput& input, const Ice::Current& = Ice::emptyCurrent) override;
- armem::data::WaitForMemoryResult waitForMemory(const armem::data::WaitForMemoryInput& input, const Ice::Current& = Ice::emptyCurrent) override;
-
-
- public:
-
- std::mutex mnsMutex;
- MemoryNameSystem mns;
-
-
- private:
-
- plugins::ComponentPlugin* plugin = nullptr;
-
-
- };
-
-
-}
diff --git a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
index ece0fa74306ed77465ce0bb0c643b781ec704178..a80a189ff585c7610bebe0978468aec82ebe48b9 100644
--- a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
+++ b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
@@ -4,186 +4,41 @@
namespace armarx::armem::mns
{
- MemoryNameSystem::MemoryNameSystem(const std::string& logTag)
+ // dto::WaitForServerResult MemoryNameSystem::waitForServer(const dto::WaitForServerInput& input)
+ void MemoryNameSystem::waitForServer_async(
+ const AMD_MemoryNameSystemInterface_waitForServerPtr& future,
+ const dto::WaitForServerInput& input)
{
- armarx::Logging::setTag(logTag);
+ waitForServerFutures[input.name].push_back(future);
+ waitForServer_processOnce();
}
- armem::data::RegisterMemoryResult MemoryNameSystem::registerMemory(const armem::data::RegisterMemoryInput& input)
+ void MemoryNameSystem::waitForServer_processOnce()
{
- armem::data::RegisterMemoryResult result;
-
- if (!input.proxy)
- {
- result.success = false;
- std::stringstream ss;
- ss << "Could not register the memory '" << input.name << "'."
- << "\nGiven proxy is null."
- << "\nIf you want to remove a memory, use `removeMemory()`.";
- result.errorMessage = ss.str();
- return result;
- }
-
- auto it = memoryMap.find(input.name);
- if (it == memoryMap.end())
- {
- it = memoryMap.emplace(input.name, MemoryInfo{}).first;
- }
- else if (!input.existOk)
- {
- result.success = false;
- std::stringstream ss;
- ss << "Could not register the memory '" << input.name << "'."
- << "\nMemory '" << input.name << "' is already registered. "
- << "\nIf this is ok, set 'existOk' to true when registering the memory.";
- result.errorMessage = ss.str();
- return result;
- }
-
- MemoryInfo& info = it->second;
- info.name = input.name;
- info.proxy = input.proxy;
- info.timeRegistered = armem::Time::now();
- ARMARX_DEBUG << "Registered memory '" << info.name << "'.";
-
- {
- std::unique_lock lock(waitForMemoryMutex);
- waitForMemoryCond.notify_all();
- }
-
- result.success = true;
- return result;
- }
-
-
- armem::data::RemoveMemoryResult MemoryNameSystem::removeMemory(const armem::data::RemoveMemoryInput& input)
- {
- armem::data::RemoveMemoryResult result;
-
- if (auto it = memoryMap.find(input.name); it != memoryMap.end())
- {
- result.success = true;
- memoryMap.erase(it);
-
- ARMARX_DEBUG << "Removed memory '" << input.name << "'.";
- }
- else if (!input.notExistOk)
- {
- result.success = false;
- std::stringstream ss;
- ss << "Could not remove the memory '" << input.name << "."
- << "\nMemory '" << input.name << "' is not registered. "
- << "\nIf this is ok, set 'notExistOk' to true when removing the memory.";
- result.errorMessage = ss.str();
- }
-
- return result;
- }
-
-
- data::GetAllRegisteredMemoriesResult
- MemoryNameSystem::getAllRegisteredMemories()
- {
- data::GetAllRegisteredMemoriesResult result;
- result.success = true;
- result.errorMessage = "";
-
- for (const auto& [name, info] : memoryMap)
+ for (auto it = waitForServerFutures.begin(); it != waitForServerFutures.end();)
{
- try
+ auto& [name, futures] = *it;
+ if (auto st = servers.find(name); st != servers.end())
{
- info.proxy->ice_ping();
- result.proxies[name] = info.proxy;
- }
- catch (const Ice::Exception&)
- {
- ;
- }
- }
-
- return result;
- }
-
-
- armem::data::ResolveMemoryNameResult MemoryNameSystem::resolveMemoryName(const armem::data::ResolveMemoryNameInput& input)
- {
- armem::data::ResolveMemoryNameResult result;
- try
- {
- MemoryInfo& info = memoryMap.at(input.name);
-
- result.success = true;
- result.proxy = info.proxy;
-
- ARMARX_DEBUG << "Resolved memory name '" << input.name << "'.";
- }
- catch (const std::out_of_range&)
- {
- result.success = false;
- std::stringstream ss;
- ss << "Could not resolve the memory name '" << input.name << "'."
- << "\nMemory '" << input.name << "' is not registered.";
- result.errorMessage = ss.str();
- }
-
- return result;
- }
-
-
- data::WaitForMemoryResult MemoryNameSystem::waitForMemory(const data::WaitForMemoryInput& input)
- {
- data::ResolveMemoryNameInput resInput;
- resInput.name = input.name;
-
- Time start = Time::now();
- data::ResolveMemoryNameResult resResult;
- {
- std::unique_lock lock(waitForMemoryMutex);
- auto pred = [this, resInput, &resResult]()
- {
- resResult = resolveMemoryName(resInput);
- return resResult.success;
- };
- if (input.timeoutMilliSeconds >= 0)
- {
- waitForMemoryCond.wait_for(lock, std::chrono::milliseconds(input.timeoutMilliSeconds), pred);
+ ServerInfo& info = st->second;
+
+ dto::WaitForServerResult result;
+ result.success = true;
+ result.server = info.server;
+
+ // Send responses and remove entry.
+ for (auto& future : futures)
+ {
+ future->ice_response(result);
+ }
+ it = waitForServerFutures.erase(it);
}
else
{
- waitForMemoryCond.wait(lock, pred);
+ ++it; // Skip.
}
}
-
- armem::data::WaitForMemoryResult result;
- result.success = resResult.success;
-
- if (resResult.success)
- {
- ARMARX_CHECK(resResult.proxy);
- result.proxy = resResult.proxy;
- }
- else
- {
- ARMARX_CHECK((Time::now() - start).toMilliSeconds() > input.timeoutMilliSeconds)
- << (Time::now() - start).toMilliSeconds() << " > " << input.timeoutMilliSeconds;
-
- std::stringstream ss;
- ss << "Timeout (" << input.timeoutMilliSeconds << " ms) while waiting for memory '" << input.name << "'.";
- if (resResult.errorMessage.size() > 0)
- {
- ss << "\n" << resResult.errorMessage;
- }
- result.errorMessage = ss.str();
- }
-
- return result;
- }
-
-
- bool MemoryNameSystem::hasMemory(const std::string& memoryName) const
- {
- return memoryMap.count(memoryName) > 0;
}
@@ -196,20 +51,41 @@ namespace armarx::armem::mns
int row = 0;
grid.add(Label("Memory Name"), {row, 0})
.add(Label("Component Name"), {row, 1})
- .add(Label("Registration Time"), {row, 2})
+ .add(Label("R/W"), {row, 2})
+ .add(Label("Registration Time"), {row, 3})
;
row++;
- for (const auto& [name, info] : memoryMap)
+ for (const auto& [name, info] : servers)
{
ARMARX_CHECK_EQUAL(name, info.name);
- grid
- .add(Label(name), {row, 0})
- .add(Label(info.proxy->ice_getIdentity().name), {row, 1})
- .add(Label(armem::toDateTimeMilliSeconds(info.timeRegistered, 0)), {row, 2})
- ;
+ std::string componentName = "";
+ std::string mode = "";
+ if (info.server.reading)
+ {
+ componentName = info.server.reading->ice_getIdentity().name;
+ mode += "R";
+ }
+ if (info.server.writing)
+ {
+ componentName = info.server.writing->ice_getIdentity().name;
+ mode += "W";
+ }
+
+ int col = 0;
+ grid.add(Label(name), {row, col});
+ ++col;
+
+ grid.add(Label(componentName), {row, col});
+ ++col;
+
+ grid.add(Label(mode), {row, col});
+ ++col;
+
+ grid.add(Label(armem::toDateTimeMilliSeconds(info.timeRegistered, 0)), {row, col});
+ ++col;
- row++;
+ ++row;
}
return grid;
diff --git a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.h b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.h
index 7372246d7e8059787f97f117931830d12c9dc082..51e4719f127ee576073e0c9b45cb0635241a185d 100644
--- a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.h
+++ b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.h
@@ -1,55 +1,36 @@
#pragma once
-#include <condition_variable>
-#include <mutex>
-
-#include <ArmarXCore/core/logging/Logging.h>
+#include "Registry.h"
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
-#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-#include "../core/Time.h"
+#include <map>
+#include <string>
namespace armarx::armem::mns
{
- class MemoryNameSystem : armarx::Logging
+ class MemoryNameSystem : public Registry
{
public:
- MemoryNameSystem(const std::string& logTag = "MemoryNameSystem");
-
-
- /**
- * @brief Register a new memory or update an existing entry.
- *
- * Causes threads waiting in `waitForMemory()` to resume if the respective
- * memory was added.
- */
- data::RegisterMemoryResult registerMemory(const data::RegisterMemoryInput& input);
- /**
- * @brief Remove a memory entry.
- */
- data::RemoveMemoryResult removeMemory(const data::RemoveMemoryInput& input);
+ using WaitForServerFuturePtr = AMD_MemoryNameSystemInterface_waitForServerPtr;
- data::GetAllRegisteredMemoriesResult getAllRegisteredMemories();
- /**
- * @brief Gets a memory entry, if it is available.
- */
- data::ResolveMemoryNameResult resolveMemoryName(const data::ResolveMemoryNameInput& input);
+ public:
/**
- * @brief Blocks until the specified memory is available, returning its proxy.
+ * @brief Store the call in a container for later response.
*/
- data::WaitForMemoryResult waitForMemory(const data::WaitForMemoryInput& input);
+ void waitForServer_async(
+ const AMD_MemoryNameSystemInterface_waitForServerPtr& future,
+ const dto::WaitForServerInput& input);
+ void waitForServer_processOnce();
- /// Indicates whether a memory entry for that name exists.
- bool hasMemory(const std::string& memoryName) const;
/// Builds a RemoteGui grid containing information about registered memories.
armarx::RemoteGui::Client::GridLayout RemoteGui_buildInfoGrid();
@@ -57,22 +38,8 @@ namespace armarx::armem::mns
public:
- /// Information about a memory entry.
- struct MemoryInfo
- {
- std::string name;
- server::MemoryInterfacePrx proxy;
- Time timeRegistered;
- };
-
- /// The registered memories.
- std::map<std::string, MemoryInfo> memoryMap;
-
-
- /// Mutex for `waitForMemoryCond`.
- std::mutex waitForMemoryMutex;
- /// Condition variable used by `waitForMemory()`.
- std::condition_variable waitForMemoryCond;
+ /// Queued calls to `waitForServer`.
+ std::map<std::string, std::vector<WaitForServerFuturePtr>> waitForServerFutures;
};
diff --git a/source/RobotAPI/libraries/armem/mns/Registry.cpp b/source/RobotAPI/libraries/armem/mns/Registry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..260cfcd818a426ae7b4f18c6cde5a4180a459f0b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/mns/Registry.cpp
@@ -0,0 +1,162 @@
+#include "Registry.h"
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+
+namespace armarx::armem::mns
+{
+
+ Registry::Registry(const std::string& logTag)
+ {
+ armarx::Logging::setTag(logTag);
+ }
+
+
+ bool Registry::hasServer(const std::string& memoryName) const
+ {
+ return servers.count(memoryName) > 0;
+ }
+
+
+ dto::RegisterServerResult Registry::registerServer(const dto::RegisterServerInput& input)
+ {
+ dto::RegisterServerResult result;
+
+ if (not(input.server.reading or input.server.writing))
+ {
+ result.success = false;
+ std::stringstream ss;
+ ss << "Could not register the memory server '" << input.name << "'."
+ << "\nGiven proxies are null."
+ << "\nIf you want to remove a memory server, use `removeServer()`.";
+ result.errorMessage = ss.str();
+ return result;
+ }
+
+ auto it = servers.find(input.name);
+ if (it == servers.end())
+ {
+ it = servers.emplace(input.name, ServerInfo{}).first;
+ }
+ else if (not input.existOk)
+ {
+ result.success = false;
+ std::stringstream ss;
+ ss << "Could not register the memory '" << input.name << "'."
+ << "\nMemory '" << input.name << "' is already registered. "
+ << "\nIf this is ok, set 'existOk' to true when registering the memory.";
+ result.errorMessage = ss.str();
+ return result;
+ }
+
+ ServerInfo& info = it->second;
+ info.name = input.name;
+ info.server = input.server;
+ info.timeRegistered = armem::Time::now();
+ ARMARX_DEBUG << "Registered memory '" << info.name << "'.";
+
+ result.success = true;
+ return result;
+ }
+
+
+ dto::RemoveServerResult Registry::removeServer(const dto::RemoveServerInput& input)
+ {
+ dto::RemoveServerResult result;
+
+ if (auto it = servers.find(input.name); it != servers.end())
+ {
+ result.success = true;
+ servers.erase(it);
+
+ ARMARX_DEBUG << "Removed memory '" << input.name << "'.";
+ }
+ else if (!input.notExistOk)
+ {
+ result.success = false;
+ std::stringstream ss;
+ ss << "Could not remove the memory '" << input.name << "."
+ << "\nMemory '" << input.name << "' is not registered. "
+ << "\nIf this is ok, set 'notExistOk' to true when removing the memory.";
+ result.errorMessage = ss.str();
+ }
+
+ return result;
+ }
+
+
+ template <class Prx>
+ bool isAvailable(const Prx& proxy)
+ {
+ if (proxy)
+ {
+ try
+ {
+ proxy->ice_ping();
+ return true;
+ }
+ catch (const Ice::Exception&)
+ {
+ }
+ }
+ return false;
+ }
+
+
+ dto::GetAllRegisteredServersResult
+ Registry::getAllRegisteredServers()
+ {
+ dto::GetAllRegisteredServersResult result;
+ result.success = true;
+ result.errorMessage = "";
+
+ for (const auto& [name, info] : servers)
+ {
+ if (isAvailable(info.server.reading) or isAvailable(info.server.writing))
+ {
+ result.servers[name] = info.server;
+ }
+ }
+
+ return result;
+ }
+
+
+ dto::ResolveServerResult Registry::resolveServer(const dto::ResolveServerInput& input)
+ {
+ dto::ResolveServerResult result;
+ try
+ {
+ ServerInfo& info = servers.at(input.name);
+
+ result.success = true;
+ result.server = info.server;
+
+ ARMARX_DEBUG << "Resolved memory name '" << input.name << "'.";
+ }
+ catch (const std::out_of_range&)
+ {
+ result.success = false;
+ std::stringstream ss;
+ ss << "Could not resolve the memory name '" << input.name << "'."
+ << "\nServer '" << input.name << "' is not registered.";
+ result.errorMessage = ss.str();
+ }
+
+ return result;
+ }
+
+
+ server::ReadingMemoryInterfacePrx getReadingInterface(const dto::MemoryServerInterfaces& server)
+ {
+ return server.reading;
+ }
+
+
+ server::WritingMemoryInterfacePrx getWritingInterface(const dto::MemoryServerInterfaces& server)
+ {
+ return server.writing;
+ }
+
+}
+
diff --git a/source/RobotAPI/libraries/armem/mns/Registry.h b/source/RobotAPI/libraries/armem/mns/Registry.h
new file mode 100644
index 0000000000000000000000000000000000000000..bf9348d157ee990f6032c4cf115b9fd56c70f716
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/mns/Registry.h
@@ -0,0 +1,71 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/interface/armem/server/WritingMemoryInterface.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <map>
+#include <string>
+
+
+namespace armarx::armem::mns
+{
+
+ /**
+ * @brief A registry for memory servers.
+ */
+ class Registry : armarx::Logging
+ {
+ public:
+
+ Registry(const std::string& logTag = "MemoryNameSystem Registry");
+
+
+ /// Indicates whether a server entry for that name exists.
+ bool hasServer(const std::string& memoryName) const;
+
+
+ /**
+ * @brief Register a new memory server or update an existing entry.
+ *
+ * Causes threads waiting in `waitForMemory()` to resume if the respective
+ * memory server was added.
+ */
+ dto::RegisterServerResult registerServer(const dto::RegisterServerInput& input);
+ /**
+ * @brief Remove a server entry.
+ */
+ dto::RemoveServerResult removeServer(const dto::RemoveServerInput& input);
+
+
+ /**
+ * @brief Gets a server entry, if it is available.
+ */
+ dto::ResolveServerResult resolveServer(const dto::ResolveServerInput& input);
+
+ dto::GetAllRegisteredServersResult getAllRegisteredServers();
+
+
+ public:
+
+ /// Information about a memory entry.
+ struct ServerInfo
+ {
+ std::string name;
+ mns::dto::MemoryServerInterfaces server;
+ Time timeRegistered;
+ };
+
+ /// The registered memories.
+ std::map<std::string, ServerInfo> servers;
+
+ };
+
+
+ server::ReadingMemoryInterfacePrx getReadingInterface(const dto::MemoryServerInterfaces& server);
+ server::WritingMemoryInterfacePrx getWritingInterface(const dto::MemoryServerInterfaces& server);
+}
diff --git a/source/RobotAPI/libraries/armem/mns/plugins/Plugin.cpp b/source/RobotAPI/libraries/armem/mns/plugins/Plugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a2e7c5dbf3fe87d6fdf24fee0181504a5f85abe9
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/mns/plugins/Plugin.cpp
@@ -0,0 +1,8 @@
+#include "Plugin.h"
+
+
+namespace armarx::armem::mns::plugins
+{
+
+}
+
diff --git a/source/RobotAPI/libraries/armem/mns/plugins/Plugin.h b/source/RobotAPI/libraries/armem/mns/plugins/Plugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..5caa973a9ebb7025f42978c608c033ce47722074
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/mns/plugins/Plugin.h
@@ -0,0 +1,24 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/mns/MemoryNameSystem.h>
+
+#include <ArmarXCore/core/ComponentPlugin.h>
+
+
+namespace armarx::armem::mns::plugins
+{
+
+ class Plugin : public armarx::ComponentPlugin
+ {
+ public:
+
+ using armarx::ComponentPlugin::ComponentPlugin;
+
+
+ public:
+
+ MemoryNameSystem mns;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/mns/plugins/PluginUser.cpp b/source/RobotAPI/libraries/armem/mns/plugins/PluginUser.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4ad29cdeec3c2b732cf1b9074931ab3c116fa42b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/mns/plugins/PluginUser.cpp
@@ -0,0 +1,69 @@
+#include "PluginUser.h"
+#include "Plugin.h"
+
+
+namespace armarx::armem::mns::plugins
+{
+
+ PluginUser::PluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+
+ dto::RegisterServerResult PluginUser::registerServer(const dto::RegisterServerInput& input, const Ice::Current&)
+ {
+ std::scoped_lock lock(mnsMutex);
+ dto::RegisterServerResult result = plugin->mns.registerServer(input);
+ return result;
+ }
+
+
+ dto::RemoveServerResult PluginUser::removeServer(const dto::RemoveServerInput& input, const Ice::Current&)
+ {
+ std::scoped_lock lock(mnsMutex);
+ dto::RemoveServerResult result = plugin->mns.removeServer(input);
+ return result;
+ }
+
+
+ dto::GetAllRegisteredServersResult
+ PluginUser::getAllRegisteredServers(const Ice::Current&)
+ {
+ std::scoped_lock lock(mnsMutex);
+ dto::GetAllRegisteredServersResult result = plugin->mns.getAllRegisteredServers();
+ return result;
+ }
+
+
+ dto::ResolveServerResult PluginUser::resolveServer(const dto::ResolveServerInput& input, const Ice::Current&)
+ {
+ std::scoped_lock lock(mnsMutex);
+ dto::ResolveServerResult result = plugin->mns.resolveServer(input);
+ return result;
+ }
+
+
+ // dto::WaitForServerResult PluginUser::waitForServer(const dto::WaitForServerInput& input, const Ice::Current&)
+ void PluginUser::waitForServer_async(
+ const AMD_MemoryNameSystemInterface_waitForServerPtr& future,
+ const dto::WaitForServerInput& input,
+ const Ice::Current&)
+ {
+ std::scoped_lock lock(mnsMutex);
+ plugin->mns.waitForServer_async(future, input);
+ }
+
+
+ MemoryNameSystem& PluginUser::mns()
+ {
+ return plugin->mns;
+ }
+
+
+ const MemoryNameSystem& PluginUser::mns() const
+ {
+ return plugin->mns;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/mns/plugins/PluginUser.h b/source/RobotAPI/libraries/armem/mns/plugins/PluginUser.h
new file mode 100644
index 0000000000000000000000000000000000000000..1e8134ad5d372b1d69252cdc0cf2f5916c948b1e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/mns/plugins/PluginUser.h
@@ -0,0 +1,55 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/mns/MemoryNameSystem.h>
+
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
+#include <mutex>
+
+
+namespace armarx::armem::mns::plugins
+{
+ class Plugin;
+
+
+ class PluginUser :
+ virtual public ManagedIceObject
+ , virtual public mns::MemoryNameSystemInterface
+ {
+ public:
+
+ PluginUser();
+
+ // mns::MemoryNameSystemInterface interface
+ public:
+
+ dto::RegisterServerResult registerServer(const dto::RegisterServerInput& input, const Ice::Current& = Ice::emptyCurrent) override;
+ dto::RemoveServerResult removeServer(const dto::RemoveServerInput& input, const Ice::Current& = Ice::emptyCurrent) override;
+
+ dto::GetAllRegisteredServersResult getAllRegisteredServers(const Ice::Current&) override;
+
+ dto::ResolveServerResult resolveServer(const dto::ResolveServerInput& input, const Ice::Current& = Ice::emptyCurrent) override;
+
+ // Uses Asynchronous Method Dispatch (AMD)
+ void waitForServer_async(
+ const AMD_MemoryNameSystemInterface_waitForServerPtr& future,
+ const dto::WaitForServerInput& input,
+ const Ice::Current& = Ice::emptyCurrent) override;
+
+
+ protected:
+
+ std::mutex mnsMutex;
+ MemoryNameSystem& mns();
+ const MemoryNameSystem& mns() const;
+
+
+ private:
+
+ plugins::Plugin* plugin = nullptr;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp b/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
deleted file mode 100644
index 8c814a160b49b0367cece67d407b986ac4d372e5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
+++ /dev/null
@@ -1,165 +0,0 @@
-#include "ComponentPlugin.h"
-
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <RobotAPI/libraries/armem/core/error.h>
-
-#include "MemoryToIceAdapter.h"
-
-//#include <RobotAPI/libraries/armem/core/io/diskWriter/NlohmannJSON/NlohmannJSONDiskWriter.h>
-
-
-namespace armarx::armem::server::plugins
-{
- ComponentPlugin::~ComponentPlugin() = default;
-
-
- void ComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
- {
- MemoryNameSystemComponentPlugin::postCreatePropertyDefinitions(properties);
-
- ComponentPluginUser& parent = this->parent<ComponentPluginUser>();
- properties->topic(memoryListener, parent.memoryListenerDefaultName);
-
- properties->optional(parent.longtermMemoryEnabled, "memory.ltm.00_enabled");
- parent.longtermMemory.defineProperties(properties, "memory");
- }
-
-
- void ComponentPlugin::postOnConnectComponent()
- {
- ComponentPluginUser& parent = this->parent<ComponentPluginUser>();
-
- if (isMemoryNameSystemEnabled() && parent.memoryNameSystem)
- {
- registerMemory(parent);
- }
- parent.iceMemory.setMemoryListener(memoryListener);
-
- // establishing connection to ltm and mongodb
- if (parent.longtermMemoryEnabled)
- {
- parent.longtermMemory.reload();
- }
- }
-
-
- void ComponentPlugin::preOnDisconnectComponent()
- {
- ComponentPluginUser& parent = this->parent<ComponentPluginUser>();
- if (isMemoryNameSystemEnabled() && parent.memoryNameSystem)
- {
- removeMemory(parent);
- }
- }
-
-
- data::RegisterMemoryResult ComponentPlugin::registerMemory(ComponentPluginUser& parent)
- {
- MemoryID id = MemoryID().withMemoryName(parent.workingMemory.name());
- MemoryInterfacePrx proxy = MemoryInterfacePrx::checkedCast(parent.getProxy());
- ARMARX_CHECK_NOT_NULL(proxy);
-
- data::RegisterMemoryResult result;
- try
- {
- parent.memoryNameSystem.registerServer(id, proxy);
- result.success = true;
- ARMARX_DEBUG << "Registered memory server for " << id << " in the Memory Name System (MNS).";
- }
- catch (const armem::error::ServerRegistrationOrRemovalFailed& e)
- {
- result.success = false;
- result.errorMessage = e.what();
- ARMARX_WARNING << e.what();
- }
- return result;
- }
-
-
- data::RemoveMemoryResult ComponentPlugin::removeMemory(ComponentPluginUser& parent)
- {
- MemoryID id = MemoryID().withMemoryName(parent.workingMemory.name());
-
- data::RemoveMemoryResult result;
- try
- {
- parent.memoryNameSystem.removeServer(id);
- result.success = true;
- ARMARX_DEBUG << "Removed memory server for " << id << " from the Memory Name System (MNS).";
- }
- catch (const armem::error::ServerRegistrationOrRemovalFailed& e)
- {
- result.success = false;
- result.errorMessage = e.what();
- ARMARX_WARNING << e.what();
- }
- catch (const Ice::NotRegisteredException&)
- {
- // It's ok, the MNS is gone.
- result.success = false;
- result.errorMessage = "Memory Name System is gone.";
- }
- return result;
- }
-
-}
-
-
-namespace armarx::armem::server
-{
- ComponentPluginUser::ComponentPluginUser()
- {
- addPlugin(plugin);
- }
-
- ComponentPluginUser::~ComponentPluginUser() = default;
-
-
- // WRITING
- data::AddSegmentsResult ComponentPluginUser::addSegments(const data::AddSegmentsInput& input, const Ice::Current&)
- {
- bool addCoreSegmentOnUsage = false;
- return addSegments(input, addCoreSegmentOnUsage);
- }
-
- data::AddSegmentsResult ComponentPluginUser::addSegments(const data::AddSegmentsInput& input, bool addCoreSegments)
- {
- // std::scoped_lock lock(workingMemoryMutex);
- data::AddSegmentsResult result = iceMemory.addSegments(input, addCoreSegments);
- return result;
- }
-
-
- data::CommitResult ComponentPluginUser::commit(const data::Commit& commitIce, const Ice::Current&)
- {
- // std::scoped_lock lock(workingMemoryMutex);
- return iceMemory.commit(commitIce);
- }
-
-
- // READING
- armem::query::data::Result ComponentPluginUser::query(const armem::query::data::Input& input, const Ice::Current&)
- {
- // std::scoped_lock lock(workingMemoryMutex);
- return iceMemory.query(input);
- }
-
-
- // LTM STORING
- data::StoreResult ComponentPluginUser::store(const data::StoreInput& input, const Ice::Current&)
- {
- std::scoped_lock lock(/*workingMemoryMutex,*/ longtermMemoryMutex);
- return iceMemory.store(input);
- }
-
-
- // LTM LOADING
- armem::query::data::Result ComponentPluginUser::load(const armem::query::data::Input& input, const Ice::Current&)
- {
- std::scoped_lock lock(longtermMemoryMutex);
- return iceMemory.load(input);
- }
-
-}
diff --git a/source/RobotAPI/libraries/armem/server/ComponentPlugin.h b/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
deleted file mode 100644
index 06ba31f7a128549f51be85de300a92bdd310cf95..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
+++ /dev/null
@@ -1,123 +0,0 @@
-#pragma once
-
-#include <mutex>
-
-#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/longtermmemory/Memory.h>
-#include <RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h>
-
-#include "MemoryToIceAdapter.h"
-
-
-namespace armarx::armem::server
-{
- class ComponentPluginUser;
-}
-
-
-namespace armarx::armem::server::plugins
-{
-
- class ComponentPlugin : public client::plugins::MemoryNameSystemComponentPlugin
- {
- using Base = client::plugins::MemoryNameSystemComponentPlugin;
- public:
-
- using Base::MemoryNameSystemComponentPlugin;
- virtual ~ComponentPlugin() override;
-
- void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
-
- void postOnConnectComponent() override;
- void preOnDisconnectComponent() override;
-
-
- /**
- * @brief Register the parent component in the MNS.
- * Called before onConnect() if MNS is enabled.
- */
- data::RegisterMemoryResult registerMemory(ComponentPluginUser& parent);
- /**
- * @brief Remove the parent component from the MNS.
- * Called before onDisconnect() if MNS is enabled.
- */
- data::RemoveMemoryResult removeMemory(ComponentPluginUser& parent);
-
- client::MemoryListenerInterfacePrx memoryListener;
-
- std::string longTermMemoryDatabaseHost;
- std::string longTermMemoryDatabaseUser;
- std::string longTermMemoryDatabasePassword;
- };
-
-}
-
-
-namespace armarx::armem::server
-{
-
- /**
- * @brief Base class of memory server components.
- */
- class ComponentPluginUser :
- virtual public ManagedIceObject
- , virtual public MemoryInterface
- , virtual public client::MemoryNameSystemComponentPluginUser
- {
- public:
-
- ComponentPluginUser();
- virtual ~ComponentPluginUser() override;
-
-
- // WritingInterface interface
- virtual data::AddSegmentsResult addSegments(const data::AddSegmentsInput& input, const Ice::Current& = Ice::emptyCurrent) override;
- data::AddSegmentsResult addSegments(const data::AddSegmentsInput& input, bool addCoreSegments);
-
- virtual data::CommitResult commit(const data::Commit& commit, const Ice::Current& = Ice::emptyCurrent) override;
-
-
- // ReadingInterface interface
- virtual armem::query::data::Result query(const armem::query::data::Input& input, const Ice::Current& = Ice::emptyCurrent) override;
-
-
- // StoringInterface interface
- virtual data::StoreResult store(const data::StoreInput&, const Ice::Current& = Ice::emptyCurrent) override;
-
-
- // LoadingInterface interface
- virtual armem::query::data::Result load(const armem::query::data::Input&, const Ice::Current& = Ice::emptyCurrent) override;
-
-
- public:
-
- /// The actual memory.
- 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;
- std::mutex longtermMemoryMutex;
-
- /// property defaults
- std::string memoryListenerDefaultName = "MemoryUpdates";
-
- /// Helps connecting `memory` to ice. Used to handle Ice callbacks.
- MemoryToIceAdapter iceMemory { &workingMemory, &longtermMemory};
-
-
- private:
-
- plugins::ComponentPlugin* plugin = nullptr;
-
- };
-
-
-}
diff --git a/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp b/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp
index 9347cb7e9234723d33407693cb68ec1f5ea8448d..57b495a527d07924176314edc70fbfa1c53f1cae 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp
@@ -1,106 +1,116 @@
#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/type/container/Object.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
+ static std::string getTypeString(const armem::base::detail::AronTyped& typed)
{
- std::scoped_lock lock(coreSegment.mutex());
+ std::stringstream type;
+ if (typed.aronType())
+ {
+ type << " (" << typed.aronType()->getName() << ")";
+ }
+ else
+ {
+ type << " (no Aron type)";
+ }
+ return type.str();
+ }
+ 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())
+ + getTypeString(coreSegment));
- 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())
+ + getTypeString(providerSegment));
- 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)
+ {
+ group.addChild(makeGroupBox(snapshot));
+ };
+
+ if (int(entity.size()) <= maxHistorySize)
{
- for (const auto& [time, snapshot] : entity.history())
- {
- group.addChild(makeGroupBox(snapshot));
- }
+ 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);
@@ -108,27 +118,78 @@ namespace armarx::armem::server
}
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const wm::EntitySnapshot& snapshot) const
+ 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 armem::server::wm::CoreSegment& coreSegment) const
+ {
+ return coreSegment.doLocked([this, &coreSegment]()
+ {
+ 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;
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 354eec9cea9d1d1e37f4dc4ea790b47ba1749755..73d00c3a370fd14e40771df77d0d96cc6b9dbf42 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -1,19 +1,24 @@
#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 <RobotAPI/libraries/aron/core/Exception.h>
-#include "../error.h"
-#include "../core/workingmemory/ice_conversions.h"
-#include "query_proc/workingmemory/MemoryQueryProcessor.h"
-#include "query_proc/longtermmemory/MemoryQueryProcessor.h"
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
namespace armarx::armem::server
{
- MemoryToIceAdapter::MemoryToIceAdapter(wm::Memory* workingMemory, ltm::Memory* longtermMemory) :
- workingMemory(workingMemory), longtermMemory(longtermMemory)
+ MemoryToIceAdapter::MemoryToIceAdapter(wm::Memory* workingMemory, server::ltm::mongodb::MemoryManager* longtermMemory) :
+ workingMemory(workingMemory), longtermMemoryManager(longtermMemory)
{
}
@@ -29,11 +34,12 @@ namespace armarx::armem::server
data::AddSegmentResult
MemoryToIceAdapter::addSegment(const data::AddSegmentInput& input, bool addCoreSegments)
{
+ ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(workingMemory);
data::AddSegmentResult output;
- armem::wm::CoreSegment* coreSegment = nullptr;
+ server::wm::CoreSegment* coreSegment = nullptr;
try
{
coreSegment = &workingMemory->getCoreSegment(input.coreSegmentName);
@@ -55,20 +61,22 @@ namespace armarx::armem::server
if (input.providerSegmentName.size() > 0)
{
- std::scoped_lock lock(coreSegment->mutex());
- try
- {
- coreSegment->addProviderSegment(input.providerSegmentName);
- }
- catch (const armem::error::ContainerEntryAlreadyExists&)
+ coreSegment->doLocked([&coreSegment, &input]()
{
- // This is ok.
- if (input.clearWhenExists)
+ try
{
- wm::ProviderSegment& provider = coreSegment->getProviderSegment(input.providerSegmentName);
- provider.clear();
+ coreSegment->addProviderSegment(input.providerSegmentName);
}
- }
+ catch (const armem::error::ContainerEntryAlreadyExists&)
+ {
+ // This is ok.
+ if (input.clearWhenExists)
+ {
+ server::wm::ProviderSegment& provider = coreSegment->getProviderSegment(input.providerSegmentName);
+ provider.clear();
+ }
+ }
+ });
}
armem::MemoryID segmentID;
@@ -85,6 +93,7 @@ namespace armarx::armem::server
data::AddSegmentsResult
MemoryToIceAdapter::addSegments(const data::AddSegmentsInput& input, bool addCoreSegments)
{
+ ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(workingMemory);
data::AddSegmentsResult output;
@@ -99,6 +108,7 @@ namespace armarx::armem::server
data::CommitResult
MemoryToIceAdapter::commit(const data::Commit& commitIce, Time timeArrived)
{
+ ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(workingMemory);
auto handleException = [](const std::string & what)
{
@@ -136,6 +146,7 @@ namespace armarx::armem::server
data::CommitResult
MemoryToIceAdapter::commit(const data::Commit& commitIce)
{
+ ARMARX_TRACE;
return commit(commitIce, armem::Time::now());
}
@@ -143,18 +154,21 @@ namespace armarx::armem::server
armem::CommitResult
MemoryToIceAdapter::commit(const armem::Commit& commit)
{
+ ARMARX_TRACE;
return this->_commit(commit, false);
}
armem::CommitResult MemoryToIceAdapter::commitLocking(const armem::Commit& commit)
{
+ ARMARX_TRACE;
return this->_commit(commit, true);
}
armem::CommitResult MemoryToIceAdapter::_commit(const armem::Commit& commit, bool locking)
{
+ ARMARX_TRACE;
std::vector<data::MemoryID> updatedIDs;
const bool publishUpdates = bool(memoryListenerTopic);
@@ -174,19 +188,9 @@ namespace armarx::armem::server
// also store in ltm if transfermode is set to always
// TODO: Move outside of loop?
- if (longtermMemory)
+ if (longtermMemoryManager)
{
- if (longtermMemory->alwaysTransferSettings.enabled)
- {
- wm::Memory tmp(longtermMemory->name());
- tmp._addMissingCoreSegmentDuringUpdate = true;
- tmp.update(update);
- longtermMemory->append(tmp);
- }
- else
- {
- // TODO: see below
- }
+
}
// TODO: Consollidate to ltm if onFilledTransfer is enabled (fabian.peller)
@@ -231,34 +235,26 @@ namespace armarx::armem::server
armem::query::data::Result
MemoryToIceAdapter::query(const armem::query::data::Input& input)
{
+ ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(workingMemory);
// Core segment processors will aquire the core segment locks.
- armem::wm::query_proc::MemoryQueryProcessor wmProcessor(
+ query_proc::wm_server::MemoryQueryProcessor wmServerProcessor(
input.withData ? armem::DataMode::WithData : armem::DataMode::NoData);
- wm::Memory wmResult = wmProcessor.process(input, *workingMemory);
+ armem::wm::Memory wmResult = wmServerProcessor.process(input.memoryQueries, *workingMemory);
- armem::ltm::query_proc::MemoryQueryProcessor ltmProcessor;
- ltm::Memory ltmResult = ltmProcessor.process(input, *longtermMemory);
+ query_proc::ltm::MemoryQueryProcessor ltmProcessor;
+ armem::wm::Memory ltmResult = ltmProcessor.process(input, longtermMemoryManager->getCacheAndLutNotConverted());
armem::query::data::Result result;
- if (ltmResult.hasData())
+ 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 ltmConverted = ltmResult.convert();
- if (!ltmConverted.hasData())
- {
- ARMARX_ERROR << "A converted memory contains no data although the original memory contained data. This indicates that something is wrong.";
- }
+ longtermMemoryManager->convert(ltmResult); // convert memory ==> meaning resolving lut references to e.g. mongodb
- wmResult.append(ltmConverted);
- if (!wmResult.hasData())
+ wmResult.append(ltmResult);
+ 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.";
}
@@ -267,16 +263,18 @@ namespace armarx::armem::server
auto queryInput = armem::client::QueryInput::fromIce(input);
queryInput.replaceQueryTarget(query::data::QueryTarget::LTM, query::data::QueryTarget::WM);
- wm::Memory merged_result = wmProcessor.process(queryInput.toIce(), wmResult);
- if (!merged_result.hasData())
+ query_proc::wm::MemoryQueryProcessor wm2wmProcessor(
+ input.withData ? armem::DataMode::WithData : armem::DataMode::NoData);
+ armem::wm::Memory mergedResult = wm2wmProcessor.process(queryInput.toIce(), wmResult);
+ if (mergedResult.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);
+ result.memory = toIce<data::MemoryPtr>(mergedResult);
// 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
@@ -299,26 +297,19 @@ namespace armarx::armem::server
client::QueryResult MemoryToIceAdapter::query(const client::QueryInput& input)
{
+ ARMARX_TRACE;
return client::QueryResult::fromIce(query(input.toIce()));
}
// LTM LOADING FROM LTM
- query::data::Result MemoryToIceAdapter::load(const armem::query::data::Input& query)
- {
- ARMARX_CHECK_NOT_NULL(longtermMemory);
- query::data::Result output;
-
- output.success = true;
- return output;
- }
-
// LTM STORING
data::StoreResult MemoryToIceAdapter::store(const armem::data::StoreInput& input)
{
+ ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(workingMemory);
- ARMARX_CHECK_NOT_NULL(longtermMemory);
+ ARMARX_CHECK_NOT_NULL(longtermMemoryManager);
data::StoreResult output;
for (const auto& query : input.query.memoryQueries)
@@ -333,16 +324,12 @@ namespace armarx::armem::server
if (queryResult.success)
{
- wm::Memory m;
+ armem::wm::Memory m;
fromIce(queryResult.memory, m);
- longtermMemory->append(m);
+ longtermMemoryManager->append(m);
}
return output;
}
-
-
-
-
}
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
index f4b4b4c94eac3a51cb8c90e2fcabfc09288e3ead..f924399c7eb749f8404f28c1c2a9bd7945112a7c 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/longtermmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/ltm/mongodb/MemoryManager.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,
+ server::ltm::mongodb::MemoryManager* longtermMemory = nullptr);
void setMemoryListener(client::MemoryListenerInterfacePrx memoryListenerTopic);
@@ -47,15 +48,14 @@ namespace armarx::armem::server
client::QueryResult query(const client::QueryInput& input);
// LTM LOADING
- query::data::Result load(const armem::query::data::Input& input);
// LTM STORING
data::StoreResult store(const armem::data::StoreInput& input);
public:
- wm::Memory* workingMemory;
- ltm::Memory* longtermMemory;
+ server::wm::Memory* workingMemory;
+ server::ltm::mongodb::MemoryManager* longtermMemoryManager;
client::MemoryListenerInterfacePrx memoryListenerTopic;
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..d65ec0f36b9f60e680c6e720d80cd9cc3d6e789f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/forward_declarations.h
@@ -0,0 +1,22 @@
+#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;
+}
+namespace armarx::armem::server::ltm::mongodb
+{
+ class MemoryManager;
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.cpp b/source/RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2e8c64d64121eccc007cad7ba2d55c2e361b94e2
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.cpp
@@ -0,0 +1,134 @@
+// Header
+#include "LongtermMemoryBase.h"
+
+// ArmarX
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+
+namespace armarx::armem::server::ltm
+{
+ void LongtermMemoryBase::setName(const std::string& name)
+ {
+ cache.name() = name;
+ lut.name() = name;
+ }
+
+ armem::wm::Memory LongtermMemoryBase::getCacheAndLutNotConverted() const
+ {
+ std::lock_guard l(cache_mutex);
+ std::lock_guard l2(lut_mutex);
+
+ armem::wm::Memory m(lut.name());
+ m.append(cache);
+ m.append(lut);
+
+ // debug output
+ //lut.forEachSnapshot([](const auto & e)
+ //{
+ // ARMARX_INFO << "The SNapshot: " << e.id().str() << " has size: " << e.size();
+ //});
+
+ return m;
+ }
+
+ void LongtermMemoryBase::append(const armem::wm::Memory& m)
+ {
+ TIMING_START(LTM_Append);
+ ARMARX_INFO << "Append memory with name '" << m.name() << "' into the LTM with name '" << cache.name() << "'";
+
+ std::lock_guard l(cache_mutex);
+ cache.append(m);
+
+ encodeAndStore();
+
+ TIMING_END(LTM_Append);
+ }
+
+ void LongtermMemoryBase::checkUpdateLatestSnapshot(const armem::wm::EntitySnapshot& newSnapshot)
+ {
+ // update map of latestSnapshots
+ if (auto it = latestSnapshots.find(newSnapshot.id().getEntityID().str()); it != latestSnapshots.end())
+ {
+ auto ptr = it->second;
+ if (ptr->id().timestamp > newSnapshot.id().timestamp)
+ {
+ ptr = &newSnapshot;
+ }
+ // else ignore ==> no update
+ }
+ else
+ {
+ // no entry yet
+ latestSnapshots.emplace(newSnapshot.id().getEntityID().str(), &newSnapshot);
+ }
+ }
+
+ bool LongtermMemoryBase::containsCoreSegment(const MemoryID& coreSegmentID) const
+ {
+ //ARMARX_INFO << "Check if lut has core seg";
+ if (lut.hasCoreSegment(coreSegmentID.coreSegmentName))
+ {
+ //ARMARX_INFO << "lus has core seg";
+ return true;
+ }
+ return false;
+ }
+
+ bool LongtermMemoryBase::containsProviderSegment(const MemoryID& providerSegmentID) const
+ {
+ //ARMARX_INFO << "Check if lut has prov seg";
+ if (lut.hasCoreSegment(providerSegmentID.coreSegmentName))
+ {
+ auto core = lut.getCoreSegment(providerSegmentID.coreSegmentName);
+ if (core.hasProviderSegment(providerSegmentID.providerSegmentName))
+ {
+ //ARMARX_INFO << "lus has prov seg";
+ return true;
+ }
+ }
+ return false;
+ }
+
+ bool LongtermMemoryBase::containsEntity(const MemoryID& entityID) const
+ {
+ //ARMARX_INFO << "Check if lut has entity";
+ if (lut.hasCoreSegment(entityID.coreSegmentName))
+ {
+ auto core = lut.getCoreSegment(entityID.coreSegmentName);
+ if (core.hasProviderSegment(entityID.providerSegmentName))
+ {
+ auto prov = core.getProviderSegment(entityID.providerSegmentName);
+ if (prov.hasEntity(entityID.entityName))
+ {
+ //ARMARX_INFO << "lus has entity";
+ return true;
+ }
+ }
+ }
+ return false;
+ }
+
+ bool LongtermMemoryBase::containsSnapshot(const MemoryID& snapshotID) const
+ {
+ //ARMARX_INFO << "Check if lut has snapshot";
+ if (lut.hasCoreSegment(snapshotID.coreSegmentName))
+ {
+ auto core = lut.getCoreSegment(snapshotID.coreSegmentName);
+ if (core.hasProviderSegment(snapshotID.providerSegmentName))
+ {
+ auto prov = core.getProviderSegment(snapshotID.providerSegmentName);
+ if (prov.hasEntity(snapshotID.entityName))
+ {
+ auto entity = prov.getEntity(snapshotID.entityName);
+ if (entity.hasSnapshot(snapshotID.timestamp))
+ {
+ //ARMARX_INFO << "lut has snapshot";
+ return true;
+ }
+ }
+ }
+ }
+ return false;
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.h b/source/RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..097f951548cc777fa03cedbc5f425e867439cd48
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.h
@@ -0,0 +1,65 @@
+#pragma once
+
+// STD / STL
+#include <optional>
+#include <mutex>
+
+// Memory
+#include "../../core/wm/memory_definitions.h"
+
+namespace armarx::armem::server::ltm
+{
+ /// @brief Interface functions for the longterm memory classes
+ class LongtermMemoryBase
+ {
+ public:
+ struct AppendResult
+ {
+ std::vector<MemoryID> addedCoreSegments;
+ std::vector<MemoryID> addedProviderSegments;
+ std::vector<MemoryID> addedEntities;
+
+ std::vector<MemoryID> addedSnapshots;
+ std::vector<MemoryID> replacedSnapshots;
+ std::vector<MemoryID> ignoredSnapshots;
+ };
+
+ struct ReloadResult
+ {
+
+ };
+
+ void append(const armem::wm::Memory&);
+
+ virtual void reload() = 0;
+ virtual void convert(armem::wm::Memory&) = 0;
+ virtual void encodeAndStore() = 0;
+
+ // pass through to internal memory
+ void setName(const std::string& name);
+
+ // get merged internal memory
+ armem::wm::Memory getCacheAndLutNotConverted() const;
+
+ protected:
+ void checkUpdateLatestSnapshot(const armem::wm::EntitySnapshot& newSnapshot);
+
+ bool containsCoreSegment(const MemoryID&) const;
+ bool containsProviderSegment(const MemoryID&) const;
+ bool containsEntity(const MemoryID&) const;
+ bool containsSnapshot(const MemoryID&) const;
+
+ protected:
+ /// Internal memory for data consolidated from wm to ltm (cache)
+ armem::wm::Memory cache;
+ mutable std::recursive_mutex cache_mutex;
+
+ /// Internal memory for indexes (lut)
+ armem::wm::Memory lut;
+ mutable std::recursive_mutex lut_mutex;
+
+ /// A map from entityID to its latest snapshot stored. When adding a new snapshot we compare it to the last one stored.
+ std::map<std::string, const armem::wm::EntitySnapshot*> latestSnapshots;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/MemoryManager.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/MemoryManager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b2360b5c6580ea37bbbbb6765817e998e910c302
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/MemoryManager.cpp
@@ -0,0 +1,402 @@
+// Header
+#include "MemoryManager.h"
+
+// STD / STL
+#include <iostream>
+#include <fstream>
+
+// Simox
+#include <SimoxUtility/json.h>
+
+// ArmarX
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/armem/core/wm/aron_conversions.h>
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+
+namespace
+{
+ // check whether a string is a number (timestamp folders)
+ bool isNumber(const std::string& s)
+ {
+ for (char const& ch : s)
+ {
+ if (std::isdigit(ch) == 0)
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+}
+
+namespace armarx::armem::server::ltm::disk
+{
+ namespace fs = std::filesystem;
+
+ bool MemoryManager::checkPath() const
+ {
+ // Check connection:
+ ARMARX_INFO << "Checking Path";
+ if (!fs::exists(basePathToMemory) || !fs::is_directory(basePathToMemory) || basePathToMemory.filename() != lut.name())
+ {
+ ARMARX_WARNING << deactivateSpam("PathIsNotValid")
+ << "The entered path is not valid. Please use a path leading to a memory folder with name: " << lut.name() << "."
+ << "\n\n";
+ return false;
+ }
+
+ return true;
+ }
+
+ void MemoryManager::reload()
+ {
+ TIMING_START(LTM_Reload);
+ ARMARX_INFO << "(Re)Loading a memory from: " << basePathToMemory.string();
+
+ if (!checkPath())
+ {
+ // abort
+ ARMARX_WARNING << "Could not (pre)load a memory from the filesystem.";
+ return;
+ }
+
+ armem::wm::Memory temp(lut.id()); // a temporary client wm. We will append temp to the lut at the end of this metho (append ignores duplicate entries)
+ ARMARX_INFO << "Loading memory: " << temp.id().str();
+
+ // ///////////////////////////////
+ // Iterate over core segments
+ // ///////////////////////////////
+ for (const auto& d : std::filesystem::directory_iterator(basePathToMemory))
+ // Although this looks like code duplication, we need a distinguition between memories,
+ // core, prov and entities because of a different structure
+ // (only core and prov have same structure)
+ {
+
+ if (!d.is_directory())
+ {
+ continue;
+ }
+
+ std::string k = d.path().filename();
+ if (temp.hasCoreSegment(k))
+ {
+ throw error::ArMemError("Somehow the (memory) container already contains the key k = " + k + ". This should not happen.");
+ }
+
+ // ///////////////////////////////
+ // Add and iterate over core segments
+ // ///////////////////////////////
+ auto& cSeg = temp.addCoreSegment(k);
+ for (const auto& d : std::filesystem::directory_iterator(d))
+ {
+ if (!d.is_directory())
+ {
+ continue;
+ }
+
+ std::string k = d.path().filename();
+ if (cSeg.hasProviderSegment(k))
+ {
+ throw error::ArMemError("Somehow the (core) container already contains the key k = " + k + ". This should not happen.");
+ }
+
+ // ///////////////////////////////
+ // Add and iterate over provider segments
+ // ///////////////////////////////
+ auto& pSeg = cSeg.addProviderSegment(k);
+ for (const auto& d : std::filesystem::directory_iterator(d))
+ {
+ if (!d.is_directory())
+ {
+ continue;
+ }
+
+ std::string k = d.path().filename();
+ if (pSeg.hasEntity(k))
+ {
+ throw error::ArMemError("Somehow the (provider) container already contains the key k = " + k + ". This should not happen.");
+ }
+
+ // ///////////////////////////////
+ // Add and iterate over entities
+ // ///////////////////////////////
+ auto& eSeg = pSeg.addEntity(k);
+ for (const auto& d : std::filesystem::directory_iterator(d))
+ {
+ if (!d.is_directory())
+ {
+ continue;
+ }
+
+ std::string k = d.path().filename();
+ if (!isNumber(k))
+ {
+ continue;
+ }
+
+ auto ts = armem::Time::microSeconds(std::stol(k));
+ // TODO catch exceptions?
+
+ if (eSeg.hasSnapshot(ts))
+ {
+ throw error::ArMemError("Somehow the (entity) container already contains the key k = " + k + ". This should not happen.");
+ }
+
+ // ///////////////////////////////
+ // Add and iterate over entities
+ // ///////////////////////////////
+ auto& sSeg = eSeg.addSnapshot(ts);
+ for (unsigned int i = 0; i < 10000; ++i) // ugly workaround to get the folders in the correct order
+ {
+ fs::path p = d / std::to_string(i);
+ if (!fs::exists(p) || !fs::is_directory(p))
+ {
+ // early stopping
+ break;
+ }
+
+ fs::path data = p / DATA_FILENAME;
+ if (!fs::exists(data) || !fs::is_regular_file(data))
+ {
+ // do not set data
+ continue;
+ }
+
+ // else we have an instance
+ /*std::ifstream ifs(data);
+ std::string file_content((std::istreambuf_iterator<char>(ifs)), (std::istreambuf_iterator<char>()));
+
+ nlohmann::json j = nlohmann::json::parse(file_content);
+ auto aron = aron::converter::AronNlohmannJSONConverter::ConvertFromNlohmannJSON(j);*/
+
+ sSeg.addInstance();
+ //from_aron(aron, instance);
+ }
+ }
+ }
+ }
+ }
+
+ std::lock_guard l(cache_mutex); // we always take the cache mutex BEFORE the lut mutex! (otherwise we may have deadlocks)
+ std::lock_guard l2(lut_mutex);
+ lut.append(temp);
+ ARMARX_INFO << "After reload memory " << lut.id().str() << " has size: " << lut.size();
+
+ TIMING_END(LTM_Reload);
+ }
+
+ void MemoryManager::convert(armem::wm::Memory& m)
+ {
+ TIMING_START(LTM_Convert);
+ if (!checkPath())
+ {
+ // abort
+ ARMARX_WARNING << "Could not convert a memory from the filesystem.";
+ return;
+ }
+
+ // update emtpy data ptr
+ m.forEachCoreSegment([this](armem::wm::CoreSegment & e)
+ {
+ e.forEachProviderSegment([this](armem::wm::ProviderSegment & e)
+ {
+ e.forEachEntity([this](armem::wm::Entity & e)
+ {
+ e.forEachSnapshot([this](armem::wm::EntitySnapshot & e)
+ {
+ // check whether data is nullptr
+ bool allDataIsNull = e.size() > 0;
+ e.forEachInstance([&allDataIsNull](armem::wm::EntityInstance & e)
+ {
+ if (e.data())
+ {
+ allDataIsNull = false;
+ return false; // means break
+ }
+ return true;
+ });
+
+ if (allDataIsNull) // an entry from the lut (probably... for now we assume that every entry either has data (cache) or has null (lut))
+ {
+ // Get data from mongodb
+ auto p = basePathToMemory / e.id().coreSegmentName / e.id().providerSegmentName / e.id().entityName / std::to_string(e.id().timestamp.toMicroSeconds());
+
+ if (fs::exists(p) && fs::is_directory(p))
+ {
+ for (unsigned int i = 0; i < e.size(); ++i)
+ {
+ auto data = p / std::to_string(i) / DATA_FILENAME;
+
+ if (fs::exists(data) && fs::is_regular_file(data))
+ {
+ auto& ins = e.getInstance(i);
+
+ std::ifstream ifs(data);
+ std::string file_content((std::istreambuf_iterator<char>(ifs)), (std::istreambuf_iterator<char>()));
+ nlohmann::json doc = nlohmann::json::parse(file_content);
+
+ auto aron = aron::converter::AronNlohmannJSONConverter::ConvertFromNlohmannJSON(doc);
+
+ wm::EntityInstance tmp(e.id().withInstanceIndex(i));
+ from_aron(aron, tmp);
+
+ ins.data() = tmp.data();
+ }
+ }
+ }
+ // else leave snapshot untouched
+ }
+ });
+ });
+ });
+ });
+ TIMING_END(LTM_Convert);
+ }
+
+ void MemoryManager::encodeAndStore()
+ {
+ TIMING_START(LTM_Encode);
+ if (!checkPath())
+ {
+ // abort
+ ARMARX_WARNING << "Could not (pre)load a memory from the filesystem.";
+ return;
+ }
+
+ std::lock_guard l(cache_mutex);
+
+ // ///////////////////////////////
+ // Iterate over core segments
+ // ///////////////////////////////
+ fs::path mPath = basePathToMemory;
+ cache.forEachCoreSegment([&mPath](armem::wm::CoreSegment & e)
+ {
+ fs::path cPath = mPath / e.id().coreSegmentName;
+ if (!fs::exists(cPath))
+ {
+ // not found
+ fs::create_directory(cPath);
+ }
+ if (!fs::is_directory(cPath))
+ {
+ throw error::ArMemError("Could not create the (core) folder: " + cPath.string());
+ }
+
+ // ///////////////////////////////
+ // Iterate over provider segments
+ // ///////////////////////////////
+ e.forEachProviderSegment([&cPath](armem::wm::ProviderSegment & e)
+ {
+ fs::path pPath = cPath / e.id().providerSegmentName;
+ if (!fs::exists(pPath))
+ {
+ // not found
+ fs::create_directory(pPath);
+ }
+ if (!fs::is_directory(pPath))
+ {
+ throw error::ArMemError("Could not create the (provider) folder: " + pPath.string());
+ }
+
+ // ///////////////////////////////
+ // Iterate over entities
+ // ///////////////////////////////
+ e.forEachEntity([&pPath](armem::wm::Entity & e)
+ {
+ fs::path ePath = pPath / e.id().entityName;
+ if (!fs::exists(ePath))
+ {
+ // not found
+ fs::create_directory(ePath);
+ }
+ if (!fs::is_directory(ePath))
+ {
+ throw error::ArMemError("Could not create the (entity) folder: " + ePath.string());
+ }
+
+ // ///////////////////////////////
+ // Iterate over snapshots
+ // ///////////////////////////////
+ e.forEachSnapshot([&ePath](armem::wm::EntitySnapshot & e)
+ {
+ fs::path sPath = ePath / std::to_string(e.id().timestamp.toMicroSeconds());
+ if (!fs::exists(sPath))
+ {
+ // not found
+ fs::create_directory(sPath);
+ }
+ else
+ {
+ ARMARX_INFO << "The snapshot " << sPath.string() << " already exists. Ingoring it.";
+ return; // continue if already exists
+ }
+
+ if (!fs::is_directory(sPath))
+ {
+ throw error::ArMemError("Could not create the (timestamp) folder: " + sPath.string());
+ }
+
+ // ///////////////////////////////
+ // Iterate over instances
+ // ///////////////////////////////
+ e.forEachInstance([&sPath](armem::wm::EntityInstance & e)
+ {
+ fs::path iPath = sPath / std::to_string(e.id().instanceIndex);
+ if (!fs::exists(iPath))
+ {
+ // not found
+ fs::create_directory(iPath);
+ }
+ else
+ {
+ // This is strange, since we know, that the snapshot folder did not exist.
+ // However, we ignore and continue
+ ARMARX_INFO << "Somehow the instance folder " << iPath.string() << " already exists, although the snapshot folder was newly created. Ignore this.";
+ return;
+ }
+ if (!fs::is_directory(iPath))
+ {
+ throw error::ArMemError("Could not create the (instance) folder: " + iPath.string());
+ }
+
+ fs::path data = iPath / DATA_FILENAME;
+ if (fs::exists(data))
+ {
+ // Should not be the case. Anyway, if it happens, create new file!
+ fs::remove(data);
+ }
+
+ std::ofstream ofs;
+ ofs.open(data);
+
+ auto aron = std::make_shared<aron::datanavigator::DictNavigator>();
+ to_aron(aron, e);
+ nlohmann::json j = aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(aron);
+
+ ofs << j.dump(2);
+ ofs.close();
+ });
+ });
+ });
+ });
+
+ });
+
+ // what to do with clear text data after encoding?
+ // TODO!
+
+ // Finaly clear cache and put reference to lut
+ cache.forEachInstance([](armem::wm::EntityInstance & i)
+ {
+ i.data() = nullptr;
+ });
+
+ std::lock_guard l2(lut_mutex);
+ lut.append(cache);
+ cache.clear();
+
+ TIMING_END(LTM_Encode);
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/MemoryManager.h b/source/RobotAPI/libraries/armem/server/ltm/disk/MemoryManager.h
new file mode 100644
index 0000000000000000000000000000000000000000..227d419119e3889006a3f0f30a4789e3b9e76dfe
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/MemoryManager.h
@@ -0,0 +1,44 @@
+#pragma once
+
+// STD / STL
+#include <mutex>
+#include <optional>
+#include <filesystem>
+
+// Base Class
+#include "../LongtermMemoryBase.h"
+
+namespace armarx::armem::server::ltm::disk
+{
+ /// @brief A memory storing data in mongodb (needs 'armarx memory start' to start the mongod instance)
+ class MemoryManager :
+ public LongtermMemoryBase
+ {
+ using Base = LongtermMemoryBase;
+
+ public:
+ MemoryManager() = default;
+
+ void reload() override;
+ void convert(armem::wm::Memory&) override;
+ void encodeAndStore() override;
+
+ void setBasePath(const std::filesystem::path& p)
+ {
+ basePathToMemory = p;
+ }
+
+ protected:
+
+
+ private:
+ bool checkPath() const;
+
+ public:
+ std::filesystem::path basePathToMemory;
+
+ private:
+ static const constexpr char* TYPE_FILENAME = "type.aron.ltm.json";
+ static const constexpr char* DATA_FILENAME = "data.aron.ltm.json";
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/mongodb/ConnectionManager.cpp b/source/RobotAPI/libraries/armem/server/ltm/mongodb/ConnectionManager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ad2d95b0fed35991ae99e050a9939537a840a2a2
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/mongodb/ConnectionManager.cpp
@@ -0,0 +1,77 @@
+#include "ConnectionManager.h"
+
+namespace armarx::armem::server::ltm::mongodb
+{
+ std::mutex ConnectionManager::initializationMutex;
+ bool ConnectionManager::initialized = false;
+ std::map<std::string, std::unique_ptr<mongocxx::pool>> ConnectionManager::Connections = {};
+
+
+ void ConnectionManager::initialize_if()
+ {
+ std::lock_guard l(initializationMutex); // all others have to wait until the initialization is complete
+ if (!initialized)
+ {
+ initialized = true;
+ mongocxx::instance instance{}; // This should be done only once.
+ }
+ }
+
+ mongocxx::pool& ConnectionManager::Connect(const MongoDBSettings& settings)
+ {
+ initialize_if();
+
+ const auto uri_str = settings.uri();
+ auto it = Connections.find(uri_str);
+ if (it == Connections.end())
+ {
+ mongocxx::uri uri(uri_str);
+ auto pool = std::make_unique<mongocxx::pool>(uri);
+ auto con = Connections.emplace(settings.key(), std::move(pool));
+ return *con.first->second;
+ }
+ else
+ {
+ // A connection already exists. We do not need to open another one.
+ return *it->second;
+ }
+ }
+
+ bool ConnectionManager::ConnectionIsValid(const MongoDBSettings& settings, bool forceNewConnection)
+ {
+ initialize_if();
+
+ try
+ {
+ if (!forceNewConnection)
+ {
+ auto it = Connections.find(settings.key());
+ if (it != Connections.end())
+ {
+ auto client = it->second->acquire();
+ auto admin = client->database("admin");
+ auto result = admin.run_command(bsoncxx::builder::basic::make_document(bsoncxx::builder::basic::kvp("isMaster", 1)));
+ return true;
+ }
+ }
+
+ mongocxx::uri uri(settings.uri());
+ auto client = mongocxx::client(uri);
+ auto admin = client["admin"];
+ auto result = admin.run_command(bsoncxx::builder::basic::make_document(bsoncxx::builder::basic::kvp("isMaster", 1)));
+ return true;
+ }
+ catch (const std::exception& xcp)
+ {
+ return false;
+ }
+ }
+
+ bool ConnectionManager::ConnectionExists(const MongoDBSettings& settings)
+ {
+ initialize_if();
+
+ auto it = Connections.find(settings.key());
+ return it != Connections.end();
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/mongodb/ConnectionManager.h b/source/RobotAPI/libraries/armem/server/ltm/mongodb/ConnectionManager.h
new file mode 100644
index 0000000000000000000000000000000000000000..15fac008d7963c034ce40fe55a4606ffe3fead3f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/mongodb/ConnectionManager.h
@@ -0,0 +1,97 @@
+#pragma once
+
+#include <string>
+#include <mutex>
+#include <map>
+#include <memory>
+#include <sstream>
+
+#include <bsoncxx/json.hpp>
+#include <mongocxx/client.hpp>
+#include <mongocxx/pool.hpp>
+#include <mongocxx/stdx.hpp>
+#include <mongocxx/uri.hpp>
+#include <mongocxx/instance.hpp>
+#include <bsoncxx/builder/stream/helpers.hpp>
+#include <bsoncxx/builder/stream/document.hpp>
+#include <bsoncxx/builder/stream/array.hpp>
+
+
+namespace armarx::armem::server::ltm::mongodb
+{
+
+ using PoolClientPtr = mongocxx::pool::entry;
+
+ /**
+ * @brief A manager of multiple mongodb connection
+ */
+ class ConnectionManager
+ {
+ public:
+ struct MongoDBSettings
+ {
+ std::string host = "localhost";
+ unsigned int port = 25276;
+ std::string user = "";
+ std::string password = "";
+ std::string database = "Test";
+ int minPoolSize = 5;
+ int maxPoolSize = 100;
+
+
+ bool isSet() const
+ {
+ // we always need a host and a port
+ return !host.empty() and port != 0;
+ }
+
+ std::string baseUri() const
+ {
+ std::stringstream ss;
+ ss << "mongodb://";
+
+ if (!user.empty())
+ {
+ ss << user;
+ if (!password.empty())
+ {
+ ss << ":" << password;
+ }
+ ss << "@";
+ }
+ ss << host;
+ return ss.str();
+ }
+
+ std::string key() const
+ {
+ // TODO: What happens if a connection exists and you would like to open another one with a different user (e.g. that sees different things)?
+ return "mongodb://" + host + ":" + std::to_string(port);
+ }
+
+ std::string uri() const
+ {
+ return baseUri() + ":" + std::to_string(port) + "/?minPoolSize=" + std::to_string(minPoolSize) + "&maxPoolSize=" + std::to_string(maxPoolSize);
+ }
+
+ std::string toString() const
+ {
+ return uri() + "&database=" + database;
+ }
+ };
+
+ static mongocxx::pool& Connect(const MongoDBSettings& settings);
+ static bool ConnectionIsValid(const MongoDBSettings& settings, bool forceNewConnection = false);
+ static bool ConnectionExists(const MongoDBSettings& settings);
+
+ private:
+ static void initialize_if();
+
+
+ private:
+ static std::mutex initializationMutex;
+ static bool initialized;
+ static std::map<std::string, std::unique_ptr<mongocxx::pool>> Connections;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/mongodb/MemoryManager.cpp b/source/RobotAPI/libraries/armem/server/ltm/mongodb/MemoryManager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..73a06f5bda18fbc90f3d95be12987fc382eb2b82
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/mongodb/MemoryManager.cpp
@@ -0,0 +1,426 @@
+// Header
+#include "MemoryManager.h"
+
+// Simox
+#include <SimoxUtility/json.h>
+
+// ArmarX
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/armem/core/wm/aron_conversions.h>
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+
+
+namespace armarx::armem::server::ltm::mongodb
+{
+ namespace bsoncxxbuilder = bsoncxx::builder::stream;
+ namespace bsoncxxdoc = bsoncxx::document;
+
+ PoolClientPtr MemoryManager::checkConnection() const
+ {
+ // Check connection:
+ ARMARX_INFO << "Checking connection";
+ if (!ConnectionManager::ConnectionIsValid(dbsettings))
+ {
+ ARMARX_WARNING << deactivateSpam("ConnectionIsNotValid")
+ << "The connection to mongocxx for ltm '" << cache.name() << "' is not valid. Settings are: " << dbsettings.toString()
+ << "\nTo start it, run e.g.: \n"
+ << "armarx memory start"
+ << "\n\n";
+ return nullptr;
+ }
+
+ auto& pool = ConnectionManager::Connect(dbsettings);
+ auto client = pool.acquire();
+
+ return client;
+ }
+
+ void MemoryManager::reload()
+ {
+ TIMING_START(LTM_Reload);
+ ARMARX_INFO << "(Re)Establishing connection to: " << dbsettings.toString();
+
+ auto client = checkConnection();
+ if (!client)
+ {
+ // abort
+ ARMARX_WARNING << "A connection to: " << dbsettings.toString() << " is not possible. Could not (pre)load data from mongodb.";
+ return;
+ }
+
+ auto databases = client->list_databases();
+ for (const auto& doc : databases)
+ {
+ auto el = doc["name"];
+ ARMARX_INFO << "Found Memory-Collection in MongoDB: " << el.get_utf8().value;
+ }
+
+ armem::wm::Memory temp(lut.id()); // a temporary client wm. We will append temp to the lut at the end of this metho (append ignores duplicate entries)
+ mongocxx::database db = client->database(dbsettings.database);
+
+ ARMARX_INFO << "Loading memory: " << temp.id().str();
+
+ // ///////////////////////////////
+ // Iterate over core segments
+ // ///////////////////////////////
+ mongocxx::collection mColl = db.collection(temp.id().str());
+ mongocxx::cursor cursor = mColl.find({});
+ for (const auto& doc : cursor) // Although this looks like code duplication, we need a distinguition between memories,
+ // core, prov and entities because of a different structure
+ // (only core and prov have same structure)
+ {
+ auto el = doc[MONGO_FOREIGN_KEY];
+ auto foreignKey = el.get_utf8().value;
+
+ MemoryID i((std::string) foreignKey);
+ if (i.memoryName != temp.id().memoryName)
+ {
+ throw error::InvalidMemoryID(i, "A MemoryID in mongodb was invalid. Found the wrong memory name: " + i.str());
+ }
+
+ std::string k = i.coreSegmentName;
+
+ if (temp.hasCoreSegment(k))
+ {
+ throw error::ArMemError("Somehow the (memory) container already contains the key k = " + k + ". Do you have double entries in mongodb?");
+ }
+ else
+ {
+ // ///////////////////////////////
+ // Add and iterate over provider segments
+ // ///////////////////////////////
+ auto& cSeg = temp.addCoreSegment(k);
+ mongocxx::collection cColl = db.collection(cSeg.id().str());
+ mongocxx::cursor cursor = cColl.find({});
+ for (const auto& doc : cursor)
+ {
+ auto el = doc[MONGO_FOREIGN_KEY];
+ auto foreignKey = el.get_utf8().value;
+
+ MemoryID i((std::string) foreignKey);
+ if (i.coreSegmentName != cSeg.id().coreSegmentName || i.memoryName != cSeg.id().memoryName)
+ {
+ throw error::InvalidMemoryID(i, "A MemoryID in mongodb was invalid. Found the wrong memory name: " + i.str());
+ }
+
+ std::string k = i.providerSegmentName;
+ if (cSeg.hasProviderSegment(k))
+ {
+ throw error::ArMemError("Somehow the (core segment) container already contains the key k = " + k + ". Do you have double entries in mongodb?");
+ }
+ else
+ {
+ // ///////////////////////////////
+ // Add and iterate over entities
+ // ///////////////////////////////
+ auto& pSeg = cSeg.addProviderSegment(k);
+ mongocxx::collection pColl = db.collection(pSeg.id().str());
+ mongocxx::cursor cursor = pColl.find({});
+ for (const auto& doc : cursor)
+ {
+ auto el = doc[MONGO_FOREIGN_KEY];
+ auto foreignKey = el.get_utf8().value;
+
+ MemoryID i((std::string) foreignKey);
+ if (i.providerSegmentName != pSeg.id().providerSegmentName || i.coreSegmentName != pSeg.id().coreSegmentName || i.memoryName != pSeg.id().memoryName)
+ {
+ throw error::InvalidMemoryID(i, "A MemoryID in mongodb was invalid. Found the wrong memory name: " + i.str());
+ }
+
+ std::string k = i.entityName;
+ if (pSeg.hasEntity(k))
+ {
+ throw error::ArMemError("Somehow the (provider segment) container already contains the key k = " + k + ". Do you have double entries in mongodb?");
+ }
+ else
+ {
+ // ///////////////////////////////
+ // Add and iterate over snapshots
+ // ///////////////////////////////
+ auto& eSeg = pSeg.addEntity(k);
+ mongocxx::collection eColl = db.collection(eSeg.id().str());
+ mongocxx::cursor cursor = eColl.find({});
+ for (const auto& doc : cursor)
+ {
+ auto ts = armem::Time::microSeconds(doc[MONGO_TIMESTAMP].get_int64().value);
+ auto& newSnapshot = eSeg.addSnapshot(ts);
+
+ auto i = doc[MONGO_INSTANCES];
+ unsigned long length = std::distance(i.get_array().value.begin(), i.get_array().value.end());
+ ARMARX_INFO << "The array legth for an snapshot '" << newSnapshot.id().str() << "' is: " << length;
+
+ for (unsigned long i = 0; i < length; ++i)
+ {
+ // add an empty instance as reference
+ newSnapshot.addInstance({});
+ }
+
+
+ // check to update lastSnapshot map
+ checkUpdateLatestSnapshot(newSnapshot);
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+
+ std::lock_guard l(cache_mutex); // we always take the cache mutex BEFORE the lut mutex! (otherwise we may have deadlocks)
+ std::lock_guard l2(lut_mutex);
+ lut.append(temp);
+ ARMARX_INFO << "After reload memory " << lut.id().str() << " has size: " << lut.size();
+
+ TIMING_END(LTM_Reload);
+ }
+
+ void MemoryManager::convert(armem::wm::Memory& m)
+ {
+ TIMING_START(LTM_Convert);
+
+ auto client = checkConnection();
+ if (!client)
+ {
+ // abort
+ ARMARX_WARNING << "A connection to: " << dbsettings.toString() << " is not possible. Could not convert information, therefore returning leaving input untouched.";
+ return;
+ }
+ mongocxx::database db = client->database(dbsettings.database);
+
+ // update emtpy data ptr
+ m.forEachCoreSegment([&db](armem::wm::CoreSegment & e)
+ {
+ e.forEachProviderSegment([&db](armem::wm::ProviderSegment & e)
+ {
+ e.forEachEntity([&db](armem::wm::Entity & e)
+ {
+ e.forEachSnapshot([&db](armem::wm::EntitySnapshot & e)
+ {
+ // check whether data is nullptr
+ bool allDataIsNull = e.size() > 0;
+ e.forEachInstance([&allDataIsNull](armem::wm::EntityInstance & e)
+ {
+ if (e.data())
+ {
+ allDataIsNull = false;
+ return false; // means break
+ }
+ return true;
+ });
+
+ if (allDataIsNull) // an entry from the lut (probably... for now we assume that every entry either has data (cache) or has null (lut))
+ {
+ // Get data from mongodb
+ auto eColl = db.collection(e.id().getEntityID().str());
+ auto q = bsoncxxbuilder::document{} << std::string(MONGO_TIMESTAMP) << e.id().timestamp.toMicroSeconds() << bsoncxxbuilder::finalize;
+ auto res = eColl.find_one(q.view());
+
+ if (!res)
+ {
+ throw error::ArMemError("Could not load an instance from the memory '" + e.id().getEntityID().str() + "'. Tried to access: " + e.id().getEntitySnapshotID().str());
+ }
+
+ // convert full json of this entry
+ nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(*res));
+ nlohmann::json instances = json[MONGO_INSTANCES];
+
+ if (instances.size() != e.size())
+ {
+ throw error::ArMemError("The size of the mongodb entity entry at id " + e.id().getEntitySnapshotID().str() + " has wrong size. Expected: " + std::to_string(e.size()) + " but got: " + std::to_string(instances.size()));
+ }
+
+ for (unsigned int i = 0; i < e.size(); ++i)
+ {
+ auto& ins = e.getInstance(i);
+
+ // get ionstance json
+ nlohmann::json doc = instances[i];
+ auto aron = aron::converter::AronNlohmannJSONConverter::ConvertFromNlohmannJSON(doc);
+
+ // remove metadata
+ wm::EntityInstance tmp(e.id().withInstanceIndex(i));
+ from_aron(aron, tmp);
+
+ // set data
+ ins.data() = tmp.data();
+ }
+ }
+ });
+ });
+ });
+ });
+ TIMING_END(LTM_Convert);
+ }
+
+ void MemoryManager::encodeAndStore()
+ {
+ TIMING_START(LTM_Encode);
+ ARMARX_INFO << "Encode cache " << cache.id().str() << " with size: " << cache.size();
+
+ auto client = checkConnection();
+ if (!client)
+ {
+ // abort
+ ARMARX_WARNING << "A connection to: " << dbsettings.toString() << " is not possible. Could not consolidate data from cache to mongodb.";
+ return;
+ }
+
+ std::lock_guard l(cache_mutex);
+ mongocxx::database db = client->database(dbsettings.database);
+ auto mColl = db.collection(cache.id().str());
+
+ // ///////////////////////////////
+ // Iterate over core segments
+ // ///////////////////////////////
+ cache.forEachCoreSegment([this, &db, &mColl](armem::wm::CoreSegment & e)
+ {
+ auto cColl = db.collection(e.id().str());
+
+ if (!containsCoreSegment(mColl, e.id()))
+ {
+ // not found
+ mongodbInsertForeignKey(mColl, e.id().str());
+ }
+
+ // ///////////////////////////////
+ // Iterate over provider segments
+ // ///////////////////////////////
+ e.forEachProviderSegment([this, &db, &cColl](armem::wm::ProviderSegment & e)
+ {
+ auto pColl = db.collection(e.id().str());
+
+ if (!containsProviderSegment(cColl, e.id()))
+ {
+ // not found
+ mongodbInsertForeignKey(cColl, e.id().str());
+ }
+
+ // ///////////////////////////////
+ // Iterate over each segments
+ // ///////////////////////////////
+ e.forEachEntity([this, &db, &pColl](armem::wm::Entity & e)
+ {
+ auto eColl = db.collection(e.id().str());
+
+ if (!containsEntity(pColl, e.id()))
+ {
+ // not found
+ mongodbInsertForeignKey(pColl, e.id().str());
+ }
+
+ // ///////////////////////////////
+ // Iterate over snapshots
+ // ///////////////////////////////
+ e.forEachSnapshot([this, &eColl](armem::wm::EntitySnapshot & e)
+ {
+ if (!this->containsSnapshot(eColl, e.id()))
+ {
+ // timestamp not found in mongodb ==> new entry
+ bsoncxxbuilder::document builder{};
+ auto in_array = builder
+ << std::string(MONGO_ID) << e.id().str()
+ << std::string(MONGO_TIMESTAMP) << e.id().timestamp.toMicroSeconds()
+ << std::string(MONGO_INSTANCES);
+ auto array_builder = bsoncxx::builder::basic::array{};
+
+ e.forEachInstance([&array_builder](const wm::EntityInstance & e)
+ {
+ auto aron = std::make_shared<aron::datanavigator::DictNavigator>();
+ to_aron(aron, e);
+ nlohmann::json j = aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(aron);
+
+ auto doc_value = bsoncxx::from_json(j.dump(2));
+ array_builder.append(doc_value);
+ });
+
+ auto after_array = in_array << array_builder;
+ bsoncxx::document::value doc = after_array << bsoncxx::builder::stream::finalize;
+ ARMARX_INFO << "Insert into mongodb: " << e.id().timestamp.toMicroSeconds();
+ eColl.insert_one(doc.view());
+
+ // check to update lastSnapshot map
+ checkUpdateLatestSnapshot(e);
+ }
+ else
+ {
+ ARMARX_INFO << "Timestamp already found for id: " << e.id().str();
+ // timestamp already in mongodb. Ignore it
+ }
+ });
+ });
+ });
+ });
+
+ // what to do with clear text data after encoding?
+ // TODO!
+
+ // Finaly clear cache and put reference to lut
+ cache.forEachInstance([](armem::wm::EntityInstance & i)
+ {
+ i.data() = nullptr;
+ });
+
+ std::lock_guard l2(lut_mutex);
+ lut.append(cache);
+ cache.clear();
+
+ TIMING_END(LTM_Encode);
+ }
+
+ void MemoryManager::mongodbInsertForeignKey(mongocxx::collection& coll, const std::string& key)
+ {
+ auto q = bsoncxxbuilder::document{} << std::string(MONGO_FOREIGN_KEY) << key << bsoncxxbuilder::finalize;
+ coll.insert_one(q.view());
+ }
+
+ bool MemoryManager::mongodbContainsForeignKey(mongocxx::collection& coll, const std::string& key) const
+ {
+ // check mongodb
+ auto q = bsoncxxbuilder::document{} << std::string(MONGO_FOREIGN_KEY) << key << bsoncxxbuilder::finalize;
+ auto res = coll.find_one(q.view());
+ return (bool) res;
+ }
+
+ bool MemoryManager::containsCoreSegment(mongocxx::collection& mColl, const MemoryID& coreSegmentID) const
+ {
+ if (Base::containsCoreSegment(coreSegmentID))
+ {
+ return true;
+ }
+ return mongodbContainsForeignKey(mColl, coreSegmentID.str());
+ }
+
+ bool MemoryManager::containsProviderSegment(mongocxx::collection& cColl, const MemoryID& providerSegmentID) const
+ {
+ if (Base::containsProviderSegment(providerSegmentID))
+ {
+ return true;
+ }
+ return mongodbContainsForeignKey(cColl, providerSegmentID.str());
+ }
+
+ bool MemoryManager::containsEntity(mongocxx::collection& pColl, const MemoryID& entityID) const
+ {
+ if (Base::containsEntity(entityID))
+ {
+ return true;
+ }
+ return mongodbContainsForeignKey(pColl, entityID.str());
+ }
+
+ bool MemoryManager::containsSnapshot(mongocxx::collection& eColl, const MemoryID& snapshotID) const
+ {
+ if (Base::containsSnapshot(snapshotID))
+ {
+ return true;
+ }
+
+ // check mongodb
+ auto q = bsoncxxbuilder::document{} << std::string(MONGO_TIMESTAMP) << snapshotID.timestamp.toMicroSeconds() << bsoncxxbuilder::finalize;
+ auto res = eColl.find_one(q.view());
+ return (bool) res;
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/mongodb/MemoryManager.h b/source/RobotAPI/libraries/armem/server/ltm/mongodb/MemoryManager.h
new file mode 100644
index 0000000000000000000000000000000000000000..189ccc42563d17dd63116dc3210d99d648b28c31
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/mongodb/MemoryManager.h
@@ -0,0 +1,51 @@
+#pragma once
+
+// STD / STL
+#include <mutex>
+#include <optional>
+
+// Base Class
+#include "../LongtermMemoryBase.h"
+
+// Data
+# include "ConnectionManager.h"
+
+namespace armarx::armem::server::ltm::mongodb
+{
+ /// @brief A memory storing data in mongodb (needs 'armarx memory start' to start the mongod instance)
+ class MemoryManager :
+ public LongtermMemoryBase
+ {
+ using Base = LongtermMemoryBase;
+
+ public:
+ MemoryManager() = default;
+
+ void reload() override;
+ void convert(armem::wm::Memory&) override;
+ void encodeAndStore() override;
+
+ protected:
+
+
+ private:
+ PoolClientPtr checkConnection() const; // return nullptr if not possible
+
+ void mongodbInsertForeignKey(mongocxx::collection&, const std::string& key);
+
+ bool mongodbContainsForeignKey(mongocxx::collection&, const std::string& key) const;
+ bool containsCoreSegment(mongocxx::collection&, const MemoryID&) const;
+ bool containsProviderSegment(mongocxx::collection&, const MemoryID&) const;
+ bool containsEntity(mongocxx::collection&, const MemoryID&) const;
+ bool containsSnapshot(mongocxx::collection&, const MemoryID&) const;
+
+ public:
+ ConnectionManager::MongoDBSettings dbsettings;
+
+ private:
+ static const constexpr char* MONGO_FOREIGN_KEY = "foreign_key";
+ static const constexpr char* MONGO_ID = "id";
+ static const constexpr char* MONGO_TIMESTAMP = "timestamp";
+ static const constexpr char* MONGO_INSTANCES = "instances";
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/plugins.h b/source/RobotAPI/libraries/armem/server/plugins.h
new file mode 100644
index 0000000000000000000000000000000000000000..9f1255994daba045756a8c90dcbcfd57a9c63a11
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/plugins.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#include "plugins/Plugin.h"
+#include "plugins/ReadOnlyPluginUser.h"
+#include "plugins/ReadWritePluginUser.h"
+
+
+namespace armarx::armem::server
+{
+ using plugins::Plugin;
+ using plugins::ReadOnlyPluginUser;
+ using plugins::ReadWritePluginUser;
+}
+
+namespace armarx::armem
+{
+ using ServerPlugin = server::Plugin;
+ using ReadOnlyServerPluginUser = server::ReadOnlyPluginUser;
+ using ReadWriteServerPluginUser = server::ReadWritePluginUser;
+}
diff --git a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..031e8fbdfe55b1024bac050b341327e6ef16e482
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
@@ -0,0 +1,125 @@
+#include "Plugin.h"
+
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/client/plugins/Plugin.h>
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx::armem::server::plugins
+{
+
+ Plugin::~Plugin() = default;
+
+
+ Plugin::Plugin(ManagedIceObject& parent, std::string prefix) :
+ armarx::ComponentPlugin(parent, prefix)
+ {
+ addPlugin(clientPlugin);
+ ARMARX_CHECK_NOT_NULL(clientPlugin);
+ addPluginDependency(clientPlugin);
+ }
+
+
+ void Plugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
+ {
+ const std::string prefix = "mem.";
+ if (not workingMemory.name().empty()
+ and not properties->hasDefinition(prefix + "MemoryName"))
+ {
+ properties->optional(workingMemory.name(), prefix + "MemoryName", "Name of this memory server.");
+ }
+ properties->optional(longtermMemoryEnabled, prefix + "ltm.00_enabled");
+
+
+ // Publish memory updates topic
+ properties->topic(memoryTopic, memoryTopicDefaultName);
+ }
+
+
+ void Plugin::postOnConnectComponent()
+ {
+ Component& parent = this->parent<Component>();
+
+ if (clientPlugin->isMemoryNameSystemEnabled() and clientPlugin->getMemoryNameSystemClient())
+ {
+ registerServer(parent);
+ }
+ iceAdapter.setMemoryListener(memoryTopic);
+
+ // establishing connection to ltm and mongodb
+ if (longtermMemoryEnabled)
+ {
+ longtermMemoryManager.reload();
+ }
+ }
+
+
+ void Plugin::preOnDisconnectComponent()
+ {
+ if (clientPlugin->isMemoryNameSystemEnabled() and clientPlugin->getMemoryNameSystemClient())
+ {
+ removeServer();
+ }
+ }
+
+
+ void Plugin::setMemoryName(const std::string& memoryName)
+ {
+ workingMemory.name() = memoryName;
+ longtermMemoryManager.setName(memoryName);
+ }
+
+
+ mns::dto::RegisterServerResult Plugin::registerServer(armarx::Component& parent)
+ {
+ MemoryID id = MemoryID().withMemoryName(workingMemory.name());
+ mns::dto::MemoryServerInterfaces server;
+ server.reading = ReadingMemoryInterfacePrx::uncheckedCast(parent.getProxy());
+ server.writing = WritingMemoryInterfacePrx::uncheckedCast(parent.getProxy());
+
+ mns::dto::RegisterServerResult result;
+ try
+ {
+ clientPlugin->getMemoryNameSystemClient().registerServer(id, server);
+ result.success = true;
+ ARMARX_DEBUG << "Registered memory server for " << id << " in the Memory Name System (MNS).";
+ }
+ catch (const armem::error::ServerRegistrationOrRemovalFailed& e)
+ {
+ result.success = false;
+ result.errorMessage = e.what();
+ ARMARX_WARNING << e.what();
+ }
+ return result;
+ }
+
+
+ mns::dto::RemoveServerResult Plugin::removeServer()
+ {
+ MemoryID id = MemoryID().withMemoryName(workingMemory.name());
+
+ mns::dto::RemoveServerResult result;
+ try
+ {
+ clientPlugin->getMemoryNameSystemClient().removeServer(id);
+ result.success = true;
+ ARMARX_DEBUG << "Removed memory server for " << id << " from the Memory Name System (MNS).";
+ }
+ catch (const armem::error::ServerRegistrationOrRemovalFailed& e)
+ {
+ result.success = false;
+ result.errorMessage = e.what();
+ ARMARX_WARNING << e.what();
+ }
+ catch (const Ice::NotRegisteredException&)
+ {
+ // It's ok, the MNS is gone.
+ result.success = false;
+ result.errorMessage = "Memory Name System is gone.";
+ }
+ return result;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/plugins/Plugin.h b/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..49cc07bb08dc7265fa797e2f5f77a3d81511c33c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
@@ -0,0 +1,105 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/ltm/mongodb/MemoryManager.h>
+
+#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+
+#include <ArmarXCore/core/ComponentPlugin.h>
+
+
+namespace armarx
+{
+ class Component;
+}
+namespace armarx::armem::client::plugins
+{
+ class Plugin;
+}
+
+namespace armarx::armem::server::plugins
+{
+
+ class Plugin :
+ public armarx::ComponentPlugin
+ {
+ public:
+
+ Plugin(ManagedIceObject& parent, std::string prefix);
+ virtual ~Plugin() override;
+
+
+ virtual void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ virtual void postOnConnectComponent() override;
+ virtual void preOnDisconnectComponent() override;
+
+
+ public:
+
+ /// Set the name of the wm and the ltm (if enabled).
+ void setMemoryName(const std::string& memoryName);
+
+
+ protected:
+
+ /**
+ * @brief Register the parent component in the MNS.
+ *
+ * Called before onConnect() if MNS is enabled.
+ */
+ mns::dto::RegisterServerResult registerServer(armarx::Component& parent);
+
+ /**
+ * @brief Remove the parent component from the MNS.
+ *
+ * Called before onDisconnect() if MNS is enabled.
+ */
+ mns::dto::RemoveServerResult removeServer();
+
+
+
+ public:
+
+ // Working Memory
+
+ /// The actual memory.
+ server::wm::Memory workingMemory;
+
+ /// Helps connecting `memory` to ice. Used to handle Ice callbacks.
+ MemoryToIceAdapter iceAdapter { &workingMemory, &longtermMemoryManager};
+
+
+ // Working Memory Updates (publishing)
+
+ std::string memoryTopicDefaultName = "MemoryUpdates";
+ client::MemoryListenerInterfacePrx memoryTopic;
+
+
+ // Long-Term Memory
+
+ /// Indicates whether to use or not to use the ltm feature.
+ bool longtermMemoryEnabled = true;
+
+ /// A manager class for the ltm. It internally holds a normal wm instance as a cache.
+ server::ltm::mongodb::MemoryManager longtermMemoryManager;
+
+ std::string longTermMemoryDatabaseHost;
+ std::string longTermMemoryDatabaseUser;
+ std::string longTermMemoryDatabasePassword;
+
+
+ private:
+
+ client::plugins::Plugin* clientPlugin;
+
+ };
+}
+
+namespace armarx::armem::server
+{
+ using plugins::Plugin;
+}
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadOnlyPluginUser.cpp b/source/RobotAPI/libraries/armem/server/plugins/ReadOnlyPluginUser.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b95b94afb85f31efd41384283409460b706a729e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadOnlyPluginUser.cpp
@@ -0,0 +1,56 @@
+#include "ReadOnlyPluginUser.h"
+#include "Plugin.h"
+
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+
+namespace armarx::armem::server::plugins
+{
+
+ ReadOnlyPluginUser::ReadOnlyPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+
+ ReadOnlyPluginUser::~ReadOnlyPluginUser()
+ {
+ }
+
+
+ void ReadOnlyPluginUser::setMemoryName(const std::string& memoryName)
+ {
+ plugin->setMemoryName(memoryName);
+ }
+
+
+ armem::query::data::Result ReadOnlyPluginUser::query(const armem::query::data::Input& input, const Ice::Current&)
+ {
+ ARMARX_TRACE;
+ return iceAdapter().query(input);
+ }
+
+
+ Plugin& ReadOnlyPluginUser::memoryServerPlugin()
+ {
+ return *plugin;
+ }
+
+
+ wm::Memory& ReadOnlyPluginUser::workingMemory()
+ {
+ return plugin->workingMemory;
+ }
+
+
+ MemoryToIceAdapter& ReadOnlyPluginUser::iceAdapter()
+ {
+ return plugin->iceAdapter;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadOnlyPluginUser.h b/source/RobotAPI/libraries/armem/server/plugins/ReadOnlyPluginUser.h
new file mode 100644
index 0000000000000000000000000000000000000000..f56d66b0f35e173d3076dedb729a7dc77ab9bb6b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadOnlyPluginUser.h
@@ -0,0 +1,61 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/server/forward_declarations.h>
+
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+#include <RobotAPI/interface/armem/server/MemoryInterface.h>
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
+
+namespace armarx::armem::server::plugins
+{
+
+ class Plugin;
+
+
+ /**
+ * @brief Base class of memory server components.
+ *
+ * Implements the server ice interfaces using the ice adapter of the plugin.
+ */
+ class ReadOnlyPluginUser :
+ virtual public ManagedIceObject
+ , virtual public ReadingMemoryInterface
+ , virtual public client::plugins::PluginUser
+ {
+ public:
+
+ ReadOnlyPluginUser();
+ virtual ~ReadOnlyPluginUser() override;
+
+
+ void setMemoryName(const std::string& memoryName);
+
+
+ // ReadingInterface interface
+ virtual armem::query::data::Result query(
+ const armem::query::data::Input& input,
+ const Ice::Current& = Ice::emptyCurrent) override;
+
+
+ public:
+
+ Plugin& memoryServerPlugin();
+
+ server::wm::Memory& workingMemory();
+ MemoryToIceAdapter& iceAdapter();
+
+
+ private:
+
+ plugins::Plugin* plugin = nullptr;
+
+ };
+
+}
+
+namespace armarx::armem::server
+{
+ using plugins::ReadOnlyPluginUser;
+}
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f3da4f3ee885a2f1cda0a86ad195ae466f4fb9e5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
@@ -0,0 +1,100 @@
+#include "ReadWritePluginUser.h"
+#include "Plugin.h"
+
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+
+namespace armarx::armem::server::plugins
+{
+
+ ReadWritePluginUser::ReadWritePluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+
+ ReadWritePluginUser::~ReadWritePluginUser()
+ {
+ }
+
+
+ void ReadWritePluginUser::setMemoryName(const std::string& memoryName)
+ {
+ plugin->setMemoryName(memoryName);
+ }
+
+
+ // WRITING
+ data::AddSegmentsResult ReadWritePluginUser::addSegments(const data::AddSegmentsInput& input, const Ice::Current&)
+ {
+ ARMARX_TRACE;
+ bool addCoreSegmentOnUsage = false;
+ return addSegments(input, addCoreSegmentOnUsage);
+ }
+
+ data::AddSegmentsResult ReadWritePluginUser::addSegments(const data::AddSegmentsInput& input, bool addCoreSegments)
+ {
+ ARMARX_TRACE;
+ data::AddSegmentsResult result = iceAdapter().addSegments(input, addCoreSegments);
+ return result;
+ }
+
+
+ data::CommitResult ReadWritePluginUser::commit(const data::Commit& commitIce, const Ice::Current&)
+ {
+ ARMARX_TRACE;
+ return iceAdapter().commit(commitIce);
+ }
+
+
+ // READING
+ armem::query::data::Result ReadWritePluginUser::query(const armem::query::data::Input& input, const Ice::Current&)
+ {
+ ARMARX_TRACE;
+ return iceAdapter().query(input);
+ }
+
+
+ // LTM STORING
+ data::StoreResult ReadWritePluginUser::store(const data::StoreInput& input, const Ice::Current&)
+ {
+ ARMARX_TRACE;
+ return iceAdapter().store(input);
+ }
+
+
+ Plugin& ReadWritePluginUser::memoryServerPlugin()
+ {
+ return *plugin;
+ }
+
+
+ wm::Memory& ReadWritePluginUser::workingMemory()
+ {
+ return plugin->workingMemory;
+ }
+
+
+ MemoryToIceAdapter& ReadWritePluginUser::iceAdapter()
+ {
+ return plugin->iceAdapter;
+ }
+
+
+ bool ReadWritePluginUser::isLongtermMemoryEnabled()
+ {
+ return plugin->longtermMemoryEnabled;
+ }
+
+
+ ltm::mongodb::MemoryManager& ReadWritePluginUser::longtermMemoryManager()
+ {
+ return plugin->longtermMemoryManager;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h
new file mode 100644
index 0000000000000000000000000000000000000000..6736224199489fb171d49b092fd91e88e59fd9b0
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h
@@ -0,0 +1,74 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/server/forward_declarations.h>
+
+#include <RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h>
+#include <RobotAPI/interface/armem/server/MemoryInterface.h>
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
+
+namespace armarx::armem::server::plugins
+{
+
+ class Plugin;
+
+
+ /**
+ * @brief Base class of memory server components.
+ *
+ * Implements the server ice interfaces using the ice adapter of the plugin.
+ */
+ class ReadWritePluginUser :
+ virtual public ManagedIceObject
+ , virtual public MemoryInterface
+ , virtual public client::plugins::ListeningPluginUser
+ {
+ public:
+
+ ReadWritePluginUser();
+ virtual ~ReadWritePluginUser() override;
+
+
+ void setMemoryName(const std::string& memoryName);
+
+
+ // WritingInterface interface
+ virtual data::AddSegmentsResult addSegments(const data::AddSegmentsInput& input, const Ice::Current& = Ice::emptyCurrent) override;
+ data::AddSegmentsResult addSegments(const data::AddSegmentsInput& input, bool addCoreSegments);
+
+ virtual data::CommitResult commit(const data::Commit& commit, const Ice::Current& = Ice::emptyCurrent) override;
+
+
+ // ReadingInterface interface
+ virtual armem::query::data::Result query(const armem::query::data::Input& input, const Ice::Current& = Ice::emptyCurrent) override;
+
+
+ // StoringInterface interface
+ virtual data::StoreResult store(const data::StoreInput&, const Ice::Current& = Ice::emptyCurrent) override;
+
+
+ public:
+
+ Plugin& memoryServerPlugin();
+
+ server::wm::Memory& workingMemory();
+ MemoryToIceAdapter& iceAdapter();
+
+ bool isLongtermMemoryEnabled();
+ server::ltm::mongodb::MemoryManager& longtermMemoryManager();
+
+
+ private:
+
+ plugins::Plugin* plugin = nullptr;
+
+ };
+
+}
+
+namespace armarx::armem::server
+{
+ using plugins::ReadWritePluginUser;
+}
+
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 34e2abd99c889d65723065c8b4726b2ea0aa9bd1..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,2 +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 2af92450309d640122bffc9066c1a1df7bdfff49..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:
virtual ~BaseQueryProcessorBase() = default;
- DataT process(const QueryT& query, const DataT& data) const
+ ResultT process(const QueryT& query, const DataT& data) const
{
- DataT result = data.copyEmpty();
- if (getTargets(query.targets).count(getTargetType()))
+ ResultT result { data.id() };
+ if (detail::getTargets(query.targets).count(queryTarget))
{
this->process(result, query, data);
}
return result;
}
- DataT process(const QueryPtrT& query, const DataT& data) const
+ ResultT process(const QueryPtrT& query, const DataT& data) const
{
return this->process(*query, *data);
}
- DataT process(const QuerySeqT& queries, const DataT& data) const
+ ResultT process(const QuerySeqT& queries, const DataT& data) const
{
- DataT result = data.copyEmpty();
+ ResultT result { data.id() };
this->process(result, queries, data);
return result;
}
- void process(DataT& result, const QuerySeqT& queries, const DataT& data) const
+ void process(ResultT& result, const QuerySeqT& queries, const DataT& data) const
{
if (queries.empty())
{
- ARMARX_DEBUG << "There are no queries to process.";
return;
}
for (const auto& query : queries)
{
- if (getTargets(query->targets).count(getTargetType()))
+ if (detail::getTargets(query->targets).count(queryTarget))
{
this->process(result, *query, data);
}
}
}
- virtual void process(DataT& result, const QueryT& query, const DataT& data) const = 0;
-
- protected:
+ /**
+ * @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;
- virtual query::data::QueryTarget getTargetType() const = 0;
-
-
- private:
+ };
- /// 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.
- static std::set<query::data::QueryTarget> 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/CoreSegmentQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h
index 9d7b2518227e4ddf0c5d759963af193656e39894..0c6199eac95465a27da8bc0d59b4fcd40607d139 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h
@@ -1,39 +1,51 @@
#pragma once
-#include <regex>
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include "BaseQueryProcessorBase.h"
-#include <RobotAPI/interface/armem/query.h>
#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/interface/armem/query.h>
-#include "BaseQueryProcessorBase.h"
-#include "ProviderSegmentQueryProcessorBase.h"
+#include <regex>
-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;
- virtual void process(_CoreSegmentT& result,
+ virtual void process(ResultCoreSegmentT& result,
const armem::query::data::CoreSegmentQuery& query,
- const _CoreSegmentT& coreSegment) const override
+ const CoreSegmentT& coreSegment) const override
{
if (auto q = dynamic_cast<const armem::query::data::core::All*>(&query))
{
@@ -53,46 +65,57 @@ 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));
- }
+ this->_processResult(result, providerSegment, query);
+ });
}
- 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.findProviderSegment(query.providerSegmentName);
+ if (providerSegment)
{
- const ProviderSegmentT& providerSegment = coreSegment.getProviderSegment(query.providerSegmentName);
- result.addProviderSegment(providerSegmentProcessorProcess(query.providerSegmentQueries, providerSegment));
- }
- catch (const error::MissingEntry&)
- {
- // Leave empty.
+ this->_processResult(result, *providerSegment, query);
}
}
- 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())
+ const std::regex regex(query.providerSegmentNameRegex);
+ coreSegment.forEachProviderSegment(
+ [this, &result, &query, ®ex](const ProviderSegmentT & providerSegment)
{
if (std::regex_search(providerSegment.name(), regex))
{
- result.addProviderSegment(providerSegmentProcessorProcess(query.providerSegmentQueries, providerSegment));
+ this->_processResult(result, providerSegment, query);
}
- }
+ });
}
+
+ private:
+
+ void _processResult(ResultCoreSegmentT& result,
+ const ProviderSegmentT& providerSegment,
+ const armem::query::data::CoreSegmentQuery& query) const
+ {
+ ResultProviderSegmentT& added = result.addProviderSegment(providerSegment.name(), providerSegment.aronType());
+ childProcessor.process(added, 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.cpp b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.cpp
index 5cbac85a8dc5f8899e7c72434516063121a2b4c1..d9df528842d11fd66ba38877318785d6c4dd5263 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.cpp
@@ -1 +1,12 @@
#include "EntityQueryProcessorBase.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx::armem::server::query_proc::base
+{
+ void detail::checkReferenceTimestampNonNegative(const Time& timestamp)
+ {
+ ARMARX_CHECK_NONNEGATIVE(timestamp.toMicroSeconds()) << "Reference timestamp must be non-negative.";
+ }
+}
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..b0210c37e4ed71b64875a79453e56d928852b54f 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,43 @@
#pragma once
-#include <cstdint>
-#include <iterator>
-
-#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 "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 "BaseQueryProcessorBase.h"
-#include "RobotAPI/libraries/armem/core/Time.h"
+#include <RobotAPI/interface/armem/query.h>
+
+#include <cstdint>
+#include <iterator>
-namespace armarx::armem::base::query_proc
+namespace armarx::armem::server::query_proc::base::detail
+{
+ void checkReferenceTimestampNonNegative(const Time& timestamp);
+}
+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,69 +73,62 @@ 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)
{
- try
+ if (const ResultSnapshotT* snapshot = entity.findLatestSnapshot())
{
- addResultSnapshot(result, entity.getLatestSnapshot());
- }
- catch (const armem::error::EntityHistoryEmpty&)
- {
- if (false)
- {
- ARMARX_IMPORTANT << "Failed to retrieve latest snapshot from entity " << entity.id() << ". "
- << "Entity has not timestamps.";
- }
+ addResultSnapshot(result, *snapshot);
}
}
else
{
const Time time = fromIce<Time>(query.timestamp);
- try
+ if (const ResultSnapshotT* snapshot = entity.findSnapshot(time))
{
- auto snapshot = entity.getSnapshot(time);
- addResultSnapshot(result, snapshot);
+ addResultSnapshot(result, *snapshot);
}
- catch (const armem::error::MissingEntry&)
+ else
{
// Leave empty.
- if (false)
+#if 0
+ std::stringstream ss;
+ ss << "Failed to retrieve snapshot with timestamp "
+ << armem::toDateTimeMilliSeconds(time)
+ << " from entity " << entity.id() << ".\n"
+ << "Entity has timestamps: ";
+ for (const Time& t : entity.getTimestamps())
{
- std::stringstream ss;
- ss << "Failed to retrieve snapshot with timestamp "
- << armem::toDateTimeMilliSeconds(time)
- << " from entity " << entity.id() << ".\n"
- << "Entity has timestamps: ";
- for (const Time& t : entity.getTimestamps())
- {
- ss << "\n- " << armem::toDateTimeMilliSeconds(t);
- }
- ARMARX_IMPORTANT << ss.str();
+ ss << "\n- " << armem::toDateTimeMilliSeconds(t);
}
+ ARMARX_IMPORTANT << ss.str();
+#endif
}
}
}
- 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,109 +138,90 @@ 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())
+ entity.forEachSnapshotInIndexRange(
+ query.first, query.last,
+ [this, &result](const EntitySnapshotT & snapshot)
{
- return;
- }
-
- size_t first = negativeIndexSemantics(query.first, entity.history().size());
- size_t last = negativeIndexSemantics(query.last, entity.history().size());
-
- if (first <= last)
- {
- 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!";
+ const Time referenceTimestamp = fromIce<Time>(query.timestamp);
+ detail::checkReferenceTimestampNonNegative(referenceTimestamp);
- try
+ if (auto* beforeOrAt = entity.findLatestSnapshotBeforeOrAt(referenceTimestamp))
{
- auto beforeOrAt = entity.getLatestSnapshotBeforeOrAt(referenceTimestamp);
- addResultSnapshot(result, beforeOrAt);
- }
- catch (const armem::error::MissingEntry&)
- {
- // Leave empty.
+ addResultSnapshot(result, *beforeOrAt);
}
}
- 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 Time referenceTimestamp = fromIce<Time>(query.timestamp);
+ detail::checkReferenceTimestampNonNegative(referenceTimestamp);
- 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
+ {
+ num = std::min(befores.size(), static_cast<size_t>(query.maxEntries));
+ }
+
+ for (size_t r = 0; r < num; ++r)
{
- // Leave empty.
+ 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!";
+ const Time referenceTimestamp = fromIce<Time>(query.timestamp);
+ detail::checkReferenceTimestampNonNegative(referenceTimestamp);
- const auto referenceTimestampMicroSeconds = referenceTimestamp.toMicroSeconds();
- const auto epsDuration = fromIce<Time>(query.eps).toMicroSeconds();
+ const float referenceTimestampMicroSeconds = referenceTimestamp.toMicroSeconds();
+ const float epsDuration = fromIce<Time>(query.eps).toMicroSeconds();
// elements have to be in range [t_ref - eps, t_ref + eps] if eps is positive
const auto isInRange = [&](const Time & t) -> bool
@@ -258,15 +234,14 @@ namespace armarx::armem::base::query_proc
return std::abs(t.toMicroSeconds() - referenceTimestampMicroSeconds) <= epsDuration;
};
- try
+ // last element before or at timestamp
+ if (auto* beforeOrAt = entity.findLatestSnapshotBeforeOrAt(referenceTimestamp))
{
- // last element before or at timestamp
- auto beforeOrAt = entity.getLatestSnapshotBeforeOrAt(referenceTimestamp);
- const auto timestampOfMatchBefore = beforeOrAt.id().timestamp;
+ const auto timestampOfMatchBefore = beforeOrAt->id().timestamp;
const auto isPerfectMatch = timestampOfMatchBefore == referenceTimestamp;
if (isInRange(timestampOfMatchBefore))
{
- addResultSnapshot(result, beforeOrAt);
+ addResultSnapshot(result, *beforeOrAt);
}
// earsly stop, not necessary to also get the next since the match is perfect
@@ -276,35 +251,25 @@ namespace armarx::armem::base::query_proc
}
// first element after or at timestamp (or at because of fewer checks, we can assure that there is not element at)
- const auto after = entity.getFirstSnapshotAfterOrAt(referenceTimestamp);
- const auto timestampOfMatchAfter = after.id().timestamp;
- if (isInRange(timestampOfMatchAfter))
+ const auto* after = entity.findFirstSnapshotAfterOrAt(referenceTimestamp);
+ if (after)
{
- addResultSnapshot(result, after);
+ const auto timestampOfMatchAfter = after->id().timestamp;
+ if (isInRange(timestampOfMatchAfter))
+ {
+ addResultSnapshot(result, *after);
+ }
}
}
- 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 55c5e51126b49775db7317d6401aa695c4b5581c..91c41d9f54cb762654db9763dfe6d9c3af1aa60d 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,53 @@
#pragma once
-#include <regex>
-
-#include <RobotAPI/interface/armem/query.h>
-
#include "BaseQueryProcessorBase.h"
-#include "CoreSegmentQueryProcessorBase.h"
-
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/interface/armem/query.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
+ ResultMemoryT process(const armem::query::data::Input& input, const MemoryT& memory) const
{
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 +63,59 @@ 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));
- }
+ this->_processResult(result, coreSegment, query);
+ });
}
- 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));
- }
- catch (const error::MissingEntry&)
+ if (const CoreSegmentT* coreSegment = memory.findCoreSegment(query.coreSegmentName))
{
+ this->_processResult(result, *coreSegment, query);
}
}
- 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));
+ this->_processResult(result, coreSegment, query);
}
- }
+ });
}
+
+ private:
+
+ void _processResult(ResultMemoryT& result,
+ const CoreSegmentT& coreSegment,
+ const armem::query::data::MemoryQuery& query) const
+ {
+ ResultCoreSegmentT& added = result.addCoreSegment(coreSegment.name(), coreSegment.aronType());
+ childProcessor.process(added, 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..46f146694825936d4834b579093881e8452db3ca 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,50 +60,60 @@ 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));
- }
+ this->_processResult(result, entity, query);
+ });
}
- 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));
- }
- catch (const error::MissingEntry&)
+ if (const EntityT* entity = providerSegment.findEntity(query.entityName))
{
- // Leave empty.
+ this->_processResult(result, *entity, query);
}
}
- 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));
+ this->_processResult(result, entity, query);
}
- }
+ return true;
+ });
}
+
+ private:
+
+ void _processResult(ResultProviderSegmentT& result,
+ const EntityT& entity,
+ const armem::query::data::ProviderSegmentQuery& query) const
+ {
+ ResultEntityT& added = result.addEntity(entity.name());
+ childProcessor.process(added, query.entityQueries, entity);
+ }
+
+
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/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 469b5f907d01bbff757b1bea14ff8e80e5769315..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/BaseQueryProcessor.h
+++ /dev/null
@@ -1,32 +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:
-
- query::data::QueryTarget getTargetType() const override
- {
- return query::data::QueryTarget::Disk;
- }
-
- };
-}
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 5c043a2d1dbe2218078591adc3fc0ebac0bfe932..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);
- }
-
- 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 c57c2c2da39ca53ce97d27b498e69b648a16f25a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/EntityQueryProcessor.h
+++ /dev/null
@@ -1,42 +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 038523950bfe399c2ef7a65b79fde43376651617..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);
- }
-
- 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 2043273ec7c5f3fee2d02410a145ef436c47459c..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);
- }
-
- 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 64f560afff93e5d0cfff4b036f68f0fb4e8cdaba..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/BaseQueryProcessor.h
+++ /dev/null
@@ -1,32 +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:
-
- query::data::QueryTarget getTargetType() const override
- {
- return query::data::QueryTarget::LTM;
- }
-
- };
-}
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 dc379f1d88fa82bbe886e412e0341079fa43fce9..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);
- }
-
- 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 a5129417262515975efa247398602272865ee5ec..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);
- }
-
- 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 e0c6db9e3ef06e6241e9af8c572bb7fc78eec452..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);
- }
-
- 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..05734c2e15d6836b69719283805fcfc5c1d7702d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/ltm.h
@@ -0,0 +1,32 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.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::wm::Entity, armem::wm::Entity>
+ {
+ };
+
+ class ProviderSegmentQueryProcessor :
+ public base::ProviderSegmentQueryProcessorBase <queryTarget, armem::wm::ProviderSegment, armem::wm::ProviderSegment, EntityQueryProcessor >
+ {
+ };
+
+ class CoreSegmentQueryProcessor :
+ public base::CoreSegmentQueryProcessorBase <queryTarget, armem::wm::CoreSegment, armem::wm::CoreSegment, ProviderSegmentQueryProcessor>
+ {
+ };
+
+ class MemoryQueryProcessor :
+ public base::MemoryQueryProcessorBase <queryTarget, armem::wm::Memory, armem::wm::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..0ace705be878087b9023ea574f50769489514cb5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm.cpp
@@ -0,0 +1,72 @@
+#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
+ {
+ coreSegment.doLocked([&]()
+ {
+ 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 d4875053c93dc2278cdb9f327fc0739370811fe7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/BaseQueryProcessor.h
+++ /dev/null
@@ -1,37 +0,0 @@
-#pragma once
-
-#include <RobotAPI/interface/armem/query.h>
-#include <RobotAPI/libraries/armem/core/DataMode.h>
-
-#include <RobotAPI/libraries/armem/server/query_proc/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:
-
- query::data::QueryTarget getTargetType() const override
- {
- return query::data::QueryTarget::WM;
- }
-
- 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 4c41f16664af134b66b00355080d8de362d7eaf9..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.cpp
+++ /dev/null
@@ -1,39 +0,0 @@
-#include "CoreSegmentQueryProcessor.h"
-
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
-
-
-namespace armarx::armem::wm::query_proc
-{
-
- CoreSegmentQueryProcessor::CoreSegmentQueryProcessor(DataMode dataMode) :
- Base(dataMode), providerSegmentProcessor(dataMode)
- {}
-
-
- CoreSegmentQueryProcessor::~CoreSegmentQueryProcessor() = default;
-
-
- void CoreSegmentQueryProcessor::process(
- CoreSegment& result, const armem::query::data::CoreSegmentQuery& query, const CoreSegment& coreSegment) const
- {
- std::scoped_lock lock(coreSegment.mutex());
- CoreSegmentQueryProcessorBase::process(result, query, coreSegment);
- }
-
-
- data::CoreSegment CoreSegmentQueryProcessor::processToIce(const armem::query::data::CoreSegmentQuery& query, const wm::CoreSegment& coreSegment) const
- {
- data::CoreSegment data;
- toIce(data, process(query, coreSegment));
- return data;
- }
-
-
- ProviderSegment CoreSegmentQueryProcessor::providerSegmentProcessorProcess(const armem::query::data::ProviderSegmentQuerySeq& q, const ProviderSegment& s) const
- {
- return providerSegmentProcessor.process(q, s);
- }
-
-
-}
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 f9e1d59c9f75866e3817440bb163ec5789c0f359..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.h
+++ /dev/null
@@ -1,48 +0,0 @@
-#pragma once
-
-#include <mutex>
-
-#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
-#include <RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h>
-
-#include "BaseQueryProcessor.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);
- virtual ~CoreSegmentQueryProcessor() override;
-
- using Base::process;
-
- /// Locks the core segment, then delegates back to `CoreSegmentQueryProcessorBase`.
- void process(CoreSegment& result,
- const armem::query::data::CoreSegmentQuery& query,
- const CoreSegment& coreSegment) const override;
-
- data::CoreSegment processToIce(const armem::query::data::CoreSegmentQuery& query, const wm::CoreSegment& coreSegment) const;
-
-
- protected:
-
- virtual ProviderSegment providerSegmentProcessorProcess(const armem::query::data::ProviderSegmentQuerySeq& q, const ProviderSegmentT& s) const override;
-
-
- 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 005649b8f670586f16177f90a43c6e0f48b3879d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.cpp
+++ /dev/null
@@ -1,43 +0,0 @@
-#include "EntityQueryProcessor.h"
-
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
-
-
-namespace armarx::armem::wm::query_proc
-{
- EntityQueryProcessor::EntityQueryProcessor(DataMode dataMode) :
- BaseQueryProcessor<wm::Entity, armem::query::data::EntityQuery>(dataMode)
- {}
-
-
- EntityQueryProcessor::~EntityQueryProcessor() = default;
-
-
- void EntityQueryProcessor::addResultSnapshot(wm::Entity& result, const wm::EntitySnapshot& snapshot) const
- {
- bool withData = (dataMode == DataMode::WithData);
- if (withData)
- {
- result.addSnapshot(snapshot.copy());
- }
- else
- {
- result.addSnapshot(snapshot.copyWithoutData());
- }
- }
-
-
- data::Entity EntityQueryProcessor::processToIce(const armem::query::data::EntityQuery& query, const wm::Entity& entity) const
- {
- data::Entity data;
- toIce(data, process(query, entity));
- return data;
- }
-
-
- void EntityQueryProcessor::addResultSnapshot(wm::Entity& result, wm::Entity::ContainerT::const_iterator it) const
- {
- addResultSnapshot(result, it->second);
- }
-
-}
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 dde53a8ff3393dde385ccc49aa937fcf196476d0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.h
+++ /dev/null
@@ -1,35 +0,0 @@
-#pragma once
-
-#include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
-#include <RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h>
-
-#include "BaseQueryProcessor.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);
- virtual ~EntityQueryProcessor() override;
-
- data::Entity processToIce(const armem::query::data::EntityQuery& query, const wm::Entity& entity) const;
-
-
- private:
-
- void addResultSnapshot(wm::Entity& result, wm::Entity::ContainerT::const_iterator it) const override;
- void addResultSnapshot(wm::Entity& result, const wm::EntitySnapshot& snapshot) const override;
-
- };
-
-}
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 7aa868af13bc525b42d93f230a8398c27531525d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.cpp
+++ /dev/null
@@ -1,22 +0,0 @@
-#include "MemoryQueryProcessor.h"
-
-
-namespace armarx::armem::wm::query_proc
-{
-
- MemoryQueryProcessor::MemoryQueryProcessor(DataMode dataMode) :
- Base(dataMode), coreSegmentProcessor(dataMode)
- {}
-
-
- MemoryQueryProcessor::~MemoryQueryProcessor()
- {
- }
-
-
- CoreSegment MemoryQueryProcessor::coreSegmentProcessorProcess(const armem::query::data::CoreSegmentQuerySeq& q, const CoreSegment& s) const
- {
- return coreSegmentProcessor.process(q, s);
- }
-
-}
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 c57eea94a464a2e2191e614b5f1d6095d716405a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h
+++ /dev/null
@@ -1,39 +0,0 @@
-#pragma once
-
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
-#include <RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h>
-
-#include "BaseQueryProcessor.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);
- virtual ~MemoryQueryProcessor() override;
-
-
- protected:
-
- virtual CoreSegment coreSegmentProcessorProcess(
- const armem::query::data::CoreSegmentQuerySeq& q, const CoreSegment& s) const override;
-
-
- 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 eb2524631b4c1d2fb89eb3e7c36a3aaf86f31e29..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.cpp
+++ /dev/null
@@ -1,30 +0,0 @@
-#include "ProviderSegmentQueryProcessor.h"
-
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
-
-
-namespace armarx::armem::wm::query_proc
-{
-
- ProviderSegmentQueryProcessor::ProviderSegmentQueryProcessor(DataMode dataMode) :
- Base(dataMode), entityProcessor(dataMode)
- {}
-
-
- ProviderSegmentQueryProcessor::~ProviderSegmentQueryProcessor() = default;
-
-
- data::ProviderSegment ProviderSegmentQueryProcessor::processToIce(const armem::query::data::ProviderSegmentQuery& query, const wm::ProviderSegment& providerSegment) const
- {
- data::ProviderSegment data;
- toIce(data, process(query, providerSegment));
- return data;
- }
-
-
- Entity ProviderSegmentQueryProcessor::entityProcessorProcess(const armem::query::data::EntityQuerySeq& q, const Entity& s) const
- {
- return entityProcessor.process(q, s);
- }
-
-}
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 8e4f5a5e85d6c3c7f58433a0c2ddcd50074c7d7e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.h
+++ /dev/null
@@ -1,40 +0,0 @@
-#pragma once
-
-#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
-#include <RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h>
-
-#include "BaseQueryProcessor.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);
- virtual ~ProviderSegmentQueryProcessor() override;
-
-
- using Base::process;
- data::ProviderSegment processToIce(const armem::query::data::ProviderSegmentQuery& query, const wm::ProviderSegment& providerSegment) const;
-
-
- protected:
-
- virtual Entity entityProcessorProcess(const armem::query::data::EntityQuerySeq& q, const Entity& s) const override;
-
- private:
-
- EntityQueryProcessor entityProcessor;
-
- };
-}
diff --git a/source/RobotAPI/libraries/armem/server/segment/Segment.cpp b/source/RobotAPI/libraries/armem/server/segment/Segment.cpp
index ec25ce7843a6cca3555dc9789f3570e0bfb51425..d538568c2430c9ffbd00c9b48bebcd46086280ea 100644
--- a/source/RobotAPI/libraries/armem/server/segment/Segment.cpp
+++ b/source/RobotAPI/libraries/armem/server/segment/Segment.cpp
@@ -1 +1,101 @@
#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);
+ }
+
+
+ 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
index 6cd9761f89e50ea1e23ce75b27263242e0fe4f29..dc35846aa89d14c125e5a6f3093e1a19ee85c6e2 100644
--- a/source/RobotAPI/libraries/armem/server/segment/Segment.h
+++ b/source/RobotAPI/libraries/armem/server/segment/Segment.h
@@ -1,15 +1,14 @@
#pragma once
-// STD/STL
-#include <mutex>
-#include <string>
+#include <RobotAPI/libraries/armem/server/forward_declarations.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
+#include <RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h>
-// ArmarX
#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/application/properties/PluginAll.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
-#include "../MemoryToIceAdapter.h"
+#include <string>
namespace armarx::armem::server::segment
@@ -25,8 +24,9 @@ namespace armarx::armem::server::segment
class SegmentBase : public armarx::Logging
{
public:
+
SegmentBase() = delete;
- SegmentBase(armem::server::MemoryToIceAdapter& iceMemory) :
+ SegmentBase(MemoryToIceAdapter& iceMemory) :
iceMemory(iceMemory)
{
Logging::setTag("armarx::armem::Segment");
@@ -36,69 +36,54 @@ namespace armarx::armem::server::segment
virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") = 0;
virtual void onInit() = 0;
- virtual void onConnect() = 0;
+ // virtual void onConnect() = 0;
+
protected:
+
// Memory connection
- armem::server::MemoryToIceAdapter& iceMemory;
+ MemoryToIceAdapter& iceMemory;
SegmentType* segment;
- private:
};
}
+
namespace wm
{
/**
* @brief A base class for core segments
*/
- class CoreSegmentBase : public detail::SegmentBase<armarx::armem::wm::CoreSegment>
+ class CoreSegmentBase : public detail::SegmentBase<server::wm::CoreSegment>
{
- using Base = detail::SegmentBase<armarx::armem::wm::CoreSegment>;
+ using Base = detail::SegmentBase<server::wm::CoreSegment>;
public:
- CoreSegmentBase(armem::server::MemoryToIceAdapter& iceMemory,
- const std::string& defaultCoreSegmentName = "",
- aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType = nullptr,
- int defaultMaxHistorySize = -1):
- Base(iceMemory),
- p({defaultCoreSegmentName, defaultMaxHistorySize}),
- coreSegmentAronType(coreSegmentAronType)
- {
- Logging::setTag("armarx::armem::wm::Segment");
- }
- virtual ~CoreSegmentBase() = default;
+ CoreSegmentBase(
+ MemoryToIceAdapter& iceMemory,
+ const std::string& defaultCoreSegmentName = "",
+ aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType = nullptr,
+ int defaultMaxHistorySize = -1);
- virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override
- {
- ARMARX_CHECK_NOT_NULL(defs);
+ virtual ~CoreSegmentBase() override;
- 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);
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ virtual void onInit() override;
+ // virtual void onConnect() override;
- segment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, coreSegmentAronType);
- segment->setMaxHistorySize(p.maxHistorySize);
- }
- virtual void onConnect() override
+ template <class FunctionT>
+ auto doLocked(FunctionT&& function) const
{
-
+ return segment->doLocked(function);
}
- std::mutex& mutex() const
- {
- ARMARX_CHECK_NOT_NULL(segment);
- return segment->mutex();
- }
protected:
+
struct Properties
{
std::string coreSegmentName;
@@ -107,66 +92,37 @@ namespace armarx::armem::server::segment
Properties p;
aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType;
+
};
/**
* @brief A base class for provider segments
*/
- class ProviderSegmentBase : public detail::SegmentBase<armarx::armem::wm::ProviderSegment>
+ class ProviderSegmentBase : public detail::SegmentBase<server::wm::ProviderSegment>
{
- using Base = detail::SegmentBase<armarx::armem::wm::ProviderSegment>;
+ using Base = detail::SegmentBase<server::wm::ProviderSegment>;
public:
- ProviderSegmentBase(armem::server::MemoryToIceAdapter& iceMemory,
- const std::string& defaultProviderSegmentName = "",
- aron::typenavigator::ObjectNavigatorPtr providerSegmentAronType = nullptr,
- const std::string& defaultCoreSegmentName = "",
- aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType = nullptr,
- int defaultMaxHistorySize = -1):
- Base(iceMemory),
- p({defaultCoreSegmentName, defaultProviderSegmentName, defaultMaxHistorySize}),
- coreSegmentAronType(coreSegmentAronType),
- providerSegmentAronType(providerSegmentAronType)
- {
- Logging::setTag("armarx::armem::wm::Segment");
- }
-
- virtual ~ProviderSegmentBase() = default;
- virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override
- {
- ARMARX_CHECK_NOT_NULL(defs);
+ ProviderSegmentBase(
+ MemoryToIceAdapter& iceMemory,
+ const std::string& defaultProviderSegmentName = "",
+ aron::typenavigator::ObjectNavigatorPtr providerSegmentAronType = nullptr,
+ const std::string& defaultCoreSegmentName = "",
+ aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType = nullptr,
+ int defaultMaxHistorySize = -1);
- 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).");
- }
+ virtual ~ProviderSegmentBase() override;
- virtual void onInit() override
- {
- 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);
- }
- virtual void onConnect() 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;
@@ -178,10 +134,11 @@ namespace armarx::armem::server::segment
aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType;
aron::typenavigator::ObjectNavigatorPtr providerSegmentAronType;
- armarx::armem::wm::CoreSegment* coreSegment;
+ server::wm::CoreSegment* coreSegment;
};
+
/**
* @brief A base class for provider segments with an aron business object
*/
@@ -191,10 +148,13 @@ namespace armarx::armem::server::segment
using Base = CoreSegmentBase;
public:
- AronTypedCoreSegmentBase(armem::server::MemoryToIceAdapter& iceMemory,
- const std::string& defaultCoreSegmentName = "",
- int defaultMaxHistorySize = -1):
- Base(iceMemory, defaultCoreSegmentName, BusinessClassObject::toAronType(), defaultMaxHistorySize)
+
+ AronTypedCoreSegmentBase(
+ MemoryToIceAdapter& iceMemory,
+ const std::string& defaultCoreSegmentName = "",
+ int defaultMaxHistorySize = -1) :
+ Base(iceMemory, defaultCoreSegmentName, BusinessClassObject::toAronType(),
+ defaultMaxHistorySize)
{
}
@@ -202,6 +162,7 @@ namespace armarx::armem::server::segment
};
+
/**
* @brief A base class for provider segments with an aron business object
*/
@@ -211,15 +172,19 @@ namespace armarx::armem::server::segment
using Base = ProviderSegmentBase;
public:
- AronTypedProviderSegmentBase(armem::server::MemoryToIceAdapter& iceMemory,
- const std::string& defaultProviderSegmentName = "",
- const std::string& defaultCoreSegmentName = "",
- int defaultMaxHistorySize = -1):
- Base(iceMemory, defaultProviderSegmentName, BusinessClassObject::toAronType(), defaultCoreSegmentName, nullptr, defaultMaxHistorySize)
+
+ 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
index 8184e27c985877a19aaa873db07883685140b17b..50f5ae6451bb49f9a4356579d78618a865297881 100644
--- a/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.cpp
@@ -12,11 +12,11 @@ namespace armarx::armem::server::segment
{
SpecializedSegment::SpecializedSegment(
- server::MemoryToIceAdapter& iceMemory,
- aron::typenavigator::ObjectNavigatorPtr aronType,
- const std::string& defaultCoreSegmentName,
- int64_t defaultMaxHistorySize
- ) :
+ server::MemoryToIceAdapter& iceMemory,
+ aron::typenavigator::ObjectNavigatorPtr aronType,
+ const std::string& defaultCoreSegmentName,
+ int64_t defaultMaxHistorySize
+ ) :
iceMemory(iceMemory), aronType(aronType)
{
setDefaultCoreSegmentName(defaultCoreSegmentName);
@@ -52,13 +52,6 @@ namespace armarx::armem::server::segment
}
- std::mutex& SpecializedSegment::mutex() const
- {
- ARMARX_CHECK_NOT_NULL(coreSegment);
- return coreSegment->mutex();
- }
-
-
void SpecializedSegment::setDefaultCoreSegmentName(const std::string& coreSegmentName)
{
this->properties.coreSegmentName = coreSegmentName;
diff --git a/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h b/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h
index fd3a456c55d785471e86a0d3f1e12a5491d3e32c..271536db21ffdfadb3fba874f60bdd8a33d21f44 100644
--- a/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h
+++ b/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h
@@ -1,28 +1,14 @@
#pragma once
-#include <mutex>
-#include <string>
+#include <RobotAPI/libraries/armem/server/forward_declarations.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h>
#include <ArmarXCore/core/application/properties/forward_declarations.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <string>
-namespace armarx::aron::typenavigator
-{
- using ObjectNavigatorPtr = std::shared_ptr<class ObjectNavigator>;
-}
-namespace armarx::armem
-{
- namespace server
- {
- class MemoryToIceAdapter;
- }
-
- namespace wm
- {
- class CoreSegment;
- }
-}
namespace armarx::armem::server::segment
{
@@ -31,15 +17,15 @@ namespace armarx::armem::server::segment
* @brief Specialized management of a core segment.
*/
class SpecializedSegment :
- public armarx::Logging
+ public armarx::Logging
{
public:
SpecializedSegment(
- server::MemoryToIceAdapter& iceMemory,
- aron::typenavigator::ObjectNavigatorPtr aronType = nullptr,
- const std::string& defaultCoreSegmentName = "",
- int64_t defaultMaxHistorySize = -1);
+ server::MemoryToIceAdapter& iceMemory,
+ aron::typenavigator::ObjectNavigatorPtr aronType = nullptr,
+ const std::string& defaultCoreSegmentName = "",
+ int64_t defaultMaxHistorySize = -1);
virtual ~SpecializedSegment();
@@ -48,7 +34,12 @@ namespace armarx::armem::server::segment
virtual void init();
// void connect();
- std::mutex& mutex() const;
+
+ template <class FunctionT>
+ auto doLocked(FunctionT&& function) const
+ {
+ return coreSegment->doLocked(function);
+ }
protected:
@@ -61,7 +52,7 @@ namespace armarx::armem::server::segment
protected:
server::MemoryToIceAdapter& iceMemory;
- wm::CoreSegment* coreSegment = nullptr;
+ server::wm::CoreSegment* coreSegment = nullptr;
aron::typenavigator::ObjectNavigatorPtr aronType;
struct 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..962179a68aeb601dc261eb6d496389295597a2ea
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h
@@ -0,0 +1,56 @@
+#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);
+ });
+ }
+
+ };
+}
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/core/workingmemory/Memory.cpp b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
similarity index 58%
rename from source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
rename to source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
index 70fd41e7c451175d5964a81d48f4e8270c2e611c..c2dc4f6b38abdd3de9e67afe0f7031f2209d2055 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
@@ -1,12 +1,48 @@
-#include "Memory.h"
+#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
+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(), size_t(_maxHistorySize));
+ }
+ return removedElements;
+ }
+
+
// TODO: add core segment if param is set
std::vector<Memory::Base::UpdateResult>
Memory::updateLocking(const Commit& commit)
@@ -29,15 +65,16 @@ namespace armarx::armem::wm
CoreSegment& coreSegment = it->second;
// Lock the core segment for the whole batch.
- std::scoped_lock lock(coreSegment.mutex());
-
- for (const EntityUpdate* update : updates)
+ coreSegment.doLocked([&result, &coreSegment, updates = &updates]()
{
- auto r = coreSegment.update(*update);
- Base::UpdateResult ret { r };
- ret.memoryUpdateType = UpdateType::UpdatedExisting;
- result.push_back(ret);
- }
+ for (const EntityUpdate* update : *updates)
+ {
+ auto r = coreSegment.update(*update);
+ Base::UpdateResult ret { r };
+ ret.memoryUpdateType = UpdateType::UpdatedExisting;
+ result.push_back(ret);
+ }
+ });
}
else
{
@@ -63,33 +100,12 @@ namespace armarx::armem::wm
CoreSegment& segment = getCoreSegment(update.entityID.coreSegmentName);
Base::UpdateResult result;
+ segment.doLocked([&result, &segment, &update]()
{
- std::scoped_lock lock(segment.mutex());
result = segment.update(update);
- }
+ });
result.memoryUpdateType = UpdateType::UpdatedExisting;
return result;
}
-
- 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/server/wm/memory_definitions.h b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
new file mode 100644
index 0000000000000000000000000000000000000000..029d185c62445a5a83dd749d1b86f2393f95e6d2
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
@@ -0,0 +1,152 @@
+#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 <RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.h>
+
+#include <mutex>
+
+
+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 armem::wm::detail::FindInstanceDataMixinForEntity<Entity>
+ {
+ 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 armem::wm::detail::FindInstanceDataMixin<ProviderSegment>
+ {
+ public:
+
+ using base::ProviderSegmentBase<Entity, ProviderSegment>::ProviderSegmentBase;
+
+
+ using ProviderSegmentBase::addEntity;
+
+ template <class ...Args>
+ Entity& addEntity(const std::string& name, Args... args)
+ {
+ Entity& added = ProviderSegmentBase::addEntity(name, args...);
+ added.setMaxHistorySize(this->getMaxHistorySize());
+ return added;
+ }
+
+ };
+
+
+
+ /// @brief base::CoreSegmentBase
+ class CoreSegment :
+ public base::CoreSegmentBase<ProviderSegment, CoreSegment>,
+ public detail::MaxHistorySizeParent<CoreSegment>
+ , public armem::wm::detail::FindInstanceDataMixin<CoreSegment>
+ {
+ using Base = base::CoreSegmentBase<ProviderSegment, CoreSegment>;
+
+ public:
+
+ using Base::CoreSegmentBase;
+
+ /// @see base::CoreSegmentBase::addProviderSegment()
+ using CoreSegmentBase::addProviderSegment;
+ template <class ...Args>
+ ProviderSegment& addProviderSegment(const std::string& name, Args... args)
+ {
+ ProviderSegmentT& added = CoreSegmentBase::addProviderSegment(name, args...);
+ added.setMaxHistorySize(this->getMaxHistorySize());
+ return added;
+ }
+
+
+ // Locking interface
+
+ template <class FunctionT>
+ auto doLocked(FunctionT&& function) const
+ {
+ std::scoped_lock lock(_mutex);
+ return function();
+ }
+
+
+ private:
+
+ mutable std::mutex _mutex;
+
+ };
+
+
+
+ /// @see base::MemoryBase
+ class Memory :
+ public base::MemoryBase<CoreSegment, Memory>
+ , public armem::wm::detail::FindInstanceDataMixin<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..9c29804ebdc56444b175b3de0abd115b3a1067c0
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/test/ArMemForEachTest.cpp
@@ -0,0 +1,188 @@
+/*
+ * 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 <RobotAPI/libraries/armem/core/operations.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" << armem::print(memory));
+
+ 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/ArMemGetFindTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemGetFindTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..29c12884d15292706ac2be33410352fe832dbc2a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/test/ArMemGetFindTest.cpp
@@ -0,0 +1,347 @@
+/*
+ * 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/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/operations.h>
+
+#include <RobotAPI/Test.h>
+
+#include <iostream>
+
+
+namespace armem = armarx::armem;
+namespace wm = armarx::armem::wm;
+namespace aron = armarx::aron;
+
+
+namespace ArMemGetFindTest
+{
+ struct Fixture
+ {
+ wm::EntitySnapshot snapshot;
+ wm::Entity entity;
+ wm::ProviderSegment provSeg;
+ wm::CoreSegment coreSeg;
+ wm::Memory memory;
+
+ armem::MemoryID instanceID;
+ armem::MemoryID snapshotID;
+ armem::MemoryID entityID;
+ armem::MemoryID provSegID;
+ armem::MemoryID coreSegID;
+ armem::MemoryID memoryID;
+
+ Fixture()
+ {
+ {
+ snapshot.time() = armem::Time::microSeconds(1000);
+ snapshot.addInstance();
+ }
+ {
+ entity.name() = "entity";
+ entity.addSnapshot(snapshot);
+ }
+ {
+ provSeg.name() = "provider segment";
+ provSeg.addEntity(entity);
+ }
+ {
+ coreSeg.name() = "core segment";
+ coreSeg.addProviderSegment(provSeg);
+ }
+ {
+ memory.name() = "memory";
+ memory.addCoreSegment(coreSeg);
+ }
+
+ memoryID = memory.id();
+ coreSegID = memoryID.withCoreSegmentName(coreSeg.name());
+ provSegID = coreSegID.withProviderSegmentName(provSeg.name());
+ entityID = provSegID.withEntityName(entity.name());
+ snapshotID = entityID.withTimestamp(snapshot.time());
+ instanceID = snapshotID.withInstanceIndex(0);
+ }
+ ~Fixture()
+ {
+ }
+
+
+ template <class ParentT>
+ void test_get_find_instance_by_id(ParentT&& parent)
+ {
+ _test_get_find_instance_by_id(const_cast<const ParentT&>(parent));
+ _test_get_find_instance_by_id(const_cast<ParentT&>(parent));
+ }
+ template <class ParentT>
+ void _test_get_find_instance_by_id(ParentT&& parent)
+ {
+ BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
+ {
+ BOOST_CHECK_EQUAL(parent.hasInstance(snapshotID.withInstanceIndex(0)), true);
+ BOOST_CHECK_EQUAL(parent.hasInstance(snapshotID.withInstanceIndex(1)), false);
+
+ BOOST_CHECK_NE(parent.findInstance(snapshotID.withInstanceIndex(0)), nullptr);
+ BOOST_CHECK_EQUAL(parent.findInstance(snapshotID.withInstanceIndex(1)), nullptr);
+
+ BOOST_CHECK_NO_THROW(parent.getInstance(snapshotID.withInstanceIndex(0)));
+ BOOST_CHECK_THROW(parent.getInstance(snapshotID.withInstanceIndex(1)), armem::error::MissingEntry);
+ }
+ }
+
+ template <class ParentT>
+ void test_get_find_snapshot_by_id(ParentT&& parent)
+ {
+ _test_get_find_snapshot_by_id(const_cast<const ParentT&>(parent));
+ _test_get_find_snapshot_by_id(const_cast<ParentT&>(parent));
+ }
+ template <class ParentT>
+ void _test_get_find_snapshot_by_id(ParentT&& parent)
+ {
+ BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
+ {
+ BOOST_CHECK_EQUAL(parent.hasSnapshot(entityID.withTimestamp(armem::Time::microSeconds(1000))), true);
+ BOOST_CHECK_EQUAL(parent.hasSnapshot(entityID.withTimestamp(armem::Time::microSeconds(2000))), false);
+
+ BOOST_CHECK_NE(parent.findSnapshot(entityID.withTimestamp(armem::Time::microSeconds(1000))), nullptr);
+ BOOST_CHECK_EQUAL(parent.findSnapshot(entityID.withTimestamp(armem::Time::microSeconds(2000))), nullptr);
+
+ BOOST_CHECK_NO_THROW(parent.getSnapshot(entityID.withTimestamp(armem::Time::microSeconds(1000))));
+ BOOST_CHECK_THROW(parent.getSnapshot(entityID.withTimestamp(armem::Time::microSeconds(2000))), armem::error::MissingEntry);
+ }
+ }
+
+
+ template <class ParentT>
+ void test_get_find_entity_by_id(ParentT&& parent)
+ {
+ _test_get_find_entity_by_id(const_cast<const ParentT&>(parent));
+ _test_get_find_entity_by_id(const_cast<ParentT&>(parent));
+ }
+ template <class ParentT>
+ void _test_get_find_entity_by_id(ParentT&& parent)
+ {
+ BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
+ {
+ BOOST_CHECK_EQUAL(parent.hasEntity(provSegID.withEntityName("entity")), true);
+ BOOST_CHECK_EQUAL(parent.hasEntity(provSegID.withEntityName("other entity")), false);
+
+ BOOST_CHECK_NE(parent.findEntity(provSegID.withEntityName("entity")), nullptr);
+ BOOST_CHECK_EQUAL(parent.findEntity(provSegID.withEntityName("other entity")), nullptr);
+
+ BOOST_CHECK_NO_THROW(parent.getEntity(provSegID.withEntityName("entity")));
+ BOOST_CHECK_THROW(parent.getEntity(provSegID.withEntityName("other entity")), armem::error::MissingEntry);
+ }
+ }
+
+ template <class ParentT>
+ void test_get_find_provider_segment_by_id(ParentT&& parent)
+ {
+ _test_get_find_provider_segment_by_id(const_cast<const ParentT&>(parent));
+ _test_get_find_provider_segment_by_id(const_cast<ParentT&>(parent));
+ }
+ template <class ParentT>
+ void _test_get_find_provider_segment_by_id(ParentT&& parent)
+ {
+ BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
+ {
+ BOOST_CHECK_EQUAL(parent.hasProviderSegment(provSegID.withProviderSegmentName("provider segment")), true);
+ BOOST_CHECK_EQUAL(parent.hasProviderSegment(provSegID.withProviderSegmentName("other provider segment")), false);
+
+ BOOST_CHECK_NE(parent.findProviderSegment(provSegID.withProviderSegmentName("provider segment")), nullptr);
+ BOOST_CHECK_EQUAL(parent.findProviderSegment(provSegID.withProviderSegmentName("other provider segment")), nullptr);
+
+ BOOST_CHECK_NO_THROW(parent.getProviderSegment(provSegID.withProviderSegmentName("provider segment")));
+ BOOST_CHECK_THROW(parent.getProviderSegment(provSegID.withProviderSegmentName("other provider segment")), armem::error::MissingEntry);
+ }
+ }
+
+ template <class ParentT>
+ void test_get_find_core_segment_by_id(ParentT&& parent)
+ {
+ _test_get_find_core_segment_by_id(const_cast<const ParentT&>(parent));
+ _test_get_find_core_segment_by_id(const_cast<ParentT&>(parent));
+ }
+ template <class ParentT>
+ void _test_get_find_core_segment_by_id(ParentT&& parent)
+ {
+ BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
+ {
+ BOOST_CHECK_EQUAL(parent.hasCoreSegment(provSegID.withCoreSegmentName("core segment")), true);
+ BOOST_CHECK_EQUAL(parent.hasCoreSegment(provSegID.withCoreSegmentName("other core segment")), false);
+
+ BOOST_CHECK_NE(parent.findCoreSegment(provSegID.withCoreSegmentName("core segment")), nullptr);
+ BOOST_CHECK_EQUAL(parent.findCoreSegment(provSegID.withCoreSegmentName("other core segment")), nullptr);
+
+ BOOST_CHECK_NO_THROW(parent.getCoreSegment(provSegID.withCoreSegmentName("core segment")));
+ BOOST_CHECK_THROW(parent.getCoreSegment(provSegID.withCoreSegmentName("other core segment")), armem::error::MissingEntry);
+ }
+ }
+ };
+}
+
+
+BOOST_FIXTURE_TEST_SUITE(ArMemGetFindTest, Fixture)
+
+
+
+BOOST_AUTO_TEST_CASE(test_snapshot_get_find_instance_by_key)
+{
+ BOOST_CHECK_EQUAL(snapshot.hasInstance(0), true);
+ BOOST_CHECK_EQUAL(snapshot.hasInstance(1), false);
+
+ BOOST_CHECK_NE(snapshot.findInstance(0), nullptr);
+ BOOST_CHECK_EQUAL(snapshot.findInstance(1), nullptr);
+
+ BOOST_CHECK_NO_THROW(snapshot.getInstance(0));
+ BOOST_CHECK_THROW(snapshot.getInstance(1), armem::error::MissingEntry);
+}
+
+
+BOOST_AUTO_TEST_CASE(test_entity_get_find_snapshot_by_key)
+{
+ BOOST_CHECK_EQUAL(entity.hasSnapshot(armem::Time::microSeconds(1000)), true);
+ BOOST_CHECK_EQUAL(entity.hasSnapshot(armem::Time::microSeconds(2000)), false);
+
+ BOOST_CHECK_NE(entity.findSnapshot(armem::Time::microSeconds(1000)), nullptr);
+ BOOST_CHECK_EQUAL(entity.findSnapshot(armem::Time::microSeconds(2000)), nullptr);
+
+ BOOST_CHECK_NO_THROW(entity.getSnapshot(armem::Time::microSeconds(1000)));
+ BOOST_CHECK_THROW(entity.getSnapshot(armem::Time::microSeconds(2000)), armem::error::MissingEntry);
+}
+
+
+BOOST_AUTO_TEST_CASE(test_provider_segment_get_find_entity_by_key)
+{
+ BOOST_CHECK_EQUAL(provSeg.hasEntity("entity"), true);
+ BOOST_CHECK_EQUAL(provSeg.hasEntity("other entity"), false);
+
+ BOOST_CHECK_NE(provSeg.findEntity("entity"), nullptr);
+ BOOST_CHECK_EQUAL(provSeg.findEntity("other entity"), nullptr);
+
+ BOOST_CHECK_NO_THROW(provSeg.getEntity("entity"));
+ BOOST_CHECK_THROW(provSeg.getEntity("other entity"), armem::error::MissingEntry);
+}
+
+
+BOOST_AUTO_TEST_CASE(test_core_segment_get_find_provider_segment_by_key)
+{
+ BOOST_CHECK_EQUAL(coreSeg.hasProviderSegment("provider segment"), true);
+ BOOST_CHECK_EQUAL(coreSeg.hasProviderSegment("other provider segment"), false);
+
+ BOOST_CHECK_NE(coreSeg.findProviderSegment("provider segment"), nullptr);
+ BOOST_CHECK_EQUAL(coreSeg.findProviderSegment("other provider segment"), nullptr);
+
+ BOOST_CHECK_NO_THROW(coreSeg.getProviderSegment("provider segment"));
+ BOOST_CHECK_THROW(coreSeg.getProviderSegment("other provider segment"), armem::error::MissingEntry);
+}
+
+
+BOOST_AUTO_TEST_CASE(test_memory_get_find_core_segment_by_key)
+{
+ BOOST_CHECK_EQUAL(memory.hasCoreSegment("core segment"), true);
+ BOOST_CHECK_EQUAL(memory.hasCoreSegment("other core segment"), false);
+
+ BOOST_CHECK_NE(memory.findCoreSegment("core segment"), nullptr);
+ BOOST_CHECK_EQUAL(memory.findCoreSegment("other core segment"), nullptr);
+
+ BOOST_CHECK_NO_THROW(memory.getCoreSegment("core segment"));
+ BOOST_CHECK_THROW(memory.getCoreSegment("other core segment"), armem::error::MissingEntry);
+}
+
+
+
+BOOST_AUTO_TEST_CASE(test_snapshot_get_find_instance_by_id)
+{
+ test_get_find_instance_by_id(snapshot);
+}
+BOOST_AUTO_TEST_CASE(test_entity_get_find_instance_by_id)
+{
+ test_get_find_instance_by_id(entity);
+}
+BOOST_AUTO_TEST_CASE(test_provider_segment_get_find_instance_by_id)
+{
+ test_get_find_instance_by_id(provSeg);
+}
+BOOST_AUTO_TEST_CASE(test_core_segment_get_find_instance_by_id)
+{
+ test_get_find_instance_by_id(coreSeg);
+}
+BOOST_AUTO_TEST_CASE(test_memory_get_find_instance_by_id)
+{
+ test_get_find_instance_by_id(memory);
+}
+
+
+BOOST_AUTO_TEST_CASE(test_entity_get_find_snapshot_by_id)
+{
+ test_get_find_snapshot_by_id(entity);
+}
+BOOST_AUTO_TEST_CASE(test_provider_segment_get_find_snapshot_by_id)
+{
+ test_get_find_snapshot_by_id(provSeg);
+}
+BOOST_AUTO_TEST_CASE(test_core_segment_get_find_snapshot_by_id)
+{
+ test_get_find_snapshot_by_id(coreSeg);
+}
+BOOST_AUTO_TEST_CASE(test_memory_get_find_snapshot_by_id)
+{
+ test_get_find_snapshot_by_id(memory);
+}
+
+
+BOOST_AUTO_TEST_CASE(test_provider_segment_get_find_entity_by_id)
+{
+ test_get_find_entity_by_id(provSeg);
+}
+BOOST_AUTO_TEST_CASE(test_core_segment_get_find_entity_by_id)
+{
+ test_get_find_entity_by_id(coreSeg);
+}
+BOOST_AUTO_TEST_CASE(test_memory_get_find_entity_by_id)
+{
+ test_get_find_entity_by_id(memory);
+}
+
+
+BOOST_AUTO_TEST_CASE(test_core_segment_get_find_provider_segment_by_id)
+{
+ test_get_find_provider_segment_by_id(coreSeg);
+}
+BOOST_AUTO_TEST_CASE(test_memory_get_find_provider_segment_by_id)
+{
+ test_get_find_provider_segment_by_id(memory);
+}
+
+BOOST_AUTO_TEST_CASE(test_memory_get_find_core_segment_by_id)
+{
+ test_get_find_core_segment_by_id(memory);
+}
+
+
+
+BOOST_AUTO_TEST_SUITE_END()
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..f0ede7b42a3bdb34d1a103ab43226adae833740a 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>
@@ -34,7 +34,6 @@
#include <RobotAPI/libraries/aron/core/Debug.h>
-#include "../core/longtermmemory/Memory.h"
//#include "../core/io/diskWriter/NlohmannJSON/NlohmannJSONDiskWriter.h"
@@ -122,7 +121,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..e9f694a27104c142603752f22f5452c8024663b0 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp
@@ -26,18 +26,21 @@
#include <RobotAPI/Test.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
-#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
-#include <RobotAPI/libraries/armem/core/diskmemory/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/error.h>
+#include <RobotAPI/libraries/armem/core/operations.h>
#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;
namespace aron = armarx::aron;
+namespace wm = armarx::armem::wm;
+
BOOST_AUTO_TEST_CASE(test_time_to_string)
{
@@ -78,6 +81,157 @@ BOOST_AUTO_TEST_CASE(test_time_to_string)
}
+// Public interface tests
+
+
+namespace ArMemMemoryTest
+{
+ struct APITestFixture
+ {
+ wm::EntityInstance instance { 0 };
+ wm::EntitySnapshot snapshot { armem::Time::microSeconds(1000) };
+ wm::Entity entity { "entity" };
+ wm::ProviderSegment provSeg { "provider" };
+ wm::CoreSegment coreSeg { "core" };
+ wm::Memory memory { "memory" };
+
+
+ void test_add_instance(wm::EntityInstance& added, const wm::EntitySnapshot& parent)
+ {
+ BOOST_TEST_CONTEXT("Added: " << armem::print(added) << "\n Parent: " << armem::print(parent))
+ {
+ const int index = 0;
+ BOOST_CHECK_EQUAL(added.index(), index);
+ BOOST_CHECK_EQUAL(parent.size(), 1);
+ BOOST_CHECK(parent.hasInstance(index));
+ BOOST_CHECK_EQUAL(&parent.getInstance(index), &added);
+ }
+ }
+ void test_add_snapshot(wm::EntitySnapshot& added, const wm::Entity& parent)
+ {
+ BOOST_TEST_CONTEXT("Added: " << armem::print(added) << "\n Parent: " << armem::print(parent))
+ {
+ const armem::Time time = armem::Time::microSeconds(1000);
+ BOOST_CHECK_EQUAL(added.time(), time);
+ BOOST_CHECK_EQUAL(parent.size(), 1);
+ BOOST_CHECK(parent.hasSnapshot(time));
+ BOOST_CHECK_EQUAL(&parent.getSnapshot(time), &added);
+ }
+ }
+ void test_add_entity(wm::Entity& added, const wm::ProviderSegment& parent)
+ {
+ BOOST_TEST_CONTEXT("Added: " << armem::print(added) << "\n Parent: " << armem::print(parent))
+ {
+ const std::string name = "entity";
+ BOOST_CHECK_EQUAL(added.name(), name);
+ BOOST_CHECK_EQUAL(parent.size(), 1);
+ BOOST_CHECK(parent.hasEntity(name));
+ BOOST_CHECK_EQUAL(&parent.getEntity(name), &added);
+ }
+ }
+ void test_add_provider_segment(wm::ProviderSegment& added, const wm::CoreSegment& parent)
+ {
+ BOOST_TEST_CONTEXT("Added: " << armem::print(added) << "\n Parent: " << armem::print(parent))
+ {
+ const std::string name = "provider";
+ BOOST_CHECK_EQUAL(added.name(), name);
+ BOOST_CHECK_EQUAL(parent.size(), 1);
+ BOOST_CHECK(parent.hasProviderSegment(name));
+ BOOST_CHECK_EQUAL(&parent.getProviderSegment(name), &added);
+ }
+ }
+ void test_add_core_segment(wm::CoreSegment& added, const wm::Memory& parent)
+ {
+ BOOST_TEST_CONTEXT("Added: " << armem::print(added) << "\n Parent: " << armem::print(parent))
+ {
+ const std::string name = "core";
+ BOOST_CHECK_EQUAL(added.name(), name);
+ BOOST_CHECK_EQUAL(parent.size(), 1);
+ BOOST_CHECK(parent.hasCoreSegment(name));
+ BOOST_CHECK_EQUAL(&parent.getCoreSegment(name), &added);
+ }
+ }
+ };
+}
+
+
+
+BOOST_FIXTURE_TEST_SUITE(APITest, ArMemMemoryTest::APITestFixture)
+
+
+BOOST_AUTO_TEST_CASE(test_add_instance_no_args)
+{
+ test_add_instance(snapshot.addInstance(), snapshot);
+}
+BOOST_AUTO_TEST_CASE(test_add_instance_copy)
+{
+ test_add_instance(snapshot.addInstance(instance), snapshot);
+}
+BOOST_AUTO_TEST_CASE(test_add_instance_move)
+{
+ test_add_instance(snapshot.addInstance(std::move(instance)), snapshot);
+}
+
+BOOST_AUTO_TEST_CASE(test_add_snapshot_time)
+{
+ test_add_snapshot(entity.addSnapshot(armem::Time::microSeconds(1000)), entity);
+}
+BOOST_AUTO_TEST_CASE(test_add_snapshot_copy)
+{
+ test_add_snapshot(entity.addSnapshot(snapshot), entity);
+}
+BOOST_AUTO_TEST_CASE(test_add_snapshot_move)
+{
+ test_add_snapshot(entity.addSnapshot(std::move(snapshot)), entity);
+}
+
+BOOST_AUTO_TEST_CASE(test_add_entity_name)
+{
+ test_add_entity(provSeg.addEntity("entity"), provSeg);
+}
+BOOST_AUTO_TEST_CASE(test_add_entity_copy)
+{
+ test_add_entity(provSeg.addEntity(entity), provSeg);
+}
+BOOST_AUTO_TEST_CASE(test_add_entity_move)
+{
+ test_add_entity(provSeg.addEntity(std::move(entity)), provSeg);
+}
+
+BOOST_AUTO_TEST_CASE(test_add_provider_segment_name)
+{
+ test_add_provider_segment(coreSeg.addProviderSegment("provider"), coreSeg);
+}
+BOOST_AUTO_TEST_CASE(test_add_provider_segment_copy)
+{
+ test_add_provider_segment(coreSeg.addProviderSegment(provSeg), coreSeg);
+}
+BOOST_AUTO_TEST_CASE(test_add_provider_segment_move)
+{
+ test_add_provider_segment(coreSeg.addProviderSegment(std::move(provSeg)), coreSeg);
+}
+
+BOOST_AUTO_TEST_CASE(test_add_core_segment_name)
+{
+ test_add_core_segment(memory.addCoreSegment("core"), memory);
+}
+BOOST_AUTO_TEST_CASE(test_add_core_segment_copy)
+{
+ test_add_core_segment(memory.addCoreSegment(coreSeg), memory);
+}
+BOOST_AUTO_TEST_CASE(test_add_core_segment_move)
+{
+ test_add_core_segment(memory.addCoreSegment(std::move(coreSeg)), memory);
+}
+
+
+BOOST_AUTO_TEST_SUITE_END()
+
+
+
+
+
+// COPY/MOVE CTOR/OP TESTS
namespace ArMemMemoryTest
@@ -87,11 +241,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 +308,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);
}
@@ -224,37 +383,26 @@ BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_CASE(test_key_ctors)
{
- armem::wm::EntityInstance instance(10);
+ wm::EntityInstance instance(10);
BOOST_CHECK_EQUAL(instance.index(), 10);
- armem::wm::EntitySnapshot snapshot(armem::Time::milliSeconds(100));
+ wm::EntitySnapshot snapshot(armem::Time::milliSeconds(100));
BOOST_CHECK_EQUAL(snapshot.time(), armem::Time::milliSeconds(100));
- armem::wm::Entity entity("entity");
+ wm::Entity entity("entity");
BOOST_CHECK_EQUAL(entity.name(), "entity");
- armem::wm::ProviderSegment provSeg("provSeg");
+ wm::ProviderSegment provSeg("provSeg");
BOOST_CHECK_EQUAL(provSeg.name(), "provSeg");
- armem::wm::CoreSegment coreSeg("coreSeg");
+ wm::CoreSegment coreSeg("coreSeg");
BOOST_CHECK_EQUAL(coreSeg.name(), "coreSeg");
- armem::wm::Memory memory("memory");
+ wm::Memory memory("memory");
BOOST_CHECK_EQUAL(memory.name(), "memory");
}
-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
@@ -282,19 +430,19 @@ void checkMoved_d_ltm(const T& moved)
}
template <>
-struct CustomChecks<armem::wm::EntityInstance>
+struct CustomChecks<wm::EntityInstance>
{
- static void checkEqual(const armem::wm::EntityInstance& lhs, const armem::wm::EntityInstance& rhs)
+ static void checkEqual(const wm::EntityInstance& lhs, const wm::EntityInstance& rhs)
{
BOOST_CHECK_EQUAL(lhs.metadata(), rhs.metadata());
BOOST_CHECK_EQUAL(lhs.data(), rhs.data());
}
- static void checkMoved(const armem::wm::EntityInstance& moved)
+ static void checkMoved(const wm::EntityInstance& moved)
{
BOOST_CHECK_EQUAL(moved.data(), nullptr);
}
};
-template <>
+/*template <>
struct CustomChecks<armem::d_ltm::EntityInstance>
{
static void checkEqual(const armem::d_ltm::EntityInstance& lhs, const armem::d_ltm::EntityInstance& rhs)
@@ -365,19 +513,22 @@ struct CustomChecks<armem::d_ltm::Memory>
{
checkMoved_d_ltm(moved);
}
-};
+};*/
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 +610,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 +647,17 @@ struct CopyMoveCtorsOpsTest : public CopyMoveCtorsOpsTestBase
void reset() override
{
in = T {id};
- add_element(in.container());
+ if constexpr(std::is_same_v<T, wm::Memory>)
+ {
+ in.addCoreSegment("C");
+ }
+ {
+ 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);
@@ -581,29 +746,29 @@ struct CopyMoveCtorsOpsTest : public CopyMoveCtorsOpsTestBase
BOOST_AUTO_TEST_CASE(test_copy_move_ctors_ops)
{
{
- InstanceCopyMoveCtorsOpsTest<armem::wm::EntityInstance>().test();
- CopyMoveCtorsOpsTest<armem::wm::EntitySnapshot>().test();
- CopyMoveCtorsOpsTest<armem::wm::Entity>().test();
- CopyMoveCtorsOpsTest<armem::wm::ProviderSegment>().test();
- CopyMoveCtorsOpsTest<armem::wm::CoreSegment>().test();
- CopyMoveCtorsOpsTest<armem::wm::Memory>().test();
+ InstanceCopyMoveCtorsOpsTest<wm::EntityInstance>().test();
+ CopyMoveCtorsOpsTest<wm::EntitySnapshot>().test();
+ CopyMoveCtorsOpsTest<wm::Entity>().test();
+ CopyMoveCtorsOpsTest<wm::ProviderSegment>().test();
+ CopyMoveCtorsOpsTest<wm::CoreSegment>().test();
+ CopyMoveCtorsOpsTest<wm::Memory>().test();
}
- {
+ /*{
InstanceCopyMoveCtorsOpsTest<armem::ltm::EntityInstance>().test();
CopyMoveCtorsOpsTest<armem::ltm::EntitySnapshot>().test();
CopyMoveCtorsOpsTest<armem::ltm::Entity>().test();
CopyMoveCtorsOpsTest<armem::ltm::ProviderSegment>().test();
CopyMoveCtorsOpsTest<armem::ltm::CoreSegment>().test();
CopyMoveCtorsOpsTest<armem::ltm::Memory>().test();
- }
- {
+ }*/
+ /*{
InstanceCopyMoveCtorsOpsTest<armem::d_ltm::EntityInstance>().test();
CopyMoveCtorsOpsTest<armem::d_ltm::EntitySnapshot>().test();
CopyMoveCtorsOpsTest<armem::d_ltm::Entity>().test();
CopyMoveCtorsOpsTest<armem::d_ltm::ProviderSegment>().test();
CopyMoveCtorsOpsTest<armem::d_ltm::CoreSegment>().test();
CopyMoveCtorsOpsTest<armem::d_ltm::Memory>().test();
- }
+ }*/
}
@@ -612,7 +777,7 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
{
armem::EntityUpdate update;
- armem::wm::Memory memory("Memory");
+ wm::Memory memory("Memory");
BOOST_CHECK_EQUAL(memory.name(), "Memory");
{
update.entityID = armem::MemoryID::fromString("OtherMemory/SomeSegment");
@@ -621,7 +786,7 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
BOOST_CHECK_THROW(memory.update(update), armem::error::MissingEntry);
}
- armem::wm::CoreSegment& coreSegment = memory.addCoreSegment("ImageRGB");
+ wm::CoreSegment& coreSegment = memory.addCoreSegment("ImageRGB");
BOOST_CHECK_EQUAL(coreSegment.name(), "ImageRGB");
BOOST_CHECK(memory.hasCoreSegment(coreSegment.name()));
{
@@ -632,7 +797,7 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
//BOOST_CHECK_THROW(coreSegment.update(update), armem::error::MissingEntry);
}
- armem::wm::ProviderSegment& providerSegment = coreSegment.addProviderSegment("SomeRGBImageProvider");
+ wm::ProviderSegment& providerSegment = coreSegment.addProviderSegment("SomeRGBImageProvider");
BOOST_CHECK_EQUAL(providerSegment.name(), "SomeRGBImageProvider");
BOOST_CHECK(coreSegment.hasProviderSegment(providerSegment.name()));
{
@@ -652,17 +817,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");
+ 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());
+ wm::EntitySnapshot& entitySnapshot = entity.getSnapshot(update.timeCreated);
+ BOOST_CHECK_EQUAL(entitySnapshot.size(), update.instancesData.size());
// Another update (on memory).
@@ -670,16 +835,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 +852,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 +864,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();
@@ -738,7 +903,7 @@ BOOST_AUTO_TEST_CASE(test_history_size_in_provider_segment)
std::vector<std::string> entityNames = { "A", "B" };
// Fill entities and histories with unlimited size.
- for (const auto& name : entityNames)
+ for (const std::string& name : entityNames)
{
update.entityID.entityName = name;
@@ -753,22 +918,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,69 +949,18 @@ 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);
entityNames.push_back("C");
- for (const auto& name : entityNames)
+ for (const std::string& name : entityNames)
{
update.entityID.entityName = name;
update.timeCreated = armem::Time::milliSeconds(5000);
providerSegment.update(update);
- BOOST_CHECK_EQUAL(providerSegment.getEntity(name).history().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();
+ BOOST_CHECK_EQUAL(providerSegment.getEntity(name).getMaxHistorySize(), -1);
+ BOOST_CHECK_EQUAL(providerSegment.getEntity(name).size(), 3);
}
}
-
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..13d050572342263d273dc5080f1631cec15faf05 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
@@ -20,28 +20,28 @@
* 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 <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 <iostream>
+#include <ArmarXCore/core/exceptions/LocalException.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 +55,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 +84,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 +125,9 @@ 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.findFirstSnapshotAfterOrAt(armem::Time::microSeconds(0));
+ BOOST_REQUIRE_NE(first, nullptr);
+ BOOST_CHECK_EQUAL(first->time(), armem::Time::microSeconds(5000));
}
}
@@ -140,8 +140,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 +168,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 +192,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 +213,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 +234,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 +267,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 +285,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 +307,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 +322,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 +347,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 +362,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 +377,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 +397,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 +423,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 +449,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 +468,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 +493,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 +518,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 +543,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 +561,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 +579,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 +600,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 +642,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 +665,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..43b75650492c4d69a6fb08d5a550796e5dfd3a3b 100644
--- a/source/RobotAPI/libraries/armem/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/test/CMakeLists.txt
@@ -2,9 +2,11 @@
# Libs required for the tests
SET(LIBS ${LIBS} ArmarXCore ${LIB_NAME})
+armarx_add_test(ArMemForEachTest ArMemForEachTest.cpp "${LIBS}")
+armarx_add_test(ArMemGetFindTest ArMemGetFindTest.cpp "${LIBS}")
armarx_add_test(ArMemIceConversionsTest ArMemIceConversionsTest.cpp "${LIBS}")
+armarx_add_test(ArMemLTMTest ArMemLTMTest.cpp "${LIBS}")
armarx_add_test(ArMemMemoryTest ArMemMemoryTest.cpp "${LIBS}")
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(ArMemQueryTest ArMemQueryTest.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/CMakeLists.txt b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
index 696c6db777d8512d49168682c8a368243f2b930d..2a781dd3989d5731393e1b5d9304b06f467611a4 100644
--- a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
@@ -18,10 +18,11 @@ set(SOURCES
gui_utils.cpp
lifecycle.cpp
- MemoryControlWidget.cpp
MemoryViewer.cpp
PeriodicUpdateWidget.cpp
+ disk/ControlWidget.cpp
+
instance/GroupBox.cpp
instance/ImageView.cpp
instance/InstanceView.cpp
@@ -53,7 +54,6 @@ set(HEADERS
gui_utils.h
lifecycle.h
- MemoryControlWidget.h
MemoryViewer.h
PeriodicUpdateWidget.h
TreeWidgetBuilder.h
@@ -61,6 +61,8 @@ set(HEADERS
PeriodicUpdateWidget.h
TreeWidgetBuilder.h
+ disk/ControlWidget.h
+
instance/GroupBox.h
instance/ImageView.h
instance/InstanceView.h
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.cpp b/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.cpp
deleted file mode 100644
index 5016db50bf5da8dc10d0fac5b5c004b9178ddc40..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.cpp
+++ /dev/null
@@ -1,65 +0,0 @@
-#include "MemoryControlWidget.h"
-
-#include <QPushButton>
-#include <QLineEdit>
-#include <QTimer>
-#include <QHBoxLayout>
-
-#include <cmath>
-
-
-namespace armarx::armem::gui
-{
-
- MemoryControlWidget::MemoryControlWidget()
- {
- setSizePolicy(QSizePolicy::Policy::Minimum, QSizePolicy::Policy::Fixed);
-
- auto vlayout = new QVBoxLayout();
- auto hlayout = new QHBoxLayout();
-
- 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);
-
- _storeOnDiskButton = new QPushButton("Store current result on local Disk", this);
-
- _storeInLTMButton = new QPushButton("Store current result in LTM", this);
-
- vlayout->addWidget(_lineEdit);
- hlayout->addWidget(_storeOnDiskButton);
- hlayout->addWidget(_storeInLTMButton);
- vlayout->addItem(hlayout);
-
- this->setLayout(vlayout);
- // Private connections.
-
- // Public connections.
- connect(_storeInLTMButton, &QPushButton::pressed, this, &This::storeInLTM);
- connect(_storeOnDiskButton, &QPushButton::pressed, this, &This::storeOnDisk);
- }
-
- QLineEdit* MemoryControlWidget::pathInputBox()
- {
- return _lineEdit;
- }
-
- QString MemoryControlWidget::getEnteredPath()
- {
- return _lineEdit->text();
- }
-
- QPushButton* MemoryControlWidget::storeInLTMButton()
- {
- return _storeInLTMButton;
- }
-
- QPushButton* MemoryControlWidget::storeOnDiskButton()
- {
- return _storeOnDiskButton;
- }
-}
-
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.h b/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.h
deleted file mode 100644
index 19831694acc7b9251c7d372b8381f755df1bc292..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.h
+++ /dev/null
@@ -1,48 +0,0 @@
-#pragma once
-
-#include <QWidget>
-
-class QPushButton;
-class QLineEdit;
-
-namespace armarx::armem::gui
-{
-
- class MemoryControlWidget : public QWidget
- {
- Q_OBJECT
- using This = MemoryControlWidget;
-
- public:
-
- MemoryControlWidget();
-
- QLineEdit* pathInputBox();
- QString getEnteredPath();
-
- QPushButton* storeInLTMButton();
- QPushButton* storeOnDiskButton();
-
- public slots:
-
- signals:
-
- void storeInLTM();
- void storeOnDisk();
-
- private slots:
-
-
- signals:
-
-
- private:
-
- QLineEdit* _lineEdit;
-
- QPushButton* _storeInLTMButton;
- QPushButton* _storeOnDiskButton;
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
index f88a2e1731c87de6fa163c307c90f4c6d3e01ae2..6e2bbb30bd3d2258796da97ce8a81b24224d201e 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
@@ -1,12 +1,10 @@
#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/wm.h>
+#include <RobotAPI/libraries/armem/server/query_proc/ltm.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
@@ -16,24 +14,28 @@
#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <QApplication>
#include <QBoxLayout>
#include <QCheckBox>
+#include <QClipboard>
#include <QDialog>
#include <QGroupBox>
#include <QMenu>
#include <QLabel>
#include <QLayout>
#include <QSettings>
+#include <QTimer>
+
+#include <iomanip>
-#include <filesystem>
namespace armarx::armem::gui
{
MemoryViewer::MemoryViewer(
QBoxLayout* updateWidgetLayout,
- QBoxLayout* memoryControlWidgetLayout,
QGroupBox* memoryGroupBox, QLayout* memoryGroupBoxParentLayout,
QGroupBox* instanceGroupBox, QLayout* instanceGroupBoxParentLayout,
+ QBoxLayout* diskControlWidgetLayout,
QLabel* statusLabel
)
{
@@ -46,6 +48,10 @@ namespace armarx::armem::gui
connect(statusLabel, &QLabel::customContextMenuRequested, [statusLabel](const QPoint & pos)
{
QMenu menu(statusLabel);
+ menu.addAction("Copy to clipboard", [statusLabel]()
+ {
+ QApplication::clipboard()->setText(statusLabel->text());
+ });
menu.addAction("Clear status", [statusLabel]()
{
statusLabel->clear();
@@ -59,13 +65,9 @@ namespace armarx::armem::gui
updateWidget = new armem::gui::PeriodicUpdateWidget(2.0, 60);
updateWidgetLayout->insertWidget(0, updateWidget);
- // LTM Control
- if (memoryControlWidgetLayout)
- {
- this->memoryControlWidgetLayout = memoryControlWidgetLayout;
- memoryControlWidget = new armem::gui::MemoryControlWidget();
- memoryControlWidgetLayout->addWidget(memoryControlWidget);
- }
+ processQueryResultTimer = new QTimer(this);
+ processQueryResultTimer->setInterval(1000 / 60); // Keep this stable.
+ processQueryResultTimer->start();
// Memory View
memoryGroup = new armem::gui::MemoryGroupBox();
@@ -78,14 +80,25 @@ namespace armarx::armem::gui
this->instanceGroup->setStatusLabel(statusLabel);
ARMARX_CHECK_NULL(instanceGroupBox);
+ // Disk Control
+ if (diskControlWidgetLayout)
+ {
+ this->diskControlLayout = diskControlWidgetLayout;
+ diskControl = new armem::gui::disk::ControlWidget();
+ diskControlWidgetLayout->addWidget(diskControl);
+ }
+
// Connections
//connect(this, &This::connected, this, &This::updateMemory);
- connect(memoryControlWidget, &armem::gui::MemoryControlWidget::storeOnDisk, this, &This::storeOnDisk);
- connect(memoryControlWidget, &armem::gui::MemoryControlWidget::storeInLTM, this, &This::storeInLTM);
+ connect(diskControl, &armem::gui::disk::ControlWidget::requestedStoreOnDisk, this, &This::storeOnDisk);
+ connect(diskControl, &armem::gui::disk::ControlWidget::requestedLoadFromDisk, this, &This::loadFromDisk);
- connect(this, &This::connected, this, &This::updateMemories);
- connect(updateWidget, &armem::gui::PeriodicUpdateWidget::update, this, &This::updateMemories);
+ connect(this, &This::connected, this, &This::startQueries);
+ connect(updateWidget, &armem::gui::PeriodicUpdateWidget::update, this, &This::startQueries);
+ connect(processQueryResultTimer, &QTimer::timeout, this, &This::processQueryResults);
+
+ connect(memoryGroup->queryWidget(), &armem::gui::QueryWidget::storeInLTM, this, &This::storeInLTM);
connect(this, &This::memoryDataChanged, this, &This::updateMemoryTree);
connect(memoryGroup->tree(), &armem::gui::MemoryTreeWidget::selectedItemChanged, this, &This::updateInstanceTree);
@@ -95,11 +108,13 @@ namespace armarx::armem::gui
connect(instanceGroup->view, &armem::gui::InstanceView::memoryIdResolutionRequested, this, &This::resolveMemoryID);
}
- void MemoryViewer::setLogTag(const std::string& tag)
+
+ void MemoryViewer::setLogTag(const std::string& _tag) // Leading _ silences a warning
{
- Logging::setTag(tag);
+ Logging::setTag(_tag);
}
+
void MemoryViewer::onInit(ManagedIceObject& component)
{
if (mnsName.size() > 0)
@@ -114,9 +129,10 @@ namespace armarx::armem::gui
emit initialized();
}
+
void MemoryViewer::onConnect(ManagedIceObject& component)
{
- if (!mnsName.empty())
+ if (not mnsName.empty())
{
armem::mns::MemoryNameSystemInterfacePrx mnsProxy;
component.getProxy(mnsProxy, mnsName);
@@ -126,7 +142,7 @@ namespace armarx::armem::gui
memoryReaders = mns.getAllReaders(update);
}
// DebugObserver is optional (check for null on every call)
- if (!debugObserverName.empty())
+ if (not debugObserverName.empty())
{
component.getProxy(debugObserver, debugObserverName, false, "", false);
}
@@ -135,6 +151,7 @@ namespace armarx::armem::gui
emit connected();
}
+
void MemoryViewer::onDisconnect(ManagedIceObject&)
{
updateWidget->stopTimer();
@@ -142,6 +159,7 @@ namespace armarx::armem::gui
emit disconnected();
}
+
const armem::wm::Memory* MemoryViewer::getSingleMemoryData(const std::string& memoryName)
{
auto it = memoryData.find(memoryName);
@@ -164,9 +182,10 @@ namespace armarx::armem::gui
}
}
+
void MemoryViewer::storeInLTM()
{
- TIMING_START(MemoryStore);
+ TIMING_START(MemoryStore)
for (auto& [name, reader] : memoryReaders)
{
@@ -175,149 +194,183 @@ namespace armarx::armem::gui
reader.readAndStore(input);
}
- TIMING_END_STREAM(MemoryStore, ARMARX_VERBOSE);
+ TIMING_END_STREAM(MemoryStore, ARMARX_VERBOSE)
}
- void MemoryViewer::storeOnDisk()
+
+ void MemoryViewer::storeOnDisk(QString directory)
{
- TIMING_START(MemoryExport);
- QString qs = memoryControlWidget->getEnteredPath();
- std::string utf8_text = qs.toUtf8().constData();
+ TIMING_START(MemoryExport)
- if (not utf8_text.empty())
- {
- ARMARX_IMPORTANT << "Exporting all memories at '" << utf8_text << "'.";
+ std::string status;
+ diskControl->storeOnDisk(directory, memoryData, &status);
- std::filesystem::path p(utf8_text);
- if (std::filesystem::is_regular_file(p))
- {
- ARMARX_WARNING << "Could not export a memory at '" << utf8_text << "'. Skipping export.";
- return;
- }
+ statusLabel->setText(QString::fromStdString(status));
+ TIMING_END_STREAM(MemoryExport, ARMARX_VERBOSE)
+ }
- std::filesystem::create_directories(p);
- for (auto& [name, reader] : memoryReaders)
- {
- armem::client::QueryInput input = memoryGroup->queryWidget()->queryInput();
- armem::client::QueryResult result = reader.query(input);
- armem::d_ltm::Memory dMem(name);
- dMem.reload(p / name);
- dMem.append(result.memory);
- }
- }
- else
+ void MemoryViewer::loadFromDisk(QString directory)
+ {
+ std::string status;
+ std::map<std::string, wm::Memory> data =
+ diskControl->loadFromDisk(directory, memoryGroup->queryWidget()->queryInput(), &status);
+
+ for (auto& [name, memory] : data)
{
- ARMARX_WARNING << "The path is empty. Could not export the memory in nirvana.";
+ this->memoryData[name] = std::move(memory);
}
+ statusLabel->setText(QString::fromStdString(status));
- TIMING_END_STREAM(MemoryExport, ARMARX_VERBOSE);
+ emit memoryDataChanged();
}
- void MemoryViewer::updateMemories()
- {
- memoryReaders.clear();
- memoryData.clear();
+ void MemoryViewer::startQueries()
+ {
memoryReaders = mns.getAllReaders(true);
+ startDueQueries(runningQueries, memoryReaders);
+ }
+
+
+ void MemoryViewer::processQueryResults()
+ {
+ const std::map<std::string, client::QueryResult> results = collectQueryResults(runningQueries, memoryReaders);
+
+ int errorCount = 0;
+ applyQueryResults(results, &errorCount);
+
+ emit memoryDataChanged();
+ updateStatusLabel(errorCount);
+ }
+
+
+ void MemoryViewer::updateStatusLabel(int errorCount)
+ {
+ // Code to output status label information
+ if (statusLabel and errorCount > 0)
+ {
+ 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.";
- bool dataChanged = false;
+ statusLabel->setText(QString::fromStdString(ss.str()));
+ }
+ }
- QString qs = memoryControlWidget->getEnteredPath();
- std::string utf8_text = qs.toUtf8().constData();
- std::filesystem::path p(utf8_text);
+ void MemoryViewer::startDueQueries(
+ std::map<std::string, Ice::AsyncResultPtr>& queries,
+ const std::map<std::string, armem::client::Reader>& readers)
+ {
+ armem::client::QueryInput input = memoryGroup->queryWidget()->queryInput();
- // first check if the local file system should be queried
- if (memoryGroup->queryWidget()->alsoQueryLocalDisk())
+ for (auto& [name, reader] : readers)
{
- if (std::filesystem::is_directory(p))
+ if (queries.count(name) == 0)
{
- for (const auto& d : std::filesystem::directory_iterator(p))
- {
- if (d.is_directory())
- {
- std::string k = d.path().filename();
- armem::d_ltm::Memory dMem(k);
- dMem.reload(p / k);
+ queries[name] = reader.memoryPrx->begin_query(input.toIce());
+ }
+ }
+ }
- armem::client::QueryInput input = memoryGroup->queryWidget()->queryInput();
- input.addQueryTargetToAll(armem::query::data::QueryTarget::Disk);
- armem::d_ltm::query_proc::MemoryQueryProcessor d_ltm_processor;
- dMem = d_ltm_processor.process(input.toIce(), dMem);
+ std::map<std::string, client::QueryResult>
+ MemoryViewer::collectQueryResults(
+ std::map<std::string, Ice::AsyncResultPtr>& queries,
+ const std::map<std::string, client::Reader>& readers)
+ {
+ TIMING_START(tCollectQueryResults)
+
+ std::map<std::string, client::QueryResult> results;
+ for (auto it = queries.begin(); it != queries.end();)
+ {
+ const std::string& name = it->first;
+ Ice::AsyncResultPtr& queryPromise = it->second;
- wm::Memory converted = dMem.convert();
- memoryData[k] = std::move(converted);
- dataChanged = true;
+ if (queryPromise->isCompleted())
+ {
+ if (auto jt = memoryReaders.find(name); jt != readers.end())
+ {
+ try
+ {
+ results[name] = client::QueryResult::fromIce(jt->second.memoryPrx->end_query(queryPromise));
+ }
+ catch (const Ice::ConnectionRefusedException&)
+ {
+ // Server is gone (MNS did not now about it yet) => Skip result.
}
}
+ // else: Server is gone (MNS new about it) => Skip result.
+
+ // Promise is completed => Clean up in any case.
+ it = runningQueries.erase(it);
}
else
{
- ARMARX_WARNING << "Could not import a memory from '" << utf8_text << "'. Skipping import.";
+ ++it; // Uncompleted => Keep.
}
}
- armem::client::QueryInput input = memoryGroup->queryWidget()->queryInput();
- for (auto& [name, reader] : memoryReaders)
+ TIMING_END_STREAM(tCollectQueryResults, ARMARX_VERBOSE)
+ if (debugObserver)
{
- TIMING_START(MemoryQuery);
+ debugObserver->begin_setDebugChannel(Logging::tag.tagName,
{
- armem::client::QueryResult result = reader.query(input);
- if (result)
- {
- if (result.memory.hasData())
- {
- if (const auto& it = memoryData.find(name); it != memoryData.end())
- {
- result.memory.append(it->second);
+ { "t Collect Query Results [ms]", new Variant(tCollectQueryResults.toMilliSecondsDouble()) },
+ { "# Collected Query Results", new Variant(static_cast<int>(results.size())) },
+ });
+ }
- // 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);
+ return results;
+ }
- 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);
- }
- else
- {
- ARMARX_INFO << "The memory " << name << " has no data after querying.";
- }
- }
- else
+ void MemoryViewer::applyQueryResults(
+ const std::map<std::string, client::QueryResult>& results,
+ int* outErrorCount)
+ {
+ TIMING_START(tProcessQueryResults)
+ for (const auto& [name, result] : results)
+ {
+ if (result.success)
+ {
+ memoryData[name] = std::move(result.memory);
+ }
+ else
+ {
+ ARMARX_WARNING << "Querying memory server '" << name << "' produced an error: \n" << result.errorMessage;
+ if (outErrorCount)
{
- ARMARX_WARNING << "A query for memory '" << name << "' produced an error: " << result.errorMessage;
- if (statusLabel)
- {
- statusLabel->setText(QString::fromStdString(result.errorMessage));
- }
+ outErrorCount++;
}
}
- TIMING_END_STREAM(MemoryQuery, ARMARX_VERBOSE);
+ }
- if (debugObserver)
+ // Drop all entries in memoryData which are not in memoryReaders anymore.
+ for (auto it = memoryData.begin(); it != memoryData.end();)
+ {
+ if (memoryReaders.count(it->first) == 0)
+ {
+ it = memoryData.erase(it);
+ }
+ else
{
- debugObserver->setDebugDatafield(Logging::tag.tagName, "Memory Query [ms]", new Variant(MemoryQuery.toMilliSecondsDouble()));
+ ++it;
}
}
- if (dataChanged)
- {
- emit memoryDataChanged();
- }
- else
+ TIMING_END_STREAM(tProcessQueryResults, ARMARX_VERBOSE)
+ if (debugObserver)
{
- if (statusLabel)
+ debugObserver->begin_setDebugChannel(Logging::tag.tagName,
{
- statusLabel->setText("No query result.");
- }
+ { "t Process Query Results [ms]", new Variant(tProcessQueryResults.toMilliSecondsDouble()) },
+ { "# Processed Query Results", new Variant(static_cast<int>(results.size())) },
+ });
}
}
@@ -327,13 +380,13 @@ namespace armarx::armem::gui
const armem::wm::Memory* data = getSingleMemoryData(selectedID.memoryName);
if (data)
{
- if (!selectedID.hasEntityName())
+ if (not selectedID.hasEntityName())
{
return;
}
armem::MemoryID id = selectedID;
const armem::wm::EntitySnapshot* snapshot = nullptr;
- if (!id.hasTimestamp())
+ if (not id.hasTimestamp())
{
const armem::wm::Entity& entity = data->getEntity(id);
if (entity.empty())
@@ -343,13 +396,13 @@ namespace armarx::armem::gui
snapshot = &entity.getLatestSnapshot();
id.timestamp = snapshot->time();
}
- if (!id.hasInstanceIndex())
+ if (not id.hasInstanceIndex())
{
- if (!snapshot)
+ if (not snapshot)
{
try
{
- snapshot = &data->getEntitySnapshot(id);
+ snapshot = &data->getSnapshot(id);
}
catch (const armem::error::ArMemError& e)
{
@@ -371,6 +424,7 @@ namespace armarx::armem::gui
}
}
+
void MemoryViewer::resolveMemoryID(const MemoryID& id)
{
// ARMARX_IMPORTANT << "Resolving memory ID: " << id;
@@ -390,18 +444,17 @@ namespace armarx::armem::gui
std::optional<wm::EntityInstance> instance;
try
{
- const wm::Memory* data = getSingleMemoryData(id.memoryName);
- if (data)
+ if (const wm::Memory* data = getSingleMemoryData(id.memoryName))
{
segmentType = data->getProviderSegment(id).aronType();
if (id.hasInstanceIndex())
{
- instance = data->getEntityInstance(id);
+ instance = data->getInstance(id);
}
else if (id.hasTimestamp())
{
- instance = data->getEntitySnapshot(id).getInstance(0);
+ instance = data->getSnapshot(id).getInstance(0);
}
else
{
@@ -409,13 +462,12 @@ namespace armarx::armem::gui
}
}
}
- catch (const armem::error::ArMemError& e)
+ catch (const armem::error::ArMemError&)
{
// May be handled by remote lookup
- (void) e;
}
- if (!instance)
+ if (not instance)
{
try
{
@@ -439,36 +491,39 @@ namespace armarx::armem::gui
}
}
+
void MemoryViewer::updateMemoryTree()
{
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);
+ TIMING_START(GuiUpdate)
memoryGroup->tree()->update(convMap);
- TIMING_END_STREAM(GuiUpdate, ARMARX_VERBOSE);
+ TIMING_END_STREAM(GuiUpdate, ARMARX_VERBOSE)
if (debugObserver)
{
- debugObserver->setDebugDatafield(Logging::tag.tagName, "GUI Update [ms]", new Variant(GuiUpdate.toMilliSecondsDouble()));
+ try
+ {
+ debugObserver->setDebugDatafield(Logging::tag.tagName, "GUI Update [ms]", new Variant(GuiUpdate.toMilliSecondsDouble()));
+ }
+ catch (const Ice::Exception&)
+ {
+ // Ignore ...
+ }
}
}
+
const static std::string CONFIG_KEY_MEMORY = "MemoryViewer.MemoryNameSystem";
const static std::string CONFIG_KEY_DEBUG_OBSERVER = "MemoryViewer.DebugObserverName";
+
void MemoryViewer::loadSettings(QSettings* settings)
{
mnsName = settings->value(QString::fromStdString(CONFIG_KEY_MEMORY), "MemoryNameSystem").toString().toStdString();
@@ -480,12 +535,12 @@ namespace armarx::armem::gui
settings->setValue(QString::fromStdString(CONFIG_KEY_DEBUG_OBSERVER), QString::fromStdString(debugObserverName));
}
+
void MemoryViewer::writeConfigDialog(SimpleConfigDialog* dialog)
{
- dialog->addProxyFinder<armarx::armem::mns::MemoryNameSystemInterfacePrx>({CONFIG_KEY_MEMORY, "MemoryNameSystem", "*"});
+ dialog->addProxyFinder<armarx::armem::mns::MemoryNameSystemInterfacePrx>({CONFIG_KEY_MEMORY, "MemoryNameSystem", "MemoryNameSystem"});
dialog->addProxyFinder<armarx::DebugObserverInterfacePrx>({CONFIG_KEY_DEBUG_OBSERVER, "Debug Observer", "DebugObserver"});
}
-
void MemoryViewer::readConfigDialog(SimpleConfigDialog* dialog)
{
mnsName = dialog->getProxyName(CONFIG_KEY_MEMORY);
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
index 71870bdd565b0b8a585808f5f0e9f0f43c93d315..e29a2352014cebaf1a700097f7409f7f32cb5229 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
@@ -10,11 +10,11 @@
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.h>
#include <RobotAPI/libraries/armem_gui/lifecycle.h>
#include <RobotAPI/libraries/armem_gui/instance/GroupBox.h>
#include <RobotAPI/libraries/armem_gui/memory/GroupBox.h>
-#include <RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.h>
-#include <RobotAPI/libraries/armem_gui/MemoryControlWidget.h>
+#include <RobotAPI/libraries/armem_gui/disk/ControlWidget.h>
class QBoxLayout;
@@ -23,6 +23,7 @@ class QGroupBox;
class QLabel;
class QLayout;
class QSettings;
+class QTimer;
namespace armarx
@@ -47,9 +48,9 @@ namespace armarx::armem::gui
MemoryViewer(
QBoxLayout* updateWidgetLayout,
- QBoxLayout* ltmControlWidgetLayout,
QGroupBox* memoryGroupBox, QLayout* memoryGroupBoxParentLayout,
QGroupBox* instanceGroupBox, QLayout* instanceGroupBoxParentLayout,
+ QBoxLayout* diskControlWidgetLayout,
QLabel* statusLabel
);
@@ -65,14 +66,16 @@ namespace armarx::armem::gui
public slots:
- void updateMemories();
void updateInstanceTree(const armem::MemoryID& selectedID);
void resolveMemoryID(const MemoryID& id);
- // LTMControlWidget
+ // Disk Control
+ void storeOnDisk(QString directory);
+ void loadFromDisk(QString directory);
+
void storeInLTM();
- void storeOnDisk();
+
signals:
@@ -87,8 +90,13 @@ namespace armarx::armem::gui
private slots:
+ void startQueries();
+ void processQueryResults();
+
void updateMemoryTree();
+
+
signals:
void memoryDataChanged();
@@ -101,6 +109,24 @@ namespace armarx::armem::gui
void onDisconnect(ManagedIceObject& component);
const armem::wm::Memory* getSingleMemoryData(const std::string& memoryName);
+ void updateStatusLabel(int errorCount);
+
+
+
+ /// Start a query for each entry in `memoryReaders` which is not in `runningQueries`.
+ void startDueQueries(
+ std::map<std::string, Ice::AsyncResultPtr>& queries,
+ const std::map<std::string, armem::client::Reader>& readers);
+
+ std::map<std::string, client::QueryResult>
+ collectQueryResults(
+ std::map<std::string, Ice::AsyncResultPtr>& queries,
+ const std::map<std::string, armem::client::Reader>& readers);
+
+ void applyQueryResults(
+ const std::map<std::string, client::QueryResult>& results,
+ int* outErrorCount = nullptr);
+
public:
@@ -111,11 +137,16 @@ namespace armarx::armem::gui
std::map<std::string, armem::client::Reader> memoryReaders;
std::map<std::string, armem::wm::Memory> memoryData;
+ std::map<std::string, Ice::AsyncResultPtr> runningQueries;
+ /// Periodically triggers query result collection.
+ QTimer* processQueryResultTimer;
+
+
QLayout* updateWidgetLayout = nullptr;
armem::gui::PeriodicUpdateWidget* updateWidget = nullptr;
- QLayout* memoryControlWidgetLayout = nullptr;
- armem::gui::MemoryControlWidget* memoryControlWidget = nullptr;
+ QLayout* diskControlLayout = nullptr;
+ armem::gui::disk::ControlWidget* diskControl = nullptr;
armem::gui::MemoryGroupBox* memoryGroup = nullptr;
diff --git a/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.cpp b/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.cpp
index a62659cd78d4448bb72913ae432fc22f8f2b6eae..2e315e6f06f8e141c3911479685424b3f5585580 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");
@@ -46,8 +46,11 @@ namespace armarx::armem::gui
connect(_frequencySpinBox, &QDoubleSpinBox::editingFinished, this, &This::_updateTimerFrequency);
// Public connections.
- connect(_updateButton, &QPushButton::pressed, this, &This::update);
- connect(_timer, &QTimer::timeout, this, &This::update);
+ connect(_updateButton, &QPushButton::pressed, this, &This::updateSingle);
+ connect(_timer, &QTimer::timeout, this, &This::updatePeriodic);
+
+ connect(this, &This::updateSingle, this, &This::update);
+ connect(this, &This::updatePeriodic, this, &This::update);
}
QPushButton* PeriodicUpdateWidget::updateButton()
@@ -55,6 +58,12 @@ namespace armarx::armem::gui
return _updateButton;
}
+
+ int PeriodicUpdateWidget::getUpdateIntervalMs() const
+ {
+ return static_cast<int>(std::round(1000 / _frequencySpinBox->value()));
+ }
+
void PeriodicUpdateWidget::startTimerIfEnabled()
{
if (_autoCheckBox->isChecked())
@@ -77,7 +86,7 @@ namespace armarx::armem::gui
void PeriodicUpdateWidget::_updateTimerFrequency()
{
- _timer->setInterval(static_cast<int>(std::round(1000 / _frequencySpinBox->value())));
+ _timer->setInterval(getUpdateIntervalMs());
}
void PeriodicUpdateWidget::_toggleAutoUpdates(bool enabled)
diff --git a/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.h b/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.h
index 41b725220d283ddcae534d68c9932f89c89169e4..db8d9d5a30264180d93243c3a6f631f5666ed784 100644
--- a/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.h
@@ -27,8 +27,9 @@ namespace armarx::armem::gui
QDoubleSpinBox* frequencySpinBox();
QPushButton* updateButton();
- bool autoEnabled() const;
- double updateFrequency() const;
+ bool isAutoEnabled() const;
+ double getUpdateFrequency() const;
+ int getUpdateIntervalMs() const;
void startTimerIfEnabled();
void stopTimer();
@@ -40,6 +41,9 @@ namespace armarx::armem::gui
void update();
+ void updateSingle();
+ void updatePeriodic();
+
private slots:
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/disk/ControlWidget.cpp b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e361438099e5e2a34b80d5f64f810a23fb3270d2
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
@@ -0,0 +1,196 @@
+#include "ControlWidget.h"
+
+#include <RobotAPI/libraries/armem/server/ltm/disk/MemoryManager.h>
+#include <RobotAPI/libraries/armem/server/query_proc/ltm.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <QHBoxLayout>
+#include <QSpacerItem>
+#include <QFileDialog>
+#include <QPushButton>
+
+#include <filesystem>
+
+#include <cmath>
+
+
+namespace armarx::armem::gui::disk
+{
+
+ ControlWidget::ControlWidget()
+ {
+ _latestDirectory = QString::fromStdString("/tmp/MemoryExport");
+
+ _loadFromDiskButton = new QPushButton(" Load from Disk", this);
+ _loadFromDiskButton->setIcon(QIcon(":/icons/document-open.svg"));
+ _storeOnDiskButton = new QPushButton(" Store on Disk", this);
+ _storeOnDiskButton->setIcon(QIcon(":/icons/document-save.svg"));
+
+ // Allow horizontal shrinking of buttons
+ std::vector<QPushButton*> buttons { _storeOnDiskButton, _loadFromDiskButton };
+ for (QPushButton* button : buttons)
+ {
+ button->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Fixed);
+ }
+
+ this->setSizePolicy(QSizePolicy::Policy::Preferred, QSizePolicy::Policy::Fixed);
+
+ QHBoxLayout* layout = new QHBoxLayout();
+ this->setLayout(layout);
+
+ const int margin = 0;
+ layout->setContentsMargins(margin, margin, margin, margin);
+
+ layout->addWidget(_loadFromDiskButton);
+ layout->addWidget(_storeOnDiskButton);
+
+
+ // Connections
+
+ connect(_loadFromDiskButton, &QPushButton::pressed, [this]()
+ {
+ QString directory = chooseDirectoryDialog();
+ if (directory.size() > 0)
+ {
+ emit requestedLoadFromDisk(directory);
+ }
+ });
+ connect(_storeOnDiskButton, &QPushButton::pressed, [this]()
+ {
+ QString directory = chooseDirectoryDialog();
+ if (directory.size() > 0)
+ {
+ emit requestedStoreOnDisk(directory);
+ }
+ });
+ }
+
+
+ static
+ const std::string&
+ handleSingular(int num, const std::string& singular, const std::string& plural)
+ {
+ return num == 1 ? singular : plural;
+ }
+
+
+ void
+ ControlWidget::storeOnDisk(
+ QString directory,
+ const std::map<std::string, wm::Memory> memoryData,
+ std::string* outStatus)
+ {
+ std::filesystem::path path(directory.toUtf8().constData());
+ ARMARX_CHECK_POSITIVE(path.string().size()); // An empty path indicates an error.
+
+ std::stringstream status;
+ if (std::filesystem::is_regular_file(path))
+ {
+ status << "Could not export memories contents to " << path << ": Cannot overwrite existing file.";
+ }
+ else
+ {
+ int numStored = 0;
+ for (const auto& [name, data] : memoryData)
+ {
+ if (std::filesystem::is_regular_file(path / name))
+ {
+ status << "Could not export memory '" << name << "' to " << path << ": Cannot overwrite existing file.\n";
+ }
+ else
+ {
+ std::filesystem::create_directories(path / name);
+
+ armem::server::ltm::disk::MemoryManager manager;
+ manager.setName(name);
+ manager.setBasePath(path / name);
+ manager.reload();
+ manager.append(data);
+
+ numStored++;
+ }
+ }
+ status << "Exported " << numStored << " " << handleSingular(numStored, "memory", "memories") << " to " << path << ".";
+ }
+
+ if (outStatus)
+ {
+ *outStatus = status.str();
+ }
+ }
+
+
+ std::map<std::string, wm::Memory>
+ ControlWidget::loadFromDisk(
+ QString directory,
+ const armem::client::QueryInput& _queryInput,
+ std::string* outStatus)
+ {
+ std::filesystem::path path(directory.toUtf8().constData());
+
+ std::map<std::string, wm::Memory> memoryData;
+ std::stringstream status;
+
+ if (not std::filesystem::is_directory(path))
+ {
+ status << "Could not import a memory from " << path << ". Skipping import.";
+ }
+ else
+ {
+ // We use LTM as query target for the disk
+ armem::client::QueryInput queryInput = _queryInput;
+ queryInput.addQueryTargetToAll(armem::query::data::QueryTarget::LTM);
+ const query::data::Input queryIce = queryInput.toIce();
+
+ int numLoaded = 0;
+ for (const auto& dir : std::filesystem::directory_iterator(path))
+ {
+ if (dir.is_directory())
+ {
+ const std::string key = dir.path().filename();
+ armem::server::ltm::disk::MemoryManager manager;
+ manager.setName(key);
+ manager.setBasePath(path / key);
+
+ manager.reload();
+
+ armem::server::query_proc::ltm::MemoryQueryProcessor ltm_processor;
+ armem::wm::Memory query_res = ltm_processor.process(queryIce, manager.getCacheAndLutNotConverted());
+
+ manager.convert(query_res);
+ memoryData[key] = std::move(query_res);
+
+ numLoaded++;
+ }
+ }
+ status << "Loaded " << numLoaded << " " << handleSingular(numLoaded, "memory", "memories") << " from " << path << ".";
+ }
+
+ if (outStatus)
+ {
+ *outStatus = status.str();
+ }
+ return memoryData;
+ }
+
+
+ QString ControlWidget::chooseDirectoryDialog()
+ {
+ QFileDialog dialog;
+ dialog.setFileMode(QFileDialog::DirectoryOnly);
+ dialog.setOption(QFileDialog::ShowDirsOnly, false);
+ dialog.setDirectory(_latestDirectory);
+ if (dialog.exec())
+ {
+ _latestDirectory = dialog.directory().path();
+ return _latestDirectory;
+ }
+ else
+ {
+ return QString::fromStdString("");
+ }
+ }
+
+}
+
diff --git a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.h b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.h
new file mode 100644
index 0000000000000000000000000000000000000000..b14c9b0a0810acbafba74638e6ebd87989e81165
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.h
@@ -0,0 +1,62 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+#include <RobotAPI/libraries/armem/client/Query.h>
+
+#include <QWidget>
+#include <QString>
+
+
+class QPushButton;
+
+
+namespace armarx::armem::gui::disk
+{
+
+ class ControlWidget : public QWidget
+ {
+ Q_OBJECT
+ using This = ControlWidget;
+
+ public:
+
+ ControlWidget();
+
+
+ void
+ storeOnDisk(
+ QString directory,
+ const std::map<std::string, wm::Memory> memoryData,
+ std::string* outStatus = nullptr);
+
+ std::map<std::string, wm::Memory>
+ loadFromDisk(
+ QString directory,
+ const armem::client::QueryInput& queryInput,
+ std::string* outStatus = nullptr);
+
+
+ signals:
+
+ void requestedLoadFromDisk(QString directory);
+ void requestedStoreOnDisk(QString directory);
+
+
+ private slots:
+
+ QString chooseDirectoryDialog();
+
+
+ signals:
+
+
+ private:
+
+ QPushButton* _loadFromDiskButton;
+ QPushButton* _storeOnDiskButton;
+
+ QString _latestDirectory;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/GroupBox.cpp b/source/RobotAPI/libraries/armem_gui/instance/GroupBox.cpp
index ff828d4437f042c7857332ec3957f9b5f55c60a5..6d233680524bef1b515236fb03ec6e7e8e965ec0 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/GroupBox.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/GroupBox.cpp
@@ -15,7 +15,7 @@ namespace armarx::armem::gui::instance
view = new armem::gui::InstanceView();
- useTypeInfoCheckBox = new QCheckBox("Use Type Info", this);
+ useTypeInfoCheckBox = new QCheckBox("Use Type Information", this);
useTypeInfoCheckBox->setChecked(true);
QHBoxLayout* checkBoxLayout = new QHBoxLayout();
diff --git a/source/RobotAPI/libraries/armem_gui/instance/ImageView.cpp b/source/RobotAPI/libraries/armem_gui/instance/ImageView.cpp
index 05979fe8f669c3d0bc0cf30c7f7ea78004e49372..8c75fbd87d715add23701374643ff22d37f281ec 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/ImageView.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/ImageView.cpp
@@ -13,7 +13,6 @@
* 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 ArmarX::
* @author Rainer Kartmann ( rainer dot kartmann at kit dot edu)
* @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
diff --git a/source/RobotAPI/libraries/armem_gui/instance/ImageView.h b/source/RobotAPI/libraries/armem_gui/instance/ImageView.h
index d684bbcccc37482f358d9350ba78812e9d065cc3..4b85e093a9ef522b7d125a6a0362a98dc4e11c52 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/ImageView.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/ImageView.h
@@ -13,7 +13,6 @@
* 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 ArmarX::
* @author Rainer Kartmann ( rainer dot kartmann at kit dot edu)
* @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
@@ -29,6 +28,9 @@
namespace armarx::armem::gui::instance
{
+ /**
+ * @brief A widget drawing an image in itself.
+ */
class ImageView : public QWidget
{
Q_OBJECT
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
index 2b4ab121dabc7d52188f1d10b8adb501f2c72d32..da6ee43303128a3a73bf8db917301c41fe4b7695 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
@@ -15,6 +15,8 @@
#include <QVBoxLayout>
#include <SimoxUtility/algorithm/string.h>
+#include <SimoxUtility/color/cmaps.h>
+#include <SimoxUtility/math/SoftMinMax.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -104,7 +106,7 @@ namespace armarx::armem::gui::instance
const armem::wm::EntityInstance* instance = nullptr;
try
{
- instance = &memory.getEntityInstance(id);
+ instance = &memory.getInstance(id);
if (useTypeInfo)
{
aronType = memory.getProviderSegment(id).aronType();
@@ -113,8 +115,7 @@ namespace armarx::armem::gui::instance
catch (const armem::error::ArMemError& e)
{
showErrorMessage(e.what());
- return;
- };
+ }
if (instance)
{
update(*instance, aronType);
@@ -146,7 +147,7 @@ namespace armarx::armem::gui::instance
void InstanceView::addInstanceView(const wm::EntityInstance& instance, aron::typenavigator::ObjectNavigatorPtr aronType)
{
- ARMARX_IMPORTANT << "Adding instance view with toolbar for instance: " << instance.id();
+ // ARMARX_IMPORTANT << "Adding instance view with toolbar for instance: " << instance.id();
InstanceView* view = new InstanceView;
view->setStatusLabel(statusLabel);
view->setUseTypeInfo(useTypeInfo);
@@ -175,6 +176,8 @@ namespace armarx::armem::gui::instance
{
if (!data)
{
+ treeItemData->setText(int(Columns::TYPE), QString::fromStdString(""));
+
armarx::gui::clearItem(treeItemData);
QTreeWidgetItem* item = new QTreeWidgetItem({"(No data.)"});
treeItemData->addChild(item);
@@ -189,6 +192,8 @@ namespace armarx::armem::gui::instance
}
else
{
+ treeItemData->setText(int(Columns::TYPE), QString::fromStdString(""));
+
DataTreeBuilder builder;
builder.setColumns(int(Columns::KEY), int(Columns::VALUE), int(Columns::TYPE));
builder.updateTree(treeItemData, data);
@@ -205,7 +210,7 @@ namespace armarx::armem::gui::instance
armem::toDateTimeMilliSeconds(metadata.timeSent),
armem::toDateTimeMilliSeconds(metadata.timeArrived)
};
- ARMARX_CHECK_EQUAL(treeItemMetadata->childCount(), items.size());
+ ARMARX_CHECK_EQUAL(static_cast<size_t>(treeItemMetadata->childCount()), items.size());
int i = 0;
for (const std::string& item : items)
{
@@ -222,11 +227,18 @@ namespace armarx::armem::gui::instance
}
- aron::Path InstanceView::getElementPath(const QTreeWidgetItem* item) const
+ std::optional<aron::Path> InstanceView::getElementPath(const QTreeWidgetItem* item) const
{
QStringList qpath = item->data(int(Columns::KEY), Qt::UserRole).toStringList();
- aron::Path path = deserializePath(qpath);
- return path;
+ if (qpath.empty())
+ {
+ return std::nullopt;
+ }
+ else
+ {
+ aron::Path path = deserializePath(qpath);
+ return path;
+ }
}
@@ -256,16 +268,38 @@ namespace armarx::armem::gui::instance
aron::type::Descriptor type = static_cast<aron::type::Descriptor>(item->data(int(Columns::TYPE), Qt::UserRole).toInt());
switch (type)
{
+ case aron::type::Descriptor::eImage:
case aron::type::Descriptor::eIVTCByteImage:
{
- const aron::Path path = getElementPath(item);
-
- QAction* viewAction = new QAction("Show image");
- menu.addAction(viewAction);
- connect(viewAction, &QAction::triggered, [this, path]()
+ if (const std::optional<aron::Path> path = getElementPath(item))
{
- this->showImageView(path);
- });
+ QAction* viewAction = new QAction("Show image");
+ menu.addAction(viewAction);
+ connect(viewAction, &QAction::triggered, [this, path]()
+ {
+ this->showImageView(path.value());
+ });
+
+ try
+ {
+ aron::datanavigator::NavigatorPtr element = currentInstance->data()->navigateAbsolute(path.value());
+ if (auto imageData = aron::datanavigator::NDArrayNavigator::DynamicCast(element))
+ {
+ const std::vector<int> shape = imageData->getDimensions();
+ if (std::find(shape.begin(), shape.end(), 0) != shape.end())
+ {
+ viewAction->setText(viewAction->text() + " (image is empty)");
+ viewAction->setEnabled(false);
+ }
+ }
+ }
+ catch (const aron::error::AronException&)
+ {
+ }
+ catch (const armarx::LocalException&)
+ {
+ }
+ }
}
break;
default:
@@ -276,17 +310,18 @@ namespace armarx::armem::gui::instance
const std::string typeName = item->text(int(Columns::TYPE)).toStdString();
if (typeName == instance::sanitizedMemoryIDTypeName)
{
- const aron::Path path = getElementPath(item);
-
- if (std::optional<MemoryID> id = getElementMemoryID(path))
+ if (const std::optional<aron::Path> path = getElementPath(item))
{
- if (QAction* action = makeActionCopyMemoryID(id.value()))
+ if (std::optional<MemoryID> id = getElementMemoryID(path.value()))
{
- menu.addAction(action);
- }
- if (QAction* action = makeActionResolveMemoryID(id.value()))
- {
- menu.addAction(action);
+ if (QAction* action = makeActionCopyMemoryID(id.value()))
+ {
+ menu.addAction(action);
+ }
+ if (QAction* action = makeActionResolveMemoryID(id.value()))
+ {
+ menu.addAction(action);
+ }
}
}
}
@@ -383,11 +418,11 @@ namespace armarx::armem::gui::instance
void InstanceView::showImageView(const aron::Path& elementPath)
{
- if (!currentInstance)
+ if (not currentInstance)
{
return;
}
- if (!imageView)
+ if (not imageView)
{
WidgetsWithToolbar* toolbar = new WidgetsWithToolbar();
@@ -413,15 +448,106 @@ namespace armarx::armem::gui::instance
}
+
+ QImage InstanceView::ImageView::convertDepth32ToRGB32(
+ const aron::datanavigator::NDArrayNavigator& aron)
+ {
+ const std::vector<int> shape = aron.getDimensions();
+ ARMARX_CHECK_EQUAL(shape.size(), 3);
+ ARMARX_CHECK_EQUAL(shape.at(2), 4) << "Expected Depth32 image to have 4 bytes per pixel.";
+
+ const int rows = shape.at(0);
+ const int cols = shape.at(1);
+
+ // Rendering seems to be optimized for RGB32
+ // rows go along 0 = height, cols go along 1 = width
+ QImage image(cols, rows, QImage::Format::Format_RGB32);
+ const float* data = reinterpret_cast<float*>(aron.getData());
+
+ auto updateLimits = [](float value, Limits& limits)
+ {
+ if (value > 0) // Exclude 0 from normalization (it may be only background)
+ {
+ limits.min = std::min(limits.min, value);
+ }
+ limits.max = std::max(limits.max, value);
+ };
+
+ // Find data range and adapt cmap.
+ Limits limits;
+ if (limitsHistory.empty())
+ {
+ const float* sourceRow = data;
+ for (int row = 0; row < rows; ++row)
+ {
+ for (int col = 0; col < cols; ++col)
+ {
+ float value = sourceRow[col];
+ updateLimits(value, limits);
+ }
+ sourceRow += cols;
+ }
+ cmap.set_vlimits(limits.min, limits.max);
+ }
+ // Only do it at the beginning and stop after enough samples were collected.
+ else if (limitsHistory.size() < limitsHistoryMaxSize)
+ {
+ simox::math::SoftMinMax softMin(0.25, limitsHistory.size());
+ simox::math::SoftMinMax softMax(0.25, limitsHistory.size());
+
+ for (auto& l : limitsHistory)
+ {
+ softMin.add(l.min);
+ softMax.add(l.max);
+ }
+
+ cmap.set_vlimits(softMin.getSoftMin(), softMax.getSoftMax());
+ }
+
+ // Update image
+ {
+ const float* sourceRow = data;
+
+ const int bytesPerLine = image.bytesPerLine();
+ uchar* targetRow = image.bits();
+
+ for (int row = 0; row < rows; ++row)
+ {
+ for (int col = 0; col < cols; ++col)
+ {
+ float value = sourceRow[col];
+ simox::Color color = value <= 0
+ ? simox::Color::white()
+ : cmap(value);
+ targetRow[col*4 + 0] = color.b;
+ targetRow[col*4 + 1] = color.g;
+ targetRow[col*4 + 2] = color.r;
+ targetRow[col*4 + 3] = color.a;
+
+ updateLimits(value, limits);
+ }
+ sourceRow += cols;
+ targetRow += bytesPerLine;
+ }
+ }
+ if (limitsHistory.size() < limitsHistoryMaxSize)
+ {
+ limitsHistory.push_back(limits);
+ }
+
+ return image;
+ }
+
+
void InstanceView::updateImageView(const aron::datanavigator::DictNavigatorPtr& data)
{
using aron::datanavigator::NDArrayNavigator;
- if (!imageView)
+ if (not imageView)
{
return;
}
- if (!data)
+ if (not data)
{
removeImageView();
return;
@@ -448,48 +574,92 @@ namespace armarx::armem::gui::instance
}
NDArrayNavigator::PointerType imageData = NDArrayNavigator::DynamicCast(element);
- if (!imageData)
+ if (not imageData)
{
showErrorMessage("Expected NDArrayNavigator, but got: " + simox::meta::get_type_name(element));
return;
}
- std::vector<int> shape = imageData->getDimensions();
+ const std::vector<int> shape = imageData->getDimensions();
if (shape.size() != 3)
{
showErrorMessage("Expected array shape with 3 dimensions, but got: "
+ NDArrayNavigator::DimensionsToString(shape));
return;
}
+ const int rows = shape.at(0);
+ const int cols = shape.at(1);
- QImage::Format format = QImage::Format::Format_Invalid;
- switch (shape.at(2))
+ using aron::typenavigator::ImagePixelType;
+ std::optional<ImagePixelType> pixelType;
+ try
{
+ pixelType = aron::typenavigator::ImageNavigator::pixelTypeFromName(imageData->getType());
+ }
+ catch (const aron::error::AronException&)
+ {
+ }
+
+ bool clearLimitsHistory = true;
+ std::optional<QImage> image;
+ if (pixelType)
+ {
+ switch (pixelType.value())
+ {
+ case ImagePixelType::Rgb24:
+ ARMARX_CHECK_EQUAL(shape.at(2), 3) << "Expected Rgb24 image to have 3 bytes per pixel.";
+ image = QImage(imageData->getData(), cols, rows, QImage::Format::Format_RGB888);
+ break;
+
+ case ImagePixelType::Depth32:
+ image = imageView->convertDepth32ToRGB32(*imageData);
+ clearLimitsHistory = false;
+ break;
+ }
+ }
+ else
+ {
+ QImage::Format format = QImage::Format_Invalid;
+ switch (shape.at(2))
+ {
case 1:
format = QImage::Format::Format_Grayscale8;
break;
+
case 3:
format = QImage::Format::Format_RGB888;
break;
+
default:
showErrorMessage("Expected 1 or 3 elements in last dimension, but got shape: "
+ NDArrayNavigator::DimensionsToString(shape));
return;
+ }
+ image = QImage(imageData->getData(), cols, rows, format);
}
+ ARMARX_CHECK(image.has_value());
+
std::stringstream title;
- title << "Image element '" << imageView->elementPath.toString() << "'"; // of entity instance " << currentInstance->id();
+ title << "Image element '" << imageView->elementPath.toString() << "'"; // of entity instance " << currentInstance->id();
imageView->setTitle(QString::fromStdString(title.str()));
- imageView->view->setImage(QImage(imageData->getData(), shape.at(0), shape.at(1), format));
+ imageView->view->setImage(image.value());
+
+ if (clearLimitsHistory)
+ {
+ imageView->limitsHistory.clear();
+ }
}
- InstanceView::ImageView::ImageView()
+ InstanceView::ImageView::ImageView() :
+ cmap(simox::color::cmaps::plasma().reversed()),
+ limitsHistoryMaxSize(32)
{
setLayout(new QHBoxLayout());
int margin = 2;
layout()->setContentsMargins(margin, margin, margin, margin);
- if (false)
+ if (/* DISABLES CODE */ (false))
{
QFont font = this->font();
font.setPointSizeF(font.pointSize() * 0.75);
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
index 26f9d3be5fccfdce4fdb17a9ddbe430c2f334433..ab434ba10c6084d4440f7ae2c08758f05f038ed4 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
@@ -1,13 +1,19 @@
#pragma once
#include <optional>
+#include <deque>
#include <QWidget>
#include <QGroupBox>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <SimoxUtility/color/ColorMap.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h>
+#include <RobotAPI/libraries/aron/core/Path.h>
+
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
class QGroupBox;
@@ -24,7 +30,7 @@ namespace armarx::armem::gui::instance
class WidgetsWithToolbar;
- class InstanceView : public QWidget, public Logging
+ class InstanceView : public QWidget, public armarx::Logging
{
Q_OBJECT
using This = InstanceView;
@@ -68,7 +74,7 @@ namespace armarx::armem::gui::instance
void showErrorMessage(const std::string& message);
- aron::Path getElementPath(const QTreeWidgetItem* item) const;
+ std::optional<aron::Path> getElementPath(const QTreeWidgetItem* item) const;
std::optional<MemoryID> getElementMemoryID(const aron::Path& elementPath);
QAction* makeActionResolveMemoryID(const MemoryID& id);
@@ -101,10 +107,26 @@ namespace armarx::armem::gui::instance
public:
ImageView();
+ QImage convertDepth32ToRGB32(const aron::datanavigator::NDArrayNavigator& aron);
+
instance::ImageView* view;
aron::Path elementPath;
WidgetsWithToolbar* toolbar;
+
+
+ struct Limits
+ {
+ float min = std::numeric_limits<float>::max();
+ float max = -std::numeric_limits<float>::max();
+ };
+
+ /// Color map to visualize depth images.
+ simox::ColorMap cmap;
+ /// History over first n extremal depth values used to calibrate the colormap.
+ std::deque<Limits> limitsHistory;
+ /// In this context, n.
+ const size_t limitsHistoryMaxSize;
};
ImageView* imageView = nullptr;
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/display_visitors/DataDisplayVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.cpp
index f41f5d4cb447e9571d5145f26af5cbff6c9db2cf..af2192c2a36de2c89fcda1caa4f556cd5781315f 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.cpp
@@ -68,7 +68,8 @@ namespace armarx::aron
bool DataDisplayVisitor::visit(NDArrayDataNavigator& n)
{
- value << "shape " << aron::datanavigator::NDArrayNavigator::DimensionsToString(n.getDimensions());
+ value << "shape " << aron::datanavigator::NDArrayNavigator::DimensionsToString(n.getDimensions())
+ << ", type '" << n.getType() << "'";
return false;
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
index 827d14a599af43e6067fd1b3fa18bb136dfcbed8..16c54007a40ad0f04d85c3bc4e9ecebcb4e74416 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
@@ -3,6 +3,9 @@
#include <iomanip> // std::setprecision
#include <SimoxUtility/algorithm/string.h>
+#include <SimoxUtility/math/pose/pose.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/aron/core/Exception.h>
#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
@@ -22,6 +25,14 @@ namespace armarx::aron
return v.value.str();
}
+
+ TypedDataDisplayVisitor::TypedDataDisplayVisitor() :
+ coeffSep(" "),
+ eigenIof(Eigen::StreamPrecision, 0, coeffSep, "\n", "", "", "", "")
+ {
+ }
+
+
bool TypedDataDisplayVisitor::visitEnter(DictTypeNavigator&, DictDataNavigator& data)
{
value << DataDisplayVisitor::getValue(data);
@@ -54,12 +65,14 @@ namespace armarx::aron
bool TypedDataDisplayVisitor::visit(DoubleTypeNavigator&, DoubleDataNavigator& data)
{
+ setStreamPrecision();
value << DataDisplayVisitor::getValue(data);
return false;
}
bool TypedDataDisplayVisitor::visit(FloatTypeNavigator&, FloatDataNavigator& data)
{
+ setStreamPrecision();
value << DataDisplayVisitor::getValue(data);
return false;
}
@@ -85,19 +98,52 @@ namespace armarx::aron
bool TypedDataDisplayVisitor::visit(TimeTypeNavigator&, LongDataNavigator& data)
{
armem::Time time = armem::Time::microSeconds(data.getValue());
- armem::toDateTimeMilliSeconds(time);
value << armem::toDateTimeMilliSeconds(time);
return false;
}
- bool TypedDataDisplayVisitor::visit(EigenMatrixTypeNavigator&, NDArrayDataNavigator& data)
+
+ template <typename ScalarT>
+ void TypedDataDisplayVisitor::processEigenMatrix(EigenMatrixTypeNavigator& type, NDArrayDataNavigator& data)
{
- value << DataDisplayVisitor::getValue(data);
+ Eigen::Map<Eigen::Matrix<ScalarT, Eigen::Dynamic, Eigen::Dynamic>> m(reinterpret_cast<ScalarT*>(data.getData()), type.getRows(), type.getCols());
+ value << m.format(eigenIof);
+ }
+
+ bool TypedDataDisplayVisitor::visit(EigenMatrixTypeNavigator& t, NDArrayDataNavigator& data)
+ {
+ if (std::max(t.getRows(), t.getCols()) > 10)
+ {
+ // Just show the shape.
+ value << DataDisplayVisitor::getValue(data);
+ }
+ else if (data.getType() == "float")
+ {
+ setStreamPrecision();
+ processEigenMatrix<float>(t, data);
+ }
+ else if (data.getType() == "double")
+ {
+ setStreamPrecision();
+ processEigenMatrix<double>(t, data);
+ }
+ else
+ {
+ value << DataDisplayVisitor::getValue(data);
+ }
return false;
}
bool TypedDataDisplayVisitor::visit(EigenQuaternionTypeNavigator&, NDArrayDataNavigator& data)
{
+ processEigenQuaternion(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(ImageNavigator&, NDArrayDataNavigator& data)
+ {
+ // aron::typenavigator::ImagePixelType pixelType = ImageNavigator::pixelTypeFromName(data.getType());
+ //value << DataDisplayVisitor::getValue(data) << " pixel type: " << data.getType()";
value << DataDisplayVisitor::getValue(data);
return false;
}
@@ -122,26 +168,62 @@ namespace armarx::aron
bool TypedDataDisplayVisitor::visit(PoseTypeNavigator&, NDArrayDataNavigator& data)
{
+ const Eigen::IOFormat eigenIof(Eigen::StreamPrecision, 0, " ", "\n", "( ", " )", "", "");
+ const std::string cdot = "\u00B7"; // center dot: https://unicode-table.com/de/00B7/
+
+ auto getLines = [&](auto&& mat)
+ {
+ std::stringstream ss;
+ setStreamPrecision(ss);
+ ss << mat.format(eigenIof);
+ std::vector<std::string> lines = simox::alg::split(ss.str(), "\n");
+ ARMARX_CHECK_EQUAL(lines.size(), 3);
+ return lines;
+ };
+
const Eigen::Matrix4f pose = aron::converter::AronEigenConverter::ConvertToMatrix4f(data);
- value << std::setprecision(2) << std::fixed;
- value << pose.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, coeffSep, "\n", "", "", "", ""));
+ const std::vector<std::string> r = getLines(simox::math::orientation(pose));
+ const std::vector<std::string> t = getLines(simox::math::position(pose));
+
+ std::vector<std::string> lines;
+ lines.push_back(r.at(0) + " \t" + t.at(0));
+ lines.push_back(r.at(1) + " " + cdot + " x + \t" + t.at(1));
+ lines.push_back(r.at(2) + " \t" + t.at(2));
+
+ value << simox::alg::join(lines, "\n");
return false;
}
+
bool TypedDataDisplayVisitor::visit(PositionTypeNavigator&, NDArrayDataNavigator& data)
{
const Eigen::Vector3f pos = aron::converter::AronEigenConverter::ConvertToVector3f(data);
- value << std::setprecision(2) << std::fixed;
- value << pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, "", coeffSep, "", "", "", ""));
+ setStreamPrecision();
+ value << pos.format(eigenIof);
return false;
}
bool TypedDataDisplayVisitor::visit(OrientationTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ processEigenQuaternion(data);
+ return false;
+ }
+
+ void TypedDataDisplayVisitor::processEigenQuaternion(visitor::TypedDataVisitor::NDArrayDataNavigator& data)
{
const Eigen::Quaternionf quat = aron::converter::AronEigenConverter::ConvertToQuaternionf(data);
- value << std::setprecision(2) << std::fixed;
+ setStreamPrecision();
value << quat.w() << coeffSep << "|" << coeffSep << quat.x() << coeffSep << quat.y() << coeffSep << quat.z();
- return false;
+ }
+
+ void TypedDataDisplayVisitor::setStreamPrecision()
+ {
+ setStreamPrecision(value);
+ }
+
+ void TypedDataDisplayVisitor::setStreamPrecision(std::ostream& os)
+ {
+ os << std::setprecision(2) << std::fixed;
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h
index 3923b7d0d74a1ee9c4b1ea7a5664ad3f83396051..4bf2ec5d8cd149eb40ad1f9a0d3a655424183e72 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h
@@ -3,6 +3,8 @@
#include <sstream>
#include <string>
+#include <Eigen/Core>
+
#include <RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h>
@@ -18,6 +20,9 @@ namespace armarx::aron
public:
+ TypedDataDisplayVisitor();
+
+
std::stringstream value;
@@ -39,6 +44,7 @@ namespace armarx::aron
bool visit(EigenMatrixTypeNavigator&, NDArrayDataNavigator& data) override;
bool visit(EigenQuaternionTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(ImageNavigator&, NDArrayDataNavigator& data) override;
bool visit(IVTCByteImageTypeNavigator&, NDArrayDataNavigator& data) override;
bool visit(OpenCVMatTypeNavigator&, NDArrayDataNavigator& data) override;
bool visit(PCLPointCloudTypeNavigator&, NDArrayDataNavigator& data) override;
@@ -49,7 +55,18 @@ namespace armarx::aron
protected:
- std::string coeffSep = " ";
+ std::string coeffSep;
+ Eigen::IOFormat eigenIof;
+
+
+ private:
+
+ template <typename ScalarT>
+ void processEigenMatrix(EigenMatrixTypeNavigator&, NDArrayDataNavigator& data);
+ void processEigenQuaternion(NDArrayDataNavigator& data);
+
+ void setStreamPrecision();
+ void setStreamPrecision(std::ostream& os);
};
diff --git a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
index 0180b51961c795ca4a45fd748078cb5cda1ca663..e8ece36b766dec34b3bce7325b7a62e78513bbc2 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
@@ -10,6 +10,22 @@
const std::string armarx::armem::gui::instance::rawMemoryIDTypeName = armarx::armem::arondto::MemoryID::toAronType()->getName();
const std::string armarx::armem::gui::instance::sanitizedMemoryIDTypeName = "MemoryID";
+
+namespace
+{
+ std::string remove_wrap(const std::string& string, const std::string& prefix, const std::string& suffix)
+ {
+ if (simox::alg::starts_with(string, prefix) && simox::alg::ends_with(string, suffix))
+ {
+ return string.substr(prefix.size(), string.size() - prefix.size() - suffix.size());
+ }
+ else
+ {
+ return string;
+ }
+ }
+}
+
std::string armarx::armem::gui::instance::sanitizeTypeName(const std::string& typeName)
{
if (typeName == rawMemoryIDTypeName)
@@ -19,16 +35,26 @@ std::string armarx::armem::gui::instance::sanitizeTypeName(const std::string& ty
namespace s = simox::alg;
std::string n = typeName;
- n = s::replace_all(n, "Aron", "");
- n = s::replace_all(n, "Type", "");
- n = s::replace_all(n, "type::", "");
- if (s::starts_with(n, "Object<") && s::ends_with(n, ">"))
+ n = s::replace_all(n, "type::Aron", "");
+ n = s::replace_all(n, "data::Aron", "");
+
+ if (false)
+ {
+ const std::vector<std::string> containers { "Dict", "List", "Object", "Tuple", "Pair", "NDArray" };
+ for (const std::string& s : containers)
+ {
+ n = s::replace_all(n, s + "Type", s);
+ }
+ }
+ else
{
- std::string begin = "Object<";
- std::string end = ">";
- n = n.substr(begin.size(), n.size() - begin.size() - end.size());
+ n = s::replace_all(n, "Type", "");
}
+
+ n = simox::alg::remove_prefix(n, "Aron");
+ n = remove_wrap(n, "Object<", ">");
+
if (true)
{
const std::string del = "::";
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..bdf6aed5bb00495bf74d0125a60b4e108e8bcd64 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,27 +36,29 @@ namespace armarx::armem::gui::instance
return true;
});
- builder.updateTree(parent, getIndex(children.size()));
+ builder.updateTreeWithContainer(parent, getIndex(children.size()));
}
void DataTreeBuilder::update(QTreeWidgetItem* item, const std::string& key, const aron::datanavigator::NavigatorPtr& data)
{
- if (!data)
+ if (data)
{
- this->setRowTexts(item, key, "null");
- return;
+ this->setRowTexts(item, key, *data);
+
+ if (auto cast = aron::datanavigator::DictNavigator::DynamicCast(data))
+ {
+ updateTree(item, cast);
+ }
+ else if (auto cast = aron::datanavigator::ListNavigator::DynamicCast(data))
+ {
+ updateTree(item, cast);
+ }
}
-
- this->setRowTexts(item, key, *data);
-
- if (auto cast = aron::datanavigator::DictNavigator::DynamicCast(data))
- {
- updateTree(item, cast);
- }
- else if (auto cast = aron::datanavigator::ListNavigator::DynamicCast(data))
+ else
{
- updateTree(item, cast);
+ // Optional data?
+ this->setRowTexts(item, key, "(none)");
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.cpp
index f837fe2fa6451cf8edd82793f2c6ec773560ce2c..8a73afd2069896bbea2577b8f6d73f7df95b1ab5 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.cpp
@@ -3,14 +3,19 @@
#include <QTreeWidgetItem>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+// #include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
+#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
#include <RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h>
namespace armarx::armem::gui::instance
{
+ const int keyIndexRole = Qt::UserRole + 10;
+
+
DataTreeBuilderBase::DataTreeBuilderBase()
{
}
@@ -34,7 +39,7 @@ namespace armarx::armem::gui::instance
QTreeWidgetItem* DataTreeBuilderBase::makeItem(size_t key) const
{
QTreeWidgetItem* item = new QTreeWidgetItem();
- item->setData(0, Qt::UserRole, static_cast<int>(key));
+ item->setData(0, keyIndexRole, static_cast<int>(key));
return item;
}
@@ -50,7 +55,7 @@ namespace armarx::armem::gui::instance
QTreeWidgetItem* item, const std::string& key, aron::datanavigator::Navigator& data)
{
const std::string value = armarx::aron::DataDisplayVisitor::getValue(data);
- setRowTexts(item, key, value, data.getName());
+ setRowTexts(item, key, value, sanitizeTypeName(data.getName()));
}
DataTreeBuilderBase::DictBuilder DataTreeBuilderBase::getDictBuilder() const
@@ -73,7 +78,11 @@ namespace armarx::armem::gui::instance
ListBuilder builder;
builder.setCompareFn([](size_t key, QTreeWidgetItem * item)
{
- return armarx::detail::compare(static_cast<int>(key), item->data(0, Qt::UserRole).toInt());
+ QVariant itemKey = item->data(0, keyIndexRole);
+ ARMARX_CHECK_EQUAL(itemKey.type(), QVariant::Type::Int);
+ // << VAROUT(key) << " | " << VAROUT(item->text(0).toStdString()) << itemKey.typeName();
+
+ return armarx::detail::compare(static_cast<int>(key), itemKey.toInt());
});
builder.setMakeItemFn([this](size_t key)
{
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..d8ce4516b43c7cbccfaff7bfbd5f362d6dbfef0a 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;
@@ -42,6 +42,7 @@ namespace armarx::armem::gui::instance
void setRowTexts(QTreeWidgetItem* item, const std::string& key, const std::string& value, const std::string& typeName = "") const;
void setRowTexts(QTreeWidgetItem* item, const std::string& key, aron::datanavigator::Navigator& data);
+
public:
int columnKey = 0;
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..bda945607d2dc2e11b40957941b370613825aad3 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
@@ -4,6 +4,7 @@
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
@@ -35,12 +36,12 @@ namespace armarx::armem::gui::instance
auto childData = data.getElement(key);
if (childData)
{
- this->update(item, key, *childType, *childData);
+ this->update(item, key, *childType, childData.get());
}
return true;
});
- builder.updateTree(parent, data.getAllKeys());
+ builder.updateTreeWithContainer(parent, data.getAllKeys());
}
}
@@ -69,14 +70,14 @@ namespace armarx::armem::gui::instance
auto childType = type.getMemberType(key);
auto childData = data.getElement(key);
- if (childType && childData)
+ if (childType)
{
- this->update(item, key, *childType, *childData);
+ this->update(item, key, *childType, childData.get());
}
return true;
});
- builder.updateTree(parent, type.getAllKeys());
+ builder.updateTreeWithContainer(parent, type.getAllKeys());
}
@@ -92,15 +93,14 @@ namespace armarx::armem::gui::instance
ListBuilder builder = getListBuilder();
builder.setUpdateItemFn([this, &children, &childType](size_t key, QTreeWidgetItem * item)
{
- auto childData = children.at(key);
- if (childData)
+ if (auto childData = children.at(key))
{
- this->update(item, std::to_string(key), *childType, *childData);
+ this->update(item, std::to_string(key), *childType, childData.get());
}
return true;
});
- builder.updateTree(parent, getIndex(children.size()));
+ builder.updateTreeWithContainer(parent, getIndex(children.size()));
}
}
@@ -119,14 +119,14 @@ namespace armarx::armem::gui::instance
auto childType = i == 0 ? childTypes.first : childTypes.second;
auto childData = data.getElement(static_cast<unsigned int>(i));
- if (childType && childData)
+ if (childType)
{
- this->update(item, std::to_string(i), *childType, *childData);
+ this->update(item, std::to_string(i), *childType, childData.get());
}
return true;
});
- builder.updateTree(parent, getIndex(data.childrenSize()));
+ builder.updateTreeWithContainer(parent, getIndex(data.childrenSize()));
}
@@ -143,14 +143,14 @@ namespace armarx::armem::gui::instance
auto childType = childTypes.at(i);
auto childData = data.getElement(static_cast<unsigned int>(i));
- if (childType && childData)
+ if (childType)
{
- this->update(item, std::to_string(i), *childType, *childData);
+ this->update(item, std::to_string(i), *childType, childData.get());
}
return true;
});
- builder.updateTree(parent, getIndex(type.getAcceptedTypes().size()));
+ builder.updateTreeWithContainer(parent, getIndex(type.getAcceptedTypes().size()));
}
@@ -158,21 +158,30 @@ namespace armarx::armem::gui::instance
QTreeWidgetItem* item,
const std::string& key,
aron::typenavigator::Navigator& type,
- aron::datanavigator::Navigator& data)
+ aron::datanavigator::Navigator* data)
{
using namespace aron;
- const std::string value = aron::TypedDataDisplayVisitor::getValue(type, data);
- const std::string typeName = instance::sanitizeTypeName(type.getName());
+ const std::string value = data ? aron::TypedDataDisplayVisitor::getValue(type, *data) : "(none)";
+ std::string typeName = instance::sanitizeTypeName(type.getName());
+ switch (type.getMaybe())
+ {
+ case aron::type::Maybe::eOptional:
+ typeName = "Optional[" + typeName + "]";
+ break;
+ default:
+ break;
+ }
+
setRowTexts(item, key, value, typeName);
- item->setData(columnKey, Qt::UserRole, instance::serializePath(data.getPath()));
+ item->setData(columnKey, Qt::UserRole, data ? instance::serializePath(data->getPath()) : QStringList());
item->setData(columnType, Qt::UserRole, static_cast<int>(type.getDescriptor()));
if (typeName == sanitizedMemoryIDTypeName)
{
MemoryIDTreeWidgetItem* memoryIDItem = dynamic_cast<MemoryIDTreeWidgetItem*>(item);
- datanavigator::DictNavigator* dictData = dynamic_cast<datanavigator::DictNavigator*>(&data);
+ datanavigator::DictNavigator* dictData = dynamic_cast<datanavigator::DictNavigator*>(data);
if (memoryIDItem && dictData)
{
arondto::MemoryID dto;
@@ -185,25 +194,28 @@ namespace armarx::armem::gui::instance
}
}
- if (auto t = dynamic_cast<typenavigator::ObjectNavigator*>(&type))
- {
- _updateTree<datanavigator::DictNavigator>(item, *t, data);
- }
- else if (auto t = dynamic_cast<typenavigator::DictNavigator*>(&type))
- {
- _updateTree<datanavigator::DictNavigator>(item, *t, data);
- }
- else if (auto t = dynamic_cast<typenavigator::ListNavigator*>(&type))
- {
- _updateTree<datanavigator::ListNavigator>(item, *t, data);
- }
- else if (auto t = dynamic_cast<typenavigator::PairNavigator*>(&type))
- {
- _updateTree<datanavigator::ListNavigator>(item, *t, data);
- }
- else if (auto t = dynamic_cast<typenavigator::TupleNavigator*>(&type))
+ if (data)
{
- _updateTree<datanavigator::ListNavigator>(item, *t, data);
+ if (auto t = dynamic_cast<typenavigator::ObjectNavigator*>(&type))
+ {
+ _updateTree<datanavigator::DictNavigator>(item, *t, *data);
+ }
+ else if (auto t = dynamic_cast<typenavigator::DictNavigator*>(&type))
+ {
+ _updateTree<datanavigator::DictNavigator>(item, *t, *data);
+ }
+ else if (auto t = dynamic_cast<typenavigator::ListNavigator*>(&type))
+ {
+ _updateTree<datanavigator::ListNavigator>(item, *t, *data);
+ }
+ else if (auto t = dynamic_cast<typenavigator::PairNavigator*>(&type))
+ {
+ _updateTree<datanavigator::ListNavigator>(item, *t, *data);
+ }
+ else if (auto t = dynamic_cast<typenavigator::TupleNavigator*>(&type))
+ {
+ _updateTree<datanavigator::ListNavigator>(item, *t, *data);
+ }
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h
index 57580a1f6d3a84ca639131e0da375a6129650e0a..0dead6736a93073854f8593d423ba03e60e8e0cb 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h
@@ -52,7 +52,7 @@ namespace armarx::armem::gui::instance
void update(QTreeWidgetItem* item,
const std::string& key,
aron::typenavigator::Navigator& type,
- aron::datanavigator::Navigator& data);
+ aron::datanavigator::Navigator* data);
template <class DataT, class TypeT>
void _updateTree(QTreeWidgetItem* item, TypeT& type, aron::datanavigator::Navigator& data);
diff --git a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
index 1d4a2b908eba38fbb4be2d028030598c220aae44..1ff5b509001d5cdb4d095442b4614878396b4157 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
@@ -4,8 +4,6 @@
#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
#include <SimoxUtility/algorithm/string.h>
#include <QHeaderView>
@@ -51,12 +49,12 @@ namespace armarx::armem::gui::memory
void TreeWidget::initBuilders()
{
- workingmemoryBuilder.setExpand(true);
- workingmemoryBuilder.setMakeItemFn([this](const std::string & name, const wm::Memory * memory)
+ memoryBuilder.setExpand(true);
+ memoryBuilder.setMakeItemFn([this](const std::string & name, const wm::Memory * memory)
{
return makeItem(name, *memory);
});
- workingmemoryBuilder.setUpdateItemFn([this](const std::string&, const wm::Memory * memory, QTreeWidgetItem * memoryItem)
+ memoryBuilder.setUpdateItemFn([this](const std::string&, const wm::Memory * memory, QTreeWidgetItem * memoryItem)
{
updateContainerItem(*memory, memoryItem);
if (memoryItem)
@@ -66,25 +64,31 @@ 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)
+ coreSegmentBuilder.setExpand(true);
+ coreSegmentBuilder.setNameFn(nameFn, int(Columns::KEY));
+ coreSegmentBuilder.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)
+ coreSegmentBuilder.setUpdateItemFn([this](const wm::CoreSegment & coreSeg, QTreeWidgetItem * coreSegItem)
{
updateContainerItem(coreSeg, coreSegItem);
updateChildren(coreSeg, coreSegItem);
return true;
});
- workingmemoryProvSegmentBuilder.setExpand(true);
- workingmemoryProvSegmentBuilder.setMakeItemFn([this](const std::string & name, const wm::ProviderSegment & provSeg)
+ provSegmentBuilder.setExpand(true);
+ provSegmentBuilder.setNameFn(nameFn, int(Columns::KEY));
+ provSegmentBuilder.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)
+ provSegmentBuilder.setUpdateItemFn([this](const wm::ProviderSegment & provSeg, QTreeWidgetItem * provSegItem)
{
updateContainerItem(provSeg, provSegItem);
updateChildren(provSeg, provSegItem);
@@ -92,46 +96,46 @@ namespace armarx::armem::gui::memory
});
// entityBuilder.setExpand(true);
- workingmemoryEntityBuilder.setMakeItemFn([this](const std::string & name, const wm::Entity & entity)
+ entityBuilder.setNameFn(nameFn, int(Columns::KEY));
+ entityBuilder.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)
+ entityBuilder.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)
+ snapshotBuilder.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)
+ snapshotBuilder.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)
+ snapshotBuilder.setUpdateItemFn([this](const wm::EntitySnapshot & snapshot, QTreeWidgetItem * snapshotItem)
{
updateContainerItem(snapshot, snapshotItem);
updateChildren(snapshot, snapshotItem);
return true;
});
- workingmemoryInstanceBuilder.setMakeItemFn([this](const wm::EntityInstance & instance)
+ instanceBuilder.setMakeItemFn([this](const wm::EntityInstance & instance)
{
QTreeWidgetItem* item = makeItem("", instance);
return item;
});
- workingmemoryInstanceBuilder.setCompareFn([](const wm::EntityInstance & lhs, QTreeWidgetItem * rhsItem)
+ instanceBuilder.setCompareFn([](const wm::EntityInstance & lhs, QTreeWidgetItem * rhsItem)
{
return armarx::detail::compare(lhs.index(), rhsItem->text(0).toInt());
});
- workingmemoryInstanceBuilder.setUpdateItemFn([this](const wm::EntityInstance & instance, QTreeWidgetItem * instanceItem)
+ instanceBuilder.setUpdateItemFn([this](const wm::EntityInstance & instance, QTreeWidgetItem * instanceItem)
{
updateItemItem(instance, instanceItem);
updateChildren(instance, instanceItem);
@@ -183,27 +187,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 +217,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)
{
@@ -221,33 +235,33 @@ namespace armarx::armem::gui::memory
void TreeWidget::updateChildren(const std::map<std::string, const armem::wm::Memory*>& memories, QTreeWidget* tree)
{
- workingmemoryBuilder.updateTree(tree, memories);
+ memoryBuilder.updateTree(tree, memories);
}
void TreeWidget::updateChildren(const armem::wm::Memory& memory, QTreeWidgetItem* memoryItem)
{
- workingmemoryCoreSegmentBuilder.updateTree(memoryItem, memory.coreSegments());
+ coreSegmentBuilder.updateTreeWithIterator(memoryItem, makeIteratorFn(memory));
}
void TreeWidget::updateChildren(const armem::wm::CoreSegment& coreSeg, QTreeWidgetItem* coreSegItem)
{
- workingmemoryProvSegmentBuilder.updateTree(coreSegItem, coreSeg.providerSegments());
+ provSegmentBuilder.updateTreeWithIterator(coreSegItem, makeIteratorFn(coreSeg));
}
void TreeWidget::updateChildren(const armem::wm::ProviderSegment& provSeg, QTreeWidgetItem* provSegItem)
{
- workingmemoryEntityBuilder.updateTree(provSegItem, provSeg.entities());
+ entityBuilder.updateTreeWithIterator(provSegItem, makeIteratorFn(provSeg));
}
void TreeWidget::updateChildren(const armem::wm::Entity& entity, QTreeWidgetItem* entityItem)
{
- workingmemorySnapshotBuilder.updateTree(entityItem, entity.history());
+ snapshotBuilder.updateTreeWithIterator(entityItem, makeIteratorFn(entity));
}
void TreeWidget::updateChildren(const armem::wm::EntitySnapshot& snapshot, QTreeWidgetItem* snapshotItem)
{
- workingmemoryInstanceBuilder.updateTree(snapshotItem, snapshot.instances());
+ instanceBuilder.updateTreeWithIterator(snapshotItem, makeIteratorFn(snapshot));
}
void TreeWidget::updateChildren(const armem::wm::EntityInstance& data, QTreeWidgetItem* dataItem)
@@ -255,19 +269,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;
}
@@ -277,16 +297,16 @@ namespace armarx::armem::gui::memory
(void) level, (void) item;
}
- template <class... T>
- void TreeWidget::updateContainerItem(
- const base::detail::MemoryContainerBase<T...>& container, QTreeWidgetItem* item)
+ template <class ContainerT>
+ void TreeWidget::updateContainerItem(const ContainerT& container, QTreeWidgetItem* item)
{
updateItemItem(container, item);
item->setText(int(Columns::SIZE), QString::number(container.size()));
- if constexpr(std::is_base_of_v<base::detail::AronTyped, base::detail::MemoryContainerBase<T...>>)
+ // Does not work
+ if constexpr(std::is_base_of_v<base::detail::AronTyped, ContainerT>)
{
- const base::detail::AronTyped& cast = dynamic_cast<const base::detail::AronTyped&>(container);
+ const base::detail::AronTyped& cast = static_cast<const base::detail::AronTyped&>(container);
std::string typeName;
if (cast.aronType())
{
diff --git a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h
index 2bcf0fa1b6135534407044a3ce341b86afeab442..75f56c9ace248e58daf3f7ee3ac69baa12b0edfe 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h
@@ -4,8 +4,7 @@
#include <QTreeWidget>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
-#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
@@ -67,21 +66,23 @@ 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>
- void updateContainerItem(const armem::base::detail::MemoryContainerBase<T...>& container, QTreeWidgetItem* item);
+ template <class ContainerT>
+ void updateContainerItem(const ContainerT& container, QTreeWidgetItem* item);
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;
+ MapTreeWidgetBuilder<std::string, const wm::Memory*> memoryBuilder;
+ TreeWidgetBuilder<wm::CoreSegment> coreSegmentBuilder;
+ TreeWidgetBuilder<wm::ProviderSegment> provSegmentBuilder;
+ TreeWidgetBuilder<wm::Entity> entityBuilder;
+ TreeWidgetBuilder<wm::EntitySnapshot> snapshotBuilder;
+ TreeWidgetBuilder<wm::EntityInstance> instanceBuilder;
std::optional<MemoryID> _selectedID;
/// While this is false, do not handle selection updates.
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
index dbb3d3fcabfa2ccf0034d1ba72027664bebe6ebc..f549a9e86046f40feb4087d28acd48726293a5c7 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
@@ -1,5 +1,8 @@
#include "QueryWidget.h"
+#include <QWidget>
+#include <QTabWidget>
+#include <QPushButton>
#include <QCheckBox>
#include <QHBoxLayout>
#include <QVBoxLayout>
@@ -12,25 +15,35 @@ namespace armarx::armem::gui
QueryWidget::QueryWidget()
{
- QVBoxLayout* layout = new QVBoxLayout();
- setLayout(layout);
+ QHBoxLayout* hlayout1 = new QHBoxLayout();
+ QHBoxLayout* hlayout2 = new QHBoxLayout();
+ QVBoxLayout* vlayout = new QVBoxLayout();
_dataCheckBox = new QCheckBox("Get Data");
_dataCheckBox->setChecked(true);
+ _storeInLTMButton = new QPushButton("Store query result in LTM");
+
_tabWidget = new QTabWidget();
_snapshotSelectorWidget = new SnapshotSelectorWidget();
_tabWidget->addTab(_snapshotSelectorWidget, QString("Snapshots"));
- layout->addWidget(_dataCheckBox);
- layout->addWidget(_tabWidget);
+ hlayout1->addWidget(_dataCheckBox);
+ hlayout1->addWidget(_storeInLTMButton);
+
+ hlayout2->addWidget(_tabWidget);
const int margin = 0;
- layout->setContentsMargins(margin, margin, margin, margin);
+ vlayout->setContentsMargins(margin, margin, margin, margin);
+
+ vlayout->addLayout(hlayout1);
+ vlayout->addLayout(hlayout2);
+ // Public connections.
+ connect(_storeInLTMButton, &QPushButton::pressed, this, &This::storeInLTM);
- // connect to queryChanged
+ setLayout(vlayout);
}
@@ -41,11 +54,6 @@ namespace armarx::armem::gui
: armem::DataMode::NoData;
}
- bool QueryWidget::alsoQueryLocalDisk() const
- {
- return _snapshotSelectorWidget->queryTargetContainsLocalDisk();
- }
-
armem::client::QueryInput QueryWidget::queryInput()
{
armem::client::query::Builder qb(dataMode());
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h
index d2c3fecd6198ad2ad5c3f37077eb07f4f365dea9..2bc7f223d5db52435b7571970bb4cd398aaeec49 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h
@@ -1,13 +1,13 @@
#pragma once
-#include <QWidget>
-#include <QTabWidget>
-
#include <RobotAPI/libraries/armem/core/DataMode.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include "SnapshotSelectorWidget.h"
+class QWidget;
+class QTabWidget;
+class QPushButton;
namespace armarx::armem::gui
{
@@ -15,19 +15,21 @@ namespace armarx::armem::gui
class QueryWidget : public QWidget
{
Q_OBJECT
+ using This = QueryWidget;
+
public:
QueryWidget();
armem::DataMode dataMode() const;
- bool alsoQueryLocalDisk() const;
armem::client::QueryInput queryInput();
public slots:
signals:
+ void storeInLTM();
// ToDo:
// void queryChanged(armem::query::data::Input query);
@@ -42,6 +44,8 @@ namespace armarx::armem::gui
private:
QCheckBox* _dataCheckBox;
+ QPushButton* _storeInLTMButton;
+
QTabWidget* _tabWidget;
SnapshotSelectorWidget* _snapshotSelectorWidget;
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
index e41bed15cecbc599c7c97b4266a04e0e9a70e47e..6b38efdddb30e67d1abfb3190d88508dff72258a 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
@@ -26,16 +26,15 @@ namespace armarx::armem::gui
{
targets.push_back(query::data::QueryTarget::LTM);
}
- if (_DiskMemoryQueryTargetCheckBox->isChecked())
- {
- targets.push_back(query::data::QueryTarget::Disk);
- }
return targets;
}
- bool SnapshotSelectorWidget::queryTargetContainsLocalDisk() const
+ void SnapshotSelectorWidget::queryTargetStateChanged()
{
- return _LocalDiskMemoryQueryTargetCheckBox->isChecked();
+ if (!_WMQueryTargetCheckBox->isChecked() && !_LTMQueryTargetCheckBox->isChecked())
+ {
+ _WMQueryTargetCheckBox->setChecked(true);
+ }
}
SnapshotSelectorWidget::SnapshotSelectorWidget()
@@ -59,14 +58,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)");
- _LocalDiskMemoryQueryTargetCheckBox = new QCheckBox("Local Disk");
+ _LTMQueryTargetCheckBox = new QCheckBox("LTM");
+
+ connect(_WMQueryTargetCheckBox, &QCheckBox::stateChanged, this, &This::queryTargetStateChanged);
+ connect(_LTMQueryTargetCheckBox, &QCheckBox::stateChanged, this, &This::queryTargetStateChanged);
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..e7ec233c896133beb1b675baf37e2d0e2f53489c 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h
@@ -37,7 +37,6 @@ namespace armarx::armem::gui
armem::DataMode dataMode() const;
client::query::SnapshotSelector selector();
query::data::QueryTargets queryTargets() const;
- bool queryTargetContainsLocalDisk() const;
public slots:
@@ -45,14 +44,13 @@ namespace armarx::armem::gui
signals:
void queryChanged();
-
private slots:
//void updateSelector();
void hideAllForms();
void showSelectedFormForQuery(QString selected);
-
+ void queryTargetStateChanged();
signals:
void queryOutdated();
@@ -68,8 +66,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..f7bb3e3e9a4cdc560838c0a3d25c0d7074a96503 100644
--- a/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp
+++ b/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp
@@ -30,27 +30,101 @@
#include <iostream>
#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
-using namespace armarx::armem::gui::instance;
+namespace dn = armarx::aron::datanavigator;
+namespace tn = armarx::aron::typenavigator;
-BOOST_AUTO_TEST_CASE(test_sanitizeTypeName)
+namespace ArMemGuiTest
{
- using namespace armarx::aron::typenavigator;
+ struct Fixture
{
- DictNavigator dict;
- dict.setAcceptedType(std::make_shared<FloatNavigator>());
- BOOST_CHECK_EQUAL(sanitizeTypeName(dict.getName()), "Dict<Float>");
+ };
+
+ void test_sanitize(const std::string& in, const std::string& expected)
+ {
+ const std::string out = armarx::armem::gui::instance::sanitizeTypeName(in);
+ BOOST_TEST_CONTEXT("in = '" << in << "'")
+ {
+ BOOST_CHECK_EQUAL(out, expected);
+ }
}
+}
+
+
+
+BOOST_FIXTURE_TEST_SUITE(ArMemGuiTest, ArMemGuiTest::Fixture)
+
+
+BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_int)
+{
+ test_sanitize(tn::IntNavigator().getName(), "Int");
+ test_sanitize(dn::IntNavigator().getName(), "Int");
+}
+
+BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_float)
+{
+ test_sanitize(tn::FloatNavigator().getName(), "Float");
+ test_sanitize(dn::FloatNavigator().getName(), "Float");
+}
+
+BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_dict)
+{
+ tn::DictNavigator dict;
+ dict.setAcceptedType(std::make_shared<tn::FloatNavigator>());
+ test_sanitize(dict.getName(), "Dict<Float>");
+
+ test_sanitize(dn::DictNavigator().getName(), "Dict");
+}
+
+BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_list)
+{
+ tn::ListNavigator list;
+ list.setAcceptedType(std::make_shared<tn::LongNavigator>());
+ test_sanitize(list.getName(), "List<Long>");
+
+ test_sanitize(dn::ListNavigator().getName(), "List");
+}
+
+BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_object)
+{
+ tn::ObjectNavigator obj;
+ obj.setObjectName("namespace::MyObjectName");
+ test_sanitize(obj.getName(), "MyObjectName (namespace)");
+}
+
+BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_tuple)
+{
+ tn::TupleNavigator type;
+ type.addAcceptedType(std::make_shared<tn::IntNavigator>());
+ type.addAcceptedType(std::make_shared<tn::FloatNavigator>());
+ test_sanitize(type.getName(), "Tuple<Int, Float>");
+}
+
+BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_pair)
+{
+ tn::PairNavigator type;
+ type.setFirstAcceptedType(std::make_shared<tn::IntNavigator>());
+ type.setSecondAcceptedType(std::make_shared<tn::FloatNavigator>());
+ test_sanitize(type.getName(), "Pair<Int, Float>");
+}
+
+BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_ndarry)
+{
{
- ListNavigator dict;
- dict.setAcceptedType(std::make_shared<LongNavigator>());
- BOOST_CHECK_EQUAL(sanitizeTypeName(dict.getName()), "Dict<Long>");
+ tn::NDArrayNavigator type;
+ type.setDimensions({ 3, 2, 1});
+ type.setTypename("float");
+ test_sanitize(type.getName(), "NDArray");
}
{
- ObjectNavigator dict;
- dict.setObjectName("namespace::MyObjectName");
- BOOST_CHECK_EQUAL(sanitizeTypeName(dict.getName()), "namespace::MyObjectName");
+ dn::NDArrayNavigator data;
+ data.setDimensions({ 3, 2, 1});
+ test_sanitize(data.getName(), "NDArray<3, 2, 1, >");
}
}
+
+
+BOOST_AUTO_TEST_SUITE_END()
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 8fab7c2c6e113e5ba5eb9b6ba8d250d8fb6fec4f..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,14 +1,21 @@
-// 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
{
@@ -72,7 +79,7 @@ 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
{
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 66e55f4107153532dd2df75a37fb369ba89817b1..f0dc4d726f83895876e23ab3bc52af36895df5b3 100644
--- a/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.h
+++ b/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.h
@@ -22,7 +22,7 @@ namespace armarx::armem::server::motions::mdb
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/client/articulated_object/ArticulatedObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
index c973cf24801967244589677f9b044be099f5c306..c4e703531a9508757a00e6c1dbf5429c7436b470 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>
@@ -43,12 +43,15 @@ namespace armarx::armem::articulated_object
return nullptr;
}
+ ARMARX_DEBUG << "Object " << name << " available";
+
auto obj =
VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(it->xml.serialize().path),
VirtualRobot::RobotIO::eStructure);
if (not obj)
{
+ ARMARX_WARNING << "Failed to load object: " << name;
return nullptr;
}
@@ -58,4 +61,4 @@ namespace armarx::armem::articulated_object
return obj;
}
-} // namespace armarx::armem::articulated_object
\ No newline at end of file
+} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
index f86f5f90d7787078572c9efa4b20cd5142100273..8caad31be27b227a8df1eb482f568ccf023b0f33 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
@@ -3,11 +3,13 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
-#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <VirtualRobot/Robot.h>
+
namespace armarx::armem::articulated_object
{
armem::articulated_object::ArticulatedObject convert(const VirtualRobot::Robot& obj,
@@ -46,4 +48,4 @@ namespace armarx::armem::articulated_object
return store(armemArticulatedObject);
}
-} // namespace armarx::armem::articulated_object
\ No newline at end of file
+} // namespace armarx::armem::articulated_object
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..202c4d6c4cc5b94e9a912ee9a24a3387b8542906 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -2,7 +2,9 @@
#include <mutex>
#include <optional>
+#include <Eigen/src/Geometry/Transform.h>
+#include "RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h"
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -13,7 +15,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>
@@ -44,7 +46,7 @@ namespace armarx::armem::articulated_object
def->optional(properties.coreClassSegmentName,
prefix + "CoreSegment",
"Name of the memory core segment to use for object classes.");
- def->optional(properties.providerName, prefix + "ProviderName");
+ def->optional(properties.providerName, prefix + "read.ProviderName");
}
void Reader::connect()
@@ -188,7 +190,7 @@ namespace armarx::armem::articulated_object
// clang-format off
qb
.coreSegments().withName(properties.coreClassSegmentName)
- .providerSegments().all() // TODO(fabian.reister): think about this: which authority is trustworthy?
+ .providerSegments().withName(properties.providerName)
.entities().all() // withName(name)
.snapshots().latest();
// clang-format on
@@ -231,43 +233,59 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
- return getRobotState(qResult.memory);
+ return getArticulatedObjectState(qResult.memory);
+ }
+
+
+ std::optional<robot::RobotState> convertToRobotState(const armem::wm::EntityInstance& instance)
+ {
+ armarx::armem::arondto::ObjectInstance aronObjectInstance;
+ try
+ {
+ aronObjectInstance.fromAron(instance.data());
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Conversion to ObjectInstance failed!";
+ return std::nullopt;
+ }
+
+ objpose::ObjectPose objectPose;
+ objpose::fromAron(aronObjectInstance.pose, objectPose);
+
+ robot::RobotState robotState
+ {
+ .timestamp = objectPose.timestamp,
+ .globalPose = Eigen::Affine3f(objectPose.objectPoseGlobal),
+ .jointMap = objectPose.objectJointValues
+ };
+
+ return robotState;
}
std::optional<robot::RobotState>
- Reader::getRobotState(const armarx::armem::wm::Memory& memory) const
+ Reader::getArticulatedObjectState(const armarx::armem::wm::Memory& memory) const
{
// clang-format off
const armem::wm::CoreSegment& coreSegment = memory
.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 convertToRobotState(instance.value());
+ // return robot::convertRobotState(instance.value());
}
-
+
+ ARMARX_FATAL << "Failed to obtain robot state";
return std::nullopt;
+
}
@@ -275,67 +293,44 @@ 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;
- }
+ instance = i;
+ });
- 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);
- }
+ if (instance.has_value())
+ {
+ return convertRobotDescription(instance.value());
}
+ ARMARX_DEBUG << "No robot description";
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/Reader.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
index f5dac4035dfb4c8ccab3e82e78af035d64fd981f..e0dca8f3ebd68e682f7bff2bcc759c6bb2673759 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -66,7 +66,7 @@ namespace armarx::armem::articulated_object
protected:
std::optional<robot::RobotState>
- getRobotState(const armarx::armem::wm::Memory& memory) const;
+ getArticulatedObjectState(const armarx::armem::wm::Memory& memory) const;
std::optional<robot::RobotDescription>
getRobotDescription(const armarx::armem::wm::Memory& memory) const;
std::vector<robot::RobotDescription>
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
index 237bc31cc8827f0fb9f80284bf0c639c23d093ec..dcd44d6739bfaa3e211a79721a330cc10b4751fc 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -1,25 +1,20 @@
#include "Writer.h"
-#include <mutex>
-#include <optional>
-
-#include <IceUtil/Time.h>
-
#include <SimoxUtility/algorithm/get_map_keys_values.h>
-#include "ArmarXCore/core/exceptions/LocalException.h"
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
-#include "RobotAPI/libraries/ArmarXObjects/ObjectInfo.h"
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectID.aron.generated.h>
#include <RobotAPI/libraries/armem/client/query.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/operations.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_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
@@ -27,6 +22,7 @@
#include "utils.h"
+
namespace armarx::armem::articulated_object
{
Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
@@ -50,7 +46,7 @@ namespace armarx::armem::articulated_object
"Name of the memory core segment to use for object classes.");
ARMARX_IMPORTANT << "Writer: add property '" << prefix << "ProviderName'";
- def->required(properties.providerName, prefix + "ProviderName", "Name of this provider");
+ def->required(properties.providerName, prefix + "write.ProviderName", "Name of this provider");
}
void Writer::connect()
@@ -86,7 +82,6 @@ namespace armarx::armem::articulated_object
void Writer::updateKnownObject(const armem::MemoryID& snapshotId)
{
-
arondto::RobotDescription aronArticulatedObjectDescription;
// aronArticulatedObjectDescription.fromAron(snapshotId.ent);
@@ -192,6 +187,8 @@ namespace armarx::armem::articulated_object
const auto& timestamp = obj.timestamp;
+ ARMARX_DEBUG << "Storing articulated object instance '" << obj.description.name << "' (provider '" << properties.providerName << "')";
+
const auto providerId = armem::MemoryID()
.withMemoryName(properties.memoryName)
.withCoreSegmentName(properties.coreInstanceSegmentName)
@@ -269,59 +266,41 @@ namespace armarx::armem::articulated_object
.getCoreSegment(properties.coreClassSegmentName)
.getProviderSegment(properties.providerName); // TODO(fabian.reister): all
// clang-format on
- const auto entities = simox::alg::get_values(providerSegment.entities());
- if (entities.empty())
+ if (const armem::wm::EntityInstance* instance = findFirstInstance(providerSegment))
{
- ARMARX_WARNING << "No entity found";
- return std::nullopt;
+ return convertRobotDescription(*instance);
}
-
- const auto entitySnapshots = simox::alg::get_values(entities.front().history());
-
- if (entitySnapshots.empty())
+ else
{
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 convertRobotDescription(instance);
}
std::unordered_map<std::string, armem::MemoryID>
Writer::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
{
- std::unordered_map<std::string, armem::MemoryID> descriptions;
-
const armem::wm::CoreSegment& coreSegment =
memory.getCoreSegment(properties.coreClassSegmentName);
- for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
+ std::unordered_map<std::string, armem::MemoryID> descriptions;
+ coreSegment.forEachEntity([&descriptions](const wm::Entity & entity)
{
- for (const auto& [name, entity] : providerSegment.entities())
+ if (entity.empty())
{
- if (entity.empty())
- {
- ARMARX_WARNING << "No entity found";
- continue;
- }
-
- const auto entitySnapshots = simox::alg::get_values(entity.history());
- const armem::wm::EntitySnapshot& sn = entitySnapshots.front();
- const armem::wm::EntityInstance& instance = sn.getInstance(0);
-
- const auto robotDescription = convertRobotDescription(instance);
-
- if (robotDescription)
- {
- const armem::MemoryID snapshotID(sn.id());
- descriptions.insert({robotDescription->name, snapshotID});
- }
+ ARMARX_WARNING << "No entity found";
+ return true;
}
- }
+
+ const armem::wm::EntitySnapshot& sn = entity.getFirstSnapshot();
+ if (const auto robotDescription = convertRobotDescription(sn.getInstance(0)))
+ {
+ const armem::MemoryID snapshotID(sn.id());
+ descriptions.insert({robotDescription->name, snapshotID});
+ }
+ return true;
+ });
return descriptions;
}
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
index 95495220780d95278685c8c349462708aa822a3e..3e827d66a496ddf8df4448788f39ca1c36722290 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
@@ -22,8 +22,9 @@
#pragma once
#include <mutex>
+#include <optional>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
@@ -39,7 +40,7 @@ namespace armarx::armem::articulated_object
virtual public WriterInterface
{
public:
- Writer(armem::client::MemoryNameSystem& memoryNameSystem);
+ Writer(armem::client::MemoryNameSystem& memoryNameSystemopti);
virtual ~Writer() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp
index c6da5e109ff356ecbe4a4f2b569c332a5e34cfcb..11c389be63160c69d449244bf6d125f71e7bc225 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp
@@ -2,6 +2,8 @@
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
namespace armarx::armem::articulated_object
{
std::optional<robot::RobotDescription>
@@ -31,4 +33,5 @@ namespace armarx::armem::articulated_object
return robotDescription;
}
-} // namespace armarx::armem::articulated_object
\ No newline at end of file
+
+} // namespace armarx::armem::articulated_object
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/attachments/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
index bb5d930da65b6ddb0e7966aa53e5e8fa766d383d..2c60cb9bd35ab7b3010e5b5f5fb0e719d0817f98 100644
--- a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
@@ -3,15 +3,15 @@
#include <sstream>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include "ArmarXCore/core/logging/Logging.h"
-#include <ArmarXCore/core/application/properties/forward_declarations.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>
@@ -50,24 +50,22 @@ namespace armarx::armem::server::obj::attachments
{
}
- 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
{
ARMARX_CHECK_NOT_NULL(coreSegment);
- std::scoped_lock(coreSegment->mutex());
-
- std::vector<armarx::armem::attachment::ObjectAttachment> attachments;
-
- for (const auto& [_, provSeg] : *coreSegment)
+ return coreSegment->doLocked([this]()
{
- for (const auto& [name, entity] : provSeg.entities())
+ std::vector<armarx::armem::attachment::ObjectAttachment> attachments;
+ coreSegment->forEachEntity([this, &attachments](const wm::Entity & entity)
{
- 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);
+ const auto aronAttachment = tryCast<armarx::armem::arondto::attachment::ObjectAttachment>(entityInstance);
if (not aronAttachment)
{
ARMARX_WARNING << "Could not convert entity instance to 'ObjectAttachment'";
- continue;
+ return true;
}
ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
@@ -76,10 +74,11 @@ namespace armarx::armem::server::obj::attachments
fromAron(*aronAttachment, attachment);
attachments.push_back(attachment);
- }
- }
+ return true;
+ });
- return attachments;
+ return attachments;
+ });
}
} // 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 0b9302d93390c473b87c67066bfe173237f3774b..4f8cd20ccd9a0ce456bb664f5f5eb8a25f1981e0 100644
--- a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h
@@ -21,41 +21,19 @@
#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/logging/Logging.h>
#include <ArmarXCore/core/application/properties/forward_declarations.h>
+#include <ArmarXCore/core/logging/Logging.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
{
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.cpp b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.cpp
index 08927d1e77d1963234f42ea9ab861ec9a33f86c4..cb0088285ca161ddc9283f3a024b65a86809fb1b 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,22 +14,25 @@ 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);
if (properties.show)
{
- const wm::Entity* entity = classCoreSegment.findEntity(MemoryID().withEntityName(properties.entityName));
+ const wm::Entity* entity = classCoreSegment.findEntity(properties.entityName);
if (entity)
{
ARMARX_INFO << "Drawing floor class '" << properties.entityName << "'.";
@@ -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 22608257c1abcf39340643870b704c2dadfcd54d..8b1ae942a95b73119eea072b7f33a539a05fbbdf 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -124,8 +124,10 @@ namespace armarx::armem::server::obj::clazz
{
try
{
- std::optional<arondto::ObjectClass> aron =
- coreSegment->getLatestEntityInstanceDataLockingAs<arondto::ObjectClass>(entityID, 0);
+ std::optional<arondto::ObjectClass> aron = coreSegment->doLocked([this, &entityID]()
+ {
+ return coreSegment->findLatestInstanceDataAs<arondto::ObjectClass>(entityID, 0);
+ });
if (not aron.has_value())
{
return;
@@ -269,12 +271,14 @@ namespace armarx::armem::server::obj::clazz
}
if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
{
- std::scoped_lock lock(segment.mutex());
- segment.properties.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- if (segment.coreSegment)
+ segment.doLocked([this, &segment]()
{
- segment.coreSegment->setMaxHistorySize(long(segment.properties.maxHistorySize));
- }
+ segment.properties.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ if (segment.coreSegment)
+ {
+ segment.coreSegment->setMaxHistorySize(long(segment.properties.maxHistorySize));
+ }
+ });
}
}
@@ -285,16 +289,13 @@ namespace armarx::armem::server::obj::clazz
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>");
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
index 15c7c4f373f20c43c0febfb142675c5058c45909..3e9717b3e51dc07491e55761664e5cc76add574b 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
@@ -9,7 +9,7 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.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>
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 09a412f4bfa40a1a6288e86dfe65fa8b50953061..9668e307ba7c7ff3ed1143087bf4aee1adf2608c 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -7,7 +7,6 @@
#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>
@@ -26,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>
@@ -143,7 +143,7 @@ namespace armarx::armem::server::obj::instance
// 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);
@@ -250,21 +250,24 @@ namespace armarx::armem::server::obj::instance
}
- 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);
@@ -273,7 +276,8 @@ namespace armarx::armem::server::obj::instance
- objpose::ObjectPoseMap Segment::getObjectPosesByProvider(
+ objpose::ObjectPoseMap
+ Segment::getObjectPosesByProvider(
const std::string& providerName,
IceUtil::Time now)
{
@@ -284,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
@@ -387,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);
@@ -402,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);
@@ -410,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);
@@ -418,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())
{
@@ -445,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());
@@ -476,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());
@@ -589,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
@@ -650,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();
@@ -662,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);
@@ -697,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.";
@@ -747,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;
@@ -781,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)
{
@@ -824,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))
{
@@ -837,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;
@@ -859,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;
@@ -894,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
{
@@ -916,10 +917,8 @@ namespace armarx::armem::server::obj::instance
{
ARMARX_WARNING_S << e.what();
}
- return false;
- };
+ });
- visitor.applyTo(*coreSegment);
return scene;
}
@@ -1040,28 +1039,29 @@ namespace armarx::armem::server::obj::instance
if (storeButton.wasClicked())
{
armem::obj::SceneSnapshot scene;
+ segment.doLocked([&scene, &segment]()
{
- std::scoped_lock lock(segment.mutex());
scene = segment.getSceneSnapshot();
- }
+ });
segment.storeScene(storeLoadLine.getValue(), scene);
}
if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
|| discardSnapshotsWhileAttached.hasValueChanged())
{
- std::scoped_lock lock(segment.mutex());
-
- if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ segment.doLocked([this, &segment]()
{
- segment.properties.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- if (segment.coreSegment)
+ if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
{
- segment.coreSegment->setMaxHistorySize(long(segment.properties.maxHistorySize));
+ segment.properties.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ if (segment.coreSegment)
+ {
+ segment.coreSegment->setMaxHistorySize(long(segment.properties.maxHistorySize));
+ }
}
- }
- segment.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 918a4efade3c596763d90fec427b18fa78a774bf..27ffbc746af05451b5fdd54f15a3afc4c7822e08 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -8,8 +8,6 @@
#include <SimoxUtility/caching/CacheMap.h>
#include <SimoxUtility/shapes/OrientedBox.h>
-#include <ArmarXCore/core/logging/Logging.h>
-
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
@@ -19,7 +17,7 @@
#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/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
#include "ArticulatedObjectVisu.h"
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index 7cd8b1202ac34600f8bfc6ecc912037b49d8bee6..ef00638e9ed76d0bbe05de3ab6af98f3687fe9d7 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -123,8 +123,8 @@ namespace armarx::armem::server::obj::instance
<< "with invalid provider proxy.\nIgnoring provider '" << providerName << "'.";
return;
}
+ segment.doLocked([this, &providerName, &info]()
{
- std::scoped_lock lock(segment.mutex());
std::stringstream ss;
for (const auto& id : info.supportedObjects)
{
@@ -133,7 +133,7 @@ namespace armarx::armem::server::obj::instance
ARMARX_VERBOSE << "Provider '" << providerName << "' available.\n"
<< "Supported objects: \n" << ss.str();
segment.providers[providerName] = info;
- }
+ });
}
@@ -164,8 +164,8 @@ namespace armarx::armem::server::obj::instance
return;
}
+ segment.doLocked([&]()
{
- std::scoped_lock lock(segment.mutex());
RemoteRobot::synchronizeLocalClone(segment.robot, segment.robotStateComponent);
if (segment.robot->hasRobotNode(calibration.robotNode))
@@ -200,7 +200,7 @@ namespace armarx::armem::server::obj::instance
{ "t MemorySetObjectPoses [ms]", new Variant(tCommitObjectPoses.toMilliSecondsDouble()) },
});
}
- }
+ });
}
@@ -218,12 +218,12 @@ namespace armarx::armem::server::obj::instance
{
TIMING_START(tGetObjectPoses);
- TIMING_START(tGetObjectPosesLock);
- std::scoped_lock lock(segment.mutex());
- TIMING_END_STREAM(tGetObjectPosesLock, ARMARX_VERBOSE);
-
const IceUtil::Time now = TimeUtil::GetTime();
- const objpose::data::ObjectPoseSeq result = objpose::toIce(simox::alg::get_values(segment.getObjectPoses(now)));
+ const objpose::ObjectPoseSeq objectPoses = segment.doLocked([this, &now]()
+ {
+ return simox::alg::get_values(segment.getObjectPoses(now));
+ });
+ const objpose::data::ObjectPoseSeq result = objpose::toIce(objectPoses);
TIMING_END_STREAM(tGetObjectPoses, ARMARX_VERBOSE);
@@ -232,7 +232,6 @@ namespace armarx::armem::server::obj::instance
debugObserver->setDebugChannel(getName(),
{
{ "t GetObjectPoses() [ms]", new Variant(tGetObjectPoses.toMilliSecondsDouble()) },
- { "t GetObjectPoses() lock [ms]", new Variant(tGetObjectPosesLock.toMilliSecondsDouble()) }
});
}
@@ -243,12 +242,12 @@ namespace armarx::armem::server::obj::instance
{
TIMING_START(GetObjectPoses);
- TIMING_START(GetObjectPosesLock);
- std::scoped_lock lock(segment.mutex());
- TIMING_END_STREAM(GetObjectPosesLock, ARMARX_VERBOSE);
-
const IceUtil::Time now = TimeUtil::GetTime();
- const objpose::data::ObjectPoseSeq result = objpose::toIce(simox::alg::get_values(segment.getObjectPosesByProvider(providerName, now)));
+ const objpose::ObjectPoseSeq objectPoses = segment.doLocked([this, &now, &providerName]()
+ {
+ return simox::alg::get_values(segment.getObjectPosesByProvider(providerName, now));
+ });
+ const objpose::data::ObjectPoseSeq result = objpose::toIce(objectPoses);
TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
@@ -257,7 +256,6 @@ namespace armarx::armem::server::obj::instance
debugObserver->setDebugChannel(getName(),
{
{ "t GetObjectPosesByProvider() [ms]", new Variant(GetObjectPoses.toMilliSecondsDouble()) },
- { "t GetObjectPosesByProvider() lock [ms]", new Variant(GetObjectPosesLock.toMilliSecondsDouble()) }
});
}
@@ -296,26 +294,28 @@ namespace armarx::armem::server::obj::instance
}
else
{
- std::scoped_lock lock(segment.mutex());
- for (const auto& objectID : input.request.objectIDs)
+ segment.doLocked([&]()
{
- bool found = true;
- for (const auto& [providerName, info] : segment.providers)
+ for (const auto& objectID : input.request.objectIDs)
{
- // ToDo: optimize look up.
- if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end())
+ bool found = true;
+ for (const auto& [providerName, info] : segment.providers)
+ {
+ // ToDo: optimize look up.
+ if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end())
+ {
+ providerRequests[providerName].objectIDs.push_back(objectID);
+ updateProxy(providerName);
+ break;
+ }
+ }
+ if (!found)
{
- providerRequests[providerName].objectIDs.push_back(objectID);
- updateProxy(providerName);
- break;
+ ARMARX_ERROR << "Did not find a provider for " << objectID << ".";
+ output.results[objectID].providerName = "";
}
}
- if (!found)
- {
- ARMARX_ERROR << "Did not find a provider for " << objectID << ".";
- output.results[objectID].providerName = "";
- }
- }
+ });
}
for (const auto& [providerName, request] : providerRequests)
@@ -343,69 +343,84 @@ namespace armarx::armem::server::obj::instance
objpose::ProviderInfoMap SegmentAdapter::getAvailableProvidersInfo(const Ice::Current&)
{
- std::scoped_lock lock(segment.mutex());
- return segment.providers;
+ return segment.doLocked([this]()
+ {
+ return segment.providers;
+ });
}
Ice::StringSeq SegmentAdapter::getAvailableProviderNames(const Ice::Current&)
{
- std::scoped_lock lock(segment.mutex());
- return simox::alg::get_keys(segment.providers);
+ return segment.doLocked([this]()
+ {
+ return simox::alg::get_keys(segment.providers);
+ });
}
objpose::ProviderInfo SegmentAdapter::getProviderInfo(const std::string& providerName, const Ice::Current&)
{
- std::scoped_lock lock(segment.mutex());
- return segment.getProviderInfo(providerName);
+ return segment.doLocked([this, &providerName]()
+ {
+ return segment.getProviderInfo(providerName);
+ });
}
bool SegmentAdapter::hasProvider(const std::string& providerName, const Ice::Current&)
{
- std::scoped_lock lock(segment.mutex());
- return segment.providers.count(providerName) > 0;
+ return segment.doLocked([this, &providerName]()
+ {
+ return segment.providers.count(providerName) > 0;
+ });
}
objpose::AttachObjectToRobotNodeOutput SegmentAdapter::attachObjectToRobotNode(
const objpose::AttachObjectToRobotNodeInput& input, const Ice::Current&)
{
- std::scoped_lock lock(segment.mutex());
- return segment.attachObjectToRobotNode(input);
+ return segment.doLocked([this, &input]()
+ {
+ return segment.attachObjectToRobotNode(input);
+ });
}
objpose::DetachObjectFromRobotNodeOutput SegmentAdapter::detachObjectFromRobotNode(
const objpose::DetachObjectFromRobotNodeInput& input, const Ice::Current&)
{
- std::scoped_lock lock(segment.mutex());
- return segment.detachObjectFromRobotNode(input);
+ return segment.doLocked([this, &input]()
+ {
+ return segment.detachObjectFromRobotNode(input);
+ });
}
objpose::DetachAllObjectsFromRobotNodesOutput SegmentAdapter::detachAllObjectsFromRobotNodes(
const objpose::DetachAllObjectsFromRobotNodesInput& input, const Ice::Current&)
{
- std::scoped_lock lock(segment.mutex());
- return segment.detachAllObjectsFromRobotNodes(input);
+ return segment.doLocked([this, &input]()
+ {
+ return segment.detachAllObjectsFromRobotNodes(input);
+ });
}
objpose::AgentFramesSeq SegmentAdapter::getAttachableFrames(const Ice::Current&)
{
- std::scoped_lock lock(segment.mutex());
-
- objpose::AgentFramesSeq output;
- std::vector<VirtualRobot::RobotPtr> agents = { segment.robot };
- for (VirtualRobot::RobotPtr agent : agents)
+ return segment.doLocked([this]()
{
- objpose::AgentFrames& frames = output.emplace_back();
- frames.agent = agent->getName();
- frames.frames = agent->getRobotNodeNames();
- }
- return output;
+ objpose::AgentFramesSeq output;
+ std::vector<VirtualRobot::RobotPtr> agents = { segment.robot };
+ for (VirtualRobot::RobotPtr agent : agents)
+ {
+ objpose::AgentFrames& frames = output.emplace_back();
+ frames.agent = agent->getName();
+ frames.frames = agent->getRobotNodeNames();
+ }
+ return output;
+ });
}
@@ -433,9 +448,9 @@ namespace armarx::armem::server::obj::instance
objpose::ObjectPoseMap objectPoses;
visu.minConfidence = -1;
- {
- std::scoped_lock lock(segment.mutex());
+ segment.doLocked([this, &objectPoses, &objectFinder]()
+ {
const IceUtil::Time now = TimeUtil::GetTime();
// Also include decayed objects in result
@@ -452,7 +467,7 @@ namespace armarx::armem::server::obj::instance
{
visu.minConfidence = segment.decay.removeObjectsBelowConfidence;
}
- }
+ });
const std::vector<viz::Layer> layers = visu.visualizeCommit(objectPoses, objectFinder);
arviz.commit(layers);
@@ -514,8 +529,10 @@ namespace armarx::armem::server::obj::instance
}
this->segment.update(adapter.segment);
{
- std::scoped_lock lock(adapter.segment.mutex());
- this->decay.update(adapter.segment.decay);
+ adapter.segment.doLocked([this, &adapter]()
+ {
+ this->decay.update(adapter.segment.decay);
+ });
}
{
std::scoped_lock lock(adapter.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/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot/CMakeLists.txt
index eb0c4fb7d37a4bb43830f224d3a81714f1175f39..43e6af6f4a45f3d638c182bef38e5cdc9cf50d24 100644
--- a/source/RobotAPI/libraries/armem_robot/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot/CMakeLists.txt
@@ -22,6 +22,7 @@ armarx_add_library(
robot_conversions.h
SOURCES
+ types.cpp
aron_conversions.cpp
robot_conversions.cpp
diff --git a/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp b/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
index 3bf4171486e35c33a1cca23a666baac2233a8356..8b7829a2f0c3f6dadd54bf2ce624c9fcbe793efd 100644
--- a/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
@@ -4,12 +4,14 @@
#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>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
namespace fs = ::std::filesystem;
diff --git a/source/RobotAPI/libraries/armem_robot/types.cpp b/source/RobotAPI/libraries/armem_robot/types.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..01847402c1e1bdde24956203620c13e15328e913
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot/types.cpp
@@ -0,0 +1,11 @@
+#include "types.h"
+
+namespace armarx::armem::robot
+{
+ std::ostream& operator<<(std::ostream &os, const RobotDescription &rhs)
+ {
+ os << "RobotDescription { name: '" << rhs.name << "', xml: '" << rhs.xml << "' }";
+ return os;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index 4fed4d1cb277ab316a318cbc13b4f32222fd8939..df39df99e945e4a1ef38c38db6f9428404a35bed 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -8,8 +8,10 @@
#include <IceUtil/Time.h>
+#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
#include <ArmarXCore/core/PackagePath.h>
+
namespace armarx::armem::robot
{
struct RobotDescription
@@ -45,4 +47,6 @@ namespace armarx::armem::robot
using RobotDescriptions = std::vector<RobotDescription>;
using RobotStates = std::vector<RobotState>;
-} // namespace armarx::armem::robot
\ No newline at end of file
+ std::ostream& operator<<(std::ostream &os, const RobotDescription &rhs);
+
+} // namespace armarx::armem::robot
diff --git a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
index defd8a3f48aa68b63673498549a731b573b9b9b4..c86ed671ef17fcd287c05a7083abf3803b852e99 100644
--- a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
@@ -6,6 +6,7 @@ armarx_set_target("Library: ${LIB_NAME}")
find_package(Eigen3 QUIET)
armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+
armarx_add_library(
LIBS
# ArmarX
@@ -19,61 +20,81 @@ armarx_add_library(
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/forward_declarations.h
- ./server/common/Visu.h
+ server/common/Visu.h
+ server/common/combine.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/common/combine.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
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
index 38eb6bb439197032cac8968a0dbee131eec8efbc..99900b7b0766613a555fb060fa19fddf987def7f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
@@ -1,79 +1,214 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!--
-Robot proprioception.
+(Armar6) Robot proprioception.
-->
<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<Eigen/Core>" />
+ <Include include="<Eigen/Geometry>" />
+ </CodeIncludes>
+
<GenerateTypes>
<Object name="armarx::armem::prop::arondto::Joints">
- <ObjectChild key="positions">
+ <ObjectChild key="position">
<Dict>
<Float />
</Dict>
</ObjectChild>
- <ObjectChild key="velocities">
+ <ObjectChild key="velocity">
<Dict>
<Float />
</Dict>
</ObjectChild>
- <ObjectChild key="accellerations">
+ <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="torques">
+
+ <ObjectChild key="torque">
<Dict>
<Float />
</Dict>
</ObjectChild>
- <ObjectChild key="inertiaTorques">
+ <ObjectChild key="inertiaTorque">
<Dict>
<Float />
</Dict>
</ObjectChild>
- <ObjectChild key="gravityTorques">
+ <ObjectChild key="gravityTorque">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="gravityCompensatedTorque">
<Dict>
<Float />
</Dict>
</ObjectChild>
- <ObjectChild key="inserveDynamicsTorques">
+ <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="velocity">
+ <ObjectChild key="relativePosition">
<EigenMatrix rows="3" cols="1" type="float" />
</ObjectChild>
-
- <ObjectChild key="acceleration">
+
+ <!-- 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="rot_velocity">
+ <ObjectChild key="acceleration">
<EigenMatrix rows="3" cols="1" type="float" />
</ObjectChild>
- <ObjectChild key="rot_acceleration">
- <EigenMatrix rows="3" cols="1" type="float" />
+ <ObjectChild key="extra">
+ <Dict>
+ <Float />
+ </Dict>
</ObjectChild>
-
+
</Object>
-
<Object name="armarx::armem::prop::arondto::ForceTorque">
<ObjectChild key="force">
@@ -92,12 +227,22 @@ Robot proprioception.
<EigenMatrix rows="3" cols="1" type="float" />
</ObjectChild>
+ <ObjectChild key="extra">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
</Object>
- <Object name="armarx::armem::prop::arondto::Proprioception">
+ <Object name="armarx::armem::arondto::Proprioception">
+ <ObjectChild key="iterationID">
+ <long />
+ </ObjectChild>
+
<ObjectChild key="joints">
<armarx::armem::prop::arondto::Joints/>
</ObjectChild>
@@ -111,7 +256,20 @@ Robot proprioception.
<armarx::armem::prop::arondto::ForceTorque/>
</Dict>
</ObjectChild>
-
+
+
+ <ObjectChild key="extraFloats">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="extraLongs">
+ <Dict>
+ <Long />
+ </Dict>
+ </ObjectChild>
+
</Object>
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..764c443a7cc595f7c1f32ca51d4bdde6ee9309a4 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -3,6 +3,7 @@
#include <mutex>
#include <optional>
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/PackagePath.h>
@@ -10,11 +11,12 @@
#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>
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
namespace fs = ::std::filesystem;
@@ -115,7 +117,7 @@ namespace armarx::armem::robot_state
if (not memoryReader)
{
- ARMARX_WARNING << "Memory reader is null";
+ ARMARX_WARNING << "Memory reader is null. Did you forget to call RobotReader::connect() in onConnectComponent()?";
return std::nullopt;
}
@@ -172,6 +174,8 @@ namespace armarx::armem::robot_state
// Query all entities from provider.
armem::client::query::Builder qb;
+ ARMARX_DEBUG << "Querying robot description for robot: " << description;
+
// clang-format off
qb
.coreSegments().withName(properties.proprioceptionCoreSegment)
@@ -214,30 +218,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 +276,28 @@ 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& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ const auto proprioception = tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+ ARMARX_CHECK(proprioception.has_value());
- const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
+ const armarx::armem::prop::arondto::Joints& joints = proprioception->joints;
+
- if (not jointState)
- {
- // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
- continue;
- }
- jointMap.emplace(jointState->name, jointState->position);
- }
- }
+ // const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
+ // if (not jointState)
+ // {
+ // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+ // return;
+ // }
+
+ jointMap = joints.position;
+
+ // jointMap.emplace(jointState->name, jointState->position);
+ });
if (jointMap.empty())
{
@@ -307,26 +316,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/localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
index 6aa026431e7d9efd0128a9007d23918b291b54bc..d46c7dfa1cc72847b27a820ecd4281ec1cb649e6 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
@@ -20,8 +20,6 @@
*/
#include "TransformReader.h"
-#include "RobotAPI/libraries/armem/core/Time.h"
-#include "RobotAPI/libraries/armem_robot_state/common/localization/types.h"
#include <algorithm>
#include <iterator>
@@ -45,20 +43,23 @@
#include <ArmarXCore/core/time/CycleUtil.h>
// this package
+#include <RobotAPI/libraries/core/FramedPose.h>
+
#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/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>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
-
+#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
+
namespace armarx::armem::client::robot_state::localization
{
@@ -135,11 +136,11 @@ namespace armarx::armem::client::robot_state::localization
TransformResult TransformReader::lookupTransform(const TransformQuery& query) const
{
const auto& timestamp = query.header.timestamp;
-
- const auto durationEpsilon = IceUtil::Time::milliSeconds(-1);
-
ARMARX_DEBUG << "Looking up transform at timestamp " << timestamp;
+ const IceUtil::Time durationEpsilon = IceUtil::Time::milliSeconds(-1);
+ (void) durationEpsilon;
+
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -152,18 +153,20 @@ namespace armarx::armem::client::robot_state::localization
// clang-format on
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
-
ARMARX_DEBUG << "Lookup result in reader: " << qResult;
if (not qResult.success)
{
- return {.transform =
+ return
+ {
+ .transform =
{
.header = query.header,
},
.status = TransformResult::Status::ErrorFrameNotAvailable,
.errorMessage = "Error in tf lookup '" + query.header.parentFrame + " -> " +
- query.header.frame + "' : " + qResult.errorMessage};
+ query.header.frame + "' : " + qResult.errorMessage
+ };
}
const auto& localizationCoreSegment = qResult.memory.getCoreSegment(properties.localizationSegment);
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 4230394aa823c96e2382fd73af68d9663513b989..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>
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 c4d0e05b4a1120b0ba6d256ad646508d7556ab7d..f20f68cf8bfda8dd57f97264e98bee84d61a83bd 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";
@@ -110,8 +125,7 @@ namespace armarx::armem::common::robot_state::localization
std::vector<std::string> chain;
- const auto& agentProviderSegment =
- localizationCoreSegment.getProviderSegment(query.header.agent);
+ const auto& agentProviderSegment = localizationCoreSegment.getProviderSegment(query.header.agent);
auto addToChain = [&](const std::string & parentFrame, const std::string & frame)
{
@@ -123,14 +137,14 @@ namespace armarx::armem::common::robot_state::localization
}
else
{
- ARMARX_WARNING << deactivateSpam(10) << "Cannot perform tf lookup '" << parentFrame << " -> " << frame
- << "'";
+ ARMARX_WARNING << deactivateSpam(10) << "Cannot perform tf lookup '" << parentFrame << " -> " << frame << "'";
}
};
std::array<std::string, 3> knownChain
{
- GlobalFrame, MapFrame, OdometryFrame}; // Robot comes next
+ GlobalFrame, MapFrame, OdometryFrame
+ }; // Robot comes next
auto* frameBeginIt =
std::find(knownChain.begin(), knownChain.end(), query.header.parentFrame);
@@ -172,8 +186,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());
@@ -203,16 +309,15 @@ namespace armarx::armem::common::robot_state::localization
const auto poseNextIt = std::find_if(transforms.begin(), transforms.end(), timestampBeyond);
- ARMARX_WARNING << "Fooasdfasdjfh";
-
ARMARX_CHECK(it == poseNextIt);
return poseNextIt;
}
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 +360,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 d6dcddd7d3fd568216a63657bfe6ae5ca1f10f9c..051ab03e793a0afceca524e007b834f4f510e6a0 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -6,8 +6,6 @@
#include <Eigen/Geometry>
-#include <IceUtil/Time.h>
-
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/math/pose.h>
@@ -15,23 +13,30 @@
#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/libraries/ArmarXObjects/ObjectFinder.h>
-#include "../description/Segment.h"
-#include "../localization/Segment.h"
-#include "../proprioception/Segment.h"
+#include <RobotAPI/components/ArViz/Client/Elements.h>
+
+#include <RobotAPI/libraries/armem_robot_state/server/description/Segment.h>
+#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
+
+#include "combine.h"
namespace armarx::armem::server::robot_state
{
- Visu::Visu(const description::Segment& descriptionSegment, const proprioception::Segment& proprioceptionSegment, const localization::Segment& localizationSegment)
+ Visu::Visu(
+ const description::Segment& descriptionSegment,
+ const proprioception::Segment& proprioceptionSegment,
+ const localization::Segment& localizationSegment)
: descriptionSegment(descriptionSegment),
proprioceptionSegment(proprioceptionSegment),
localizationSegment(localizationSegment)
- {}
+ {
+ Logging::setTag("Visu");
+ }
void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
@@ -56,6 +61,12 @@ namespace armarx::armem::server::robot_state
this->debugObserver = DebugObserverHelper("RobotStateMemory", debugObserver, batchMode);
}
+ if (updateTask)
+ {
+ updateTask->stop();
+ updateTask->join();
+ updateTask = nullptr;
+ }
updateTask = new SimpleRunningTask<>([this]()
{
this->visualizeRun();
@@ -66,24 +77,21 @@ namespace armarx::armem::server::robot_state
void Visu::visualizeRobots(viz::Layer& layer, const robot::Robots& robots)
{
- const auto visualizeRobot = [&](const robot::Robot & robot)
+ for (const robot::Robot& robot : robots)
{
- const auto xmlPath = robot.description.xml.serialize();
+ const data::PackagePath xmlPath = robot.description.xml.serialize();
// clang-format off
- auto robotVisu = viz::Robot(robot.description.name)
- // .file(xmlPath.package, xmlPath.path)
- .file(xmlPath.package, xmlPath.path)
- .joints(robot.config.jointMap)
- .pose(robot.config.globalPose);
+ viz::Robot robotVisu = viz::Robot(robot.description.name)
+ .file(xmlPath.package, xmlPath.path)
+ .joints(robot.config.jointMap)
+ .pose(robot.config.globalPose);
robotVisu.useFullModel();
// clang-format on
layer.add(robotVisu);
- };
-
- std::for_each(robots.begin(), robots.end(), visualizeRobot);
+ }
}
@@ -107,161 +115,117 @@ namespace armarx::armem::server::robot_state
}
- robot::Robots combine(
- const std::unordered_map<std::string, robot::RobotDescription>& robotDescriptions,
- const std::unordered_map<std::string, Eigen::Affine3f>& globalRobotPoseMap,
- const std::unordered_map<std::string, std::map<std::string, float>>& robotJointPositionMap)
+ void Visu::visualizeRun()
{
-
- robot::Robots robots;
-
- for (const auto &[robotName, robotDescription] : robotDescriptions)
+ CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
+ while (updateTask and not updateTask->isStopped())
{
- const auto& globalPose = globalRobotPoseMap.find(robotName);
- if (globalPose == globalRobotPoseMap.end())
+ if (p.enabled)
{
- ARMARX_WARNING << deactivateSpam(10) << "No global pose for robot '" << robotName
- << "'";
- continue;
- }
+ const Time timestamp = Time::now();
+ ARMARX_DEBUG << "Visu task at " << armem::toStringMilliSeconds(timestamp);
- const auto& jointMap = robotJointPositionMap.find(robotName);
- if (jointMap == robotJointPositionMap.end())
- {
- ARMARX_WARNING << deactivateSpam(10) << "No joint positions for robot '"
- << robotName << "'";
- continue;
+ try
+ {
+ visualizeOnce(timestamp);
+ }
+ catch (const std::exception& e)
+ {
+ ARMARX_WARNING << "Caught exception while visualizing robots: \n" << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Caught unknown exception while visualizing robots.";
+ }
+
+ if (debugObserver.has_value())
+ {
+ debugObserver->sendDebugObserverBatch();
+ }
}
+ cycle.waitForCycleDuration();
+ }
+ }
- ARMARX_DEBUG << "Found the following joints for robot " << robotName << ": "
- << simox::alg::get_keys(jointMap->second);
- // TODO(fabian.reister): based on data
- const armem::Time timestamp = IceUtil::Time::now();
+ void Visu::visualizeOnce(const Time& timestamp)
+ {
+ TIMING_START(tVisuTotal);
- robots.emplace_back(robot::Robot
- {
- .description = robotDescription,
- .instance = "", // TODO(fabian.reister): set this properly
- .config = {
- .timestamp = timestamp,
- .globalPose = globalPose->second,
- .jointMap = jointMap->second
- },
- .timestamp = timestamp,
- });
- }
+ // TODO(fabian.reister): use timestamp
- return robots;
- }
+ // Get data.
+ TIMING_START(tVisuGetData);
+ TIMING_START(tRobotDescriptions);
+ const description::RobotDescriptionMap robotDescriptions =
+ descriptionSegment.getRobotDescriptionsLocking(timestamp);
+ TIMING_END_STREAM(tRobotDescriptions, ARMARX_DEBUG);
- void Visu::visualizeRun()
- {
- CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
- while (updateTask && not updateTask->isStopped())
+ TIMING_START(tGlobalPoses);
+ const auto globalPoses = localizationSegment.getRobotGlobalPosesLocking(timestamp);
+ TIMING_END_STREAM(tGlobalPoses, ARMARX_DEBUG);
+
+ TIMING_START(tRobotFramePoses);
+ const auto frames = localizationSegment.getRobotFramePosesLocking(timestamp);
+ TIMING_END_STREAM(tRobotFramePoses, ARMARX_DEBUG);
+
+ TIMING_START(tJointPositions);
+ const auto jointPositions =
+ proprioceptionSegment.getRobotJointPositionsLocking(
+ timestamp, debugObserver ? &*debugObserver : nullptr);
+ TIMING_END_STREAM(tJointPositions, ARMARX_DEBUG);
+
+ TIMING_END_STREAM(tVisuGetData, ARMARX_DEBUG);
+
+
+ // Build layers.
+ TIMING_START(tVisuBuildLayers);
+
+ // We need all 3 information:
+ // - robot description
+ // - global pose
+ // - joint positions
+ // => this is nothing but an armem::Robot
+ ARMARX_DEBUG << "Combining robot ..."
+ << "\n- " << robotDescriptions.size() << " descriptions"
+ << "\n- " << globalPoses.size() << " global poses"
+ << "\n- " << jointPositions.size() << " joint positions";
+
+ const robot::Robots robots =
+ combine(robotDescriptions, globalPoses, jointPositions, timestamp);
+
+ ARMARX_DEBUG << "Visualize " << robots.size() << " robots ...";
+ viz::Layer layer = arviz.layer("Robots");
+ visualizeRobots(layer, robots);
+
+ ARMARX_DEBUG << "Visualize frames ...";
+ viz::Layer layerFrames = arviz.layer("Frames");
+ visualizeFrames(layerFrames, frames);
+
+ TIMING_END_STREAM(tVisuBuildLayers, ARMARX_DEBUG);
+
+
+ // Commit layers.
+
+ 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())
{
- {
- // std::scoped_lock lock(visuMutex);
- ARMARX_DEBUG << "Update task";
- if (p.enabled)
- {
- TIMING_START(tVisuTotal);
-
- // TODO(fabian.reister): use timestamp
- const Time timestamp = Time::now();
-
- try
- {
- // 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.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.getRobotJointPositionsLocking(
- timestamp, debugObserver ? &*debugObserver : nullptr);
- TIMING_END_STREAM(tRobotJointPositionMap, ARMARX_DEBUG);
-
- TIMING_END_STREAM(tVisuGetData, ARMARX_DEBUG);
-
-
- // Build layers.
- TIMING_START(tVisuBuildLayers);
-
- // we need all 3 informations:
- // - robot description
- // - global pose
- // - joint positions
- // => this is nothing but a armem::Robot
-
- const robot::Robots robots =
- combine(robotDescriptions, globalRobotPoseMap, robotJointPositionMap);
-
- ARMARX_DEBUG << "Visualize robots ...";
- viz::Layer layer = arviz.layer("Robots");
- visualizeRobots(layer, robots);
-
- ARMARX_DEBUG << "Visualize frames ...";
- viz::Layer layerFrames = arviz.layer("Frames");
- visualizeFrames(layerFrames, frames);
-
- TIMING_END_STREAM(tVisuBuildLayers, ARMARX_DEBUG);
-
-
- // Commit layers.
-
- 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 << "Caught exception while visualizing robots: \n" << ex.what();
- continue;
- }
- catch (...)
- {
- ARMARX_WARNING << "Caught unknown exception while visualizing robots.";
- continue;
- }
-
- if (debugObserver.has_value())
- {
- debugObserver->sendDebugObserverBatch();
- }
- }
- }
- cycle.waitForCycleDuration();
+ 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 Poses (ms)", tGlobalPoses.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.3 Frames (ms)", tRobotFramePoses.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.4 Joint Positions (ms)", tJointPositions.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 2 Build Layers (ms)", tVisuBuildLayers.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 3 Commit (ms)", tVisuCommit.toMilliSecondsDouble());
}
}
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 95efae41963c63669e4e4bb11b202ba9ef948ec9..ce69772c1b1c55553c62e39d3db5e8e4503d2592 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -31,27 +31,11 @@
#include <RobotAPI/libraries/armem_objects/types.h>
+#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
-namespace armarx
-{
- class ObjectFinder;
-}
namespace armarx::armem::server::robot_state
{
- namespace localization
- {
- class Segment;
- }
- namespace proprioception
- {
- class Segment;
- }
- namespace description
- {
- class Segment;
- }
-
/**
* @brief Models decay of object localizations by decreasing the confidence
@@ -71,14 +55,19 @@ namespace armarx::armem::server::robot_state
void connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver = nullptr);
- protected:
+ private:
+
+ void visualizeRun();
+ void visualizeOnce(const Time& timestamp);
- static void visualizeRobots(
+
+ static
+ void visualizeRobots(
viz::Layer& layer,
- const robot::Robots& robots
- );
+ const robot::Robots& robots);
- static void visualizeFrames(
+ static
+ void visualizeFrames(
viz::Layer& layerFrames,
const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames);
@@ -100,7 +89,6 @@ namespace armarx::armem::server::robot_state
SimpleRunningTask<>::pointer_type updateTask;
- void visualizeRun();
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..abe3f0bbb024d971052aad031b366a3d54b15ffe
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp
@@ -0,0 +1,64 @@
+#include "combine.h"
+
+#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <sstream>
+
+
+namespace armarx::armem::server
+{
+
+ robot::Robots
+ robot_state::combine(
+ const description::RobotDescriptionMap& robotDescriptions,
+ const localization::RobotPoseMap& globalPoses,
+ const proprioception::RobotJointPositionMap& jointPositions,
+ const armem::Time& timestamp)
+ {
+ std::stringstream logs;
+
+ robot::Robots robots;
+ for (const auto& [robotName, robotDescription] : robotDescriptions)
+ {
+ // Handle missing values gracefully instead of skipping the robot altogether.
+
+ robot::Robot& robot = robots.emplace_back();
+
+ robot.description = robotDescription;
+ robot.instance = ""; // TODO(fabian.reister): set this properly
+ robot.config.timestamp = timestamp;
+ robot.config.globalPose = Eigen::Affine3f::Identity();
+ robot.config.jointMap = {};
+ robot.timestamp = timestamp;
+
+ if (auto it = globalPoses.find(robotName); it != globalPoses.end())
+ {
+ robot.config.globalPose = it->second;
+ }
+ else
+ {
+ logs << "\nNo global pose for robot '" << robotName << "'.";
+ }
+ if (auto it = jointPositions.find(robotName); it != jointPositions.end())
+ {
+ robot.config.jointMap = it->second;
+ }
+ else
+ {
+ logs << "\nNo joint positions for robot '" << robotName << "'.";
+ }
+ }
+
+ if (not logs.str().empty())
+ {
+ // These are handled, so they are no warnings.
+ ARMARX_VERBOSE << deactivateSpam(60) << logs.str();
+ }
+
+ return robots;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h
new file mode 100644
index 0000000000000000000000000000000000000000..811599b69d8fe853154520f5b72b4ca95eb9488f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h
@@ -0,0 +1,38 @@
+/*
+ * 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 Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
+
+
+namespace armarx::armem::server::robot_state
+{
+
+ robot::Robots
+ combine(
+ const description::RobotDescriptionMap& robotDescriptions,
+ const localization::RobotPoseMap& globalPoses,
+ const proprioception::RobotJointPositionMap& jointPositions,
+ const armem::Time& timestamp);
+
+} // 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 bd1e9c4fc9f8d95f2abbfd86f3eee5081e8f8ab4..7ca3f370184163d06419d84bfee6e7f4fd2876ff 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -1,10 +1,5 @@
#include "Segment.h"
-#include <sstream>
-#include <thread>
-
-#include <IceUtil/Time.h>
-
#include <ArmarXCore/core/application/properties/PluginAll.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
@@ -19,7 +14,6 @@
#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>
@@ -41,8 +35,6 @@ namespace armarx::armem::server::robot_state::description
void Segment::onConnect(const RobotUnitInterfacePrx& robotUnitPrx)
{
- Base::onConnect();
-
robotUnit = robotUnitPrx;
// store the robot description linked to the robot unit in this segment
@@ -104,39 +96,37 @@ namespace armarx::armem::server::robot_state::description
}
- Segment::RobotDescriptionMap Segment::getRobotDescriptionsLocking(const armem::Time& timestamp) const
+ RobotDescriptionMap
+ Segment::getRobotDescriptionsLocking(const armem::Time& timestamp) const
{
- std::scoped_lock lock(mutex());
- return getRobotDescriptions(timestamp);
+ return doLocked([this, ×tamp]()
+ {
+ return getRobotDescriptions(timestamp);
+ });
}
- Segment::RobotDescriptionMap Segment::getRobotDescriptions(const armem::Time& timestamp) const
+ 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)
{
- for (const auto& [_, provSeg] : *segment)
+ 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 (description)
- {
- ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
- robotDescriptions.emplace(description->name, *description);
- }
- else
- {
- ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
- }
- }
+ ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
+ robotDescriptions.emplace(description->name, *description);
}
- }
+ else
+ {
+ ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
+ }
+ });
ARMARX_INFO << deactivateSpam(60) << "Number of known robot descriptions: " << robotDescriptions.size();
return robotDescriptions;
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 c62821c4afacbf6caa87b5540a4c05c778254f25..40eac57706b1cfc14d2cd823cda222d673f958c2 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
@@ -28,12 +28,9 @@
#include <unordered_map>
// ArmarX
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-
#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>
+#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
// Aron
#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
@@ -41,6 +38,7 @@
namespace armarx::armem::server::robot_state::description
{
+
class Segment : public segment::wm::AronTypedCoreSegmentBase<arondto::RobotDescription>
{
using Base = segment::wm::AronTypedCoreSegmentBase<arondto::RobotDescription>;
@@ -54,9 +52,6 @@ namespace armarx::armem::server::robot_state::description
void onConnect(const RobotUnitInterfacePrx& robotUnitPrx);
- /// 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;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/forward_declarations.h b/source/RobotAPI/libraries/armem_robot_state/server/forward_declarations.h
new file mode 100644
index 0000000000000000000000000000000000000000..e730e29f1d5c8e83f38701a2e26cfc7a55b23039
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/forward_declarations.h
@@ -0,0 +1,80 @@
+/*
+ * 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 Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <map>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include <Eigen/Geometry>
+
+
+
+namespace armarx::armem::arondto
+{
+ struct Transform;
+ struct TransformHeader;
+
+ struct JointState;
+
+ class RobotDescription;
+}
+
+namespace armarx::armem::robot
+{
+ struct RobotDescription;
+ struct RobotState;
+ struct Robot;
+
+ using Robots = std::vector<Robot>;
+ using RobotDescriptions = std::vector<RobotDescription>;
+ using RobotStates = std::vector<RobotState>;
+}
+
+namespace armarx::armem::robot_state
+{
+ struct JointState;
+
+ struct Transform;
+ struct TransformHeader;
+}
+
+namespace armarx::armem::server::robot_state::description
+{
+ using RobotDescriptionMap = std::unordered_map<std::string, robot::RobotDescription>;
+ class Segment;
+}
+
+namespace armarx::armem::server::robot_state::localization
+{
+ using RobotPoseMap = std::unordered_map<std::string, Eigen::Affine3f>;
+ using RobotFramePoseMap = std::unordered_map<std::string, std::vector<Eigen::Affine3f>>;
+ class Segment;
+}
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+ using RobotJointPositionMap = std::unordered_map<std::string, std::map<std::string, float>>;
+ class Segment;
+}
+
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 ce82f0afad8872d6e660078ce9e2425ce824ad3b..1a73d571d47912809e8f5eb63b20cff5e9678020 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -2,37 +2,22 @@
// STL
#include <iterator>
-#include <sstream>
-// Ice
-#include <IceUtil/Time.h>
-
-// Eigen
-#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>
#include <RobotAPI/libraries/core/FramedPose.h>
-
#include <RobotAPI/libraries/aron/common/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/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_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
@@ -46,32 +31,44 @@ namespace armarx::armem::server::robot_state::localization
}
- Segment::~Segment() = default;
+ Segment::~Segment()
+ {
+ }
+
+
+ void Segment::onConnect()
+ {
+ }
- Segment::RobotFramePoseMap
+ RobotFramePoseMap
Segment::getRobotFramePosesLocking(const armem::Time& timestamp) const
{
- std::scoped_lock lock(mutex());
- return getRobotFramePoses(timestamp);
+ return this->doLocked([this, ×tamp]()
+ {
+ return getRobotFramePoses(timestamp);
+ });
}
- Segment::RobotFramePoseMap
+ RobotFramePoseMap
Segment::getRobotFramePoses(const armem::Time& timestamp) const
{
using common::robot_state::localization::TransformHelper;
using common::robot_state::localization::TransformQuery;
RobotFramePoseMap frames;
-
- const auto knownRobots = simox::alg::get_keys(*segment);
- for (const auto& robotName : knownRobots)
+ for (const std::string& robotName : segment->getProviderSegmentNames())
{
- TransformQuery query{.header{.parentFrame = GlobalFrame,
- .frame = "root", // TODO, FIXME
- .agent = robotName,
- .timestamp = timestamp}};
+ TransformQuery query
+ {
+ .header = {
+ .parentFrame = armarx::GlobalFrame,
+ .frame = "root", // TODO, FIXME
+ .agent = robotName,
+ .timestamp = timestamp
+ }
+ };
const auto result = TransformHelper::lookupTransformChain(*segment, query);
if (not result)
@@ -90,40 +87,44 @@ namespace armarx::armem::server::robot_state::localization
}
- Segment::RobotPoseMap
+ RobotPoseMap
Segment::getRobotGlobalPosesLocking(const armem::Time& timestamp) const
{
- std::scoped_lock lock(mutex());
- return getRobotGlobalPoses(timestamp);
+ return this->doLocked([this, ×tamp]()
+ {
+ return getRobotGlobalPoses(timestamp);
+ });
}
- Segment::RobotPoseMap
+ RobotPoseMap
Segment::getRobotGlobalPoses(const armem::Time& timestamp) const
{
using common::robot_state::localization::TransformHelper;
using common::robot_state::localization::TransformQuery;
RobotPoseMap robotGlobalPoses;
-
- const std::vector<std::string> knownRobots = simox::alg::get_keys(*segment);
-
- for (const std::string& robotName : knownRobots)
+ 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(*segment, query);
- if (not result)
+ TransformQuery query
+ {
+ .header =
+ {
+ .parentFrame = GlobalFrame,
+ .frame = "root", // TODO, FIXME
+ .agent = robotName,
+ .timestamp = timestamp
+ }
+ };
+
+ if (const auto result = TransformHelper::lookupTransform(*segment, query))
+ {
+ robotGlobalPoses.emplace(robotName, result.transform.transform);
+ }
+ else
{
// TODO
- continue;
}
-
- robotGlobalPoses.emplace(robotName, result.transform.transform);
}
ARMARX_INFO << deactivateSpam(60)
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 53d11236231f9e6e7017f4de984c5ad4da97e6c2..f0b228800b66426da38391ebf60c57d290926959 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
@@ -30,20 +30,13 @@
#include <Eigen/Geometry>
// ArmarX
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include <RobotAPI/libraries/armem/server/segment/Segment.h>
-#include <RobotAPI/libraries/armem_objects/types.h>
-#include <RobotAPI/libraries/armem_robot_state/types.h>
-// Aron
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
-namespace armarx::armem
-{
- class EntityUpdate;
-} // namespace armarx::armem
-
namespace armarx::armem::server::robot_state::localization
{
class Segment : public segment::wm::AronTypedCoreSegmentBase<arondto::Transform>
@@ -52,14 +45,12 @@ namespace armarx::armem::server::robot_state::localization
public:
- using RobotPoseMap = std::unordered_map<std::string, Eigen::Affine3f>;
- using RobotFramePoseMap = std::unordered_map<std::string, std::vector<Eigen::Affine3f>>;
+ Segment(server::MemoryToIceAdapter& iceMemory);
+ virtual ~Segment() override;
- public:
+ void onConnect();
- Segment(server::MemoryToIceAdapter& iceMemory);
- virtual ~Segment() override;
RobotPoseMap getRobotGlobalPoses(const armem::Time& timestamp) const;
RobotPoseMap getRobotGlobalPosesLocking(const armem::Time& timestamp) const;
@@ -67,13 +58,13 @@ namespace armarx::armem::server::robot_state::localization
RobotFramePoseMap getRobotFramePoses(const armem::Time& timestamp) const;
RobotFramePoseMap getRobotFramePosesLocking(const armem::Time& timestamp) const;
- bool commitTransform(const armarx::armem::robot_state::Transform& transform);
- bool commitTransformLocking(const armarx::armem::robot_state::Transform& transform);
+ bool commitTransform(const armem::robot_state::Transform& transform);
+ bool commitTransformLocking(const armem::robot_state::Transform& transform);
private:
- EntityUpdate makeUpdate(const armarx::armem::robot_state::Transform& transform) const;
+ EntityUpdate makeUpdate(const armem::robot_state::Transform& transform) const;
};
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
similarity index 50%
rename from source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.cpp
rename to source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
index 39439e2e77bbd8c3ebea44782ae44d62286c6ed9..c2ed636a93e6a1b8feb9331daf68812160a44ba8 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
@@ -39,20 +39,18 @@
#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
+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
- );
- }
+ // Thread-local copy of debug observer helper.
+ this->debugObserver =
+ DebugObserverHelper(Logging::tag.tagName, debugObserver.getDebugObserver(), true);
}
@@ -65,9 +63,12 @@ namespace armarx::armem::server::robot_state
}
- void RobotStateWriter::run(float pollFrequency,
- std::queue<RobotUnitData>& dataQueue, std::mutex& dataMutex,
- MemoryToIceAdapter& memory, localization::Segment& localizationSegment)
+ 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())
@@ -86,34 +87,42 @@ namespace armarx::armem::server::robot_state
{
debugObserver->setDebugObserverDatafield("RobotStateWriter | Queue Size", queueSize);
}
- if (batch.size() > 0)
+ if (not batch.empty())
{
- Update update = buildUpdate(batch);
-
auto start = std::chrono::high_resolution_clock::now();
- auto endProprioception = start, endLocalization = start;
+ auto endBuildUpdate = start, endProprioception = start, endLocalization = start;
- armem::CommitResult result;
- {
- // Commits lock the core segments.
+ Update update = buildUpdate(batch);
+ endBuildUpdate = std::chrono::high_resolution_clock::now();
+
+ // Commits lock the core segments.
- result = memory.commitLocking(update.proprioception);
- endProprioception = std::chrono::high_resolution_clock::now();
+ // Proprioception
+ armem::CommitResult result = memory.commitLocking(update.proprioception);
+ endProprioception = std::chrono::high_resolution_clock::now();
- localizationSegment.commitTransformLocking(update.localization);
- endLocalization = std::chrono::high_resolution_clock::now();
+ // Localization
+ for (const armem::robot_state::Transform& transform : update.localization)
+ {
+ localizationSegment.doLocked([&localizationSegment, &transform]()
+ {
+ localizationSegment.commitTransform(transform);
+ });
}
+ endLocalization = std::chrono::high_resolution_clock::now();
+
if (not result.allSuccess())
{
- ARMARX_WARNING << "Could not add data to memory. Error message: " << result.allErrorMessages();
+ ARMARX_WARNING << "Could not commit 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));
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit (ms)", toDurationMs(start, end));
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit 1. Build Update (ms)", toDurationMs(start, endBuildUpdate));
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit 2. Proprioception (ms)", toDurationMs(endBuildUpdate, endProprioception));
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit 3. Localization (ms)", toDurationMs(endProprioception, endLocalization));
}
}
@@ -126,73 +135,73 @@ namespace armarx::armem::server::robot_state
}
- RobotStateWriter::Update RobotStateWriter::buildUpdate(std::queue<RobotUnitData> dataQueue)
+ 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& convertedAndGroupedData = dataQueue.front();
+ const RobotUnitData& data = dataQueue.front();
- for (const auto& [encName, encTimestep] : convertedAndGroupedData.groups)
{
- ARMARX_CHECK_EQUAL(encName, encTimestep.name);
-
armem::EntityUpdate& up = update.proprioception.add();
- up.entityID = properties.robotUnitProviderID.withEntityName(encName);
- up.timeCreated = encTimestep.timestamp;
- up.instancesData = { encTimestep.data };
+ up.entityID = properties.robotUnitProviderID.withEntityName(properties.robotUnitProviderID.providerSegmentName);
+ up.timeCreated = data.timestamp;
+ up.instancesData = { data.proprioception };
}
- // odometry pose -> localization segment
- auto it = convertedAndGroupedData.groups.find(properties.platformGroupName);
- if (it != convertedAndGroupedData.groups.end())
+ // Extract odometry data.
+ const std::string platformKey = "platform";
+ if (data.proprioception->hasElement(platformKey))
{
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);
-
- 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 = it->second.timestamp;
- transform.transform = odometryPose;
+ auto platformData = aron::datanavigator::DictNavigator::DynamicCastAndCheck(data.proprioception->getElement(platformKey));
+ update.localization.emplace_back(getTransform(platformData, data.timestamp));
}
- else if (!noOdometryDataLogged)
+ else
{
- ARMARX_INFO << "No odometry data! (No group with name '" << properties.platformGroupName << "'.). \n"
- << "If you are using a mobile platform this should not have happened.";
+ ARMARX_INFO << "No odometry data! "
+ << "(No element '" << platformKey << "' in proprioception data.)"
+ << "\nIf you are using a mobile platform this should not have happened."
+ << "\nThis error is only logged once."
+ << "\nThese keys exist: " << data.proprioception->getAllKeys()
+ ;
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;
}
+
+ armem::robot_state::Transform
+ RobotStateWriter::getTransform(
+ const aron::datanavigator::DictNavigatorPtr& platformData,
+ const Time& timestamp) const
+ {
+ prop::arondto::Platform platform;
+ platform.fromAron(platformData);
+
+ 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;
+ transform.header.parentFrame = armarx::OdometryFrame;
+ transform.header.frame = "root"; // TODO: robot root node
+ transform.header.agent = properties.robotUnitProviderID.providerSegmentName;
+ transform.header.timestamp = timestamp;
+ transform.transform = odometryPose;
+
+ return transform;
+ }
+
}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
similarity index 86%
rename from source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.h
rename to source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
index 93ba74d2d9d622f4d72d1d5a4d08acd97f3eb64b..10c5a8b9e4c52c2a45198d62f7661cda042ba8a4 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
@@ -48,9 +48,8 @@ namespace armarx::armem::server::robot_state::localization
{
class Segment;
}
-namespace armarx::armem::server::robot_state
+namespace armarx::armem::server::robot_state::proprioception
{
-
class RobotStateWriter : public armarx::Logging
{
public:
@@ -68,9 +67,17 @@ namespace armarx::armem::server::robot_state
struct Update
{
armem::Commit proprioception;
- armem::robot_state::Transform localization;
+ std::vector<armem::robot_state::Transform> localization;
};
- Update buildUpdate(std::queue<RobotUnitData> dataQueue);
+ Update buildUpdate(std::queue<RobotUnitData>& dataQueue);
+
+
+ private:
+
+ armem::robot_state::Transform
+ getTransform(
+ const aron::datanavigator::DictNavigatorPtr& platformData,
+ const Time& timestamp) const;
public:
@@ -79,7 +86,6 @@ namespace armarx::armem::server::robot_state
{
unsigned int memoryBatchSize = 50;
armem::MemoryID robotUnitProviderID;
- std::string platformGroupName = "sens.Platform";
};
Properties properties;
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitData.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.cpp
similarity index 63%
rename from source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitData.cpp
rename to source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.cpp
index 9a9b72e2a74203cb9f44dd54796b397770257ecd..4262bf76e634ff03445a7460cd4fd55a4df861d9 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitData.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.cpp
@@ -3,7 +3,7 @@
#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
-namespace armarx::armem::server::robot_state
+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..4fb42da5e5fa804fad70b3978766323a487ad1fb
--- /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: " << 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/components/armem/server/RobotStateMemory/RobotUnitReader.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
similarity index 70%
rename from source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.h
rename to source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
index a44c9c0f19a586f48b3f899ae314ca70cc223abe..efb7a0d8175de36ebf963fd035d176442df822f2 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
@@ -10,11 +10,12 @@
#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
@@ -27,29 +28,28 @@ namespace armarx
using RobotUnitDataStreamingReceiverPtr = std::shared_ptr<class RobotUnitDataStreamingReceiver>;
}
-namespace armarx::armem::server::robot_state
+namespace armarx::armem::server::robot_state::proprioception
{
-
class RobotUnitReader : public armarx::Logging
{
public:
- static std::map<std::string, std::string> readConfig(const std::string& configPath);
+ RobotUnitReader();
- public:
-
- void connect(armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
- armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin);
+ void connect(
+ armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
+ armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
+ const std::string& robotTypeName);
- /// Reads data from `handler` and fills `robotUnitDataQueue`.
+ /// Reads data from `handler` and fills `dataQueue`.
void run(float pollFrequency,
std::queue<RobotUnitData>& dataQueue,
std::mutex& dataMutex);
- std::optional<RobotUnitData> fetchAndGroupLatestRobotUnitData();
+ std::optional<RobotUnitData> fetchAndConvertLatestRobotUnitData();
private:
@@ -73,8 +73,8 @@ namespace armarx::armem::server::robot_state
RobotUnitDataStreamingReceiverPtr receiver;
RobotUnitDataStreaming::DataStreamingDescription description;
- /// RobotUnit name -> group name.
- std::map<std::string, std::string> configSensorMapping;
+ 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 de880c85df95dd8e0238defcce0a1831ddbbc0c9..6321dd355bb6defc1f7bb19fbbdebcf77976aba2 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -1,36 +1,22 @@
#include "Segment.h"
-#include <sstream>
-
#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>
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
+#include <ArmarXCore/observers/ObserverObjectFactories.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/aron/core/navigator/data/container/Dict.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/aron/Robot.aron.generated.h>
-
-#include <RobotAPI/libraries/armem_objects/aron_conversions.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) :
- Base(memoryToIceAdapter, "Proprioception", nullptr, 1024)
+ Base(memoryToIceAdapter, "Proprioception", arondto::Proprioception::toAronType(), 1024)
{
}
@@ -39,11 +25,10 @@ namespace armarx::armem::server::robot_state::proprioception
void Segment::onConnect(RobotUnitInterfacePrx robotUnitPrx)
{
- Base::onConnect();
-
this->robotUnit = robotUnitPrx;
std::string providerSegmentName = "Robot";
+
KinematicUnitInterfacePrx kinematicUnit = robotUnit->getKinematicUnit();
if (kinematicUnit)
{
@@ -59,22 +44,31 @@ namespace armarx::armem::server::robot_state::proprioception
- Segment::RobotJointPositionMap Segment::getRobotJointPositionsLocking(
+ 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)
+ return doLocked([this, ×tamp, &debugObserver]()
+ {
+ return getRobotJointPositions(timestamp, debugObserver);
+ });
+ }
+
+
+ static
+ aron::datanavigator::DictNavigatorPtr
+ getDictElement(const aron::datanavigator::DictNavigator& dict, const std::string& key)
+ {
+ if (dict.hasElement(key))
{
- debugObserver->setDebugObserverDatafield(dp + "t 0 Lock Core Segment (ms)", tRobotJointPositionsLock.toMilliSecondsDouble());
+ return aron::datanavigator::DictNavigator::DynamicCastAndCheck(dict.getElement(key));
}
- return getRobotJointPositions(timestamp, debugObserver);
+ return nullptr;
}
- Segment::RobotJointPositionMap Segment::getRobotJointPositions(
+ RobotJointPositionMap
+ Segment::getRobotJointPositions(
const armem::Time& timestamp,
DebugObserverHelper* debugObserver) const
{
@@ -82,76 +76,48 @@ namespace armarx::armem::server::robot_state::proprioception
ARMARX_CHECK_NOT_NULL(segment);
RobotJointPositionMap jointMap;
+ int i = 0;
- TIMING_START(tProcessProviders);
- for (const auto& [robotName, provSeg] : *segment)
+ Duration tFindData = Duration::milliSeconds(0), tReadJointPositions = Duration::milliSeconds(0);
+ TIMING_START(tProcessEntities)
+ segment->forEachEntity([&](const wm::Entity & entity)
{
- TIMING_START(tProcessEntities);
- int i = 0;
- for (const auto& [name, entity] : provSeg.entities())
+ adn::DictNavigatorPtr data;
{
- TIMING_START(tProcessEntity);
-
- TIMING_START(tGetLatestInstance);
- if (entity.empty())
- {
- continue;
- }
- const wm::EntitySnapshot& snapshot = entity.getLatestSnapshot();
- if (snapshot.empty())
- {
- continue;
- }
- const wm::EntityInstance& entityInstance = snapshot.getInstance(0);
- TIMING_END_COMMENT_STREAM(tGetLatestInstance, "tGetLatestInstance " + std::to_string(i), ARMARX_DEBUG);
-
- TIMING_START(tCastJointState);
- aron::datanavigator::DictNavigatorPtr data = entityInstance.data();
- // Avoid throwing an exception.
- if (!(data->hasElement("name") && data->hasElement("position")))
- {
- continue;
- }
- const std::string jointName = adn::StringNavigator::DynamicCastAndCheck(data->getElement("name"))->getValue();
- const float jointPosition = adn::FloatNavigator::DynamicCastAndCheck(data->getElement("position"))->getValue();
- TIMING_END_COMMENT_STREAM(tCastJointState, "tCastJointState " + std::to_string(i), ARMARX_DEBUG);
+ TIMING_START(_tFindData)
- TIMING_START(tEmplace);
- if (not jointName.empty())
+ const wm::EntitySnapshot* snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
+ if (not snapshot)
{
- jointMap[robotName].emplace(jointName, jointPosition);
+ // Got no snapshot <= timestamp. Take latest instead (if present).
+ snapshot = entity.findLatestSnapshot();
}
- else
+ if (snapshot)
{
- // This may happen, just ignore it.
- // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+ data = snapshot->findInstanceData();
}
- TIMING_END_COMMENT_STREAM(tEmplace, "tEmplace " + 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 CastJointState (ms)", tCastJointState.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(dp + "t 1.1.1.3 Emplace (ms)", tEmplace.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(dp + "t 1.1.1.T Process Entity (ms)", tProcessEntity.toMilliSecondsDouble());
- }
- ++i;
+ TIMING_END_COMMENT_STREAM(_tFindData, "tFindData " + std::to_string(i), ARMARX_DEBUG)
+ tFindData += _tFindData;
}
- TIMING_END_STREAM(tProcessEntities, ARMARX_DEBUG);
- if (debugObserver)
+ if (data)
{
- debugObserver->setDebugObserverDatafield(dp + "t 1.1 Process Entities (ms)", tProcessEntities.toMilliSecondsDouble());
- }
- }
- TIMING_END_STREAM(tProcessProviders, ARMARX_DEBUG);
+ TIMING_START(_tReadJointPositions)
- ARMARX_INFO << deactivateSpam(60) << "Number of known robot joint maps: " << jointMap.size();
+ jointMap.emplace(entity.id().providerSegmentName, readJointPositions(*data));
+
+ TIMING_END_COMMENT_STREAM(_tReadJointPositions, "tReadJointPositions " + std::to_string(i), ARMARX_DEBUG)
+ tReadJointPositions += _tReadJointPositions;
+ }
+ ++i;
+ });
+ TIMING_END_STREAM(tProcessEntities, ARMARX_DEBUG)
if (debugObserver)
{
- debugObserver->setDebugObserverDatafield(dp + "t 1 Process Providers (ms)", tProcessProviders.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1 Process Entities (ms)", tProcessEntities.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1.1 FindData (ms)", tFindData.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1.2 ReadJointPositions (ms)", tReadJointPositions.toMilliSecondsDouble());
}
return jointMap;
@@ -163,4 +129,26 @@ namespace armarx::armem::server::robot_state::proprioception
return robotUnitProviderID;
}
+
+ std::map<std::string, float>
+ Segment::readJointPositions(const wm::EntityInstanceData& data)
+ {
+ namespace adn = aron::datanavigator;
+
+ // Just get what we need without casting the whole data.
+ std::map<std::string, float> jointPositions;
+ if (adn::DictNavigatorPtr joints = getDictElement(data, "joints"))
+ {
+ if (adn::DictNavigatorPtr jointsPosition = getDictElement(*joints, "position"))
+ {
+ for (const auto& [name, value] : jointsPosition->getElements())
+ {
+ const float jointPosition = adn::FloatNavigator::DynamicCastAndCheck(value)->getValue();
+ jointPositions[name] = jointPosition;
+ }
+ }
+ }
+ return jointPositions;
+ }
+
} // 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 cca0b35a0e179a15880bc38a08a1f1f20ce9f42f..e27984bba26136defa5e2ef2b940790abe44f3f7 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
@@ -14,6 +14,7 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
* @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
@@ -22,20 +23,18 @@
#pragma once
// STD / STL
-#include <string>
+#include <map>
#include <optional>
-#include <unordered_map>
+#include <string>
// ArmarX
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-#include <RobotAPI/components/ArViz/Client/Client.h>
+// RobotAPI
#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/server/forward_declarations.h>
-// Aron
-#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
namespace armarx
{
@@ -47,10 +46,6 @@ namespace armarx::armem::server::robot_state::proprioception
{
using Base = segment::wm::CoreSegmentBase;
- public:
-
- using RobotJointPositionMap = std::unordered_map<std::string, std::map<std::string, float>>;
-
public:
Segment(server::MemoryToIceAdapter& iceMemory);
@@ -67,6 +62,13 @@ namespace armarx::armem::server::robot_state::proprioception
const armem::MemoryID& getRobotUnitProviderID() const;
+ private:
+
+ static
+ std::map<std::string, float>
+ readJointPositions(const wm::EntityInstanceData& data);
+
+
private:
RobotUnitInterfacePrx robotUnit;
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.cpp
similarity index 100%
rename from source/RobotAPI/components/armem/server/RobotStateMemory/aron_conversions.cpp
rename to source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.cpp
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h
similarity index 100%
rename from source/RobotAPI/components/armem/server/RobotStateMemory/aron_conversions.h
rename to source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h
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..cb1192954cf52fedeec2040e161c72fae15479c6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp
@@ -0,0 +1,212 @@
+#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 <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.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 (not 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:
+ ARMARX_DEBUG << "Cannot handle extra field '" << key << "' of type "
+ << RobotUnitDataStreaming::DataEntryNames.to_name(value.entry.type);
+ 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..03c0a05d0387cae6faba1a0a779ba874a3006730
--- /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_robot_state/types.h b/source/RobotAPI/libraries/armem_robot_state/types.h
index 39daba5d2d7db0148cb9df264d8da2acac54d036..b6d2b67dec2b28425e7b3dd4b0630fc39df02017 100644
--- a/source/RobotAPI/libraries/armem_robot_state/types.h
+++ b/source/RobotAPI/libraries/armem_robot_state/types.h
@@ -23,7 +23,8 @@
#include <Eigen/Geometry>
-#include "RobotAPI/libraries/armem/core/Time.h"
+#include <RobotAPI/libraries/armem/core/Time.h>
+
namespace armarx::armem::robot_state
{
@@ -34,7 +35,7 @@ namespace armarx::armem::robot_state
std::string agent;
- armem::Time timestamp;
+ armem::Time timestamp;
};
struct Transform
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/converter/CMakeLists.txt b/source/RobotAPI/libraries/aron/converter/CMakeLists.txt
index 5f8db702e460cbb2e729862458314920823be780..e93d191b2f99b0eccb973a925a77ebdf674ffaee 100644
--- a/source/RobotAPI/libraries/aron/converter/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/converter/CMakeLists.txt
@@ -3,6 +3,7 @@ add_subdirectory(ivt)
add_subdirectory(pcl)
add_subdirectory(eigen)
add_subdirectory(opencv)
+add_subdirectory(json)
add_library(AronConverter INTERFACE)
@@ -13,7 +14,8 @@ target_link_libraries(AronConverter
RobotAPI::aron::converter::ivt
RobotAPI::aron::converter::pcl
RobotAPI::aron::converter::eigen
- RobotAPI::aron::converter::opencv
+ RobotAPI::aron::converter::opencv
+ RobotAPI::aron::converter::json
)
add_library(aron::converter ALIAS AronConverter)
diff --git a/source/RobotAPI/libraries/aron/converter/json/CMakeLists.txt b/source/RobotAPI/libraries/aron/converter/json/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8f5c8006f3b16d7343a2f74b1c6b6562de426165
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/converter/json/CMakeLists.txt
@@ -0,0 +1,24 @@
+set(LIB_NAME aronjsonconverter)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+find_package(IVT COMPONENTS ivt ivtopencv QUIET)
+armarx_build_if(IVT_FOUND "IVT not available")
+
+set(LIBS
+ aron
+)
+
+set(LIB_FILES
+ NLohmannJSONConverter.cpp
+)
+
+set(LIB_HEADERS
+ NLohmannJSONConverter.h
+)
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+
+add_library(RobotAPI::aron::converter::json ALIAS aronjsonconverter)
diff --git a/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3eb6f82327e13ed89629e5de76a41909a9f0f9e2
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp
@@ -0,0 +1,42 @@
+#include "NLohmannJSONConverter.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::aron::converter
+{
+ nlohmann::json AronNlohmannJSONConverter::ConvertToNlohmannJSON(const datanavigator::DictNavigatorPtr& aron)
+ {
+ nlohmann::json j;
+ ConvertToNlohmannJSON(aron, j);
+ return j;
+ }
+
+ void AronNlohmannJSONConverter::ConvertToNlohmannJSON(const aron::datanavigator::DictNavigatorPtr& aron, nlohmann::json& j)
+ {
+ aron::dataIO::writer::NlohmannJSONWriter dataWriter;
+ aron::dataIO::Visitor::VisitAndSetup(dataWriter, aron);
+ j = dataWriter.getResult();
+ }
+
+
+
+ datanavigator::DictNavigatorPtr AronNlohmannJSONConverter::ConvertFromNlohmannJSON(const nlohmann::json& j)
+ {
+ auto aron = std::make_shared<aron::datanavigator::DictNavigator>();
+ ConvertFromNlohmannJSON(aron, j);
+ return aron;
+ }
+
+ void AronNlohmannJSONConverter::ConvertFromNlohmannJSON(aron::datanavigator::DictNavigatorPtr& a, const nlohmann::json& e, const aron::typenavigator::NavigatorPtr& expectedStructure)
+ {
+ aron::dataIO::reader::NlohmannJSONReader dataReader(e);
+ aron::dataIO::writer::NavigatorWriter navWriter;
+ aron::dataIO::Converter::ReadAndConvert(dataReader, navWriter, expectedStructure);
+ a = aron::datanavigator::DictNavigator::DynamicCastAndCheck(navWriter.getResult());
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h
new file mode 100644
index 0000000000000000000000000000000000000000..2e1a89c63511d9806951887a7461fa09dcb17da4
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h
@@ -0,0 +1,29 @@
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <string>
+#include <numeric>
+
+// Memory
+#include <RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h>
+
+// JSON
+#include <SimoxUtility/json/json.hpp>
+
+namespace armarx::aron::converter
+{
+ class AronNlohmannJSONConverter
+ {
+
+ public:
+ AronNlohmannJSONConverter() = delete;
+
+ static nlohmann::json ConvertToNlohmannJSON(const datanavigator::DictNavigatorPtr&);
+ static void ConvertToNlohmannJSON(const datanavigator::DictNavigatorPtr&, nlohmann::json&);
+
+ static datanavigator::DictNavigatorPtr ConvertFromNlohmannJSON(const nlohmann::json&);
+ static void ConvertFromNlohmannJSON(datanavigator::DictNavigatorPtr&, const nlohmann::json&, const aron::typenavigator::NavigatorPtr& = nullptr);
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/CMakeLists.txt b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
index 62548c29719f3942afee49a45e9a3f24a4232634..6fb2477169dd25daf0c77564e49d2e737825ef7c 100644
--- a/source/RobotAPI/libraries/aron/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
@@ -45,6 +45,7 @@ set(LIB_FILES
navigator/type/ndarray/NDArray.cpp
navigator/type/ndarray/EigenMatrix.cpp
navigator/type/ndarray/EigenQuaternion.cpp
+ navigator/type/ndarray/Image.cpp
navigator/type/ndarray/IVTCByteImage.cpp
navigator/type/ndarray/OpenCVMat.cpp
navigator/type/ndarray/PCLPointCloud.cpp
@@ -102,6 +103,7 @@ set(LIB_FILES
codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.cpp
codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.cpp
codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.cpp
+ codegenerator/codeWriter/cpp/serializer/ndarray/Image.cpp
codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.cpp
codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.cpp
codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
@@ -151,6 +153,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
@@ -166,6 +169,7 @@ set(LIB_HEADERS
navigator/type/ndarray/EigenMatrix.h
navigator/type/ndarray/EigenQuaternion.h
navigator/type/ndarray/IVTCByteImage.h
+ navigator/type/ndarray/Image.h
navigator/type/ndarray/OpenCVMat.h
navigator/type/ndarray/PCLPointCloud.h
navigator/type/ndarray/Position.h
@@ -181,6 +185,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
@@ -246,6 +251,7 @@ set(LIB_HEADERS
codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.h
codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.h
codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.h
+ codegenerator/codeWriter/cpp/serializer/ndarray/Image.h
codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.h
codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.h
codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.h
diff --git a/source/RobotAPI/libraries/aron/core/Config.h b/source/RobotAPI/libraries/aron/core/Config.h
index 7db99c2f1aaa298eb50f076a371677408dac113e..a8c7d0fb044619530635da4c3cb709f579fe7757 100644
--- a/source/RobotAPI/libraries/aron/core/Config.h
+++ b/source/RobotAPI/libraries/aron/core/Config.h
@@ -89,6 +89,7 @@ namespace armarx
RUN_ARON_MACRO(NDArray, ndarray, NDARRAY) \
RUN_ARON_MACRO(EigenMatrix, eigenmatrix, EIGEN_MATRIX) \
RUN_ARON_MACRO(EigenQuaternion, eigenquaternion, EIGEN_QUATERNION) \
+ RUN_ARON_MACRO(Image, image, IMAGE) \
RUN_ARON_MACRO(IVTCByteImage, ivtcbyteimage, IVT_CBYTE_IMAGE) \
RUN_ARON_MACRO(OpenCVMat, opencvmat, OPENCV_MAT) \
RUN_ARON_MACRO(PCLPointCloud, pclpointcloud, PCL_POINTCLOUD) \
@@ -168,6 +169,7 @@ namespace armarx
RUN_ARON_MACRO(NDArray, ndarray, NDARRAY, NDArray, ndarray, NDARRAY) \
RUN_ARON_MACRO(EigenMatrix, eigenmatrix, EIGEN_MATRIX, NDArray, ndarray, NDARRAY) \
RUN_ARON_MACRO(EigenQuaternion, eigenmatrix, EIGEN_MATRIX, NDArray, ndarray, NDARRAY) \
+ RUN_ARON_MACRO(Image, image, IMAGE, NDArray, ndarray, NDARRAY) \
RUN_ARON_MACRO(IVTCByteImage, ivtcbyteimage, IVT_CBYTE_IMAGE, NDArray, ndarray, NDARRAY) \
RUN_ARON_MACRO(OpenCVMat, opencvmat, OPENCV_MAT, NDArray, ndarray, NDARRAY) \
RUN_ARON_MACRO(PCLPointCloud, pclpointcloud, PCL_POINTCLOUD, NDArray, ndarray, NDARRAY) \
diff --git a/source/RobotAPI/libraries/aron/core/Randomizer.h b/source/RobotAPI/libraries/aron/core/Randomizer.h
index 00f355baa9c01e66792a886b027ad4186b808016..170f4b2b8b1a471a09ff0e4cf9b51a05c1470fe7 100644
--- a/source/RobotAPI/libraries/aron/core/Randomizer.h
+++ b/source/RobotAPI/libraries/aron/core/Randomizer.h
@@ -24,8 +24,10 @@
#pragma once
// STD/STL
+#include <map>
#include <memory>
#include <numeric>
+#include <set>
// ArmarX
#include <RobotAPI/libraries/aron/core/Exception.h>
@@ -152,6 +154,12 @@ namespace armarx::aron
t->setTypename(type);
return t;
}
+ case type::Descriptor::eImage:
+ {
+ auto t = std::make_shared<typenavigator::ImageNavigator>();
+ auto type = getRandomKey(typenavigator::ImageNavigator::pixelTypeNames());
+ return t;
+ }
case type::Descriptor::eIVTCByteImage:
{
auto t = std::make_shared<typenavigator::IVTCByteImageNavigator>();
@@ -359,8 +367,8 @@ case type::Descriptor::e##upperType: \
return vec.at(i);
}
- template <class T>
- std::string getRandomKey(const std::map<std::string, T>& m) const
+ template <class ValueT>
+ std::string getRandomKey(const std::map<std::string, ValueT>& m) const
{
std::vector<std::string> keys;
for (const auto [k, _] : m)
@@ -370,6 +378,16 @@ case type::Descriptor::e##upperType: \
return getRandomElement(keys);
}
+ std::string getRandomKey(const std::set<std::string>& set) const
+ {
+ std::vector<std::string> keys;
+ for (const auto k : set)
+ {
+ keys.push_back(k);
+ }
+ return getRandomElement(keys);
+ }
+
bool fiftyPercentChance() const
{
return generateRandom(2, 0);
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h
index 790bb1e62538da3cdd0c25908118514f9be3cddd..30f5e54450020945bf1caec30e8be1c153fb17d9 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h
@@ -47,6 +47,7 @@ namespace armarx::aron::cppserializer
public:
AronCppClass() = default;
+ virtual ~AronCppClass() = default;
/// Reset all member values of this class to default (as stated in the XML). This may mean that maybe types are null or false and images may be created as headers_only
virtual void resetHard() = 0;
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/AllSerializers.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/AllSerializers.h
index dd01bd127edd43a72cdafa370d96b68d8ebd77fa..7e1934e88f17b309ce3117d4312557359c300c07 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/AllSerializers.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/AllSerializers.h
@@ -8,6 +8,7 @@
#include "ndarray/NDArray.h"
#include "ndarray/EigenMatrix.h"
#include "ndarray/EigenQuaternion.h"
+#include "ndarray/Image.h"
#include "ndarray/IVTCByteImage.h"
#include "ndarray/OpenCVMat.h"
#include "ndarray/PCLPointCloud.h"
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.cpp
index 5169a4fcc46d3142ce61df9d717cc03942f62781..7cc42a41503cf690be9fccab82acd41b342cfb7b 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.cpp
@@ -23,21 +23,22 @@
// STD/STL
-// Header
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h>
+#include "Serializer.h"
+#include "SerializerFactory.h"
+
+#include <SimoxUtility/meta/type_name.h>
+#include <SimoxUtility/algorithm/string.h>
-// ArmarX
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/SerializerFactory.h>
namespace armarx::aron::cppserializer
{
// constantes
- const std::string Serializer::ARON_DATA_NAME = simox::meta::get_type_name(typeid(armarx::aron::data::AronData));
+ const std::string Serializer::ARON_DATA_NAME = simox::meta::get_type_name<armarx::aron::data::AronData>();
const std::string Serializer::ARON_DATA_PTR_NAME = Serializer::ARON_DATA_NAME + "::PointerType";
const std::string Serializer::READ_START_RETURN_TYPE_ACCESSOR = "_return_type";
- const std::string Serializer::ARON_TYPE_NAME = simox::meta::get_type_name(typeid(armarx::aron::type::AronType));
+ const std::string Serializer::ARON_TYPE_NAME = simox::meta::get_type_name<armarx::aron::type::AronType>();
const std::string Serializer::ARON_TYPE_PTR_NAME = Serializer::ARON_TYPE_NAME + "::PointerType";
const std::map<std::string, std::string> Serializer::ESCAPE_ACCESSORS =
@@ -263,7 +264,7 @@ namespace armarx::aron::cppserializer
}
else
{
- b->addLine("// Ressetting soft the type " + ExtractCppTypename(typenavigator));
+ b->addLine("// Resetting soft the type " + ExtractCppTypename(typenavigator));
}
switch (typenavigator->getMaybe())
@@ -310,7 +311,7 @@ namespace armarx::aron::cppserializer
}
else
{
- b->addLine("// Ressetting hard the type " + ExtractCppTypename(typenavigator));
+ b->addLine("// Resetting hard the type " + ExtractCppTypename(typenavigator));
}
switch (typenavigator->getMaybe())
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h
index e7a544b8fb8342c0b83b0c27e4ecaf35e82ba933..0bee9f4be10ef71312e56e89122fc1ef100bca18 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h
@@ -23,27 +23,21 @@
#pragma once
-// STD/STL
-#include <memory>
-#include <vector>
-#include <map>
-#include <string>
-
-// Simox
-#include <SimoxUtility/meta/type_name.h>
-#include <SimoxUtility/algorithm/string.h>
-#include <SimoxUtility/algorithm/vector.hpp>
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
-// ArmarX
#include <ArmarXCore/libraries/cppgen/CppBlock.h>
#include <ArmarXCore/libraries/cppgen/CppField.h>
#include <ArmarXCore/libraries/cppgen/CppCtor.h>
#include <ArmarXCore/libraries/cppgen/CppMethod.h>
#include <ArmarXCore/libraries/cppgen/CppClass.h>
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
+#include <memory>
+#include <map>
+#include <string>
+#include <vector>
+
namespace armarx::aron::cppserializer
{
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.cpp
index f494c591c7828dbd4435a02336a03d52361620ad..4bb11ad7cafabe9d329217cf003c3b7aa82a61bd 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.cpp
@@ -24,14 +24,18 @@
// Header
#include "Dict.h"
+#include <SimoxUtility/meta/type_name.h>
+
namespace armarx::aron::cppserializer::serializer
{
// constructors
DictSerializer::DictSerializer(const typenavigator::DictNavigatorPtr& e) :
- detail::ContainerSerializerBase<typenavigator::DictNavigator, DictSerializer>("std::map<std::string, " + FromAronTypeNaviagtorPtr(e->getAcceptedType())->getFullCppTypename() + ">",
- simox::meta::get_type_name(typeid(data::AronDict)),
- simox::meta::get_type_name(typeid(type::AronDict)), e)
+ detail::ContainerSerializerBase<typenavigator::DictNavigator, DictSerializer>(
+ "std::map<std::string, " + FromAronTypeNaviagtorPtr(e->getAcceptedType())->getFullCppTypename() + ">",
+ simox::meta::get_type_name<data::AronDict>(),
+ simox::meta::get_type_name<type::AronDict>(),
+ e)
{
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.h
index 9bd48144aeb8cdf578ac4d8c491343f539e0bcc8..a8f4734f4f5c0f0050dd9758bb9fa6e36637a2a6 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.h
@@ -23,15 +23,8 @@
#pragma once
-// STD/STL
-#include <string>
-#include <map>
-
-// Base Class
-#include "../detail/ContainerSerializerBase.h"
-
-// ArmarX
-#include "../../../../../navigator/type/container/Dict.h"
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h>
namespace armarx::aron::cppserializer::serializer
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.cpp
index 8483320c75efeca4c8cf573358d04d0539fb36d4..53e8bbcb28809e8a077e0724b05d26469040e875 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.cpp
@@ -24,13 +24,18 @@
// Header
#include "List.h"
+#include <SimoxUtility/meta/type_name.h>
+
+
namespace armarx::aron::cppserializer::serializer
{
// constructors
ListSerializer::ListSerializer(const typenavigator::ListNavigatorPtr& e) :
- detail::ContainerSerializerBase<typenavigator::ListNavigator, ListSerializer>("std::vector<" + FromAronTypeNaviagtorPtr(e->getAcceptedType())->getFullCppTypename() + ">",
- simox::meta::get_type_name(typeid(data::AronList)),
- simox::meta::get_type_name(typeid(type::AronList)), e)
+ detail::ContainerSerializerBase<typenavigator::ListNavigator, ListSerializer>(
+ "std::vector<" + FromAronTypeNaviagtorPtr(e->getAcceptedType())->getFullCppTypename() + ">",
+ simox::meta::get_type_name<data::AronList>(),
+ simox::meta::get_type_name<type::AronList>(),
+ e)
{
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.h
index 6b8afaabeef27186acee30d0167be5f1632d1ad7..6358a08ce649a5d482d9906af861d1e5bb8841a5 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.h
@@ -23,15 +23,9 @@
#pragma once
-// STD/STL
-#include <string>
-#include <vector>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/List.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h>
-// Base Class
-#include "../detail/ContainerSerializerBase.h"
-
-// ArmarX
-#include "../../../../../navigator/type/container/List.h"
namespace armarx::aron::cppserializer::serializer
{
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.cpp
index 49e06676e286dc773da88d6c12ab50062e2f1eb5..8b2bea70b9cbf9506e59fd3c56f04044400db9c7 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.cpp
@@ -24,14 +24,18 @@
// Header
#include "Object.h"
+#include <SimoxUtility/meta/type_name.h>
+
namespace armarx::aron::cppserializer::serializer
{
// constructors
ObjectSerializer::ObjectSerializer(const typenavigator::ObjectNavigatorPtr& e) :
- detail::ContainerSerializerBase<typenavigator::ObjectNavigator, ObjectSerializer>(e->getObjectName(),
- simox::meta::get_type_name(typeid(data::AronDict)),
- simox::meta::get_type_name(typeid(type::AronObject)), e)
+ detail::ContainerSerializerBase<typenavigator::ObjectNavigator, ObjectSerializer>(
+ e->getObjectName(),
+ simox::meta::get_type_name<data::AronDict>(),
+ simox::meta::get_type_name<type::AronObject>(),
+ e)
{
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.h
index d04a44557a19b5337f8678b788c68361a7b0ff0a..222e37c166b790cc3092a2c8a4fc04f6cd5ec892 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.h
@@ -23,15 +23,8 @@
#pragma once
-// STD/STL
-#include <string>
-#include <map>
-
-// Base Class
-#include "../detail/ContainerSerializerBase.h"
-
-// ArmarX
-#include "../../../../../navigator/type/container/Object.h"
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
namespace armarx::aron::cppserializer::serializer
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.cpp
index c465862acc74bac36c8f9f8e1c1cbf3b341890e3..ab4013664318c76526ecbebec1ac4c90ade479a1 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.cpp
@@ -24,13 +24,17 @@
// Header
#include "Pair.h"
+#include <SimoxUtility/meta/type_name.h>
+
namespace armarx::aron::cppserializer::serializer
{
PairSerializer::PairSerializer(const typenavigator::PairNavigatorPtr& e) :
- detail::ContainerSerializerBase<typenavigator::PairNavigator, PairSerializer>("std::pair<" + ExtractCppTypename(e->getFirstAcceptedType()) + ", " + ExtractCppTypename(e->getSecondAcceptedType()) + ">",
- simox::meta::get_type_name(typeid(data::AronList)),
- simox::meta::get_type_name(typeid(type::AronPair)), e)
+ detail::ContainerSerializerBase<typenavigator::PairNavigator, PairSerializer>(
+ "std::pair<" + ExtractCppTypename(e->getFirstAcceptedType()) + ", " + ExtractCppTypename(e->getSecondAcceptedType()) + ">",
+ simox::meta::get_type_name<data::AronList>(),
+ simox::meta::get_type_name<type::AronPair>(),
+ e)
{
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.h
index a7ef2495bcd4939820808a1298dcda92d1c9e12c..fab1eb592ceb5c0f1b373a3170f0908953545be4 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.h
@@ -23,15 +23,9 @@
#pragma once
-// STD/STL
-#include <string>
-#include <map>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Pair.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h>
-// Base Class
-#include "../detail/ContainerSerializerBase.h"
-
-// ArmarX
-#include "../../../../../navigator/type/container/Pair.h"
namespace armarx::aron::cppserializer::serializer
{
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.cpp
index a6b5e2080e97d0514cbb4bbda9131adb68a3a860..188ff3a449994c71109d1fd7f1e213c78c6189f8 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.cpp
@@ -24,13 +24,17 @@
// Header
#include "Tuple.h"
+#include <SimoxUtility/meta/type_name.h>
+
namespace armarx::aron::cppserializer::serializer
{
TupleSerializer::TupleSerializer(const typenavigator::TupleNavigatorPtr& e) :
- detail::ContainerSerializerBase<typenavigator::TupleNavigator, TupleSerializer>("std::tuple<" + simox::alg::join(ExtractCppTypenames(e->getAcceptedTypes()), ", ") + ">",
- simox::meta::get_type_name(typeid(data::AronList)),
- simox::meta::get_type_name(typeid(type::AronTuple)), e)
+ detail::ContainerSerializerBase<typenavigator::TupleNavigator, TupleSerializer>(
+ "std::tuple<" + simox::alg::join(ExtractCppTypenames(e->getAcceptedTypes()), ", ") + ">",
+ simox::meta::get_type_name<data::AronList>(),
+ simox::meta::get_type_name<type::AronTuple>(),
+ e)
{
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.h
index d8ec0bf428b98cc8290600b7556fc503d4acf5ee..e8039607eedb467608482459770e40a345d74157 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.h
@@ -23,15 +23,9 @@
#pragma once
-// STD/STL
-#include <string>
-#include <map>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Tuple.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h>
-// Base Class
-#include "../detail/ContainerSerializerBase.h"
-
-// ArmarX
-#include "../../../../../navigator/type/container/Tuple.h"
namespace armarx::aron::cppserializer::serializer
{
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h
index d2ca8f77efce232d34c15c7d9f7cd24513a46f53..baf4be57effcf3bff72b53ad4bf5a668ee471990 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h
@@ -23,15 +23,8 @@
#pragma once
-// STD/STL
-#include <memory>
-#include <string>
-#include <unordered_map>
-
-// Base class
#include "SerializerBase.h"
-// ArmarX
namespace armarx::aron::cppserializer::detail
{
@@ -40,9 +33,8 @@ namespace armarx::aron::cppserializer::detail
public SerializerBase<TypenavigatorT, DerivedT>
{
public:
- ContainerSerializerBase(const std::string& cppName, const std::string& aronDataTypename, const std::string& aronTypeTypename, const typename TypenavigatorT::PointerType& t) :
- SerializerBase<TypenavigatorT, DerivedT>(cppName, aronDataTypename, aronTypeTypename, t)
- {
- }
+
+ using SerializerBase<TypenavigatorT, DerivedT>::SerializerBase;
+
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h
index f0af4cfdd143da41629057e68a1291643b079eec..7acb76328abdc2be4ee0b08577e546900c4e3d46 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h
@@ -23,15 +23,8 @@
#pragma once
-// STD/STL
-#include <memory>
-#include <string>
-#include <unordered_map>
-
-// Base class
#include "SerializerBase.h"
-// ArmarX
namespace armarx::aron::cppserializer::detail
{
@@ -40,7 +33,12 @@ namespace armarx::aron::cppserializer::detail
public SerializerBase<TypenavigatorT, DerivedT>
{
public:
- NDArraySerializerBase(const std::string& cppName, const std::string& aronDataTypename, const std::string& aronTypeTypename, const typename TypenavigatorT::PointerType& t) :
+
+ NDArraySerializerBase(
+ const std::string& cppName,
+ const std::string& aronDataTypename,
+ const std::string& aronTypeTypename,
+ const typename TypenavigatorT::PointerType& t) :
SerializerBase<TypenavigatorT, DerivedT>(cppName, aronDataTypename, aronTypeTypename, t)
{}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h
index cf84eec049e341a83318dd3544fca0684e64236a..b20979f807c42981c0f9de6bf3106506424f9ea0 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h
@@ -23,15 +23,10 @@
#pragma once
-// STD/STL
-#include <memory>
-#include <string>
-#include <unordered_map>
-
-// Base class
#include "SerializerBase.h"
-// ArmarX
+#include <string>
+
namespace armarx::aron::cppserializer::detail
{
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.h
index 4b2dd5f8918bf0859b9e577d58dcd357a2fe7e37..92fde748539283fa35cb3ab858155d3e4f88d154 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.h
@@ -23,15 +23,11 @@
#pragma once
-// STD/STL
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h>
+
#include <memory>
#include <string>
-#include <unordered_map>
-
-// Base class
-#include "../Serializer.h"
-// ArmarX
namespace armarx::aron::cppserializer::detail
{
@@ -44,7 +40,11 @@ namespace armarx::aron::cppserializer::detail
using TypenavigatorType = TypenavigatorT;
public:
- SerializerBase(const std::string& cppName, const std::string& aronDataTypename, const std::string& aronTypeTypename, const typename TypenavigatorType::PointerType& t) :
+ SerializerBase(
+ const std::string& cppName,
+ const std::string& aronDataTypename,
+ const std::string& aronTypeTypename,
+ const typename TypenavigatorType::PointerType& t) :
Serializer(cppName, aronDataTypename, aronTypeTypename),
typenavigator(t)
{
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.cpp
index 2ae444db3f04f48865dc7e9842d5ae521e17cd0b..dc00ff9b0271b310b91be4656721eb6ed98039b1 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.cpp
@@ -24,13 +24,18 @@
// Header
#include "IntEnum.h"
+#include <SimoxUtility/meta/type_name.h>
+
+
namespace armarx::aron::cppserializer::serializer
{
// constructors
IntEnumSerializer::IntEnumSerializer(const typenavigator::IntEnumNavigatorPtr& e) :
- detail::SerializerBase<typenavigator::IntEnumNavigator, IntEnumSerializer>(e->getEnumName(),
- simox::meta::get_type_name(typeid(data::AronNDArray)),
- simox::meta::get_type_name(typeid(type::AronIntEnum)), e)
+ detail::SerializerBase<typenavigator::IntEnumNavigator, IntEnumSerializer>(
+ e->getEnumName(),
+ simox::meta::get_type_name<data::AronNDArray>(),
+ simox::meta::get_type_name<type::AronIntEnum>(),
+ e)
{
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.h
index af290bfe03f46cd5faa5cad4e218a6e3f5bd9e65..ce5feac09dcd1c2d802b176cb2d553d48adf7170 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.h
@@ -23,15 +23,11 @@
#pragma once
-// STD/STL
-#include <string>
-#include <map>
-
-// Base Class
-#include "../detail/SerializerBase.h"
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/enum/IntEnum.h>
-// ArmarX
-#include "../../../../../navigator/type/enum/IntEnum.h"
+#include <map>
+#include <string>
namespace armarx::aron::cppserializer::serializer
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.cpp
index c59e70d713528f0db11d4bf0d2212f3f3b53de77..54a4668819869c3305d79a98531cd8cdd348bf14 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.cpp
@@ -24,6 +24,9 @@
// Header
#include "EigenMatrix.h"
+#include <SimoxUtility/meta/type_name.h>
+
+
namespace armarx::aron::cppserializer::serializer
{
@@ -40,13 +43,27 @@ namespace armarx::aron::cppserializer::serializer
// constructors
EigenMatrixSerializer::EigenMatrixSerializer(const typenavigator::EigenMatrixNavigatorPtr& n) :
- detail::NDArraySerializerBase<typenavigator::EigenMatrixNavigator, EigenMatrixSerializer>("Eigen::Matrix<" + ACCEPTED_TYPES.at(n->getTypename()).first + ", " + std::to_string(n->getRows()) + ", " + std::to_string(n->getCols()) + ">",
- simox::meta::get_type_name(typeid(data::AronNDArray)),
- simox::meta::get_type_name(typeid(type::AronEigenMatrix)), n)
+ detail::NDArraySerializerBase<typenavigator::EigenMatrixNavigator, EigenMatrixSerializer>(
+ "Eigen::Matrix<" + ACCEPTED_TYPES.at(n->getTypename()).first + ", " + std::to_string(n->getRows()) + ", " + std::to_string(n->getCols()) + ">",
+ simox::meta::get_type_name<data::AronNDArray>(),
+ simox::meta::get_type_name<type::AronEigenMatrix>(),
+ n)
{
ARMARX_CHECK_NOT_NULL(typenavigator);
}
+ CppBlockPtr EigenMatrixSerializer::getResetHardBlock(const std::string& accessor) const
+ {
+ CppBlockPtr block = std::make_shared<CppBlock>();
+ block->addLine(accessor + ".setZero();");
+ return this->ResolveMaybeResetHardBlock(accessor, block, this->typenavigator);
+ }
+
+ CppBlockPtr EigenMatrixSerializer::getResetSoftBlock(const std::string& accessor) const
+ {
+ return getResetHardBlock(accessor);
+ }
+
CppBlockPtr EigenMatrixSerializer::getWriteTypeBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.h
index 13e435bfeed1161f6a349eae2507828b6a39a31c..1885efb76b1b53ab533d83fd4b071592fa040aeb 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.h
@@ -23,20 +23,14 @@
#pragma once
-// STD/STL
-#include <string>
-#include <map>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenMatrix.h>
-// Base Class
-#include "../detail/NDArraySerializerBase.h"
+#include <map>
-// ArmarX
-#include "../../../../../navigator/type/ndarray/EigenMatrix.h"
namespace armarx::aron::cppserializer::serializer
{
- class EigenMatrixSerializer;
- typedef std::shared_ptr<EigenMatrixSerializer> EigenMatrixSerializerPtr;
class EigenMatrixSerializer :
virtual public detail::NDArraySerializerBase<typenavigator::EigenMatrixNavigator, EigenMatrixSerializer>
@@ -46,6 +40,8 @@ namespace armarx::aron::cppserializer::serializer
EigenMatrixSerializer(const typenavigator::EigenMatrixNavigatorPtr&);
// virtual implementations
+ virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.cpp
index 6594fd20bc9d0915cfb95ed45a1fe821f44a58eb..41e3faf1bd3797cc721b27bfa49e0b70740c76b2 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.cpp
@@ -24,6 +24,9 @@
// Header
#include "EigenQuaternion.h"
+#include <SimoxUtility/meta/type_name.h>
+
+
namespace armarx::aron::cppserializer::serializer
{
@@ -35,13 +38,27 @@ namespace armarx::aron::cppserializer::serializer
// constructors
EigenQuaternionSerializer::EigenQuaternionSerializer(const typenavigator::EigenQuaternionNavigatorPtr& n) :
- detail::NDArraySerializerBase<typenavigator::EigenQuaternionNavigator, EigenQuaternionSerializer>("Eigen::Quaternion<" + ACCEPTED_TYPES.at(n->getTypename()).first + ">",
- simox::meta::get_type_name(typeid(data::AronNDArray)),
- simox::meta::get_type_name(typeid(type::AronEigenQuaternion)), n)
+ detail::NDArraySerializerBase<typenavigator::EigenQuaternionNavigator, EigenQuaternionSerializer>(
+ "Eigen::Quaternion<" + ACCEPTED_TYPES.at(n->getTypename()).first + ">",
+ simox::meta::get_type_name<data::AronNDArray>(),
+ simox::meta::get_type_name<type::AronEigenQuaternion>(),
+ n)
{
ARMARX_CHECK_NOT_NULL(typenavigator);
}
+ CppBlockPtr EigenQuaternionSerializer::getResetHardBlock(const std::string& accessor) const
+ {
+ CppBlockPtr block = std::make_shared<CppBlock>();
+ block->addLine(accessor + ".setIdentity();");
+ return this->ResolveMaybeResetHardBlock(accessor, block, this->typenavigator);
+ }
+
+ CppBlockPtr EigenQuaternionSerializer::getResetSoftBlock(const std::string& accessor) const
+ {
+ return getResetHardBlock(accessor);
+ }
+
CppBlockPtr EigenQuaternionSerializer::getWriteTypeBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.h
index 9a42c8cf67b3c67f9cb5664bd69a8ae2fc8d24dd..8f471d8f3e2570d51fac512d14b1b06bbbed692d 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.h
@@ -23,33 +23,29 @@
#pragma once
-// STD/STL
-#include <string>
-#include <map>
-// Base Class
-#include "../detail/NDArraySerializerBase.h"
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.h>
+
+#include <map>
-// ArmarX
-#include "../../../../../navigator/type/ndarray/EigenQuaternion.h"
namespace armarx::aron::cppserializer::serializer
{
- class EigenQuaternionSerializer;
- typedef std::shared_ptr<EigenQuaternionSerializer> EigenQuaternionSerializerPtr;
-
class EigenQuaternionSerializer :
virtual public detail::NDArraySerializerBase<typenavigator::EigenQuaternionNavigator, EigenQuaternionSerializer>
{
public:
- using PointerType = EigenQuaternionSerializerPtr;
+ using PointerType = std::shared_ptr<EigenQuaternionSerializer>;
public:
// constructors
EigenQuaternionSerializer(const typenavigator::EigenQuaternionNavigatorPtr&);
// virtual implementations
+ virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.cpp
index 4a986761bd57f9b7b2a2810fab8c19e5ffb43067..94e7bfe85af077cfaa9c69aa5b8a51869f7b5c60 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.cpp
@@ -24,6 +24,9 @@
// Header
#include "IVTCByteImage.h"
+#include <SimoxUtility/meta/type_name.h>
+
+
namespace armarx::aron::cppserializer::serializer
{
@@ -36,9 +39,11 @@ namespace armarx::aron::cppserializer::serializer
// constructors
IVTCByteImageSerializer::IVTCByteImageSerializer(const typenavigator::IVTCByteImageNavigatorPtr& n) :
- detail::NDArraySerializerBase<typenavigator::IVTCByteImageNavigator, IVTCByteImageSerializer>("CByteImage",
- simox::meta::get_type_name(typeid(data::AronNDArray)),
- simox::meta::get_type_name(typeid(type::AronIVTCByteImage)), n)
+ detail::NDArraySerializerBase<typenavigator::IVTCByteImageNavigator, IVTCByteImageSerializer>(
+ "CByteImage",
+ simox::meta::get_type_name<data::AronNDArray>(),
+ simox::meta::get_type_name<type::AronIVTCByteImage>(),
+ n)
{
ARMARX_CHECK_NOT_NULL(typenavigator);
if (typenavigator->getMaybe() == type::Maybe::eNone or typenavigator->getMaybe() == type::Maybe::eOptional)
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.h
index 1be69bdef6815d3c09f7d0cf16a06d2dda5cd81e..3e978ca5f69441e960e2b221f3a06779d0f1d7b7 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.h
@@ -23,15 +23,11 @@
#pragma once
-// STD/STL
-#include <string>
-#include <map>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/IVTCByteImage.h>
-// Base Class
-#include "../detail/NDArraySerializerBase.h"
+#include <map>
-// ArmarX
-#include "../../../../../navigator/type/ndarray/IVTCByteImage.h"
namespace armarx::aron::cppserializer::serializer
{
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Image.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Image.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..771ffb4f301fb705b19474f5a9c28ef8da5e3395
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Image.cpp
@@ -0,0 +1,261 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 Rainer Kartmann (rainer dot kartmann at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "Image.h"
+
+#include <SimoxUtility/meta/type_name.h>
+
+
+namespace armarx::aron::cppserializer::serializer
+{
+
+ const std::map<typenavigator::ImagePixelType, std::string>
+ ImageSerializer::pixelTypeMap =
+ {
+ { typenavigator::ImagePixelType::Rgb24, "CV_8UC3" },
+ { typenavigator::ImagePixelType::Depth32, "CV_32FC1"},
+ };
+
+
+ // constructors
+ ImageSerializer::ImageSerializer(const typenavigator::ImageNavigatorPtr& n) :
+ detail::NDArraySerializerBase<typenavigator::ImageNavigator, ImageSerializer>(
+ "cv::Mat",
+ simox::meta::get_type_name<data::AronNDArray>(),
+ simox::meta::get_type_name<type::AronImage>(),
+ n)
+ {
+ ARMARX_CHECK_NOT_NULL(typenavigator);
+ }
+
+
+ CppBlockPtr ImageSerializer::getResetHardBlock(const std::string& accessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+
+ std::string typeConstant;
+ {
+ if (auto it = pixelTypeMap.find(typenavigator->getPixelType()); it != pixelTypeMap.end())
+ {
+ typeConstant = it->second;
+ }
+ else
+ {
+ std::stringstream ss;
+ ss << "Pixel type '" << static_cast<int>(typenavigator->getPixelType()) << "' "
+ << "is not mapped to an OpenCV pixel type.";
+ throw error::AronException("ImageSerializer", __FUNCTION__, ss.str());
+ }
+ }
+
+ block_if_data->addLine(accessor + ".create(0, 0, " + typeConstant + ");");
+ return this->ResolveMaybeResetHardBlock(accessor, block_if_data, this->typenavigator);
+ }
+
+
+ CppBlockPtr ImageSerializer::getResetSoftBlock(const std::string& accessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+
+ block_if_data->addLine(accessor + " = 0;");
+
+ return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
+ }
+
+
+ CppBlockPtr ImageSerializer::getWriteTypeBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+
+ std::string typeName;
+ switch (typenavigator->getPixelType())
+ {
+#define CASE( typeConstant ) \
+case typeConstant : \
+ typeName = "::armarx::aron::typenavigator::ImageNavigator::pixelTypeToName( " #typeConstant " )"; \
+ break;
+
+ CASE(::armarx::aron::typenavigator::ImagePixelType::Rgb24)
+ CASE(::armarx::aron::typenavigator::ImagePixelType::Depth32)
+
+#undef CASE
+ }
+
+ b->addLine("w.writeImage({" + typeName + ", " + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
+ return b;
+ }
+
+
+ CppBlockPtr ImageSerializer::getWriteBlock(const std::string& accessor) const
+ {
+ CppBlockPtr block = std::make_shared<CppBlock>();
+
+ std::string escaped_accessor = EscapeAccessor(accessor);
+ std::string accessor_shape = "shape";
+
+ block->addLine("std::vector<int> " + accessor_shape + "(" + accessor + ".size.p, " + accessor + ".size.p + " + accessor + ".dims);");
+ block->addLine(accessor_shape + ".push_back(" + accessor + nextEl() + "elemSize());");
+
+
+#if 0 // Target code:
+ std::string type;
+ switch (the_depth32_image.type())
+ {
+ case CV_8UC3:
+ type = ::armarx::aron::typenavigator::ImageNavigator::pixelTypeToName(::armarx::aron::typenavigator::ImagePixelType::RGB24);
+ break;
+ case CV_32FC1:
+ type = ::armarx::aron::typenavigator::ImageNavigator::pixelTypeToName(::armarx::aron::typenavigator::ImagePixelType::DEPTH32);
+ break;
+ default:
+ {
+ std::stringstream ss;
+ ss << "OpenCV image type " << the_depth32_image.type() << " cannot be serialized.";
+ throw ::armarx::aron::error::AronException("ImageTest", __FUNCTION__, ss.str());
+ }
+ }
+#endif
+
+ block->addLine("std::string arrayType;");
+ block->addLine("switch (" + accessor + ".type())");
+ {
+ CppBlockPtr block_switch = std::make_shared<CppBlock>();
+
+#define CASE( typeConstant ) \
+ block_switch->addLine("case " + pixelTypeMap.at(typeConstant) + ":"); \
+ block_switch->addLine("arrayType = ::armarx::aron::typenavigator::ImageNavigator::pixelTypeToName(" #typeConstant ");"); \
+ block_switch->addLine("break;")
+
+ CASE(::armarx::aron::typenavigator::ImagePixelType::Rgb24);
+ CASE(::armarx::aron::typenavigator::ImagePixelType::Depth32);
+
+#undef CASE
+
+ block_switch->addLine("default:");
+ {
+ CppBlockPtr block_default = std::make_shared<CppBlock>();
+ block_default->addLine("std::stringstream ss;");
+ block_default->addLine("ss << \"OpenCV image type \" << " + accessor + ".type() << \" cannot be serialized.\";");
+ block_default->addLine("throw ::armarx::aron::error::AronException(\"" + typenavigator->getName() + "\", __FUNCTION__, ss.str());");
+
+ block_switch->addBlock(block_default);
+ }
+ block->addBlock(block_switch);
+ }
+
+
+ block->addLine("w.writeNDArray(" + accessor_shape + ", arrayType, reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "data)); // of " + accessor);
+
+ return ResolveMaybeWriteBlock(accessor, block, typenavigator);
+ }
+
+
+ CppBlockPtr ImageSerializer::getReadBlock(const std::string& accessor) const
+ {
+ const std::string escaped_accessor = EscapeAccessor(accessor);
+ const std::string read_start_result_accessor = escaped_accessor + READ_START_RETURN_TYPE_ACCESSOR;
+
+#if 0 // Target code
+ auto the_rgb24_image_return_type = r.readStartNDArray(); // of the_rgb24_image
+
+ if (!the_rgb24_image_return_type.dims.empty())
+ {
+ std::vector<int> shape{the_rgb24_image_return_type.dims.begin(), std::prev(the_rgb24_image_return_type.dims.end())};
+
+ ::armarx::aron::typenavigator::ImagePixelType pixelType = ::armarx::aron::typenavigator::ImageNavigator::pixelTypeFromName(the_rgb24_image_return_type.type);
+ int cvMatType = 0;
+ switch (pixelType)
+ {
+ case ::armarx::aron::typenavigator::ImagePixelType::RGB24:
+ cvMatType = CV_8UC3;
+ case ::armarx::aron::typenavigator::ImagePixelType::DEPTH32:
+ cvMatType = CV_32FC1;
+ default:
+ {
+ std::stringstream ss;
+ ss << "NDArray Type '" << the_rgb24_image_return_type.type << "' cannot be deserialized as image.";
+ throw ::armarx::aron::error::AronException("AronImageType", __FUNCTION__, ss.str());
+ }
+ }
+
+ the_rgb24_image.create(shape, cvMatType);
+ r.readEndNDArray(reinterpret_cast<unsigned char*>(the_rgb24_image.data)); // of the_rgb24_image
+ }
+#endif
+
+ CppBlockPtr block = std::make_shared<CppBlock>();
+
+ block->addLine("if (!" + read_start_result_accessor + ".dims.empty())");
+ {
+ CppBlockPtr block_if = std::make_shared<CppBlock>();
+
+ block_if->addLine("std::vector<int> shape{" + read_start_result_accessor + ".dims.begin(), std::prev(" + read_start_result_accessor + ".dims.end())};");
+
+ block_if->addLine("::armarx::aron::typenavigator::ImagePixelType pixelType = ::armarx::aron::typenavigator::ImageNavigator::pixelTypeFromName(" + read_start_result_accessor + ".type);");
+ block_if->addLine("int cvMatType = 0;");
+ block_if->addLine("switch (pixelType)");
+ {
+ CppBlockPtr block_switch = std::make_shared<CppBlock>();
+
+#define CASE( typeConstant ) \
+ block_switch->addLine("case " #typeConstant ":"); \
+ block_switch->addLine("cvMatType = " + pixelTypeMap.at(typeConstant) + ";"); \
+ block_switch->addLine("break;")
+
+ CASE(::armarx::aron::typenavigator::ImagePixelType::Rgb24);
+ CASE(::armarx::aron::typenavigator::ImagePixelType::Depth32);
+
+#undef CASE
+
+ block_switch->addLine("default:");
+ {
+ CppBlockPtr block_default = std::make_shared<CppBlock>();
+ block_default->addLine("std::stringstream ss;");
+ block_default->addLine("ss << \"NdArray Type '\" << " + read_start_result_accessor + ".type << \"' cannot be deserialized as image.\";");
+ block_default->addLine("throw ::armarx::aron::error::AronException(\"AronImageType\", __FUNCTION__, ss.str());");
+
+ block_switch->addBlock(block_default);
+ }
+ block_if->addBlock(block_switch);
+ }
+
+ block_if->addLine(accessor + ".create(shape, cvMatType);");
+ block_if->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "data)); // of " + accessor);
+ block->addBlock(block_if);
+ }
+
+ return ResolveMaybeReadBlock(accessor, "r.readStartNDArray()", block, typenavigator, true);
+ }
+
+
+ CppBlockPtr ImageSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine("if (cv::countNonZero(" + accessor + " != " + otherInstanceAccessor + ") != 0)");
+ block_if_data->addLineAsBlock("return false;");
+ return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
+ }
+}
+
+
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Image.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Image.h
new file mode 100644
index 0000000000000000000000000000000000000000..a44cafbe199c352576c1307d6cdcd20debb3f871
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Image.h
@@ -0,0 +1,67 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 Rainer Kartmann (rainer dot kartmann at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/Image.h>
+
+#include <map>
+#include <memory>
+#include <string>
+
+
+namespace armarx::aron::cppserializer::serializer
+{
+
+ class ImageSerializer :
+ virtual public detail::NDArraySerializerBase<typenavigator::ImageNavigator, ImageSerializer>
+ {
+ public:
+ using PointerType = std::shared_ptr<ImageSerializer>;
+
+ public:
+
+ ImageSerializer(const typenavigator::ImageNavigatorPtr& n);
+
+ // virtual implementations
+ virtual CppBlockPtr getResetHardBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
+ virtual CppBlockPtr getWriteBlock(const std::string&) const override;
+ virtual CppBlockPtr getReadBlock(const std::string&) const override;
+ virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
+
+
+ private:
+
+ static constexpr const char* ITERATOR_ACCESSOR = "_iterator";
+ static constexpr const char* SHAPE_ACCESSOR = "_shape";
+ static constexpr const char* NDIM_ACCESSOR = "_ndim";
+
+ /// Maps ARON pixel types to OpenCV image type constants.
+ static const std::map<typenavigator::ImagePixelType, std::string> pixelTypeMap;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.cpp
index dfd9e90c390c358aa1aba0fa6af2c3f843f6013a..f9597ff4540e02bb7aba3e7ecf990467d88ae8be 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.cpp
@@ -24,13 +24,18 @@
// Header
#include "NDArray.h"
+#include <SimoxUtility/meta/type_name.h>
+
+
namespace armarx::aron::cppserializer::serializer
{
// constructors
NDArraySerializer::NDArraySerializer(const typenavigator::NDArrayNavigatorPtr& n) :
- detail::NDArraySerializerBase<typenavigator::NDArrayNavigator, NDArraySerializer>("NDArray",
- simox::meta::get_type_name(typeid(data::AronNDArray)),
- simox::meta::get_type_name(typeid(type::AronNDArray)), n)
+ detail::NDArraySerializerBase<typenavigator::NDArrayNavigator, NDArraySerializer>(
+ "NDArray",
+ simox::meta::get_type_name<data::AronNDArray>(),
+ simox::meta::get_type_name<type::AronNDArray>(),
+ n)
{
ARMARX_CHECK_NOT_NULL(typenavigator);
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.h
index 540b38d930ec9a2911c8bb058b856031cc430683..6e6a72f01b84db55c6b059a74c16eb59aaa27b2d 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.h
@@ -23,15 +23,11 @@
#pragma once
-// STD/STL
-#include <string>
-#include <map>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/NDArray.h>
-// Base Class
-#include "../detail/NDArraySerializerBase.h"
+#include <string>
-// ArmarX
-#include "../../../../../navigator/type/ndarray/NDArray.h"
namespace armarx::aron::cppserializer::serializer
{
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.cpp
index 37de66674278dc5a8439a2b06d3e634dab7e42ba..0528b58b4efd7a05484c376c6dc85d722f536459 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.cpp
@@ -24,54 +24,86 @@
// Header
#include "OpenCVMat.h"
+#include <SimoxUtility/meta/type_name.h>
+
+
namespace armarx::aron::cppserializer::serializer
{
const std::map<std::string, std::pair<std::string, int>> OpenCVMatSerializer::ACCEPTED_TYPES =
{
- {"CV_8U", {"CV_8U", 1}},
- {"CV_8S", {"CV_8S", 1}},
- {"CV_16U", {"CV_16U", 2}},
- {"CV_16S", {"CV_16S", 2}},
- {"CV_32S", {"CV_32S", 4}},
- {"CV_32F", {"CV_32F", 4}},
- {"CV_64F", {"CV_64F", 8}},
+ { "8u", {"CV_8U", 1}},
+ { "8s", {"CV_8S", 1}},
+ {"16u", {"CV_16U", 2}},
+ {"16s", {"CV_16S", 2}},
+ {"32s", {"CV_32S", 4}},
+ {"32f", {"CV_32F", 4}},
+ {"64f", {"CV_64F", 8}},
};
// constructors
OpenCVMatSerializer::OpenCVMatSerializer(const typenavigator::OpenCVMatNavigatorPtr& n) :
- detail::NDArraySerializerBase<typenavigator::OpenCVMatNavigator, OpenCVMatSerializer>("cv::Mat",
- simox::meta::get_type_name(typeid(data::AronNDArray)),
- simox::meta::get_type_name(typeid(type::AronOpenCVMat)), n)
+ detail::NDArraySerializerBase<typenavigator::OpenCVMatNavigator, OpenCVMatSerializer>(
+ "cv::Mat",
+ simox::meta::get_type_name<data::AronNDArray>(),
+ simox::meta::get_type_name<type::AronOpenCVMat>(),
+ n)
{
ARMARX_CHECK_NOT_NULL(typenavigator);
}
- std::pair<CppBlockPtr, std::string> OpenCVMatSerializer::getDimensionsFromAccessor(const std::string& accessor) const
+ std::pair<CppBlockPtr, std::string> OpenCVMatSerializer::getShapeFromAccessor(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
std::string escaped_accessor = EscapeAccessor(accessor);
- std::string accessor_dimensions = escaped_accessor + DIMENSION_ACCESSOR;
+ std::string accessor_shape = escaped_accessor + SHAPE_ACCESSOR;
std::string accessor_iterator = escaped_accessor + ITERATOR_ACCESSOR;
- b->addLine("std::vector<int> " + accessor_dimensions + ";");
+ b->addLine("std::vector<int> " + accessor_shape + ";");
b->addLine("for (int " + accessor_iterator + " = 0; " + accessor_iterator + " < " + accessor + nextEl() + "dims; ++" + accessor_iterator + ")");
CppBlockPtr b2 = CppBlockPtr(new CppBlock());
- b2->addLine(accessor_dimensions + ".push_back(" + accessor + nextEl() + "size[" + accessor_iterator + "]);");
+ b2->addLine(accessor_shape + ".push_back(" + accessor + nextEl() + "size[" + accessor_iterator + "]);");
b->addBlock(b2);
- return {b, accessor_dimensions};
+ return {b, accessor_shape};
}
+
+ CppBlockPtr OpenCVMatSerializer::getResetHardBlock(const std::string& accessor) const
+ {
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+
+ std::string sizes;
+ {
+ const std::vector<int> shape = typenavigator->getShape();
+ sizes = simox::alg::join(simox::alg::multi_to_string(shape), ", ");
+ sizes = "std::vector<int>{ " + sizes + " }";
+ }
+ std::string type = typenavigator->getTypeName();
+ {
+ type = simox::alg::to_lower(type);
+ type = simox::alg::remove_prefix(type, "cv_");
+ if (auto it = ACCEPTED_TYPES.find(type); it != ACCEPTED_TYPES.end())
+ {
+ type = it->second.first;
+ }
+ }
+
+ block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator()
+ + "(" + sizes + ", " + type + ");");
+ return this->ResolveMaybeResetHardBlock(accessor, block_if_data, this->typenavigator);
+ }
+
+
CppBlockPtr OpenCVMatSerializer::getResetSoftBlock(const std::string& accessor) const
{
CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- auto [get_dim_block, accessor_dimensions] = getDimensionsFromAccessor(accessor);
+ auto [get_dim_block, accessor_shape] = getShapeFromAccessor(accessor);
block_if_data->appendBlock(get_dim_block);
- block_if_data->addLine("if (!" + accessor_dimensions + ".empty())");
+ block_if_data->addLine("if (!" + accessor_shape + ".empty())");
CppBlockPtr b2 = CppBlockPtr(new CppBlock());
- b2->addLine(accessor + " = " + getFullCppTypename() + "(" + accessor_dimensions + ", " + accessor + nextEl() + "type());");
+ b2->addLine(accessor + " = " + getFullCppTypename() + "(" + accessor_shape + ", " + accessor + nextEl() + "type());");
block_if_data->addBlock(b2);
return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
@@ -90,10 +122,10 @@ namespace armarx::aron::cppserializer::serializer
std::string escaped_accessor = EscapeAccessor(accessor);
- auto [get_dim_block, accessor_dimensions] = getDimensionsFromAccessor(accessor);
+ auto [get_dim_block, accessor_shape] = getShapeFromAccessor(accessor);
block_if_data->appendBlock(get_dim_block);
- block_if_data->addLine(accessor_dimensions + ".push_back(" + accessor + nextEl() + "elemSize());");
- block_if_data->addLine("w.writeNDArray(" + accessor_dimensions + ", std::to_string(" + accessor + nextEl() + "type()), reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "data)); // of " + accessor);
+ block_if_data->addLine(accessor_shape + ".push_back(" + accessor + nextEl() + "elemSize());");
+ block_if_data->addLine("w.writeNDArray(" + accessor_shape + ", std::to_string(" + accessor + nextEl() + "type()), reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "data)); // of " + accessor);
return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.h
index 27ec677b25d20dab8d2e966ac6e0065ad1db98dd..08aacde0f0d11e47cd8cca3ae17057928bf2daf3 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.h
@@ -23,33 +23,30 @@
#pragma once
-// STD/STL
-#include <string>
-#include <map>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.h>
-// Base Class
-#include "../detail/NDArraySerializerBase.h"
+#include <map>
+#include <memory>
+#include <string>
+#include <utility> // std::pair
-// ArmarX
-#include "../../../../../navigator/type/ndarray/OpenCVMat.h"
namespace armarx::aron::cppserializer::serializer
{
- class OpenCVMatSerializer;
- typedef std::shared_ptr<OpenCVMatSerializer> AronOpenCVMatTypeCppSerializerPtr;
-
class OpenCVMatSerializer :
virtual public detail::NDArraySerializerBase<typenavigator::OpenCVMatNavigator, OpenCVMatSerializer>
{
public:
- using PointerType = AronOpenCVMatTypeCppSerializerPtr;
+ using PointerType = std::shared_ptr<OpenCVMatSerializer>;
public:
// constructors
OpenCVMatSerializer(const typenavigator::OpenCVMatNavigatorPtr& n);
// virtual implementations
+ virtual CppBlockPtr getResetHardBlock(const std::string& accessor) const override;
virtual CppBlockPtr getResetSoftBlock(const std::string& accessor) const override;
virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
@@ -57,13 +54,14 @@ namespace armarx::aron::cppserializer::serializer
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
private:
- std::pair<CppBlockPtr, std::string> getDimensionsFromAccessor(const std::string& accessor) const;
+ std::pair<CppBlockPtr, std::string> getShapeFromAccessor(const std::string& accessor) const;
private:
// members
static const std::map<std::string, std::pair<std::string, int>> ACCEPTED_TYPES;
static constexpr const char* ITERATOR_ACCESSOR = "_iterator";
- static constexpr const char* DIMENSION_ACCESSOR = "_dimensions";
- static constexpr const char* NUM_DIMENSION_ACCESSOR = "_num_dimensions";
+ static constexpr const char* SHAPE_ACCESSOR = "_shape";
+ static constexpr const char* NDIM_ACCESSOR = "_ndim";
};
+
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp
index c908a0c44a8b0d1d940b9a64be5e79558b1dc949..1bd39c73c977b6cf4da7a7de01b3346083c7c742 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp
@@ -24,27 +24,44 @@
// Header
#include "Orientation.h"
+#include <SimoxUtility/meta/type_name.h>
+
+
namespace armarx::aron::cppserializer::serializer
{
// constructors
OrientationSerializer::OrientationSerializer(const typenavigator::OrientationNavigatorPtr& n) :
- detail::NDArraySerializerBase<typenavigator::OrientationNavigator, OrientationSerializer>("Eigen::Quaternion<" + n->ACCEPTED_TYPE + ">",
- simox::meta::get_type_name(typeid(data::AronNDArray)),
- simox::meta::get_type_name(typeid(type::AronOrientation)), n)
+ detail::NDArraySerializerBase<typenavigator::OrientationNavigator, OrientationSerializer>(
+ "Eigen::Quaternion<" + n->ACCEPTED_TYPE + ">",
+ simox::meta::get_type_name<data::AronNDArray>(),
+ simox::meta::get_type_name<type::AronOrientation>(),
+ n)
{
ARMARX_CHECK_NOT_NULL(typenavigator);
}
+ CppBlockPtr OrientationSerializer::getResetHardBlock(const std::string& accessor) const
+ {
+ CppBlockPtr block = std::make_shared<CppBlock>();
+ block->addLine(accessor + ".setIdentity();");
+ return this->ResolveMaybeResetHardBlock(accessor, block, this->typenavigator);
+ }
+
+ CppBlockPtr OrientationSerializer::getResetSoftBlock(const std::string& accessor) const
+ {
+ return getResetHardBlock(accessor);
+ }
+
CppBlockPtr OrientationSerializer::getWriteTypeBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
+ CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("w.writeOrientation({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
return b;
}
CppBlockPtr OrientationSerializer::getWriteBlock(const std::string& accessor) const
{
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->ACCEPTED_DIMENSION, ", ") + ", 4}, \"" + typenavigator->ACCEPTED_TYPE + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "coeffs().data())); // of " + accessor);
return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
@@ -52,14 +69,14 @@ namespace armarx::aron::cppserializer::serializer
CppBlockPtr OrientationSerializer::getReadBlock(const std::string& accessor) const
{
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "coeffs().data())); // of " + accessor);
return ResolveMaybeReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
}
CppBlockPtr OrientationSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
{
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" + otherInstanceAccessor + ")))");
block_if_data->addLineAsBlock("return false;");
return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.h
index ece7bb620a16d40184518585ed0b0592a8264639..b4851e4bc6806fc60b157302a7bfc53adc6a110a 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.h
@@ -23,39 +23,32 @@
#pragma once
-// STD/STL
-#include <string>
-#include <map>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/Orientation.h>
-// Base Class
-#include "../detail/NDArraySerializerBase.h"
-
-// ArmarX
-#include "../../../../../navigator/type/ndarray/Orientation.h"
namespace armarx::aron::cppserializer::serializer
{
- class OrientationSerializer;
- typedef std::shared_ptr<OrientationSerializer> AronOrientationTypeCppSerializerPtr;
class OrientationSerializer :
virtual public detail::NDArraySerializerBase<typenavigator::OrientationNavigator, OrientationSerializer>
{
public:
- using PointerType = AronOrientationTypeCppSerializerPtr;
+ using PointerType = std::shared_ptr<class OrientationSerializer>;
public:
+
// constructors
OrientationSerializer(const typenavigator::OrientationNavigatorPtr&);
// virtual implementations
+ virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
- private:
- static constexpr const char* DIMENSION_ACCESSOR = "_dimensions";
- static constexpr const char* TYPE_ACCESSOR = "_type";
};
+
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
index 5a6155b230e3fd5267ef30dcf665ca60df97ab1f..2e0234f3aff59d141988304448c128717bd9476d 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
@@ -24,6 +24,9 @@
// Header
#include "PCLPointCloud.h"
+#include <SimoxUtility/meta/type_name.h>
+
+
namespace armarx::aron::cppserializer::serializer
{
@@ -41,9 +44,11 @@ namespace armarx::aron::cppserializer::serializer
// constructors
PCLPointCloudSerializer::PCLPointCloudSerializer(const typenavigator::PCLPointCloudNavigatorPtr& n) :
- detail::NDArraySerializerBase<typenavigator::PCLPointCloudNavigator, PCLPointCloudSerializer>("pcl::PointCloud<" + ACCEPTED_TYPES.at(n->getTypename()).first + ">",
- simox::meta::get_type_name(typeid(data::AronNDArray)),
- simox::meta::get_type_name(typeid(type::AronPCLPointCloud)), n)
+ detail::NDArraySerializerBase<typenavigator::PCLPointCloudNavigator, PCLPointCloudSerializer>(
+ "pcl::PointCloud<" + ACCEPTED_TYPES.at(n->getTypename()).first + ">",
+ simox::meta::get_type_name<data::AronNDArray>(),
+ simox::meta::get_type_name<type::AronPCLPointCloud>(),
+ n)
{
ARMARX_CHECK_NOT_NULL(typenavigator);
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.h
index 3f182f4e24f775166a4e64d12f8933fffa771fc6..a0692c64ef06e92875ad66079f871fecc211d0fe 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.h
@@ -23,15 +23,11 @@
#pragma once
-// STD/STL
-#include <string>
-#include <map>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.h>
-// Base Class
-#include "../detail/NDArraySerializerBase.h"
+#include <map>
-// ArmarX
-#include "../../../../../navigator/type/ndarray/PCLPointCloud.h"
namespace armarx::aron::cppserializer::serializer
{
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp
index eccbbca04eeeced4c106707d5963222f0250db54..b6be09e9f94b972f23e226a6dc8cbd64e11c6040 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp
@@ -24,27 +24,44 @@
// Header
#include "Pose.h"
+#include <SimoxUtility/meta/type_name.h>
+
+
namespace armarx::aron::cppserializer::serializer
{
// constructors
PoseSerializer::PoseSerializer(const typenavigator::PoseNavigatorPtr& n) :
- detail::NDArraySerializerBase<typenavigator::PoseNavigator, PoseSerializer>("Eigen::Matrix<" + n->ACCEPTED_TYPE + ", " + simox::alg::to_string(n->ACCEPTED_DIMENSION, ", ") + ">",
- simox::meta::get_type_name(typeid(data::AronNDArray)),
- simox::meta::get_type_name(typeid(type::AronPose)), n)
+ detail::NDArraySerializerBase<typenavigator::PoseNavigator, PoseSerializer>(
+ "Eigen::Matrix<" + n->ACCEPTED_TYPE + ", " + simox::alg::to_string(n->ACCEPTED_DIMENSION, ", ") + ">",
+ simox::meta::get_type_name<data::AronNDArray>(),
+ simox::meta::get_type_name<type::AronPose>(),
+ n)
{
ARMARX_CHECK_NOT_NULL(typenavigator);
}
+ CppBlockPtr PoseSerializer::getResetHardBlock(const std::string& accessor) const
+ {
+ CppBlockPtr block = std::make_shared<CppBlock>();
+ block->addLine(accessor + ".setIdentity();");
+ return this->ResolveMaybeResetHardBlock(accessor, block, this->typenavigator);
+ }
+
+ CppBlockPtr PoseSerializer::getResetSoftBlock(const std::string& accessor) const
+ {
+ return getResetHardBlock(accessor);
+ }
+
CppBlockPtr PoseSerializer::getWriteTypeBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
+ CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("w.writePose({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
return b;
}
CppBlockPtr PoseSerializer::getWriteBlock(const std::string& accessor) const
{
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->ACCEPTED_DIMENSION, ", ") + ", 4}, \"" + typenavigator->ACCEPTED_TYPE + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "data())); // of " + accessor);
return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
@@ -52,14 +69,14 @@ namespace armarx::aron::cppserializer::serializer
CppBlockPtr PoseSerializer::getReadBlock(const std::string& accessor) const
{
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "data())); // of " + accessor);
return ResolveMaybeReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
}
CppBlockPtr PoseSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
{
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" + otherInstanceAccessor + ")))");
block_if_data->addLineAsBlock("return false;");
return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.h
index 45b8eb78c64e62a2ea7713ac977c9fb3e675de05..195a04015ea58ae14b419dcda0bf163981cecff6 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.h
@@ -23,39 +23,29 @@
#pragma once
-// STD/STL
-#include <string>
-#include <map>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/Pose.h>
-// Base Class
-#include "../detail/NDArraySerializerBase.h"
-
-// ArmarX
-#include "../../../../../navigator/type/ndarray/Pose.h"
namespace armarx::aron::cppserializer::serializer
{
- class PoseSerializer;
- typedef std::shared_ptr<PoseSerializer> AronPoseTypeCppSerializerPtr;
-
class PoseSerializer :
virtual public detail::NDArraySerializerBase<typenavigator::PoseNavigator, PoseSerializer>
{
public:
- using PointerType = AronPoseTypeCppSerializerPtr;
+ using PointerType = std::shared_ptr<PoseSerializer>;
public:
// constructors
PoseSerializer(const typenavigator::PoseNavigatorPtr&);
// virtual implementations
+ virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
- private:
- static constexpr const char* DIMENSION_ACCESSOR = "_dimensions";
- static constexpr const char* TYPE_ACCESSOR = "_type";
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp
index 199c7e3053af9cc84016d14a320092667ce87c71..6c9e54ecd0a074cc1c56a63f090fc56c06152004 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp
@@ -24,27 +24,44 @@
// Header
#include "Position.h"
+#include <SimoxUtility/meta/type_name.h>
+
+
namespace armarx::aron::cppserializer::serializer
{
// constructors
PositionSerializer::PositionSerializer(const typenavigator::PositionNavigatorPtr& n) :
- detail::NDArraySerializerBase<typenavigator::PositionNavigator, PositionSerializer>("Eigen::Matrix<" + n->ACCEPTED_TYPE + ", " + simox::alg::to_string(n->ACCEPTED_DIMENSION, ", ") + ">",
- simox::meta::get_type_name(typeid(data::AronNDArray)),
- simox::meta::get_type_name(typeid(type::AronPosition)), n)
+ detail::NDArraySerializerBase<typenavigator::PositionNavigator, PositionSerializer>(
+ "Eigen::Matrix<" + n->ACCEPTED_TYPE + ", " + simox::alg::to_string(n->ACCEPTED_DIMENSION, ", ") + ">",
+ simox::meta::get_type_name<data::AronNDArray>(),
+ simox::meta::get_type_name<type::AronPosition>(),
+ n)
{
ARMARX_CHECK_NOT_NULL(typenavigator);
}
+ CppBlockPtr PositionSerializer::getResetHardBlock(const std::string& accessor) const
+ {
+ CppBlockPtr block = std::make_shared<CppBlock>();
+ block->addLine(accessor + ".setZero();");
+ return this->ResolveMaybeResetHardBlock(accessor, block, this->typenavigator);
+ }
+
+ CppBlockPtr PositionSerializer::getResetSoftBlock(const std::string& accessor) const
+ {
+ return getResetHardBlock(accessor);
+ }
+
CppBlockPtr PositionSerializer::getWriteTypeBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
+ CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("w.writePosition({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
return b;
}
CppBlockPtr PositionSerializer::getWriteBlock(const std::string& accessor) const
{
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->ACCEPTED_DIMENSION, ", ") + ", 4}, \"" + typenavigator->ACCEPTED_TYPE + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "data())); // of " + accessor);
return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
@@ -52,14 +69,14 @@ namespace armarx::aron::cppserializer::serializer
CppBlockPtr PositionSerializer::getReadBlock(const std::string& accessor) const
{
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "data())); // of " + accessor);
return ResolveMaybeReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
}
CppBlockPtr PositionSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
{
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" + otherInstanceAccessor + ")))");
block_if_data->addLineAsBlock("return false;");
return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.h
index acb8f9912f8169b161ccd5b6d80c30e546b7ec75..61026f4617401bdc64393c4e813c3fd535dce301 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.h
@@ -23,39 +23,30 @@
#pragma once
-// STD/STL
-#include <string>
-#include <map>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/Position.h>
-// Base Class
-#include "../detail/NDArraySerializerBase.h"
-
-// ArmarX
-#include "../../../../../navigator/type/ndarray/Position.h"
namespace armarx::aron::cppserializer::serializer
{
- class PositionSerializer;
- typedef std::shared_ptr<PositionSerializer> AronPositionTypeCppSerializerPtr;
class PositionSerializer :
virtual public detail::NDArraySerializerBase<typenavigator::PositionNavigator, PositionSerializer>
{
public:
- using PointerType = AronPositionTypeCppSerializerPtr;
+ using PointerType = std::shared_ptr<PositionSerializer>;
public:
// constructors
PositionSerializer(const typenavigator::PositionNavigatorPtr&);
// virtual implementations
+ virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
- private:
- static constexpr const char* DIMENSION_ACCESSOR = "_dimensions";
- static constexpr const char* TYPE_ACCESSOR = "_type";
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Bool.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Bool.cpp
index c0bb65832b4bda3aaea920efd48fc19b98d752f3..33283bde524169ec20e01bd659de94547b662212 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Bool.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Bool.cpp
@@ -21,19 +21,20 @@
* GNU General Public License
*/
+#include "Bool.h"
-// STD/STL
-#include <string>
-#include <map>
+#include <SimoxUtility/meta/type_name.h>
-// Header
-#include "Bool.h"
namespace armarx::aron::cppserializer::serializer
{
/* constructors */
BoolSerializer::BoolSerializer(const typenavigator::BoolNavigatorPtr& e) :
- detail::PrimitiveSerializerBase<typenavigator::BoolNavigator, BoolSerializer>("bool", simox::meta::get_type_name(typeid(data::AronBool)), simox::meta::get_type_name(typeid(type::AronBool)), e)
+ detail::PrimitiveSerializerBase<typenavigator::BoolNavigator, BoolSerializer>(
+ "bool",
+ simox::meta::get_type_name<data::AronBool>(),
+ simox::meta::get_type_name<type::AronBool>(),
+ e)
{
ARMARX_CHECK_NOT_NULL(typenavigator);
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Bool.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Bool.h
index ba1dd70de0220840e9feda9d18304d631142ae06..688aeb2d365a5e785e575050f8e01983b3211e9e 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Bool.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Bool.h
@@ -23,14 +23,9 @@
#pragma once
-// STD/STL
-#include <string>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/primitive/Bool.h>
-// Base Class
-#include "../detail/PrimitiveSerializerBase.h"
-
-// ArmarX
-#include "../../../../../navigator/type/primitive/Bool.h"
namespace armarx::aron::cppserializer::serializer
{
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Double.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Double.cpp
index 62a47a280f4bfea571384b30d9674fa08d8aa801..c7bbce2eebe2367b83ce5d63a4b7b77b1da7acc6 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Double.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Double.cpp
@@ -21,19 +21,20 @@
* GNU General Public License
*/
+#include "Double.h"
-// STD/STL
-#include <string>
-#include <map>
+#include <SimoxUtility/meta/type_name.h>
-// Header
-#include "Double.h"
namespace armarx::aron::cppserializer::serializer
{
/* constructors */
DoubleSerializer::DoubleSerializer(const typenavigator::DoubleNavigatorPtr& e) :
- detail::PrimitiveSerializerBase<typenavigator::DoubleNavigator, DoubleSerializer>("double", simox::meta::get_type_name(typeid(data::AronDouble)), simox::meta::get_type_name(typeid(type::AronDouble)), e)
+ detail::PrimitiveSerializerBase<typenavigator::DoubleNavigator, DoubleSerializer>(
+ "double",
+ simox::meta::get_type_name<data::AronDouble>(),
+ simox::meta::get_type_name<type::AronDouble>(),
+ e)
{
ARMARX_CHECK_NOT_NULL(typenavigator);
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Double.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Double.h
index 6a965e2034d3924b842634d7dbb8219467a17294..b8258a0b028ac020c61af412d402599367587e6a 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Double.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Double.h
@@ -23,14 +23,9 @@
#pragma once
-// STD/STL
-#include <string>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/primitive/Double.h>
-// Base Class
-#include "../detail/PrimitiveSerializerBase.h"
-
-// ArmarX
-#include "../../../../../navigator/type/primitive/Double.h"
namespace armarx::aron::cppserializer::serializer
{
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Float.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Float.cpp
index 06cbd8c1a4304f095fc85887060f696335b05f55..34593db2f5b0b7fd7113395d79233b2a9c93b6d4 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Float.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Float.cpp
@@ -21,19 +21,20 @@
* GNU General Public License
*/
+#include "Float.h"
-// STD/STL
-#include <string>
-#include <map>
+#include <SimoxUtility/meta/type_name.h>
-// Header
-#include "Float.h"
namespace armarx::aron::cppserializer::serializer
{
/* constructors */
FloatSerializer::FloatSerializer(const typenavigator::FloatNavigatorPtr& e) :
- detail::PrimitiveSerializerBase<typenavigator::FloatNavigator, FloatSerializer>("float", simox::meta::get_type_name(typeid(data::AronFloat)), simox::meta::get_type_name(typeid(type::AronFloat)), e)
+ detail::PrimitiveSerializerBase<typenavigator::FloatNavigator, FloatSerializer>(
+ "float",
+ simox::meta::get_type_name<data::AronFloat>(),
+ simox::meta::get_type_name<type::AronFloat>(),
+ e)
{
ARMARX_CHECK_NOT_NULL(typenavigator);
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Float.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Float.h
index b2139ed2a368912adba187220a84fb6565b760ee..835ebc61264b9dbb636e12a9469a0deff0bf1e90 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Float.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Float.h
@@ -23,14 +23,9 @@
#pragma once
-// STD/STL
-#include <string>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/primitive/Float.h>
-// Base Class
-#include "../detail/PrimitiveSerializerBase.h"
-
-// ArmarX
-#include "../../../../../navigator/type/primitive/Float.h"
namespace armarx::aron::cppserializer::serializer
{
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Int.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Int.cpp
index 7458faa2858bd545c77d9c7014556632bdc0c196..84299f5843002d822eae0845f748a9ecf4e491f9 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Int.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Int.cpp
@@ -21,19 +21,20 @@
* GNU General Public License
*/
+#include "Int.h"
-// STD/STL
-#include <string>
-#include <map>
+#include <SimoxUtility/meta/type_name.h>
-// Header
-#include "Int.h"
namespace armarx::aron::cppserializer::serializer
{
/* constructors */
IntSerializer::IntSerializer(const typenavigator::IntNavigatorPtr& e) :
- detail::PrimitiveSerializerBase<typenavigator::IntNavigator, IntSerializer>("int", simox::meta::get_type_name(typeid(data::AronInt)), simox::meta::get_type_name(typeid(type::AronInt)), e)
+ detail::PrimitiveSerializerBase<typenavigator::IntNavigator, IntSerializer>(
+ "int",
+ simox::meta::get_type_name<data::AronInt>(),
+ simox::meta::get_type_name<type::AronInt>(),
+ e)
{
ARMARX_CHECK_NOT_NULL(typenavigator);
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Int.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Int.h
index 7bb6311bdd60de2374a11b37741070da3e8c737a..6d26dc8e63f6372a2e0210816f9addb394af0294 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Int.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Int.h
@@ -23,14 +23,9 @@
#pragma once
-// STD/STL
-#include <string>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/primitive/Int.h>
-// Base Class
-#include "../detail/PrimitiveSerializerBase.h"
-
-// ArmarX
-#include "../../../../../navigator/type/primitive/Int.h"
namespace armarx::aron::cppserializer::serializer
{
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Long.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Long.cpp
index 754f4818ddd5f08dc224a62ebfbb95cf89e1673a..2d6f466342a6747688180245f2892284ea956196 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Long.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Long.cpp
@@ -21,19 +21,20 @@
* GNU General Public License
*/
+#include "Long.h"
-// STD/STL
-#include <string>
-#include <map>
+#include <SimoxUtility/meta/type_name.h>
-// Header
-#include "Long.h"
namespace armarx::aron::cppserializer::serializer
{
/* constructors */
LongSerializer::LongSerializer(const typenavigator::LongNavigatorPtr& e) :
- detail::PrimitiveSerializerBase<typenavigator::LongNavigator, LongSerializer>("long", simox::meta::get_type_name(typeid(data::AronLong)), simox::meta::get_type_name(typeid(type::AronLong)), e)
+ detail::PrimitiveSerializerBase<typenavigator::LongNavigator, LongSerializer>(
+ "long",
+ simox::meta::get_type_name<data::AronLong>(),
+ simox::meta::get_type_name<type::AronLong>(),
+ e)
{
ARMARX_CHECK_NOT_NULL(typenavigator);
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Long.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Long.h
index 5d0d0948ee029b6d5718f9ee740f7ae2b0d99533..227c91bfded830f5744c35ddd3ab39ad79897775 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Long.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Long.h
@@ -23,14 +23,9 @@
#pragma once
-// STD/STL
-#include <string>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/primitive/Long.h>
-// Base Class
-#include "../detail/PrimitiveSerializerBase.h"
-
-// ArmarX
-#include "../../../../../navigator/type/primitive/Long.h"
namespace armarx::aron::cppserializer::serializer
{
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/String.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/String.cpp
index 89e3603ee8afcb7ca786097d391c21e4478b3bb3..4246e1157739f62934d58f1037dcc4098aa52fdc 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/String.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/String.cpp
@@ -21,19 +21,20 @@
* GNU General Public License
*/
+#include "String.h"
-// STD/STL
-#include <string>
-#include <map>
+#include <SimoxUtility/meta/type_name.h>
-// Header
-#include "String.h"
namespace armarx::aron::cppserializer::serializer
{
/* constructors */
StringSerializer::StringSerializer(const typenavigator::StringNavigatorPtr& e) :
- detail::PrimitiveSerializerBase<typenavigator::StringNavigator, StringSerializer>("std::string", simox::meta::get_type_name(typeid(data::AronString)), simox::meta::get_type_name(typeid(type::AronString)), e)
+ detail::PrimitiveSerializerBase<typenavigator::StringNavigator, StringSerializer>(
+ "std::string",
+ simox::meta::get_type_name<data::AronString>(),
+ simox::meta::get_type_name<type::AronString>(),
+ e)
{
ARMARX_CHECK_NOT_NULL(typenavigator);
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/String.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/String.h
index d6bc43272efabf2b93a55bce3eec77e68274afd4..59bf310ccdac6edc8a14fe2fd4e07d9fa610f371 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/String.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/String.h
@@ -23,14 +23,9 @@
#pragma once
-// STD/STL
-#include <string>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/primitive/String.h>
-// Base Class
-#include "../detail/PrimitiveSerializerBase.h"
-
-// ArmarX
-#include "../../../../../navigator/type/primitive/String.h"
namespace armarx::aron::cppserializer::serializer
{
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Time.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Time.cpp
index e76d5e1ba97eea373fa1eb9af90ec1f3dc5838cf..a44838e4a29a2705cf4ddf16da1b479827f5ad46 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Time.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Time.cpp
@@ -21,19 +21,20 @@
* GNU General Public License
*/
+#include "Time.h"
-// STD/STL
-#include <string>
-#include <map>
+#include <SimoxUtility/meta/type_name.h>
-// Header
-#include "Time.h"
namespace armarx::aron::cppserializer::serializer
{
/* constructors */
TimeSerializer::TimeSerializer(const typenavigator::TimeNavigatorPtr& e) :
- detail::PrimitiveSerializerBase<typenavigator::TimeNavigator, TimeSerializer>("IceUtil::Time", simox::meta::get_type_name(typeid(data::AronLong)), simox::meta::get_type_name(typeid(type::AronTime)), e)
+ detail::PrimitiveSerializerBase<typenavigator::TimeNavigator, TimeSerializer>(
+ "IceUtil::Time",
+ simox::meta::get_type_name<data::AronLong>(),
+ simox::meta::get_type_name<type::AronTime>(),
+ e)
{
ARMARX_CHECK_NOT_NULL(typenavigator);
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Time.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Time.h
index ff06aff3e934bd23aa52d6b86bf3f45a403bf1ef..96b0322c0ab8a1b45d74a1560ec08ba5d3c9db14 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Time.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Time.h
@@ -23,14 +23,9 @@
#pragma once
-// STD/STL
-#include <string>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/primitive/Time.h>
-// Base Class
-#include "../detail/PrimitiveSerializerBase.h"
-
-// ArmarX
-#include "../../../../../navigator/type/primitive/Time.h"
namespace armarx::aron::cppserializer::serializer
{
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
index 39f178fd00661fe78478ee3e74fe0872b2e762a5..0db5f2d68e1de29da18c9327a5b25ee5bd834bc5 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
@@ -24,11 +24,18 @@
// Header
#include "IntEnumClass.h"
+#include <SimoxUtility/meta/type_name.h>
+
+
namespace armarx::aron::cppserializer::serializer
{
// constructors
IntEnumClassSerializer::IntEnumClassSerializer(const typenavigator::IntEnumNavigatorPtr& n) :
- detail::SerializerBase<typenavigator::IntEnumNavigator, IntEnumClassSerializer>(n->getEnumName(), simox::meta::get_type_name(typeid(data::AronNDArray)), simox::meta::get_type_name(typeid(type::AronIntEnum)), n)
+ detail::SerializerBase<typenavigator::IntEnumNavigator, IntEnumClassSerializer>(
+ n->getEnumName(),
+ simox::meta::get_type_name<data::AronNDArray>(),
+ simox::meta::get_type_name<type::AronIntEnum>(),
+ n)
{
ARMARX_CHECK_NOT_NULL(typenavigator);
if (typenavigator->getMaybe() != type::Maybe::eNone)
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h
index 8bc3ce9e21e220fc34c710b7bb5a7da59521dd60..bd4a0f8128b19bbe28b388e3fa97faddf8f743b1 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h
@@ -23,15 +23,12 @@
#pragma once
-// STD/STL
-#include <string>
-#include <map>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/enum/IntEnum.h>
-// Base Class
-#include "../detail/SerializerBase.h"
+#include <map>
+#include <utility> // std::pair
-// ArmarX
-#include "../../../../../navigator/type/enum/IntEnum.h"
namespace armarx::aron::cppserializer::serializer
{
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.cpp
index 7653acba47e178b6337f8fbc9602037070b76836..f5f62b9215ebd3904405184605d6d5df9fc1298e 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.cpp
@@ -24,12 +24,18 @@
// Header
#include "ObjectClass.h"
+#include <SimoxUtility/meta/type_name.h>
+
namespace armarx::aron::cppserializer::serializer
{
// constructors
ObjectClassSerializer::ObjectClassSerializer(const typenavigator::ObjectNavigatorPtr& e) :
- detail::SerializerBase<typenavigator::ObjectNavigator, ObjectClassSerializer>(e->getObjectName(), simox::meta::get_type_name(typeid(data::AronDict)), simox::meta::get_type_name(typeid(type::AronObject)), e)
+ detail::SerializerBase<typenavigator::ObjectNavigator, ObjectClassSerializer>(
+ e->getObjectName(),
+ simox::meta::get_type_name<data::AronDict>(),
+ simox::meta::get_type_name<type::AronObject>(),
+ e)
{
ARMARX_CHECK_NOT_NULL(typenavigator);
if (typenavigator->getMaybe() != type::Maybe::eNone)
@@ -143,7 +149,8 @@ namespace armarx::aron::cppserializer::serializer
child_b->addLine("w.writeKey(\"" + key + "\");");
child_b->appendBlock(child_s->getWriteBlock(key));
- block_if_data->appendBlock(child_b);
+ block_if_data->addBlock(child_b);
+ block_if_data->addLine("");
}
block_if_data->addLine("if (!__aronExtends)");
@@ -167,7 +174,7 @@ namespace armarx::aron::cppserializer::serializer
{
const auto child_s = FromAronTypeNaviagtorPtr(child);
block_if_data->addLine("r.loadMember(\"" + key + "\");");
- block_if_data->appendBlock(child_s->getReadBlock(key));
+ block_if_data->addBlock(child_s->getReadBlock(key));
}
block_if_data->addLine("r.readEndDict(); // of top level object " + getCoreCppTypename());
return ResolveMaybeReadBlock(accessor, "", block_if_data, typenavigator);
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.h
index 03f8817a5c4e64dfbf15e9a54e8e0f6ae88f4ff3..3a7a314aa14113990d9d4aad5303b21f153c674c 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.h
@@ -23,15 +23,8 @@
#pragma once
-// STD/STL
-#include <string>
-#include <map>
-
-// Base Class
-#include "../detail/SerializerBase.h"
-
-// ArmarX
-#include "../../../../../navigator/type/container/Object.h"
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
namespace armarx::aron::cppserializer::serializer
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.h b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.h
index 695050d69d765585b3192e9a2874c721afe8693c..c67f5c1b4845bdc838c33300c92722343f831bba 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.h
@@ -74,6 +74,7 @@ namespace armarx::aron::xmltypereader
static constexpr const char* ROWS_ATTRIBUTE_NAME = "rows";
static constexpr const char* COLS_ATTRIBUTE_NAME = "cols";
static constexpr const char* DIMENSIONS_ATTRIBUTE_NAME = "dimensions";
+ static constexpr const char* SHAPE_ATTRIBUTE_NAME = "shape";
static constexpr const char* OPTIONAL_NAME = "optional";
static constexpr const char* RAW_PTR_NAME = "raw_ptr";
static constexpr const char* SHARED_PTR_NAME = "shared_ptr";
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/ReaderFactory.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/ReaderFactory.cpp
index 42c4ae9eea4efcd7dc3688164aec22bb49507959..e9795175ad11e15872b4f40594b328e50d5b99b1 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/ReaderFactory.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/ReaderFactory.cpp
@@ -289,15 +289,6 @@ namespace armarx::aron::xmltypereader
{
return nullptr;
}
-
- // Complex type (IVTCByteImage)
- typenavigator::NavigatorPtr IVTCByteImageReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
- {
- Data::EnforceTagName(node, Data::GENERATE_IVT_CBYTE_IMAGE_MEMBER_TAG);
- typenavigator::IVTCByteImageNavigatorPtr complex(new typenavigator::IVTCByteImageNavigator(path));
- return complex;
- }
-
// Complex type (EigenMatrix)
typenavigator::NavigatorPtr EigenMatrixReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
{
@@ -328,12 +319,52 @@ namespace armarx::aron::xmltypereader
return complex;
}
+
+ typenavigator::NavigatorPtr ImageReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
+ {
+ Data::EnforceTagName(node, Data::GENERATE_IMAGE_MEMBER_TAG);
+
+ typenavigator::ImageNavigatorPtr complex = std::make_shared<typenavigator::ImageNavigator>(path);
+ complex->setPixelType(node.attribute_value("pixelType"));
+
+ return complex;
+ }
+
+ // Complex type (IVTCByteImage)
+ typenavigator::NavigatorPtr IVTCByteImageReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
+ {
+ Data::EnforceTagName(node, Data::GENERATE_IVT_CBYTE_IMAGE_MEMBER_TAG);
+ typenavigator::IVTCByteImageNavigatorPtr complex(new typenavigator::IVTCByteImageNavigator(path));
+ return complex;
+ }
+
+
+
// Complex type (OpenCVMat)
typenavigator::NavigatorPtr OpenCVMatReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
{
Data::EnforceTagName(node, Data::GENERATE_OPENCV_MAT_MEMBER_TAG);
typenavigator::OpenCVMatNavigatorPtr complex(new typenavigator::OpenCVMatNavigator(path));
+
+ std::vector<int> shape;
+ {
+ std::string shapeStr = node.attribute_value(Data::SHAPE_ATTRIBUTE_NAME);
+ const bool trim = true, removeEmpty = false;
+ std::vector<std::string> split = simox::alg::split(shapeStr, ",", trim, removeEmpty);
+ shape.reserve(split.size());
+ for (const std::string& entry : split)
+ {
+ // What happens if this throws an error?
+ int size = std::stoi(entry);
+ shape.push_back(size);
+ }
+ }
+ complex->setShape(shape);
+
+ std::string typeName = node.attribute_value(Data::TYPE_ATTRIBUTE_NAME);
+ complex->setTypeName(typeName);
+
return complex;
}
@@ -342,7 +373,7 @@ namespace armarx::aron::xmltypereader
{
Data::EnforceTagName(node, Data::GENERATE_PCL_POINTCLOUD_MEMBER_TAG);
Data::EnforceAttribute(node, Data::TYPE_ATTRIBUTE_NAME);
- std::string type = node.attribute_value(Data::TYPE_ATTRIBUTE_NAME);;
+ std::string type = node.attribute_value(Data::TYPE_ATTRIBUTE_NAME);
typenavigator::PCLPointCloudNavigatorPtr complex(new typenavigator::PCLPointCloudNavigator(path));
complex->setTypename(type);
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp
index c431b5b49ea2547f262da4fd657be6aa6fed1fac..d5e2ebe0fe9eb12a834bef86cd673faa41369912 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp
@@ -150,7 +150,7 @@ namespace armarx::aron::dataIO::reader
void NavigatorReader::readEndNDArray(unsigned char* data)
{
- if (data == NULL)
+ if (data == nullptr)
{
throw error::AronException("NavigatorReader", "readEndNDArray", "The corresponding data pointer of an complex aron ptr is NULL.");
}
@@ -161,7 +161,8 @@ namespace armarx::aron::dataIO::reader
auto casted = datanavigator::NDArrayNavigator::DynamicCastAndCheck(current_nav);
std::vector<int> dims = casted->getDimensions();
- memcpy(data, casted->getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
+ int size = std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>());
+ std::memcpy(data, casted->getData(), static_cast<size_t>(size));
}
// Read primitives
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.cpp
index 8777b5d0c9691d75a6dce7c780c4e3f6c5c6ce04..b1c25a788a0c1380ef2f8e0f4aa2d9f2acf967b6 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.cpp
@@ -96,7 +96,8 @@ namespace armarx::aron::dataIO::writer
datanavigator::NDArrayNavigatorPtr aron(new datanavigator::NDArrayNavigator(path));
aron->setDimensions(dims);
aron->setType(t);
- aron->setData(std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()), data);
+ int size = std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>());
+ aron->setData(static_cast<unsigned int>(size), data);
token->addElement(aron);
}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/Writer.h b/source/RobotAPI/libraries/aron/core/io/typeIO/Writer.h
index db7b4ed1adc62ea9962bd14ce9993be51bb0cbec..0790dacbc1e0a83e497253fc9e4ee6d17c2da9c2 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/Writer.h
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/Writer.h
@@ -30,7 +30,6 @@
#include <RobotAPI/libraries/aron/core/Config.h>
-
namespace armarx::aron::typeIO
{
class WriterInterface;
@@ -91,6 +90,13 @@ namespace armarx::aron::typeIO
};
virtual void writeEigenQuaternion(const WriteEigenQuaternionInput&) = 0;
+ struct WriteImageInput
+ {
+ std::string pixelType;
+ type::Maybe maybe;
+ };
+ virtual void writeImage(const WriteImageInput&) = 0;
+
struct WriteIVTCByteImageInput
{
type::Maybe maybe;
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.cpp b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.cpp
index 6a23d549a4ea915cfecc8e7645c88c7945355797..96290019ed99cb9ee80b489d6668cdb54769e38d 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.cpp
@@ -18,17 +18,15 @@
* GNU General Public License
*/
-// STD/STL
-#include <memory>
-#include <numeric>
-
-// Header
#include "NavigatorWriter.h"
-// ArmarX
#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
+#include <memory>
+#include <numeric>
+
+
namespace armarx::aron::typeIO::writer
{
// generate Path info
@@ -154,144 +152,111 @@ namespace armarx::aron::typeIO::writer
}
}
- void NavigatorWriter::writeEigenMatrix(const WriteEigenMatrixInput& o)
+
+ template <class ElementT, class InputT>
+ std::shared_ptr<ElementT>
+ NavigatorWriter::writeElement(const InputT& input)
{
Path path = generatePath();
NavigatorWriterTokenPtr token = stack.top();
- auto type = std::make_shared<typenavigator::EigenMatrixNavigator>(path);
+ auto type = std::make_shared<ElementT>(path);
+ type->setMaybe(input.maybe);
+ token->addElement(type);
+
+ return type;
+ }
+
+
+ void NavigatorWriter::writeEigenMatrix(const WriteEigenMatrixInput& o)
+ {
+ auto type = writeElement<typenavigator::EigenMatrixNavigator>(o);
+
type->setRows(o.rows);
type->setCols(o.cols);
type->setTypename(o.type);
- type->setMaybe(o.maybe);
- token->addElement(type);
}
void NavigatorWriter::writeEigenQuaternion(const WriteEigenQuaternionInput& o)
{
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- auto type = std::make_shared<typenavigator::EigenQuaternionNavigator>(path);
+ auto type = writeElement<typenavigator::EigenQuaternionNavigator>(o);
+
type->setTypename(o.type);
- type->setMaybe(o.maybe);
- token->addElement(type);
}
+
+ void NavigatorWriter::writeImage(const WriteImageInput& o)
+ {
+ auto type = writeElement<typenavigator::ImageNavigator>(o);
+
+ type->setPixelType(o.pixelType);
+ }
+
+
void NavigatorWriter::writeIVTCByteImage(const WriteIVTCByteImageInput& o)
{
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- auto type = std::make_shared<typenavigator::IVTCByteImageNavigator>(path);
- type->setMaybe(o.maybe);
- token->addElement(type);
+ writeElement<typenavigator::IVTCByteImageNavigator>(o);
}
void NavigatorWriter::writeOpenCVMat(const WriteOpenCVMatInput& o)
{
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- auto type = std::make_shared<typenavigator::OpenCVMatNavigator>(path);
- type->setMaybe(o.maybe);
- token->addElement(type);
+ writeElement<typenavigator::OpenCVMatNavigator>(o);
}
void NavigatorWriter::writePCLPointCloud(const WritePCLPointCloudInput& o)
{
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- auto type = std::make_shared<typenavigator::PCLPointCloudNavigator>(path);
+ auto type = writeElement<typenavigator::PCLPointCloudNavigator>(o);
+
type->setTypename(o.type);
- type->setMaybe(o.maybe);
- token->addElement(type);
}
void NavigatorWriter::writePosition(const WritePositionInput& o)
{
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- auto type = std::make_shared<typenavigator::PositionNavigator>(path);
- type->setMaybe(o.maybe);
- token->addElement(type);
+ writeElement<typenavigator::PositionNavigator>(o);
}
void NavigatorWriter::writeOrientation(const WriteOrientationInput& o)
{
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- auto type = std::make_shared<typenavigator::OrientationNavigator>(path);
- type->setMaybe(o.maybe);
- token->addElement(type);
+ writeElement<typenavigator::OrientationNavigator>(o);
}
void NavigatorWriter::writePose(const WritePoseInput& o)
{
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- auto type = std::make_shared<typenavigator::PoseNavigator>(path);
- type->setMaybe(o.maybe);
- token->addElement(type);
+ writeElement<typenavigator::PoseNavigator>(o);
}
void NavigatorWriter::writeInt(const WritePrimitiveInput& o)
{
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- auto type = std::make_shared<typenavigator::IntNavigator>(path);
- type->setMaybe(o.maybe);
- token->addElement(type);
+ writeElement<typenavigator::IntNavigator>(o);
}
void NavigatorWriter::writeLong(const WritePrimitiveInput& o)
{
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- auto type = std::make_shared<typenavigator::LongNavigator>(path);
- type->setMaybe(o.maybe);
- token->addElement(type);
+ writeElement<typenavigator::LongNavigator>(o);
}
void NavigatorWriter::writeFloat(const WritePrimitiveInput& o)
{
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- auto type = std::make_shared<typenavigator::FloatNavigator>(path);
- type->setMaybe(o.maybe);
- token->addElement(type);
+ writeElement<typenavigator::FloatNavigator>(o);
}
void NavigatorWriter::writeDouble(const WritePrimitiveInput& o)
{
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- auto type = std::make_shared<typenavigator::DoubleNavigator>(path);
- type->setMaybe(o.maybe);
- token->addElement(type);
+ writeElement<typenavigator::DoubleNavigator>(o);
}
void NavigatorWriter::writeString(const WritePrimitiveInput& o)
{
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- auto type = std::make_shared<typenavigator::StringNavigator>(path);
- type->setMaybe(o.maybe);
- token->addElement(type);
+ writeElement<typenavigator::StringNavigator>(o);
}
void NavigatorWriter::writeBool(const WritePrimitiveInput& o)
{
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- auto type = std::make_shared<typenavigator::BoolNavigator>(path);
- type->setMaybe(o.maybe);
- token->addElement(type);
+ writeElement<typenavigator::BoolNavigator>(o);
}
void NavigatorWriter::writeTime(const WritePrimitiveInput& o)
{
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- auto type = std::make_shared<typenavigator::TimeNavigator>(path);
- type->setMaybe(o.maybe);
- token->addElement(type);
+ writeElement<typenavigator::TimeNavigator>(o);
}
void NavigatorWriter::writeKey(const std::string& k)
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h
index 2586935e519799b7f7dd9c3f971a45bc4ab45d8f..80ca17960f4a454bf7c431ad64ce429835797403 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h
@@ -20,17 +20,14 @@
#pragma once
-// STD/STL
-#include <memory>
-#include <stack>
-
-// BaseClass
+#include <RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriterToken.h>
#include <RobotAPI/libraries/aron/core/io/typeIO/Writer.h>
-
-// ArmarX
#include <RobotAPI/libraries/aron/core/Concepts.h>
#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriterToken.h>
+
+#include <memory>
+#include <stack>
+
namespace armarx::aron::typeIO::writer
{
@@ -56,6 +53,7 @@ namespace armarx::aron::typeIO::writer
virtual void writeEigenMatrix(const WriteEigenMatrixInput&) override;
virtual void writeEigenQuaternion(const WriteEigenQuaternionInput&) override;
+ virtual void writeImage(const WriteImageInput&) override;
virtual void writeIVTCByteImage(const WriteIVTCByteImageInput&) override;
virtual void writeOpenCVMat(const WriteOpenCVMatInput&) override;
virtual void writePCLPointCloud(const WritePCLPointCloudInput&) override;
@@ -80,10 +78,16 @@ namespace armarx::aron::typeIO::writer
}
private:
+
+ Path generatePath();
+
+ template <class ElementT, class InputT>
+ std::shared_ptr<ElementT> writeElement(const InputT&);
+
+
bool wroteInitialStartObject = false;
NavigatorWriterTokenPtr lastRemovedToken = nullptr;
std::stack<NavigatorWriterTokenPtr> stack = {};
- Path generatePath();
};
}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
index 7a4359559aaf5169ef1bdac195937d540e8b3646..aaafe280661a87ad411ab072a3dca5eed0fd7d53 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
@@ -148,6 +148,18 @@ namespace armarx::aron::typeIO::writer
token->addElement(j);
}
+
+ void NlohmannJSONWriter::writeImage(const WriteImageInput& o)
+ {
+ auto token = stack.top();
+ nlohmann::json j;
+ j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
+ j[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] = "Image";
+ j["pixelType"] = o.pixelType;
+ token->addElement(j);
+ }
+
+
void NlohmannJSONWriter::writeIVTCByteImage(const WriteIVTCByteImageInput& o)
{
auto token = stack.top();
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.h b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.h
index 055a493c878fbb76b4fed7dc41d08e680a71d976..b1a9b65483db764f7fa576c9f626a0e5b70477ca 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.h
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.h
@@ -53,6 +53,7 @@ namespace armarx::aron::typeIO::writer
virtual void writeEigenMatrix(const WriteEigenMatrixInput&) override;
virtual void writeEigenQuaternion(const WriteEigenQuaternionInput&) override;
+ virtual void writeImage(const WriteImageInput&) override;
virtual void writeIVTCByteImage(const WriteIVTCByteImageInput&) override;
virtual void writeOpenCVMat(const WriteOpenCVMatInput&) override;
virtual void writePCLPointCloud(const WritePCLPointCloudInput&) override;
diff --git a/source/RobotAPI/libraries/aron/core/navigator/Navigator.h b/source/RobotAPI/libraries/aron/core/navigator/Navigator.h
index 8077649e619f023f73c79e7dc636ec9acb4a5643..24d31001590a87cb976f6b97f362c1262e7da288 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/Navigator.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/Navigator.h
@@ -47,6 +47,7 @@ namespace armarx::aron
class Navigator
{
public:
+
// constructors
Navigator() = default;
Navigator(const Descriptor& descriptor, const Path& path) :
@@ -54,6 +55,7 @@ namespace armarx::aron
path(path)
{
}
+ virtual ~Navigator() = default;
// public member functions
Descriptor getDescriptor() const
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp
index eb1a486374c836d8156d8055e543379de9840a47..cf568cef484224a6b24e5d292cfa910dee0b8994 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp
@@ -115,7 +115,7 @@ namespace armarx::aron::datanavigator
void NDArrayNavigator::setData(unsigned int elements, const unsigned char* src)
{
aron->data = std::vector<unsigned char>(elements);
- memcpy(aron->data.data(), src, elements);
+ std::memcpy(aron->data.data(), src, elements);
}
std::vector<int> NDArrayNavigator::getDimensions() const
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..3a09c0a35f884d02f904eeaceee9dbd8b7a0c3c4 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h
@@ -8,11 +8,11 @@
#include "ndarray/NDArray.h"
#include "ndarray/EigenMatrix.h"
#include "ndarray/EigenQuaternion.h"
+#include "ndarray/Image.h"
#include "ndarray/IVTCByteImage.h"
#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..e735771a906f47337a2db04a71448c8932a40b28
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h
@@ -0,0 +1,35 @@
+#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 ImageNavigatorPtr = std::shared_ptr<class ImageNavigator>;
+ 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>;
+
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Image.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Image.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dec13ece6cd10683ac5ffd6ac0f16af9c5a7e1a9
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Image.cpp
@@ -0,0 +1,130 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 Rainer Kartmann (rainer dot kartmann at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "Image.h"
+
+#include <RobotAPI/libraries/aron/core/Exception.h>
+
+#include <SimoxUtility/algorithm/string.h>
+#include <SimoxUtility/meta/EnumNames.hpp>
+
+
+namespace armarx::aron::typenavigator
+{
+
+ static const simox::meta::EnumNames<ImagePixelType> ImagePixelTypeNames
+ {
+ { ImagePixelType::Rgb24, "rgb24" },
+ { ImagePixelType::Depth32, "depth32" },
+ };
+
+
+ // constructors
+ ImageNavigator::ImageNavigator(const Path& path) :
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eImage, path)
+ {
+ }
+
+
+ ImageNavigator::ImageNavigator(const type::AronImagePtr& o, const Path& path) :
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eImage, path),
+ detail::NavigatorBase<type::AronImage, ImageNavigator>(o)
+ {
+ }
+
+
+ type::AronImagePtr ImageNavigator::toAronImagePtr() const
+ {
+ return this->aron;
+ }
+
+
+ ImagePixelType ImageNavigator::getPixelType() const
+ {
+ return pixelTypeFromName(this->aron->pixelType);
+ }
+
+ void ImageNavigator::setPixelType(const ImagePixelType& pixelType) const
+ {
+ this->aron->pixelType = pixelTypeToName(pixelType);
+ }
+
+ void ImageNavigator::setPixelType(const std::string& typeName) const
+ {
+ // Try a lookup, which throws when failing.
+ pixelTypeFromName(typeName);
+ this->aron->pixelType = typeName;
+ }
+
+
+ // virtual implementations
+ std::string ImageNavigator::getName() const
+ {
+ return "AronImageType";
+ }
+
+
+ bool ImageNavigator::operator==(const ImageNavigator& other) const
+ {
+ return *this->aron == *other.aron;
+ }
+
+
+ std::string ImageNavigator::pixelTypeToName(ImagePixelType type)
+ {
+ try
+ {
+ return ImagePixelTypeNames.to_name(type);
+ }
+ catch (const simox::meta::error::UnknownEnumValue& e)
+ {
+ throw aron::error::AronException("ImageNavigator", __FUNCTION__, e.what());
+ }
+ }
+
+
+ ImagePixelType ImageNavigator::pixelTypeFromName(const std::string& name)
+ {
+ try
+ {
+ return ImagePixelTypeNames.from_name(simox::alg::to_lower(name));
+ }
+ catch (const simox::meta::error::UnknownEnumValue& e)
+ {
+ throw aron::error::AronException("ImageNavigator", __FUNCTION__, e.what());
+ }
+ }
+
+
+ std::set<ImagePixelType> ImageNavigator::pixelTypes()
+ {
+ return ImagePixelTypeNames.values();
+ }
+
+ std::set<std::string> ImageNavigator::pixelTypeNames()
+ {
+ return ImagePixelTypeNames.names();
+ }
+
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Image.h b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Image.h
new file mode 100644
index 0000000000000000000000000000000000000000..a725dfd161c9534902e10acd4c4a20e8183320ca
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Image.h
@@ -0,0 +1,79 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 Rainer Kartmann (rainer dot kartmann at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/navigator/type/detail/NDArrayNavigatorBase.h>
+
+#include <memory>
+#include <string>
+
+
+namespace armarx::aron::typenavigator
+{
+ enum class ImagePixelType
+ {
+ /// 3 (8-bit) byte channels for R, G, and B.
+ Rgb24, // ==> CV_8UC1
+ /// 1 (32-bit) float channel for depth.
+ Depth32, // ==> CV_32FC1
+ };
+
+
+ class ImageNavigator :
+ virtual public detail::NDArrayNavigatorBase<type::AronImage, ImageNavigator>
+ {
+ public:
+
+ ImageNavigator(const Path& path = {});
+ ImageNavigator(const type::AronImagePtr&, const Path& path);
+
+
+ type::AronImagePtr toAronImagePtr() const;
+
+ /// Get the pixel type.
+ ImagePixelType getPixelType() const;
+ void setPixelType(const ImagePixelType& type) const;
+ void setPixelType(const std::string& typeName) const;
+
+
+ // virtual implementations
+ virtual std::string getName() const override;
+
+
+ // operators
+ virtual bool operator==(const ImageNavigator&) const override;
+
+
+ public:
+
+ static std::string pixelTypeToName(ImagePixelType type);
+ static ImagePixelType pixelTypeFromName(const std::string& name);
+ static std::set<ImagePixelType> pixelTypes();
+ static std::set<std::string> pixelTypeNames();
+
+ };
+
+ using ImageNavigatorPtr = std::shared_ptr<ImageNavigator>;
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.cpp
index 2d41e10ab7c13af0fbfcba8d59919830ce37dd1a..bf805025d848c2d405c359d595edd7d4e7a94adc 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.cpp
@@ -26,18 +26,21 @@
namespace armarx::aron::typenavigator
{
+
// constructors
OpenCVMatNavigator::OpenCVMatNavigator(const Path& path) :
aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eOpenCVMat, path)
{
}
+
OpenCVMatNavigator::OpenCVMatNavigator(const type::AronOpenCVMatPtr& o, const Path& path) :
aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eOpenCVMat, path),
detail::NavigatorBase<type::AronOpenCVMat, OpenCVMatNavigator>(o)
{
}
+
// operators
bool OpenCVMatNavigator::operator==(const OpenCVMatNavigator& other) const
{
@@ -48,15 +51,42 @@ namespace armarx::aron::typenavigator
return true;
}
+
type::AronOpenCVMatPtr OpenCVMatNavigator::toAronOpenCVMatPtr() const
{
return this->aron;
}
+
+ std::vector<int> OpenCVMatNavigator::getShape() const
+ {
+ return this->aron->shape;
+ }
+
+
+ void OpenCVMatNavigator::setShape(const std::vector<int>& shape) const
+ {
+ this->aron->shape = shape;
+ }
+
+
+ std::string OpenCVMatNavigator::getTypeName() const
+ {
+ return this->aron->typeName;
+ }
+
+
+ void OpenCVMatNavigator::setTypeName(const std::string& typeName)
+ {
+ this->aron->typeName = typeName;
+ }
+
+
// virtual implementations
std::string OpenCVMatNavigator::getName() const
{
return "AronOpenCVMatType";
}
+
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.h b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.h
index 74e4e285540a66823409c8272b707d825e78ba40..46c62277da75e4c785137aa948b5388fefb8035e 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.h
@@ -30,6 +30,7 @@
// Base class
#include "../detail/NDArrayNavigatorBase.h"
+
namespace armarx::aron::typenavigator
{
class OpenCVMatNavigator;
@@ -49,9 +50,16 @@ namespace armarx::aron::typenavigator
// public member functions
type::AronOpenCVMatPtr toAronOpenCVMatPtr() const;
+ std::vector<int> getShape() const;
+ void setShape(const std::vector<int>& shape) const;
+
+ std::string getTypeName() const;
+ void setTypeName(const std::string& typeName);
+
// virtual implementations
virtual std::string getName() const override;
+
public:
const std::map<std::string, std::vector<std::string>> ACCEPTED_TYPES =
{
diff --git a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp b/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp
index e99a7f91658f88611d9884d9795fb8e36b2a8485..fe98e3abe1d839928f9e3a78c2e52cb0e0717a20 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp
@@ -155,6 +155,10 @@ namespace armarx::aron::visitor
{
return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
}
+ else if (auto t = dynamic_cast<ImageNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
else if (auto t = dynamic_cast<IVTCByteImageTypeNavigator*>(&type))
{
return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
diff --git a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h b/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h
index f577903a4524da4ce220d6d402e9719f1afdac7a..b2efdaa79f0a8813c0a753822851789378ce73c7 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h
@@ -79,6 +79,7 @@ namespace armarx::aron::visitor
// Array-valued
using EigenMatrixTypeNavigator = typenavigator::EigenMatrixNavigator;
using EigenQuaternionTypeNavigator = typenavigator::EigenQuaternionNavigator;
+ using ImageNavigator = typenavigator::ImageNavigator;
using IVTCByteImageTypeNavigator = typenavigator::IVTCByteImageNavigator;
using OpenCVMatTypeNavigator = typenavigator::OpenCVMatNavigator;
using PCLPointCloudTypeNavigator = typenavigator::PCLPointCloudNavigator;
@@ -193,6 +194,11 @@ namespace armarx::aron::visitor
(void) type, (void) data;
return true;
}
+ virtual bool visit(ImageNavigator& type, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) data;
+ return true;
+ }
virtual bool visit(IVTCByteImageTypeNavigator& type, NDArrayDataNavigator& data)
{
(void) type, (void) data;
@@ -323,6 +329,11 @@ namespace armarx::aron::visitor
(void) type, (void) key;
return visit(type, data);
}
+ virtual bool visit(ImageNavigator& type, const std::string& key, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) key;
+ return visit(type, data);
+ }
virtual bool visit(IVTCByteImageTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
{
(void) type, (void) key;
diff --git a/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt b/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt
index 9ea4438bbe3381ee9c92ae3cd4d6c656606e916c..4bc9abcc6c2e42529d3c80a5036a5af1a8e4f100 100644
--- a/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt
@@ -49,6 +49,7 @@ armarx_add_test(
aron/EigenQuaternionTest.xml
aron/EnumTest.xml
aron/HumanPoseTest.xml
+ aron/ImageTest.xml
aron/IVTCByteImageTest.xml
aron/ListTest.xml
aron/NaturalIKTest.xml
diff --git a/source/RobotAPI/libraries/aron/core/test/aron/EigenQuaternionTest.xml b/source/RobotAPI/libraries/aron/core/test/aron/EigenQuaternionTest.xml
index a5760ba3b259ab6824f8a9bcb1fb9c5cd5855f53..cc175020464ffa683fe102c0fc93a7c27011c34a 100644
--- a/source/RobotAPI/libraries/aron/core/test/aron/EigenQuaternionTest.xml
+++ b/source/RobotAPI/libraries/aron/core/test/aron/EigenQuaternionTest.xml
@@ -2,19 +2,23 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<CodeIncludes>
- <Include include="<Eigen/Core>" />
<Include include="<Eigen/Geometry>" />
</CodeIncludes>
<AronIncludes>
</AronIncludes>
<GenerateTypes>
+
<Object name='armarx::EigenQuaternionTest'>
- <ObjectChild key='the_float_eigen_matrix'>
+
+ <ObjectChild key='the_float_quaternion'>
<EigenQuaternion type="float" />
</ObjectChild>
- <ObjectChild key='the_double_eigen_matrix'>
+
+ <ObjectChild key='the_double_quaternion'>
<EigenQuaternion type="double" />
</ObjectChild>
+
</Object>
+
</GenerateTypes>
</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/core/test/aron/ImageTest.xml b/source/RobotAPI/libraries/aron/core/test/aron/ImageTest.xml
new file mode 100644
index 0000000000000000000000000000000000000000..ab88fd71efc2e0e654296c734fdcb07d0eef7b12
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/test/aron/ImageTest.xml
@@ -0,0 +1,20 @@
+<!-- An RGB and a depth image. -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<opencv2/core/core.hpp>" />
+ </CodeIncludes>
+ <GenerateTypes>
+ <Object name='armarx::ImageTest'>
+
+ <ObjectChild key='the_rgb24_image'>
+ <Image pixelType="rgb24" />
+ </ObjectChild>
+
+ <ObjectChild key='the_depth32_image'>
+ <Image pixelType="depth32" />
+ </ObjectChild>
+
+ </Object>
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/core/test/aron/OpenCVMatTest.xml b/source/RobotAPI/libraries/aron/core/test/aron/OpenCVMatTest.xml
index f4179798c6ed36ee9f48373ebe57c921bfcf448d..1eb67f338db680eb65aa3e8c2bf25e9229f4b0fa 100644
--- a/source/RobotAPI/libraries/aron/core/test/aron/OpenCVMatTest.xml
+++ b/source/RobotAPI/libraries/aron/core/test/aron/OpenCVMatTest.xml
@@ -6,18 +6,27 @@
</CodeIncludes>
<GenerateTypes>
<Object name='armarx::OpenCVMatTest'>
- <ObjectChild key='the_2d_opencv_matrix'>
- <OpenCVMat dimensions="25, 25" type="64F" />
+
+ <ObjectChild key='the_rgb24_image'>
+ <Image pixelType="rgb24" />
+ </ObjectChild>
+
+ <ObjectChild key='the_depth32_image'>
+ <Image pixelType="depth32" />
+ </ObjectChild>
+
+ <!--ObjectChild key='the_2d_opencv_matrix'>
+ <OpenCVMat shape="25, 25" type="64F" />
</ObjectChild>
<ObjectChild key='the_3d_opencv_matrix'>
- <OpenCVMat dimensions="25, 25, 25" type="CV_32F" />
+ <OpenCVMat shape="25, 25, 25" type="CV_32F" />
</ObjectChild>
<ObjectChild key='the_4d_opencv_matrix'>
- <OpenCVMat dimensions="25, 25, 25, 25" type="CV_32S" />
+ <OpenCVMat shape="25, 25, 25, 25" type="CV_32S" />
</ObjectChild>
<ObjectChild key='the_5d_opencv_matrix'>
- <OpenCVMat dimensions="25, 25, 25, 25, 25" type="16U" />
- </ObjectChild>
+ <OpenCVMat shape="25, 25, 25, 25, 25" type="16U" />
+ </ObjectChild-->
</Object>
</GenerateTypes>
</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/core/test/aronCodeGenerationTest.cpp b/source/RobotAPI/libraries/aron/core/test/aronCodeGenerationTest.cpp
index c1db4b859aa464794c3bba2e1025feb5436a5bba..44bbb19a9bad2cb37d6ba4fc9305a0f4bb05f979 100644
--- a/source/RobotAPI/libraries/aron/core/test/aronCodeGenerationTest.cpp
+++ b/source/RobotAPI/libraries/aron/core/test/aronCodeGenerationTest.cpp
@@ -30,9 +30,6 @@
#include <ctime>
#include <numeric>
-// Boost
-#include <boost/algorithm/string.hpp>
-
// Test
#include <RobotAPI/Test.h>
@@ -50,6 +47,7 @@
#include <RobotAPI/libraries/aron/core/test/aron/DictTest.aron.generated.h>
#include <RobotAPI/libraries/aron/core/test/aron/PrimitiveTest.aron.generated.h>
#include <RobotAPI/libraries/aron/core/test/aron/ObjectTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/ImageTest.aron.generated.h>
#include <RobotAPI/libraries/aron/core/test/aron/IVTCByteImageTest.aron.generated.h>
#include <RobotAPI/libraries/aron/core/test/aron/EigenMatrixTest.aron.generated.h>
#include <RobotAPI/libraries/aron/core/test/aron/EigenQuaternionTest.aron.generated.h>
diff --git a/source/RobotAPI/libraries/aron/core/test/aronExtendsTest.cpp b/source/RobotAPI/libraries/aron/core/test/aronExtendsTest.cpp
index c9feaee398008091d3ef9e97161fec750e0b95ac..4cef5c63fa14fcee4121851458802ddd0338bb77 100644
--- a/source/RobotAPI/libraries/aron/core/test/aronExtendsTest.cpp
+++ b/source/RobotAPI/libraries/aron/core/test/aronExtendsTest.cpp
@@ -30,9 +30,6 @@
#include <ctime>
#include <numeric>
-// Boost
-#include <boost/algorithm/string.hpp>
-
// Test
#include <RobotAPI/Test.h>
diff --git a/source/RobotAPI/libraries/aron/core/test/aronNavigateTest.cpp b/source/RobotAPI/libraries/aron/core/test/aronNavigateTest.cpp
index 233814ffd7c60a09fa9588093c7d1e2f726e282e..70adfc5e7647f7a4118d750ca060c5804ae8d84a 100644
--- a/source/RobotAPI/libraries/aron/core/test/aronNavigateTest.cpp
+++ b/source/RobotAPI/libraries/aron/core/test/aronNavigateTest.cpp
@@ -30,9 +30,6 @@
#include <ctime>
#include <numeric>
-// Boost
-#include <boost/algorithm/string.hpp>
-
// Test
#include <RobotAPI/Test.h>
diff --git a/source/RobotAPI/libraries/aron/core/test/aronOperatorTest.cpp b/source/RobotAPI/libraries/aron/core/test/aronOperatorTest.cpp
index 539b6fb0449f1ba3e7258c502e28602a07a5b796..6c1e44fdb6c6164c74dbe73ab067f0526dea7317 100644
--- a/source/RobotAPI/libraries/aron/core/test/aronOperatorTest.cpp
+++ b/source/RobotAPI/libraries/aron/core/test/aronOperatorTest.cpp
@@ -30,9 +30,6 @@
#include <ctime>
#include <numeric>
-// Boost
-#include <boost/algorithm/string.hpp>
-
// Test
#include <RobotAPI/Test.h>
diff --git a/source/RobotAPI/libraries/aron/core/test/aronRandomizedTest.cpp b/source/RobotAPI/libraries/aron/core/test/aronRandomizedTest.cpp
index d579f3a6dab0e0a432b8a383f47be4496dcca137..4ff0724c95ca3c0cbc31dd051b6ba3afdff37fe9 100644
--- a/source/RobotAPI/libraries/aron/core/test/aronRandomizedTest.cpp
+++ b/source/RobotAPI/libraries/aron/core/test/aronRandomizedTest.cpp
@@ -24,11 +24,46 @@
#define ARMARX_BOOST_TEST
-// STD/STL
-#include <iostream>
-#include <cstdlib>
-#include <ctime>
-#include <numeric>
+
+// Generated File - include them first to check whether their includes are complete.
+#include <RobotAPI/libraries/aron/core/test/aron/ListTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/DictTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PrimitiveTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/ObjectTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/ImageTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/IVTCByteImageTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/EigenMatrixTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/EigenQuaternionTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/OpenCVMatTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PCLPointCloudTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PositionTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/OrientationTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PoseTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/EnumTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/OptionalTest.aron.generated.h>
+
+// Aron
+#include <RobotAPI/libraries/aron/core/Debug.h>
+#include <RobotAPI/libraries/aron/core/Randomizer.h>
+
+#include <RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h>
+#include <RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h>
+
+#include <RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.h>
+#include <RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
+
+#include <RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.h>
+#include <RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.h>
+
+#include <RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h>
+#include <RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
+
+
+// ArmarX
+#include <ArmarXCore/libraries/cppgen/CppMethod.h>
+#include <ArmarXCore/libraries/cppgen/CppClass.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+
// IVT
#include <Image/ByteImage.h>
@@ -43,229 +78,201 @@
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
-// Boost
-#include <boost/algorithm/string.hpp>
-
-// Test
-#include <RobotAPI/Test.h>
-
-// ArmarX
-#include <ArmarXCore/libraries/cppgen/CppMethod.h>
-#include <ArmarXCore/libraries/cppgen/CppClass.h>
-#include <RobotAPI/libraries/aron/core/Exception.h>
-
-// Aron
-#include <RobotAPI/libraries/aron/core/Debug.h>
-#include <RobotAPI/libraries/aron/core/Randomizer.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h>
+// STD/STL
+#include <iostream>
+#include <cstdlib>
+#include <ctime>
+#include <numeric>
-#include <RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.h>
+// Test
+#include <RobotAPI/Test.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
// Files without code generation
//#include "aronDataWithoutCodeGeneration.h"
-// Generated File
-#include <RobotAPI/libraries/aron/core/test/aron/ListTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/DictTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/PrimitiveTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/ObjectTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/IVTCByteImageTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/EigenMatrixTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/EigenQuaternionTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/OpenCVMatTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/PCLPointCloudTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/PositionTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/OrientationTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/PoseTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/EnumTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/OptionalTest.aron.generated.h>
using namespace armarx;
using namespace aron;
+
template <typename T>
-void runTestWithInstances(T& k1, T& k2)
+void test_toAronType(T& in, T& out)
{
- // assumes not nullptrs as k1 and k2. If you have a maybe type then make sure that it is set properly
+ nlohmann::json in_type_json;
+ nlohmann::json out_type_json;
- int echo_debug_lv = 1;
+ BOOST_TEST_CONTEXT("getting in type")
+ {
+ typenavigator::ObjectNavigatorPtr in_type_nav = in.toAronType();
- Randomizer r;
+ type::AronObjectPtr in_type = in_type_nav->toAronObjectPtr();
+ BOOST_CHECK(in_type);
+
+ armarx::aron::typeIO::writer::NlohmannJSONWriter json_writer_in;
+ in.writeType(json_writer_in);
+ in_type_json = json_writer_in.getResult();
+ BOOST_CHECK(in_type_json.is_object());
+ }
+ BOOST_TEST_CONTEXT("getting out type")
{
- if (echo_debug_lv)
- {
- std::cout << "\t getting type 1" << std::endl;
- }
- typenavigator::ObjectNavigatorPtr k1_type_nav = k1.toAronType();
- type::AronObjectPtr k1_type = k1_type_nav->toAronObjectPtr();
- BOOST_CHECK_EQUAL((bool) k1_type, true);
+ typenavigator::ObjectNavigatorPtr out_type_nav = out.toAronType();
- armarx::aron::typeIO::writer::NlohmannJSONWriter json_writer_k1;
- k1.writeType(json_writer_k1);
- nlohmann::json k1_type_json = json_writer_k1.getResult();
- BOOST_CHECK_EQUAL(k1_type_json.is_object(), true);
+ type::AronObjectPtr out_type = out_type_nav->toAronObjectPtr();
+ BOOST_CHECK(out_type);
- if (echo_debug_lv)
- {
- std::cout << "\t getting type 2" << std::endl;
- }
- typenavigator::ObjectNavigatorPtr k2_type_nav = k2.toAronType();
- type::AronObjectPtr k2_type = k2_type_nav->toAronObjectPtr();
- BOOST_CHECK_EQUAL((bool) k2_type, true);
-
- armarx::aron::typeIO::writer::NlohmannJSONWriter json_writer_k2;
- k2.writeType(json_writer_k2);
- nlohmann::json k2_type_json = json_writer_k2.getResult();
- BOOST_CHECK_EQUAL(k2_type_json.is_object(), true);
- BOOST_CHECK_EQUAL(k1_type_json.dump(2), k2_type_json.dump(2));
+ armarx::aron::typeIO::writer::NlohmannJSONWriter json_writer_out;
+ out.writeType(json_writer_out);
+ out_type_json = json_writer_out.getResult();
+ BOOST_CHECK(out_type_json.is_object());
}
- {
- typenavigator::ObjectNavigatorPtr k1_type_nav = k1.toAronType();
- datanavigator::DictNavigatorPtr k1_aron_nav = k1.toAron();
+ BOOST_CHECK_EQUAL(out_type_json.dump(2), in_type_json.dump(2));
+}
- if (echo_debug_lv)
- {
- std::cout << "\t initialize aron 1 randomly" << std::endl;
- }
- r.initializeRandomly(k1_aron_nav, k1_type_nav);
- if (echo_debug_lv)
- {
- std::cout << "\t getting aron 1" << std::endl;
- }
- data::AronDictPtr k1_aron = k1_aron_nav->toAronDictPtr();
- BOOST_CHECK_EQUAL((bool) k1_aron, true);
- if (echo_debug_lv >= 2)
- {
- std::cout << "K1 Aron:" << std::endl;
- std::cout << armarx::aron::Debug::AronDataPtrToString(k1_aron) << std::endl;
- std::cout << "K2 Aron:" << std::endl;
- std::cout << armarx::aron::Debug::AronDataPtrToString(k2.toAron()->toAronDictPtr()) << std::endl;
- }
- if (echo_debug_lv)
- {
- std::cout << "\t setting aron 2 from aron 1" << std::endl;
- }
- k2.fromAron(k1_aron);
+template <typename T>
+void test_toAron(T& in, T& out)
+{
+ datanavigator::DictNavigatorPtr in_aron_nav = in.toAron();
+ Randomizer r;
- if (echo_debug_lv)
- {
- std::cout << "\t getting aron 2" << std::endl;
- }
- datanavigator::DictNavigatorPtr k2_aron_nav = k2.toAron();
- BOOST_CHECK_EQUAL((*k1_aron_nav == *k2_aron_nav), true);
+ BOOST_TEST_CONTEXT("initialize in aron randomly")
+ {
+ typenavigator::ObjectNavigatorPtr in_type_nav = in.toAronType();
+ r.initializeRandomly(in_aron_nav, in_type_nav);
+ }
- data::AronDictPtr k2_aron = k2_aron_nav->toAronDictPtr();
- BOOST_CHECK_EQUAL((bool) k2_aron, true);
+ BOOST_TEST_INFO("getting in aron");
+ data::AronDictPtr in_aron = in_aron_nav->toAronDictPtr();
+ BOOST_CHECK(in_aron);
- if (echo_debug_lv)
- {
- std::cout << "\t setting aron 1 from aron 2 and check for equality" << std::endl;
- }
- k1.fromAron(k2_aron_nav);
+ BOOST_TEST_MESSAGE("in aron: \n" << armarx::aron::Debug::AronDataPtrToString(in_aron));
+ BOOST_TEST_MESSAGE("out aron: \n" << armarx::aron::Debug::AronDataPtrToString(out.toAron()->toAronDictPtr()));
- if (echo_debug_lv >= 2)
- {
- std::cout << "K1 Aron:" << std::endl;
- std::cout << armarx::aron::Debug::AronDataPtrToString(k1.toAron()->toAronDictPtr()) << std::endl;
- std::cout << "K2 Aron:" << std::endl;
- std::cout << armarx::aron::Debug::AronDataPtrToString(k2.toAron()->toAronDictPtr()) << std::endl;
- }
- BOOST_CHECK_EQUAL((k1 == k2), true);
+ datanavigator::DictNavigatorPtr out_aron_nav;
+ BOOST_TEST_CONTEXT("setting out aron from in aron")
+ {
+ out.fromAron(in_aron);
- datanavigator::DictNavigatorPtr k1_aron_nav_again = k1.toAron();
- BOOST_CHECK_EQUAL((*k1_aron_nav == *k1_aron_nav_again), true);
+ BOOST_TEST_INFO("getting out aron");
+ out_aron_nav = out.toAron();
+ BOOST_CHECK(*in_aron_nav == *out_aron_nav);
+
+ data::AronDictPtr out_aron = out_aron_nav->toAronDictPtr();
+ BOOST_CHECK(out_aron);
}
+ BOOST_TEST_CONTEXT("setting in aron from out aron and check for equality")
{
- typenavigator::ObjectNavigatorPtr k1_type_nav = k1.toAronType();
- datanavigator::DictNavigatorPtr k1_aron_nav = k1.toAron();
+ in.fromAron(out_aron_nav);
- r.initializeRandomly(k1_aron_nav, k1_type_nav);
+ BOOST_TEST_MESSAGE("in Aron: \n" << armarx::aron::Debug::AronDataPtrToString(in.toAron()->toAronDictPtr()));
+ BOOST_TEST_MESSAGE("out Aron: \n" << armarx::aron::Debug::AronDataPtrToString(out.toAron()->toAronDictPtr()));
+ BOOST_CHECK(in == out);
- data::AronDictPtr k1_aron = k1_aron_nav->toAronDictPtr();
- k1.fromAron(k1_aron);
+ datanavigator::DictNavigatorPtr in_aron_nav_again = in.toAron();
+ BOOST_CHECK(*in_aron_nav == *in_aron_nav_again);
+ }
+}
- if (echo_debug_lv)
- {
- std::cout << "\t check JSON export of k and k2 for equality" << std::endl;
- }
- armarx::aron::dataIO::writer::NlohmannJSONWriter json_writer_for_k1;
- armarx::aron::dataIO::writer::NlohmannJSONWriter json_writer_for_k2;
- k1.write(json_writer_for_k1);
- nlohmann::json k1_aron_json = json_writer_for_k1.getResult();
- std::string k1_aron_json_str = k1_aron_json.dump(4);
+template <typename T>
+void test_toJson(T& in, T& out)
+{
+ datanavigator::DictNavigatorPtr in_aron_nav = in.toAron();
- armarx::aron::dataIO::writer::NlohmannJSONWriter direct_json_writer_for_k1;
- armarx::aron::dataIO::Visitor::VisitAndSetup(direct_json_writer_for_k1, k1_aron);
- nlohmann::json direct_k1_aron_json = direct_json_writer_for_k1.getResult();
- std::string direct_k1_aron_json_str = direct_k1_aron_json.dump(4);
+ Randomizer r;
+ {
+ typenavigator::ObjectNavigatorPtr in_type_nav = in.toAronType();
+ r.initializeRandomly(in_aron_nav, in_type_nav);
+ }
- if (echo_debug_lv >= 2)
- {
- std::cout << "\t K1 as json: " << std::endl << k1_aron_json_str << std::endl;
- std::cout << "\t K1 as direct json: " << std::endl << direct_k1_aron_json_str << std::endl;
- }
- BOOST_CHECK_EQUAL((k1_aron_json_str == direct_k1_aron_json_str), true);
+ data::AronDictPtr in_aron = in_aron_nav->toAronDictPtr();
+ in.fromAron(in_aron);
- armarx::aron::dataIO::reader::NlohmannJSONReader json_reader_for_k2(k1_aron_json);
+ armarx::aron::dataIO::writer::NlohmannJSONWriter json_writer_for_in;
+ armarx::aron::dataIO::writer::NlohmannJSONWriter json_writer_for_out;
- k2.read(json_reader_for_k2);
- k2.write(json_writer_for_k2);
+ nlohmann::json in_aron_json;
+ std::string in_aron_json_str;
+ BOOST_TEST_CONTEXT("check JSON export of k and out for equality")
+ {
+ in.write(json_writer_for_in);
+ in_aron_json = json_writer_for_in.getResult();
+ in_aron_json_str = in_aron_json.dump(4);
- nlohmann::json k2_aron_json = json_writer_for_k2.getResult();
- std::string k2_aron_json_str = k2_aron_json.dump(4);
+ armarx::aron::dataIO::writer::NlohmannJSONWriter direct_json_writer_for_in;
+ armarx::aron::dataIO::Visitor::VisitAndSetup(direct_json_writer_for_in, in_aron);
+ nlohmann::json direct_in_aron_json = direct_json_writer_for_in.getResult();
+ std::string direct_in_aron_json_str = direct_in_aron_json.dump(4);
- if (echo_debug_lv >= 2)
+ BOOST_TEST_CONTEXT( "\n> in as json: \n" << in_aron_json_str
+ << "\n> n as direct json: \n" << direct_in_aron_json_str)
{
- std::cout << "\t K1 as json: " << std::endl << k1_aron_json_str << std::endl;
- std::cout << "\t K2 as json: " << std::endl << k2_aron_json_str << std::endl;
+ BOOST_CHECK(in_aron_json_str == direct_in_aron_json_str);
}
- BOOST_CHECK_EQUAL((k1_aron_json_str == k2_aron_json_str), true);
+ }
+
+ armarx::aron::dataIO::reader::NlohmannJSONReader json_reader_for_out(in_aron_json);
+
+ out.read(json_reader_for_out);
+ out.write(json_writer_for_out);
+
+ nlohmann::json out_aron_json = json_writer_for_out.getResult();
+ std::string out_aron_json_str = out_aron_json.dump(4);
+
+ BOOST_TEST_CONTEXT( "\n> in as json: \n" << in_aron_json_str
+ << "\n> out as json: \n" << out_aron_json_str)
+ {
+ BOOST_CHECK(in_aron_json_str == out_aron_json_str);
}
}
-BOOST_AUTO_TEST_CASE(AronListTest)
+
+template <typename T>
+void runTestWithInstances(T& in, T& out)
+{
+ // assumes not nullptrs as in and out. If you have a maybe type then make sure that it is set properly
+
+ test_toAronType(in, out);
+ test_toAron(in, out);
+ test_toJson(in, out);
+}
+
+
+BOOST_AUTO_TEST_CASE(test_List)
{
- std::cout << "Running List test" << std::endl;
+ BOOST_TEST_MESSAGE("Running List test");
ListTest l;
ListTest l2;
runTestWithInstances<ListTest>(l, l2);
}
-BOOST_AUTO_TEST_CASE(AronDictTest)
+BOOST_AUTO_TEST_CASE(test_Dict)
{
- std::cout << "Running Dict test" << std::endl;
+ BOOST_TEST_MESSAGE("Running Dict test");
DictTest d;
DictTest d2;
runTestWithInstances<DictTest>(d, d2);
}
-BOOST_AUTO_TEST_CASE(AronPrimitiveTest)
+BOOST_AUTO_TEST_CASE(test_Primitive)
{
- std::cout << "Running Primitive test" << std::endl;
+ BOOST_TEST_MESSAGE("Running Primitive test");
PrimitiveTest p;
PrimitiveTest p2;
runTestWithInstances<PrimitiveTest>(p, p2);
}
-BOOST_AUTO_TEST_CASE(AronObjectTest)
+BOOST_AUTO_TEST_CASE(test_Object)
{
- std::cout << "Running Object test" << std::endl;
+ BOOST_TEST_MESSAGE("Running Object test");
ObjectTest1 o1;
ObjectTest1 o12;
runTestWithInstances<ObjectTest1>(o1, o12);
@@ -275,9 +282,9 @@ BOOST_AUTO_TEST_CASE(AronObjectTest)
runTestWithInstances<ObjectTest2>(o2, o22);
}
-BOOST_AUTO_TEST_CASE(AronImageTest)
+BOOST_AUTO_TEST_CASE(test_IVTCByteImage)
{
- std::cout << "Running Image test" << std::endl;
+ BOOST_TEST_MESSAGE("Running Image test");
IVTCByteImageTest ii;
IVTCByteImageTest ii2;
@@ -296,32 +303,36 @@ BOOST_AUTO_TEST_CASE(AronImageTest)
runTestWithInstances<IVTCByteImageTest>(ii, ii2);
}
-BOOST_AUTO_TEST_CASE(AronEigenMatrixTest)
+BOOST_AUTO_TEST_CASE(test_EigenMatrix)
{
// Eigen may cause problems with dimensions > 145
- std::cout << "Running EigenMatrix test" << std::endl;
+ BOOST_TEST_MESSAGE("Running EigenMatrix test");
EigenMatrixTest em;
EigenMatrixTest em2;
runTestWithInstances<EigenMatrixTest>(em, em2);
}
-BOOST_AUTO_TEST_CASE(AronOpenCVTest)
+BOOST_AUTO_TEST_CASE(test_Image)
{
- std::cout << "Running OpenCVMat test" << std::endl;
- OpenCVMatTest ocv;
- OpenCVMatTest ocv2;
-
- ocv.the_2d_opencv_matrix = cv::Mat(2, 2, CV_64F);
- ocv.the_3d_opencv_matrix = cv::Mat(std::vector<int>({2, 2, 2}), CV_32F);
- ocv.the_4d_opencv_matrix = cv::Mat(std::vector<int>({2, 2, 2, 2}), CV_16S);
- ocv.the_5d_opencv_matrix = cv::Mat(std::vector<int>({2, 2, 2, 2, 2}), CV_8U);
-
- runTestWithInstances<OpenCVMatTest>(ocv, ocv2);
+ BOOST_TEST_MESSAGE("Running Image test");
+ ImageTest image;
+ ImageTest image2;
+
+ image.the_rgb24_image.create(3, 4, image.the_rgb24_image.type());
+ image.the_depth32_image.create(3, 4, image.the_depth32_image.type());
+
+ /* Could be used for ndarray test
+ image.the_2d_opencv_matrix = cv::Mat(2, 2, CV_64F);
+ image.the_3d_opencv_matrix = cv::Mat(std::vector<int>({2, 2, 2}), CV_32F);
+ image.the_4d_opencv_matrix = cv::Mat(std::vector<int>({2, 2, 2, 2}), CV_16S);
+ image.the_5d_opencv_matrix = cv::Mat(std::vector<int>({2, 2, 2, 2, 2}), CV_8U);
+ */
+ runTestWithInstances<ImageTest>(image, image2);
}
-BOOST_AUTO_TEST_CASE(AronPCLPointCloudTest)
+BOOST_AUTO_TEST_CASE(test_PCLPointCloud)
{
- std::cout << "Running PCLPointCloud test" << std::endl;
+ BOOST_TEST_MESSAGE("Running PCLPointCloud test");
PointCloudTest pc;
PointCloudTest pc2;
@@ -334,41 +345,41 @@ BOOST_AUTO_TEST_CASE(AronPCLPointCloudTest)
runTestWithInstances<PointCloudTest>(pc, pc2);
}
-BOOST_AUTO_TEST_CASE(AronEigenQuaternionTest)
+BOOST_AUTO_TEST_CASE(test_EigenQuaternion)
{
- std::cout << "Running EigenQuaternion test" << std::endl;
+ BOOST_TEST_MESSAGE("Running EigenQuaternion test");
EigenQuaternionTest eq;
EigenQuaternionTest eq2;
runTestWithInstances<EigenQuaternionTest>(eq, eq2);
}
-BOOST_AUTO_TEST_CASE(AronPositionTest)
+BOOST_AUTO_TEST_CASE(test_Position)
{
- std::cout << "Running Position test" << std::endl;
+ BOOST_TEST_MESSAGE("Running Position test");
PositionTest pc;
PositionTest pc2;
runTestWithInstances<PositionTest>(pc, pc2);
}
-BOOST_AUTO_TEST_CASE(AronOrientationTest)
+BOOST_AUTO_TEST_CASE(test_Orientation)
{
- std::cout << "Running Orientation test" << std::endl;
+ BOOST_TEST_MESSAGE("Running Orientation test");
OrientationTest pc;
OrientationTest pc2;
runTestWithInstances<OrientationTest>(pc, pc2);
}
-BOOST_AUTO_TEST_CASE(AronPoseTest)
+BOOST_AUTO_TEST_CASE(test_Pose)
{
- std::cout << "Running Pose test" << std::endl;
+ BOOST_TEST_MESSAGE("Running Pose test");
PoseTest pc;
PoseTest pc2;
runTestWithInstances<PoseTest>(pc, pc2);
}
-BOOST_AUTO_TEST_CASE(AronOptionalTest)
+BOOST_AUTO_TEST_CASE(test_Optional)
{
- std::cout << "Running Optional test" << std::endl;
+ BOOST_TEST_MESSAGE("Running Optional test");
OptionalTest pc;
OptionalTest pc2;
runTestWithInstances<OptionalTest>(pc, pc2);
diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index 9babf0b23f1eeae6bc2bc01e9238d93809c6c40c..1e4b08820998f187015b03173f5e38c6fee21a14 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -31,7 +31,9 @@ set(LIB_FILES
RobotStatechartContext.cpp
RobotPool.cpp
checks/ConditionCheckMagnitudeChecks.cpp
+ observerfilters/OffsetFilter.cpp
observerfilters/PoseAverageFilter.cpp
+ observerfilters/PoseMedianFilter.cpp
observerfilters/PoseMedianOffsetFilter.cpp
observerfilters/MedianDerivativeFilterV3.cpp
RobotAPIObjectFactories.cpp
@@ -58,7 +60,6 @@ set(LIB_FILES
)
set(LIB_HEADERS
- EigenStl.h
PIDController.h
MultiDimPIDController.h
Trajectory.h
diff --git a/source/RobotAPI/libraries/core/EigenStl.h b/source/RobotAPI/libraries/core/EigenStl.h
deleted file mode 100644
index bf2bb0655ca86687aaf46ef7cbe5136ee42240a8..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/core/EigenStl.h
+++ /dev/null
@@ -1,37 +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 Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-
-// allow std vector to be used with Eigen objects
-#include<Eigen/StdVector>
-#ifndef EIGEN_STL_VECTOR_SPECIFICATION_DEFINED
-#define EIGEN_STL_VECTOR_SPECIFICATION_DEFINED
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2f)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3f)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector4f)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXf)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix2f)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix3f)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix4f)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::MatrixXf)
-
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3i)
-#endif
diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp
index f8a2c3f48687c98c96b56d44cd574f07c5a5bae9..852477268e44969670b78789e31f1aae57848fac 100644
--- a/source/RobotAPI/libraries/core/FramedPose.cpp
+++ b/source/RobotAPI/libraries/core/FramedPose.cpp
@@ -26,11 +26,14 @@
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/RobotConfig.h>
+
+
using namespace Eigen;
template class ::IceInternal::Handle<::armarx::FramedPose>;
diff --git a/source/RobotAPI/libraries/core/LinkedPose.cpp b/source/RobotAPI/libraries/core/LinkedPose.cpp
index 00391df0cba8d2a9b94cb368cdf16738c7e4c2c8..10b93907b5873f521378987329895c1a7033a960 100644
--- a/source/RobotAPI/libraries/core/LinkedPose.cpp
+++ b/source/RobotAPI/libraries/core/LinkedPose.cpp
@@ -290,4 +290,10 @@ namespace armarx
}
+ void VariantType::suppressWarningUnusedVariableForLinkedPoseAndDirection()
+ {
+ ARMARX_DEBUG_S << VAROUT(LinkedPose);
+ ARMARX_DEBUG_S << VAROUT(LinkedDirection);
+ }
+
}
diff --git a/source/RobotAPI/libraries/core/LinkedPose.h b/source/RobotAPI/libraries/core/LinkedPose.h
index c0cceefce563a026803846773fd805d522031186..71a4240c81060d86e75cbf204ef3de726bd685a3 100644
--- a/source/RobotAPI/libraries/core/LinkedPose.h
+++ b/source/RobotAPI/libraries/core/LinkedPose.h
@@ -42,11 +42,8 @@ namespace armarx::VariantType
// variant types
const VariantTypeId LinkedPose = Variant::addTypeName("::armarx::LinkedPoseBase");
const VariantTypeId LinkedDirection = Variant::addTypeName("::armarx::LinkedDirectionBase");
- inline void suppressWarningUnusedVariableForLinkedPoseAndDirection()
- {
- ARMARX_DEBUG_S << VAROUT(LinkedPose);
- ARMARX_DEBUG_S << VAROUT(LinkedDirection);
- }
+
+ void suppressWarningUnusedVariableForLinkedPoseAndDirection();
}
namespace armarx
diff --git a/source/RobotAPI/libraries/core/RobotStatechartContext.cpp b/source/RobotAPI/libraries/core/RobotStatechartContext.cpp
index bef441657c7653bfb75458789516a13074576a9d..0aa460711fa83b19dc8902bd0c4cfd604d8b633b 100644
--- a/source/RobotAPI/libraries/core/RobotStatechartContext.cpp
+++ b/source/RobotAPI/libraries/core/RobotStatechartContext.cpp
@@ -28,9 +28,7 @@
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <ArmarXCore/statechart/Statechart.h>
-#include <boost/algorithm/string/split.hpp>
-#include <boost/algorithm/string/classification.hpp>
-#include <boost/algorithm/string/trim.hpp>
+#include <SimoxUtility/algorithm/string/string_tools.h>
namespace armarx
{
@@ -51,12 +49,11 @@ namespace armarx
if (!getProperty<std::string>("HandUnits").getValue().empty())
{
std::string handUnitsProp = getProperty<std::string>("HandUnits").getValue();
- std::vector<std::string> handUnitList;
- boost::split(handUnitList, handUnitsProp, boost::is_any_of(","));
+ std::vector<std::string> handUnitList = simox::alg::split(handUnitsProp, ",");
for (size_t i = 0; i < handUnitList.size(); i++)
{
- boost::algorithm::trim(handUnitList.at(i));
+ simox::alg::trim(handUnitList.at(i));
usingProxy(handUnitList.at(i));
}
}
@@ -96,12 +93,11 @@ namespace armarx
if (!getProperty<std::string>("HandUnits").getValue().empty())
{
std::string handUnitsProp = getProperty<std::string>("HandUnits").getValue();
- std::vector<std::string> handUnitList;
- boost::split(handUnitList, handUnitsProp, boost::is_any_of(","));
+ std::vector<std::string> handUnitList = simox::alg::split(handUnitsProp, ",");
for (size_t i = 0; i < handUnitList.size(); i++)
{
- boost::algorithm::trim(handUnitList.at(i));
+ simox::alg::trim(handUnitList.at(i));
HandUnitInterfacePrx handPrx = getProxy<HandUnitInterfacePrx>(handUnitList.at(i));
handUnits[handUnitList.at(i)] = handPrx;
ARMARX_INFO << "Fetched handUnit proxy " << handUnitList.at(i) << ": " << handPrx << flush;
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index 990de647c0bb74092d96a7774830cd1fb6f96f98..774b324ee1d50515576625900555a8ef15510758 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -30,15 +30,11 @@
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/observers/variant/Variant.h>
-#include <boost/tokenizer.hpp>
-#include <boost/unordered_map.hpp>
#include <boost/multi_index_container.hpp>
#include <boost/multi_index/hashed_index.hpp>
#include <boost/multi_index/random_access_index.hpp>
#include <boost/multi_index/ordered_index.hpp>
#include <boost/multi_index/member.hpp>
-#include <boost/typeof/typeof.hpp>
-#include <boost/type_traits/is_arithmetic.hpp>
#include <Eigen/Core>
@@ -200,7 +196,7 @@ namespace armarx
Trajectory() {}
Trajectory(const Trajectory& source);
template <typename T>
- Trajectory(const std::vector<T>& data, const Ice::DoubleSeq timestamps = Ice::DoubleSeq(), const std::string& dimensionName = "", typename boost::enable_if<boost::is_arithmetic< T > >::type* t = 0)
+ Trajectory(const std::vector<T>& data, const Ice::DoubleSeq timestamps = Ice::DoubleSeq(), const std::string& dimensionName = "", typename std::enable_if_t<std::is_arithmetic_v< T > >* t = 0)
{
if (data.size() == 0)
{
diff --git a/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.cpp b/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.cpp
index 4232b3c8dcf97dfe7ce3ad70987c36c558561fa5..9f1ee0943cdbc899358d2f074bac5151932e3703 100644
--- a/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.cpp
+++ b/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.cpp
@@ -25,6 +25,7 @@
#include <ArmarXCore/core/util/StringHelpers.h>
+
namespace armarx
{
diff --git a/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h b/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h
index 1009903e4fb1ddb631f11482a459098452e15419..6c936c777acfbdd0b1d066ca0f5b2fcbbfef4072 100644
--- a/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h
+++ b/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h
@@ -23,11 +23,13 @@
*/
#pragma once
-#include <ArmarXCore/core/system/ImportExport.h>
-#include <ArmarXCore/observers/ConditionCheck.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/LinkedPose.h>
+#include <ArmarXCore/observers/ConditionCheck.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+
namespace armarx
{
diff --git a/source/RobotAPI/libraries/core/math/StatUtils.h b/source/RobotAPI/libraries/core/math/StatUtils.h
index d2245928ea648583f10fc6ff132cafd063e3f1e9..679878188285c3ef977746d7f7e1431ea7e00319 100644
--- a/source/RobotAPI/libraries/core/math/StatUtils.h
+++ b/source/RobotAPI/libraries/core/math/StatUtils.h
@@ -22,7 +22,9 @@
#pragma once
-#include <math.h>
+#include <ArmarXCore/core/exceptions/LocalException.h>
+
+#include <cmath>
#include <vector>
namespace armarx::math
diff --git a/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h b/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
index 2e330626166b264497319869534e99a2f093896e..569afc8d18e400dac922e9793b8ba0dc256e1cc5 100644
--- a/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
+++ b/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
@@ -22,9 +22,12 @@
#pragma once
-#include <ArmarXCore/observers/filters/DatafieldFilter.h>
#include <RobotAPI/interface/observers/ObserverFilters.h>
+
+#include <ArmarXCore/observers/filters/DatafieldFilter.h>
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
#include <algorithm>
namespace armarx::filters
diff --git a/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp b/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp
index eaf21147840e8ec1c2e95fa7eedbb218f5518ed0..699dc495760c1fb1862fe4eb0a28986361448ce8 100644
--- a/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp
+++ b/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp
@@ -21,8 +21,12 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
+
#include "MedianDerivativeFilterV3.h"
+#include <ArmarXCore/core/logging/Logging.h>
+
+
using namespace armarx;
using namespace filters;
diff --git a/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.h b/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.h
index c7881099cb51adbf5fcd1be05034d81698f889c6..382e5c3d6334d2d3c24ccf8186b68226232c6dcd 100644
--- a/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.h
+++ b/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.h
@@ -23,7 +23,6 @@
*/
#pragma once
-#include <RobotAPI/libraries/core/EigenStl.h>
#include <ArmarXCore/observers/filters/MedianFilter.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/interface/core/PoseBase.h>
diff --git a/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.cpp b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d8a3246de16fa15282559204e96f45f516567d7c
--- /dev/null
+++ b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.cpp
@@ -0,0 +1,109 @@
+#include "OffsetFilter.h"
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
+
+
+namespace armarx::filters
+{
+
+ OffsetFilter::OffsetFilter()
+ {
+ firstRun = true;
+ windowFilterSize = 1;
+ }
+
+
+ VariantBasePtr OffsetFilter::calculate(const Ice::Current&) const
+ {
+ std::unique_lock lock(historyMutex);
+
+ VariantPtr newVariant;
+
+ if (initialValue && dataHistory.size() > 0)
+ {
+ VariantTypeId type = dataHistory.begin()->second->getType();
+ VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
+
+ if (currentValue->getType() != initialValue->getType())
+ {
+ ARMARX_ERROR_S << "Types in OffsetFilter are different: " << Variant::typeToString(currentValue->getType()) << " and " << Variant::typeToString(initialValue->getType());
+ return nullptr;
+ }
+
+ if (type == VariantType::Int)
+ {
+ int newValue = dataHistory.rbegin()->second->getInt() - initialValue->getInt();
+ newVariant = new Variant(newValue);
+ }
+ else if (type == VariantType::Float)
+ {
+ float newValue = dataHistory.rbegin()->second->getFloat() - initialValue->getFloat();
+ newVariant = new Variant(newValue);
+ }
+ else if (type == VariantType::Double)
+ {
+ double newValue = dataHistory.rbegin()->second->getDouble() - initialValue->getDouble();
+ newVariant = new Variant(newValue);
+ }
+ else if (type == VariantType::FramedDirection)
+ {
+ FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(currentValue->get<FramedDirection>());
+ FramedDirectionPtr intialVec = FramedDirectionPtr::dynamicCast(initialValue->get<FramedDirection>());
+ Eigen::Vector3f newValue = vec->toEigen() - intialVec->toEigen();
+
+ newVariant = new Variant(new FramedDirection(newValue, vec->frame, vec->agent));
+ }
+ else if (type == VariantType::FramedPosition)
+ {
+ FramedPositionPtr pos = FramedPositionPtr::dynamicCast(currentValue->get<FramedPosition>());
+ FramedPositionPtr intialPos = FramedPositionPtr::dynamicCast(initialValue->get<FramedPosition>());
+ Eigen::Vector3f newValue = pos->toEigen() - intialPos->toEigen();
+ newVariant = new Variant(new FramedPosition(newValue, pos->frame, pos->agent));
+ }
+ else if (type == VariantType::MatrixFloat)
+ {
+ MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
+ MatrixFloatPtr initialMatrix = MatrixFloatPtr::dynamicCast(initialValue->get<MatrixFloat>());
+ Eigen::MatrixXf newMatrix = matrix->toEigen() - initialMatrix->toEigen();
+ newVariant = new Variant(new MatrixFloat(newMatrix));
+ }
+ }
+
+ return newVariant;
+
+ }
+
+ ParameterTypeList OffsetFilter::getSupportedTypes(const Ice::Current&) const
+ {
+ ParameterTypeList result;
+ result.push_back(VariantType::Int);
+ result.push_back(VariantType::Float);
+ result.push_back(VariantType::Double);
+ result.push_back(VariantType::FramedDirection);
+ result.push_back(VariantType::FramedPosition);
+ result.push_back(VariantType::MatrixFloat);
+ return result;
+ }
+
+ void OffsetFilter::update(Ice::Long timestamp, const VariantBasePtr& value, const Ice::Current& c)
+ {
+ DatafieldFilter::update(timestamp, value, c);
+
+ if (firstRun)
+ {
+ std::unique_lock lock(historyMutex);
+
+ if (dataHistory.size() == 0)
+ {
+ return;
+ }
+
+ initialValue = VariantPtr::dynamicCast(dataHistory.begin()->second);
+ firstRun = false;
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
index 68b2cc80905a7747ab73c04f9d19c873b4e3fb77..4af1082d7fb7ac6fb8aeea67f1491d7f94d2f39d 100644
--- a/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
@@ -23,9 +23,9 @@
#pragma once
#include <RobotAPI/interface/observers/ObserverFilters.h>
+
#include <ArmarXCore/observers/filters/DatafieldFilter.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
+
namespace armarx::filters
{
@@ -43,109 +43,27 @@ namespace armarx::filters
public DatafieldFilter
{
public:
- OffsetFilter()
- {
- firstRun = true;
- windowFilterSize = 1;
- }
+
+ OffsetFilter();
// DatafieldFilterBase interface
public:
- VariantBasePtr calculate(const Ice::Current& c = Ice::emptyCurrent) const override
- {
-
- std::unique_lock lock(historyMutex);
-
- VariantPtr newVariant;
-
- if (initialValue && dataHistory.size() > 0)
- {
- VariantTypeId type = dataHistory.begin()->second->getType();
- VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
-
- if (currentValue->getType() != initialValue->getType())
- {
- ARMARX_ERROR_S << "Types in OffsetFilter are different: " << Variant::typeToString(currentValue->getType()) << " and " << Variant::typeToString(initialValue->getType());
- return NULL;
- }
-
- if (type == VariantType::Int)
- {
- int newValue = dataHistory.rbegin()->second->getInt() - initialValue->getInt();
- newVariant = new Variant(newValue);
- }
- else if (type == VariantType::Float)
- {
- float newValue = dataHistory.rbegin()->second->getFloat() - initialValue->getFloat();
- newVariant = new Variant(newValue);
- }
- else if (type == VariantType::Double)
- {
- double newValue = dataHistory.rbegin()->second->getDouble() - initialValue->getDouble();
- newVariant = new Variant(newValue);
- }
- else if (type == VariantType::FramedDirection)
- {
- FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(currentValue->get<FramedDirection>());
- FramedDirectionPtr intialVec = FramedDirectionPtr::dynamicCast(initialValue->get<FramedDirection>());
- Eigen::Vector3f newValue = vec->toEigen() - intialVec->toEigen();
-
- newVariant = new Variant(new FramedDirection(newValue, vec->frame, vec->agent));
- }
- else if (type == VariantType::FramedPosition)
- {
- FramedPositionPtr pos = FramedPositionPtr::dynamicCast(currentValue->get<FramedPosition>());
- FramedPositionPtr intialPos = FramedPositionPtr::dynamicCast(initialValue->get<FramedPosition>());
- Eigen::Vector3f newValue = pos->toEigen() - intialPos->toEigen();
- newVariant = new Variant(new FramedPosition(newValue, pos->frame, pos->agent));
- }
- else if (type == VariantType::MatrixFloat)
- {
- MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
- MatrixFloatPtr initialMatrix = MatrixFloatPtr::dynamicCast(initialValue->get<MatrixFloat>());
- Eigen::MatrixXf newMatrix = matrix->toEigen() - initialMatrix->toEigen();
- newVariant = new Variant(new MatrixFloat(newMatrix));
- }
- }
-
- return newVariant;
-
- }
- ParameterTypeList getSupportedTypes(const Ice::Current&) const override
- {
- ParameterTypeList result;
- result.push_back(VariantType::Int);
- result.push_back(VariantType::Float);
- result.push_back(VariantType::Double);
- result.push_back(VariantType::FramedDirection);
- result.push_back(VariantType::FramedPosition);
- result.push_back(VariantType::MatrixFloat);
- return result;
- }
+
+ VariantBasePtr calculate(const Ice::Current& = Ice::emptyCurrent) const override;
+
+ ParameterTypeList getSupportedTypes(const Ice::Current&) const override;
+
+
private:
bool firstRun;
VariantPtr initialValue;
+
// DatafieldFilterBase interface
public:
- void update(Ice::Long timestamp, const VariantBasePtr& value, const Ice::Current& c) override
- {
- DatafieldFilter::update(timestamp, value, c);
-
- if (firstRun)
- {
- std::unique_lock lock(historyMutex);
-
- if (dataHistory.size() == 0)
- {
- return;
- }
-
- initialValue = VariantPtr::dynamicCast(dataHistory.begin()->second);
- firstRun = false;
- }
- }
+ void update(Ice::Long timestamp, const VariantBasePtr& value, const Ice::Current& c) override;
+
};
}
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp b/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp
index aa3b14347927cbbcd068f0164f2c92b86012fabd..e0d61134cfb5e85319d9a45af55418cf9f1a9a07 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp
@@ -23,6 +23,9 @@
*/
#include "PoseAverageFilter.h"
+#include <ArmarXCore/core/logging/Logging.h>
+
+
namespace armarx::filters
{
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.cpp b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..72c6e8e8e798c43046f14c3dbcabc73233ab9a0d
--- /dev/null
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.cpp
@@ -0,0 +1,112 @@
+#include "PoseMedianFilter.h"
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/interface/core/PoseBase.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+
+namespace armarx::filters
+{
+
+ PoseMedianFilter::PoseMedianFilter(int windowSize)
+ {
+ this->windowFilterSize = windowSize;
+ }
+
+
+ VariantBasePtr PoseMedianFilter::calculate(const Ice::Current& c) const
+ {
+ std::unique_lock lock(historyMutex);
+
+ if (dataHistory.size() == 0)
+ {
+ return nullptr;
+ }
+
+ VariantTypeId type = dataHistory.begin()->second->getType();
+
+ if (type == VariantType::Vector3
+ or type == VariantType::FramedDirection
+ or type == VariantType::FramedPosition)
+ {
+ Eigen::Vector3f vec;
+ vec.setZero();
+ std::string frame = "";
+ std::string agent = "";
+ VariantPtr var = VariantPtr::dynamicCast(dataHistory.begin()->second);
+
+ if (type == VariantType::FramedDirection)
+ {
+ FramedDirectionPtr p = var->get<FramedDirection>();
+ frame = p->frame;
+ agent = p->agent;
+ }
+ else if (type == VariantType::FramedPosition)
+ {
+ FramedPositionPtr p = var->get<FramedPosition>();
+ frame = p->frame;
+ agent = p->agent;
+ }
+
+ for (int i = 0; i < 3; ++i)
+ {
+ std::vector<double> values;
+
+ for (auto v : dataHistory)
+ {
+ VariantPtr v2 = VariantPtr::dynamicCast(v.second);
+ values.push_back(v2->get<Vector3>()->toEigen()[i]);
+ }
+
+ std::sort(values.begin(), values.end());
+ vec[i] = values.at(values.size() / 2);
+ }
+
+ if (type == VariantType::Vector3)
+ {
+ Vector3Ptr vecVar = new Vector3(vec);
+ return new Variant(vecVar);
+ }
+ else if (type == VariantType::FramedDirection)
+ {
+
+ FramedDirectionPtr vecVar = new FramedDirection(vec, frame, agent);
+ return new Variant(vecVar);
+ }
+ else if (type == VariantType::FramedPosition)
+ {
+ FramedPositionPtr vecVar = new FramedPosition(vec, frame, agent);
+ return new Variant(vecVar);
+ }
+ else
+ {
+ ARMARX_WARNING_S << "Implementation missing here";
+ return nullptr;
+ }
+ }
+ else if (type == VariantType::Double)
+ {
+ // auto values = SortVariants<double>(dataHistory);
+ // return new Variant(values.at(values.size()/2));
+ }
+ else if (type == VariantType::Int)
+ {
+ // auto values = SortVariants<int>(dataHistory);
+ // return new Variant(values.at(values.size()/2));
+ }
+
+ return MedianFilter::calculate(c);
+ }
+
+
+ ParameterTypeList PoseMedianFilter::getSupportedTypes(const Ice::Current& c) const
+ {
+ ParameterTypeList result = MedianFilter::getSupportedTypes(c);
+ result.push_back(VariantType::Vector3);
+ result.push_back(VariantType::FramedDirection);
+ result.push_back(VariantType::FramedPosition);
+ return result;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
index 33bbed17812e9c8d06336da6ce37ca9b7e73703e..bc03128714c8685f07b45b09e1e41996437e697f 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
@@ -23,9 +23,10 @@
*/
#pragma once
+#include <RobotAPI/interface/core/FramedPoseBase.h>
+
#include <ArmarXCore/observers/filters/MedianFilter.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/interface/core/PoseBase.h>
+
namespace armarx::filters
{
@@ -41,108 +42,17 @@ namespace armarx::filters
public MedianFilter
{
public:
- PoseMedianFilter(int windowSize = 11)
- {
- this->windowFilterSize = windowSize;
- }
+ PoseMedianFilter(int windowSize = 11);
// DatafieldFilterBase interface
public:
- VariantBasePtr calculate(const Ice::Current& c) const override
- {
- std::unique_lock lock(historyMutex);
-
- if (dataHistory.size() == 0)
- {
- return NULL;
- }
-
- VariantTypeId type = dataHistory.begin()->second->getType();
-
- if ((type == VariantType::Vector3) || (type == VariantType::FramedDirection) || (type == VariantType::FramedPosition))
- {
-
- Eigen::Vector3f vec;
- vec.setZero();
- std::string frame = "";
- std::string agent = "";
- VariantPtr var = VariantPtr::dynamicCast(dataHistory.begin()->second);
-
- if (type == VariantType::FramedDirection)
- {
- FramedDirectionPtr p = var->get<FramedDirection>();
- frame = p->frame;
- agent = p->agent;
- }
- else if (type == VariantType::FramedPosition)
- {
- FramedPositionPtr p = var->get<FramedPosition>();
- frame = p->frame;
- agent = p->agent;
- }
-
- for (int i = 0; i < 3; ++i)
- {
- std::vector<double> values;
-
- for (auto v : dataHistory)
- {
- VariantPtr v2 = VariantPtr::dynamicCast(v.second);
- values.push_back(v2->get<Vector3>()->toEigen()[i]);
- }
-
- std::sort(values.begin(), values.end());
- vec[i] = values.at(values.size() / 2);
- }
-
- if (type == VariantType::Vector3)
- {
- Vector3Ptr vecVar = new Vector3(vec);
- return new Variant(vecVar);
- }
- else if (type == VariantType::FramedDirection)
- {
-
- FramedDirectionPtr vecVar = new FramedDirection(vec, frame, agent);
- return new Variant(vecVar);
- }
- else if (type == VariantType::FramedPosition)
- {
- FramedPositionPtr vecVar = new FramedPosition(vec, frame, agent);
- return new Variant(vecVar);
- }
- else
- {
- ARMARX_WARNING_S << "Implementation missing here";
- return NULL;
- }
- }
- else if (type == VariantType::Double)
- {
- // auto values = SortVariants<double>(dataHistory);
- // return new Variant(values.at(values.size()/2));
- }
- else if (type == VariantType::Int)
- {
- // auto values = SortVariants<int>(dataHistory);
- // return new Variant(values.at(values.size()/2));
- }
-
- return MedianFilter::calculate(c);
- }
+ VariantBasePtr calculate(const Ice::Current& c) const override;
/**
* @brief This filter supports: Vector3, FramedDirection, FramedPosition
* @return List of VariantTypes
*/
- ParameterTypeList getSupportedTypes(const Ice::Current& c) const override
- {
- ParameterTypeList result = MedianFilter::getSupportedTypes(c);
- result.push_back(VariantType::Vector3);
- result.push_back(VariantType::FramedDirection);
- result.push_back(VariantType::FramedPosition);
- return result;
- }
+ ParameterTypeList getSupportedTypes(const Ice::Current& c) const override;
};
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp b/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp
index e4d4a1444065ad83bbc681fa83832bca5ccabb29..47e3606321c73477abeb0a31bb4cf9990469c075 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp
@@ -21,8 +21,11 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
+
#include "PoseMedianOffsetFilter.h"
+#include <ArmarXCore/core/logging/Logging.h>
+
using namespace armarx;
using namespace filters;
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h b/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h
index 73d79140ce53d28656e5d103f1a38cde1906b403..7e8cc3e23e7fb69753232ffc64e3ce4435b6a11b 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h
@@ -23,7 +23,6 @@
*/
#pragma once
-#include <RobotAPI/libraries/core/EigenStl.h>
#include <ArmarXCore/observers/filters/MedianFilter.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/interface/core/PoseBase.h>
@@ -69,5 +68,3 @@ namespace armarx::filters
};
} // namespace Filters
-
-
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index 65db64e872112548b72048542cfdecb7835cd81f..60577ef2dfb7b01eaf648a1a86008f7a969061dd 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -23,11 +23,6 @@
*/
#include "RemoteRobot.h"
-#include <boost/foreach.hpp>
-#include <boost/format.hpp>
-#include <boost/algorithm/string/split.hpp>
-#include <boost/algorithm/string/classification.hpp>
-
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/RobotConfig.h>
@@ -43,6 +38,8 @@
#include <RobotAPI/libraries/core/FramedPose.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
#include <Eigen/Geometry>
//#define DMES(Obj) cout << format("[%1%|%2%|%3%] %4%") % __FILE__ % __LINE__ % __func__ % Obj;
@@ -153,7 +150,7 @@ namespace armarx
}
NameList nodes = _robot->getRobotNodes();
- BOOST_FOREACH(std::string name, nodes)
+ for (std::string const& name : nodes)
{
storeNodes.push_back(this->getRobotNode(name));
}
@@ -177,7 +174,7 @@ namespace armarx
{
NameList sets = _robot->getRobotNodeSets();
- BOOST_FOREACH(std::string name, sets)
+ for (std::string const& name : sets)
{
storeNodeSet.push_back(this->getRobotNodeSet(name));
}
@@ -312,13 +309,6 @@ namespace armarx
{
SharedRobotNodeInterfacePrx rnRemote = _robot->getRobotNode(childrenBase[i]);
VirtualRobot::RobotNodePtr localNode = createLocalNode(rnRemote, allNodes, childrenMap, robo);
- /* boost::shared_ptr< RemoteRobotNode<VirtualRobotNodeType> > rnRemote = boost::dynamic_pointer_cast<RemoteRobotNode>(rnRemoteBase);
- if (!rnRemote)
- {
- ARMARX_ERROR_S << "RemoteRobot does not know robot node with name " << children[i];
- continue;
- }*/
-
if (!localNode)
{
@@ -413,13 +403,10 @@ namespace armarx
}
CMakePackageFinder project(projectName);
- Ice::StringSeq projectIncludePaths;
+
auto pathsString = project.getDataDir();
ARMARX_DEBUG_S << "Data paths of ArmarX package " << projectName << ": " << pathsString;
- boost::split(projectIncludePaths,
- pathsString,
- boost::is_any_of(";,"),
- boost::token_compress_on);
+ Ice::StringSeq projectIncludePaths = simox::alg::split(pathsString, ";,");
ARMARX_DEBUG_S << "Result: Data paths of ArmarX package " << projectName << ": " << projectIncludePaths;
includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
index 2c259eafc983fc5b78cdffb42c5471578f2ac3c8..e6cc65c23c13e154382042424db8483d879a89b4 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
@@ -22,13 +22,15 @@
* GNU General Public License
*/
#include "RemoteRobot.h"
-#include <boost/foreach.hpp>
-
-#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/libraries/core/FramedPose.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/interface/core/BasicTypes.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <VirtualRobot/VirtualRobot.h>
+
namespace armarx
{
@@ -166,7 +168,7 @@ namespace armarx
vector<RobotNodePtr> result;
RobotPtr robot = this->robot.lock();
- BOOST_FOREACH(string name, nodes)
+ for (std::string const& name : nodes)
{
result.push_back(robot->getRobotNode(name));
}
@@ -201,7 +203,7 @@ namespace armarx
{
NameList nodes = _node->getChildren();
vector<SceneObjectPtr> result;
- BOOST_FOREACH(string name, nodes)
+ for (string const& name : nodes)
{
result.push_back(this->robot.lock()->getRobotNode(name));
}
diff --git a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
index 9f2dc660d9945879bf66c7d5d37e9d15a0865926..a140749a911bcf68bf4ba0c84417509ae8b21101 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
@@ -39,9 +39,7 @@
#include <ArmarXCore/observers/variant/DatafieldRef.h>
#include <VirtualRobot/IK/DifferentialIK.h>
-#include <boost/algorithm/string/trim.hpp>
-#include <boost/algorithm/string/split.hpp>
-#include <boost/algorithm/string/classification.hpp>
+#include <SimoxUtility/algorithm/string/string_tools.h>
#define TCP_POSE_CHANNEL "TCPPose"
#define TCP_TRANS_VELOCITIES_CHANNEL "TCPVelocities"
@@ -350,15 +348,11 @@ void RobotStateObserver::setRobot(RobotPtr robot)
}
else
{
- std::vector<std::string> nodesetNames;
- boost::split(nodesetNames,
- nodesetsString,
- boost::is_any_of(","),
- boost::token_compress_on);
+ std::vector<std::string> nodesetNames = simox::alg::split(nodesetsString, ",");
for (auto name : nodesetNames)
{
- boost::trim(name);
+ simox::alg::trim(name);
auto node = robot->getRobotNode(name);
if (node)
diff --git a/source/RobotAPI/libraries/diffik/DiffIKProvider.h b/source/RobotAPI/libraries/diffik/DiffIKProvider.h
index a75434af401538fc92697010ee6fc3d6b43e9071..2e3b0967afe3971e25c94a4d57f1b5a10b9ae638 100644
--- a/source/RobotAPI/libraries/diffik/DiffIKProvider.h
+++ b/source/RobotAPI/libraries/diffik/DiffIKProvider.h
@@ -23,12 +23,13 @@
#pragma once
-#include <boost/shared_ptr.hpp>
-#include <Eigen/Dense>
+#include <Eigen/Core>
+
+#include <memory>
namespace armarx
{
- typedef boost::shared_ptr<class DiffIKProvider> DiffIKProviderPtr;
+ typedef std::shared_ptr<class DiffIKProvider> DiffIKProviderPtr;
struct DiffIKResult
{
@@ -38,10 +39,13 @@ namespace armarx
Eigen::VectorXf jointValues;
};
+
class DiffIKProvider
{
public:
virtual DiffIKResult SolveAbsolute(const Eigen::Matrix4f& targetPose) = 0;
virtual DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues) = 0;
+
+ virtual ~DiffIKProvider() = default;
};
}
diff --git a/source/RobotAPI/libraries/diffik/GraspTrajectory.h b/source/RobotAPI/libraries/diffik/GraspTrajectory.h
index 3e5633d59e9a2f2c8ad533735370a1f008694089..8cc0a7cad1046733e4e7d52a4fe2d69eff28b660 100644
--- a/source/RobotAPI/libraries/diffik/GraspTrajectory.h
+++ b/source/RobotAPI/libraries/diffik/GraspTrajectory.h
@@ -23,31 +23,38 @@
#pragma once
-#include <boost/shared_ptr.hpp>
-#include <Eigen/Dense>
-#include <VirtualRobot/math/AbstractFunctionR1R6.h>
-#include <VirtualRobot/math/Helpers.h>
-#include <map>
-#include <ArmarXCore/core/exceptions/Exception.h>
-#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlWriter.h>
+
+
#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
-#include <ArmarXGui/libraries/StructuralJson/JPathNavigator.h>
-#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h>
#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+
+#include <ArmarXGui/libraries/StructuralJson/JPathNavigator.h>
+#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h>
+
+#include <ArmarXCore/core/exceptions/Exception.h>
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlWriter.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/interface/serialization/Eigen.h>
+#include <VirtualRobot/math/AbstractFunctionR1R6.h>
+#include <VirtualRobot/math/Helpers.h>
+
+#include <Eigen/Core>
+
+#include <vector>
+#include <map>
+
namespace armarx
{
- typedef boost::shared_ptr<class GraspTrajectory> GraspTrajectoryPtr;
+ typedef std::shared_ptr<class GraspTrajectory> GraspTrajectoryPtr;
class GraspTrajectory
{
public:
class Keypoint;
- typedef boost::shared_ptr<Keypoint> KeypointPtr;
+ typedef std::shared_ptr<Keypoint> KeypointPtr;
class Keypoint
{
diff --git a/source/RobotAPI/libraries/diffik/RobotPlacement.h b/source/RobotAPI/libraries/diffik/RobotPlacement.h
index a677a7a1181b1a88dc731da6f4859c188453f5bf..0eb34d3f3c4cbf43c8b50c78e7e8c9a41a179ec6 100644
--- a/source/RobotAPI/libraries/diffik/RobotPlacement.h
+++ b/source/RobotAPI/libraries/diffik/RobotPlacement.h
@@ -26,11 +26,11 @@
#include "DiffIKProvider.h"
#include "GraspTrajectory.h"
-#include <boost/shared_ptr.hpp>
+#include <memory>
namespace armarx
{
- typedef boost::shared_ptr<class RobotPlacement> RobotPlacementPtr;
+ typedef std::shared_ptr<class RobotPlacement> RobotPlacementPtr;
class RobotPlacement
{
diff --git a/source/RobotAPI/libraries/diffik/SimpleDiffIK.h b/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
index e51be99211a5c8c71f612ab920be6a65bfabdcab..d71ebbee9157cd27e643860fa68287d362d2524a 100644
--- a/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
+++ b/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
@@ -32,7 +32,7 @@
namespace armarx
{
- typedef std::shared_ptr<class SimpleDiffIK> SimpleDiffIKPtr;
+ using SimpleDiffIKPtr = std::shared_ptr<class SimpleDiffIK>;
class SimpleDiffIK
{
@@ -126,4 +126,3 @@ namespace armarx
SimpleDiffIK::Parameters params;
};
}
-