diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
index 487c9c6fc4c732fc1d4fe2f126724cb74c8dd810..72e5b8f33073f716e9d3a299ea26dda288dc6066 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
@@ -42,7 +42,7 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/armem_objects/types.h>
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
index f85a804e97337e1193ca1f38048beb103e65ce59..b4cab3fc71e6d6b1dd92e3128fd3a90a8c191109 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
@@ -14,7 +14,7 @@
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <RobotAPI/libraries/armem/client/ComponentPlugin.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h>
#include <RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h>
diff --git a/source/RobotAPI/components/KITHandUnit/KITHandUnit.cpp b/source/RobotAPI/components/KITHandUnit/KITHandUnit.cpp
index e7d20106efb18cad009e22a32e3528d6c8197e06..fb4cb1d94baf63e8013099a443d18890fb5c6352 100644
--- a/source/RobotAPI/components/KITHandUnit/KITHandUnit.cpp
+++ b/source/RobotAPI/components/KITHandUnit/KITHandUnit.cpp
@@ -165,7 +165,8 @@ namespace armarx
{
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);
}
@@ -173,7 +174,8 @@ namespace armarx
{
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 a47012e9cd16988958672ab6de5f3ece1aedd9d3..997be9e96b87d88593d27fd7120f332a21912b05 100644
--- a/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp
+++ b/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp
@@ -197,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/client/ExampleMemoryClient/ExampleMemoryClient.cpp b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
index 8ae42e00bd5385886696dd6ead35feb927665947..3678ecd55d64e033e03c1e5f7400db875054cc87 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
@@ -34,7 +34,8 @@
#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>
@@ -257,40 +258,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;
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
index 5680e316a6228021932afea5d73abd02fdcda6d8..ca28ab86bc1e755c49b93649fd77b26aac0a4250 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
@@ -34,7 +34,7 @@
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/client/ComponentPlugin.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
namespace armarx
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
index 6650e573785cce732f0f2bbe7c128aa34fc58626..8ab75efc78ddad8492598356df374266947fe515 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
@@ -23,7 +23,7 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/armem/core/Time.h>
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
index 4e1d01f5564e96c7e46ad2db33081835873dd7a6..33770ad5425f8824fca2f40ae45323861f29cc91 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
@@ -33,7 +33,7 @@
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/client/ComponentPlugin.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
index e007e8f5e3ad47c57f9bb71c75e517d57af1addf..879809064b9cdb1429b49925e54ba5765cb534cf 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
@@ -60,8 +60,7 @@ namespace armarx
void ExampleMemory::onInitComponent()
{
- workingMemory.name() = p.memoryName;
- longtermMemory.name() = p.memoryName;
+ this->setMemoryName(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());
@@ -70,7 +69,11 @@ namespace armarx
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()
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp
index c5ba35a13aa4ec127f64d0992021d29262d54407..56c9d08f49759de2f75c8085551ea63fb9263935 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp
@@ -41,7 +41,7 @@ BOOST_AUTO_TEST_CASE(test_ExampleData_type)
{
armarx::aron::typenavigator::ObjectNavigatorPtr type = ExampleData::toAronType();
- BOOST_CHECK_EQUAL(type->childrenSize(), 15);
+ BOOST_CHECK_EQUAL(type->childrenSize(), 16);
armem::wm::Memory memory;
armem::wm::CoreSegment& core = memory.addCoreSegment("ExampleData", type);
diff --git a/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp b/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp
index 350810558ce9d2200e61bda865dd1633da15c128..15253dffb3eb8c7e49c114d2a7659627cd797071 100644
--- a/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp
+++ b/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp
@@ -55,8 +55,7 @@ namespace armarx
void MotionMemory::onInitComponent()
{
- workingMemory.name() = memoryName;
- longtermMemory.name() = memoryName;
+ this->setMemoryName(memoryName);
mdbMotions.onInit();
}
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index c624379b89ae336aa561e364706f3780b7fbf0e9..40ceb2864360a7f9412091a39870c8a51846f015 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
{
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index b90406be31ac76a40172d67076f931ebb1a6ba1a..44332653fc095f826aac0ccf5e5e79c124769dfd 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/ComponentPlugin.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
@@ -79,7 +76,7 @@ namespace armarx::armem::server::obj
public:
ObjectMemory();
- virtual ~ObjectMemory();
+ virtual ~ObjectMemory() override;
/// @see armarx::ManagedIceObject::getDefaultName()
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
index 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..582cf20586d119f1ef232cb32c706bbd1eef1e4e 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -38,7 +38,7 @@
#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
-#include "aron_conversions.h"
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
namespace armarx::armem::server::robot_state
@@ -68,8 +68,7 @@ namespace armarx::armem::server::robot_state
const std::string prefix = "mem.";
- workingMemory.name() = "RobotState";
- longtermMemory.name() = "RobotState";
+ this->setMemoryName("RobotState");
defs->optional(workingMemory.name(), prefix + "MemoryName", "Name of this memory server.");
const std::string robotUnitPrefix{sensorValuePrefix};
@@ -83,8 +82,6 @@ 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.");
descriptionSegment.defineProperties(defs, prefix + "desc.");
proprioceptionSegment.defineProperties(defs, prefix + "prop.");
@@ -127,8 +124,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 +143,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();
@@ -210,18 +206,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();
}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index c89a51ae47158d0c646a63f216f0a4ff397ba5d6..2cd2be9e3a5838f16b1a385e829416429b7869cd 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -35,13 +35,12 @@
#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
#include <RobotAPI/libraries/armem_robot_state/server/description/Segment.h>
-#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
-
-#include "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 +48,6 @@ namespace armarx::plugins
class DebugObserverComponentPlugin;
class RobotUnitComponentPlugin;
}
-
namespace armarx::armem::server::robot_state
{
@@ -90,11 +88,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();
@@ -123,14 +116,13 @@ namespace armarx::armem::server::robot_state
struct RobotUnit
{
int pollFrequency = 50;
- std::string configPath = "NO CONFIG SET";
armarx::plugins::RobotUnitComponentPlugin* plugin = nullptr;
- RobotUnitReader reader;
- RobotStateWriter writer;
+ proprioception::RobotUnitReader reader;
+ proprioception::RobotStateWriter writer;
// queue
- std::queue<RobotUnitData> dataQueue;
+ std::queue<proprioception::RobotUnitData> dataQueue;
mutable std::mutex dataMutex;
};
RobotUnit 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/SkillsMemory.cpp b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
index c0a9fd2f33211d9a130f236b9350836b75b51466..6c86b6afa76fe043b4dea16affb609f7db049caf 100644
--- a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
+++ b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
@@ -29,7 +29,8 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/armem_skills/aron_conversions.h>
@@ -70,7 +71,7 @@ namespace armarx
workingMemory.name() = p.memoryName;
{
- armarx::armem::wm::CoreSegment& c = workingMemory.addCoreSegment(p.statechartCoreSegmentName, armarx::armem::arondto::Statechart::Transition::toAronType());
+ armarx::armem::server::wm::CoreSegment& c = workingMemory.addCoreSegment(p.statechartCoreSegmentName, armarx::armem::arondto::Statechart::Transition::toAronType());
c.addProviderSegment("Transitions", armarx::armem::arondto::Statechart::Transition::toAronType());
}
}
diff --git a/source/RobotAPI/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/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
index dffe7711debc728d596f5ecd7cd851df2527e120..adb7c77bd1e04491b7e16ecb1599119df0ee2ce6 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
@@ -201,7 +201,7 @@ namespace armarx
{
bool expand = item->childCount() == 0;
- TreeWidgetBuilder builder(objectPoses);
+ TreeWidgetBuilder<objpose::ObjectPose> builder;
builder.setNameFn([](const objpose::ObjectPose & pose)
{
return pose.objectID.str();
@@ -245,7 +245,7 @@ namespace armarx
return true;
});
- builder.updateTree(item, objectPoses);
+ builder.updateTreeWithContainer(item, objectPoses);
if (expand)
{
@@ -299,7 +299,7 @@ namespace armarx
{
(void) dataset;
- TreeWidgetBuilder builder(datasetData);
+ TreeWidgetBuilder<std::pair<std::string, std::string>> builder;
builder.setCompareFn([](const std::pair<std::string, std::string>& lhs, QTreeWidgetItem * item)
{
auto rhs = std::make_pair(item->text(0).toStdString(), item->text(1).toStdString());
@@ -324,7 +324,7 @@ namespace armarx
}
return true;
});
- builder.updateTree(datasetItem, datasetData);
+ builder.updateTreeWithContainer(datasetItem, datasetData);
return true;
});
diff --git a/source/RobotAPI/interface/armem/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/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/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 90e2cb1f2278cdac7b6bdcc469314a2ff84a9f3a..e5077dbad6abca53970f20a82f2b4c7534333cc7 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
{
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index ef5f227686bf252f3f7941fd3f0d96aed72442d1..1fdf140a9507cd439b45226e4886ec89fe0ccfcf 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,54 +28,34 @@ set(LIB_FILES
core/Commit.cpp
core/MemoryID.cpp
core/MemoryID_operators.cpp
+ core/operations.cpp
core/SuccessHeader.cpp
core/Time.cpp
- core/ice_conversions.cpp
core/aron_conversions.cpp
+ core/ice_conversions.cpp
core/json_conversions.cpp
core/base/detail/MemoryItem.cpp
- core/base/detail/MaxHistorySize.cpp
core/base/detail/MemoryContainerBase.cpp
- core/base/detail/EntityContainerBase.cpp
core/base/detail/AronTyped.cpp
+ core/base/detail/iteration_mixins.cpp
+ core/base/detail/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
@@ -98,6 +81,15 @@ set(LIB_FILES
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/segment/Segment.cpp
server/segment/SpecializedSegment.cpp
@@ -107,23 +99,8 @@ set(LIB_FILES
server/query_proc/base/CoreSegmentQueryProcessorBase.cpp
server/query_proc/base/MemoryQueryProcessorBase.cpp
- server/query_proc/workingmemory/BaseQueryProcessor.cpp
- server/query_proc/workingmemory/EntityQueryProcessor.cpp
- server/query_proc/workingmemory/ProviderSegmentQueryProcessor.cpp
- server/query_proc/workingmemory/CoreSegmentQueryProcessor.cpp
- server/query_proc/workingmemory/MemoryQueryProcessor.cpp
-
- server/query_proc/longtermmemory/BaseQueryProcessor.cpp
- server/query_proc/longtermmemory/EntityQueryProcessor.cpp
- server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.cpp
- server/query_proc/longtermmemory/CoreSegmentQueryProcessor.cpp
- server/query_proc/longtermmemory/MemoryQueryProcessor.cpp
-
- server/query_proc/diskmemory/BaseQueryProcessor.cpp
- server/query_proc/diskmemory/EntityQueryProcessor.cpp
- server/query_proc/diskmemory/ProviderSegmentQueryProcessor.cpp
- server/query_proc/diskmemory/CoreSegmentQueryProcessor.cpp
- server/query_proc/diskmemory/MemoryQueryProcessor.cpp
+ server/query_proc/ltm.cpp
+ server/query_proc/wm.cpp
mns/MemoryNameSystem.cpp
mns/ComponentPlugin.cpp
@@ -133,15 +110,18 @@ set(LIB_FILES
set(LIB_HEADERS
core.h
+ core/forward_declarations.h
+
core/Commit.h
core/DataMode.h
core/MemoryID.h
core/MemoryID_operators.h
+ core/operations.h
core/SuccessHeader.h
core/Time.h
core/aron_conversions.h
- core/json_conversions.h
core/ice_conversions.h
+ core/json_conversions.h
core/ice_conversions_boost_templates.h
core/ice_conversions_templates.h
@@ -150,10 +130,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,41 +143,18 @@ 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/ice_conversions_templates.h
- core/ice_conversions.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
client.h
+ client/forward_declarations.h
client/ComponentPlugin.h
client/MemoryNameSystem.h
client/MemoryNameSystemComponentPlugin.h
@@ -217,45 +176,42 @@ set(LIB_HEADERS
client/util/SimpleWriterBase.h
server.h
+ server/forward_declarations.h
+
server/ComponentPlugin.h
server/MemoryToIceAdapter.h
server/MemoryRemoteGui.h
server/RemoteGuiAronDataVisitor.h
+ server/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/segment/Segment.h
server/segment/SpecializedSegment.h
server/query_proc.h
+
+ server/query_proc/base.h
server/query_proc/base/BaseQueryProcessorBase.h
server/query_proc/base/EntityQueryProcessorBase.h
server/query_proc/base/ProviderSegmentQueryProcessorBase.h
server/query_proc/base/CoreSegmentQueryProcessorBase.h
server/query_proc/base/MemoryQueryProcessorBase.h
- server/query_proc/workingmemory/BaseQueryProcessor.h
- server/query_proc/workingmemory/EntityQueryProcessor.h
- server/query_proc/workingmemory/ProviderSegmentQueryProcessor.h
- server/query_proc/workingmemory/CoreSegmentQueryProcessor.h
- server/query_proc/workingmemory/MemoryQueryProcessor.h
-
- server/query_proc/longtermmemory/BaseQueryProcessor.h
- server/query_proc/longtermmemory/EntityQueryProcessor.h
- server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.h
- server/query_proc/longtermmemory/CoreSegmentQueryProcessor.h
- server/query_proc/longtermmemory/MemoryQueryProcessor.h
-
- server/query_proc/diskmemory/BaseQueryProcessor.h
- server/query_proc/diskmemory/EntityQueryProcessor.h
- server/query_proc/diskmemory/ProviderSegmentQueryProcessor.h
- server/query_proc/diskmemory/CoreSegmentQueryProcessor.h
- server/query_proc/diskmemory/MemoryQueryProcessor.h
+ server/query_proc/ltm.h
+ server/query_proc/wm.h
mns.h
mns/MemoryNameSystem.h
mns/ComponentPlugin.h
util/util.h
-
)
armarx_add_library(
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
index 4395b55fd45e6bdb019ee1285fbc5cc3dac648f9..79140344ee99a143e29bcc655d6e1ed23d77b041 100644
--- a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
@@ -39,7 +39,32 @@ namespace armarx::armem::client
if (result.success)
{
- this->servers = result.proxies;
+ for (const auto& [name, proxy] : result.proxies)
+ {
+ auto [it, inserted] = servers.try_emplace(name, proxy);
+ // inserted: OK
+ if (not inserted)
+ {
+ // Compare ice identities, update if it changed.
+ if (it->second->ice_getIdentity() != proxy->ice_getIdentity())
+ {
+ // Replace old proxy by new one.
+ it->second = proxy;
+ }
+ }
+ }
+ // Remove all entries which are not there anymore.
+ for (auto it = servers.begin(); it != servers.end();)
+ {
+ if (result.proxies.count(it->first) == 0)
+ {
+ it = servers.erase(it);
+ }
+ else
+ {
+ ++it;
+ }
+ }
}
else
{
@@ -272,11 +297,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())
{
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
index 3ac9f7438408460c157f7484d313ef0d6a3808a6..c2e4d5ed6df5c3e12c0d3e2ada0b93c91e232133 100644
--- a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp
@@ -45,16 +45,4 @@ namespace armarx::armem::client
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
index c0139e609e830e2c07517837f6cc3d48aff434db..059c5f93d5e8d872ecdc9bff86593db2c3de1163 100644
--- a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
@@ -50,10 +50,6 @@ namespace armarx::armem::client
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:
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/forward_declarations.h b/source/RobotAPI/libraries/armem/client/forward_declarations.h
new file mode 100644
index 0000000000000000000000000000000000000000..205f56634456d4179d1baac494837d1ecdfe7069
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/forward_declarations.h
@@ -0,0 +1,24 @@
+#pragma once
+
+// #include <RobotAPI/libraries/armem/core/forward_declarations.h>
+
+
+
+namespace armarx::armem::client::query
+{
+ class Builder;
+}
+namespace armarx::armem::client
+{
+ class ComponentPluginUser;
+ class MemoryNameSystem;
+
+ class Reader;
+ class Writer;
+
+ using QueryBuilder = query::Builder;
+ struct QueryInput;
+ struct QueryResult;
+
+}
+
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/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 4fd0611bb61bcd513de5d9b4c1d066c3dd5814af..f45018ff955fac99b7cf0824eaff7539eee9ac29 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
@@ -5,15 +5,14 @@
#include <SimoxUtility/algorithm/get_map_keys_values.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/logging/Logging.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
@@ -40,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>;
@@ -53,6 +53,8 @@ namespace armarx::armem::base
using EntitySnapshotT = _EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
+ using ChildT = EntitySnapshotT;
+
struct UpdateResult
{
armarx::armem::UpdateType entityUpdateType;
@@ -67,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)
{
}
@@ -83,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.
@@ -372,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
@@ -381,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 31636867698321b0d04a52d4c8946d82d7b78eb8..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
+++ /dev/null
@@ -1,230 +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),
- periodicTransferSettings(other.periodicTransferSettings),
- onFullTransferSettings(other.onFullTransferSettings),
- mongoDBMutex()
- {
- // Do not copy _mutex.
- }
-
-
- Memory::Memory(Memory&& other) :
- Base(other),
- dbsettings(other.dbsettings),
- periodicTransferSettings(other.periodicTransferSettings),
- onFullTransferSettings(other.onFullTransferSettings),
- reloaded(other.reloaded)
- {
- // Do not move _mutex.
- }
-
-
- Memory& Memory::operator=(const Memory& other)
- {
- Base::operator=(other);
- dbsettings = other.dbsettings;
- periodicTransferSettings = other.periodicTransferSettings;
- onFullTransferSettings = other.onFullTransferSettings;
-
- // Don't copy _mutex.
- return *this;
- }
-
-
- Memory& Memory::operator=(Memory&& other)
- {
- Base::operator=(other);
- dbsettings = std::move(other.dbsettings);
- periodicTransferSettings = std::move(other.periodicTransferSettings);
- onFullTransferSettings = std::move(other.onFullTransferSettings);
- reloaded = other.reloaded;
-
- // Don't move _mutex.
- return *this;
- }
-
- bool Memory::checkConnection() const
- {
- // Check connection:
- ARMARX_INFO << "Checking connection";
- if (!MongoDBConnectionManager::ConnectionIsValid(dbsettings))
- {
- ARMARX_WARNING << deactivateSpam("ConnectionIsNotValid")
- << "The connection to mongocxx for memory '" << name() << "' is not valid. Settings are: " << dbsettings.toString()
- << "\nTo start it, run e.g.: \n"
- << "armarx memory start"
- << "\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(periodicTransferSettings.enabled, prefix + "ltm.30_enablePeriodicTransfer", "Enable transfer based on periodic interval.");
- defs->optional(onFullTransferSettings.enabled, prefix + "ltm.31_enableOnFullTransfer", "Enable transfer whenever the wm is full (see maxHistorySize).");
- }
-
- wm::Memory Memory::convert() const
- {
- 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 4902701b33374c75affc2499d9ec93d3a18a9912..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
+++ /dev/null
@@ -1,99 +0,0 @@
-#pragma once
-
-// BaseClass
-#include "../base/MemoryBase.h"
-
-// CoreSegment
-#include "CoreSegment.h"
-
-// WM
-#include "../workingmemory/Memory.h"
-
-// Properties
-#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 PeriodicTransferSettings : public TransferSettings
- {
- bool deleteFromWMOnTransfer = false;
- int frequencyHz = 1;
- };
-
- struct OnFullTransferSettings : public TransferSettings
- {
- enum class Mode
- {
- TRANSFER_LATEST,
- TRANSFER_LEAST_USED
- };
-
- Mode mode;
- int batch_size = 20;
- };
-
-
- 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;
-
- 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 5093ffce2adddfcdd27e1f08529475335fc84ff2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
+++ /dev/null
@@ -1,64 +0,0 @@
-#pragma once
-
-// Base Class
-#include <RobotAPI/libraries/armem/core/base/MemoryBase.h>
-
-// ArmarX
-#include "CoreSegment.h"
-#include "detail/CopyWithoutData.h"
-
-// Properties
-#include <ArmarXCore/core/application/properties/PluginAll.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.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;
-
- // PropertyDefinitions related to LTM
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
-
- /**
- * @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/MemoryNameSystem.cpp b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
index ece0fa74306ed77465ce0bb0c643b781ec704178..6e001784b90104580a49b2510bdb6583fa36783c 100644
--- a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
+++ b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
@@ -98,7 +98,6 @@ namespace armarx::armem::mns
}
catch (const Ice::Exception&)
{
- ;
}
}
diff --git a/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp b/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
index 8c814a160b49b0367cece67d407b986ac4d372e5..18a24d77f82baf4f07879caeb21bb52b308fdac7 100644
--- a/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
@@ -1,13 +1,11 @@
#include "ComponentPlugin.h"
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include "MemoryToIceAdapter.h"
#include <RobotAPI/libraries/armem/core/error.h>
-#include "MemoryToIceAdapter.h"
-
-//#include <RobotAPI/libraries/armem/core/io/diskWriter/NlohmannJSON/NlohmannJSONDiskWriter.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
namespace armarx::armem::server::plugins
@@ -22,8 +20,7 @@ namespace armarx::armem::server::plugins
ComponentPluginUser& parent = this->parent<ComponentPluginUser>();
properties->topic(memoryListener, parent.memoryListenerDefaultName);
- properties->optional(parent.longtermMemoryEnabled, "memory.ltm.00_enabled");
- parent.longtermMemory.defineProperties(properties, "memory");
+ properties->optional(parent.longtermMemoryEnabled, "mem.ltm.00_enabled");
}
@@ -40,7 +37,7 @@ namespace armarx::armem::server::plugins
// establishing connection to ltm and mongodb
if (parent.longtermMemoryEnabled)
{
- parent.longtermMemory.reload();
+ parent.longtermMemoryManager.reload();
}
}
@@ -116,17 +113,25 @@ namespace armarx::armem::server
ComponentPluginUser::~ComponentPluginUser() = default;
+ // Set the name of a memory
+ void ComponentPluginUser::setMemoryName(const std::string& memoryName)
+ {
+ workingMemory.name() = memoryName;
+ longtermMemoryManager.setName(memoryName);
+ }
+
// WRITING
data::AddSegmentsResult ComponentPluginUser::addSegments(const data::AddSegmentsInput& input, const Ice::Current&)
{
+ ARMARX_TRACE;
bool addCoreSegmentOnUsage = false;
return addSegments(input, addCoreSegmentOnUsage);
}
data::AddSegmentsResult ComponentPluginUser::addSegments(const data::AddSegmentsInput& input, bool addCoreSegments)
{
- // std::scoped_lock lock(workingMemoryMutex);
+ ARMARX_TRACE;
data::AddSegmentsResult result = iceMemory.addSegments(input, addCoreSegments);
return result;
}
@@ -134,7 +139,7 @@ namespace armarx::armem::server
data::CommitResult ComponentPluginUser::commit(const data::Commit& commitIce, const Ice::Current&)
{
- // std::scoped_lock lock(workingMemoryMutex);
+ ARMARX_TRACE;
return iceMemory.commit(commitIce);
}
@@ -142,7 +147,7 @@ namespace armarx::armem::server
// READING
armem::query::data::Result ComponentPluginUser::query(const armem::query::data::Input& input, const Ice::Current&)
{
- // std::scoped_lock lock(workingMemoryMutex);
+ ARMARX_TRACE;
return iceMemory.query(input);
}
@@ -150,16 +155,11 @@ namespace armarx::armem::server
// LTM STORING
data::StoreResult ComponentPluginUser::store(const data::StoreInput& input, const Ice::Current&)
{
- std::scoped_lock lock(/*workingMemoryMutex,*/ longtermMemoryMutex);
+ ARMARX_TRACE;
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
index 06ba31f7a128549f51be85de300a92bdd310cf95..f55bfc6c024922f10b84d76a52b21135db58e928 100644
--- a/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
@@ -8,8 +8,8 @@
#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/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/ltm/mongodb/MemoryManager.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h>
#include "MemoryToIceAdapter.h"
@@ -75,6 +75,9 @@ namespace armarx::armem::server
ComponentPluginUser();
virtual ~ComponentPluginUser() override;
+ /// Set the name of the wm and the ltm (if enabled)
+ void setMemoryName(const std::string& memoryName);
+
// WritingInterface interface
virtual data::AddSegmentsResult addSegments(const data::AddSegmentsInput& input, const Ice::Current& = Ice::emptyCurrent) override;
@@ -92,25 +95,26 @@ namespace armarx::armem::server
// 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;
+ server::wm::Memory workingMemory;
// [[deprecated ("The global working memory mutex is deprecated. Use the core segment mutexes instead.")]]
// std::mutex workingMemoryMutex;
+ /// Parameter to indicate whether to use or not to use the ltm feature
bool longtermMemoryEnabled = true;
- ltm::Memory longtermMemory;
- std::mutex longtermMemoryMutex;
+
+ /// A manager class for the ltm. It internally holds a normal wm instance
+ server::ltm::mongodb::MemoryManager longtermMemoryManager;
/// property defaults
std::string memoryListenerDefaultName = "MemoryUpdates";
/// Helps connecting `memory` to ice. Used to handle Ice callbacks.
- MemoryToIceAdapter iceMemory { &workingMemory, &longtermMemory};
+ MemoryToIceAdapter iceMemory { &workingMemory, &longtermMemoryManager};
private:
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 65bd2f6429c6fb2c25f48d2fa1218bc5fa308409..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 "../error.h"
-#include "../core/workingmemory/ice_conversions.h"
-#include "query_proc/workingmemory/MemoryQueryProcessor.h"
-#include "query_proc/longtermmemory/MemoryQueryProcessor.h"
+#include <RobotAPI/libraries/aron/core/Exception.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
namespace armarx::armem::server
{
- 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->doLocked([&coreSegment, &input]()
{
- coreSegment->addProviderSegment(input.providerSegmentName);
- }
- catch (const armem::error::ContainerEntryAlreadyExists&)
- {
- // 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,7 +188,7 @@ namespace armarx::armem::server
// also store in ltm if transfermode is set to always
// TODO: Move outside of loop?
- if (longtermMemory)
+ if (longtermMemoryManager)
{
}
@@ -221,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.";
}
@@ -257,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
@@ -289,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)
@@ -323,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..6872e2584352195ca8e587043e275acfbffb3739
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/forward_declarations.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+
+
+namespace armarx::armem::server
+{
+ class MemoryToIceAdapter;
+}
+namespace armarx::armem::server::wm
+{
+ using EntityInstance = armem::wm::EntityInstance;
+ using EntitySnapshot = armem::wm::EntitySnapshot;
+ class Entity;
+ class ProviderSegment;
+ class CoreSegment;
+ class Memory;
+}
diff --git a/source/RobotAPI/libraries/armem/server/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/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 0e3ddac45c6a8eee377f468767a3bc39a580c554..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::LTM;
- }
-
- };
-}
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 3d0888c7ff4d2f1b46f54e9d51a1a3c62b7dd395..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
{
@@ -55,11 +92,6 @@ namespace armarx::armem::wm
}
- void Memory::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- }
-
-
// TODO: Add core segment if param is set
Memory::Base::UpdateResult
Memory::updateLocking(const EntityUpdate& update)
@@ -68,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 38e4d8c88719541e7c4fca92964cfb069c22ba5e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.cpp
+++ /dev/null
@@ -1,87 +0,0 @@
-#include "MemoryControlWidget.h"
-
-#include <QPushButton>
-#include <QLineEdit>
-#include <QTimer>
-#include <QHBoxLayout>
-#include <QFileDialog>
-
-#include <cmath>
-
-
-namespace armarx::armem::gui
-{
-
- MemoryControlWidget::MemoryControlWidget()
- {
- setSizePolicy(QSizePolicy::Policy::Minimum, QSizePolicy::Policy::Fixed);
-
- auto vlayout = new QVBoxLayout();
- auto hlayout1 = new QHBoxLayout();
- auto hlayout2 = new QHBoxLayout();
-
- hlayout1->setContentsMargins(10, 2, 10, 2);
- hlayout2->setContentsMargins(10, 2, 10, 2);
-
- const int margin = 0;
- vlayout->setContentsMargins(margin, margin, margin, margin);
-
- _lineEdit = new QLineEdit("/tmp/MemoryExport", this);
- _lineEdit->setSizePolicy(QSizePolicy::Ignored, QSizePolicy::Preferred);
- _lineEdit->setMinimumWidth(150);
-
- _openFileBrowserButton = new QPushButton("Search files", this);
-
- _storeOnDiskButton = new QPushButton("Store query (Disk)", this);
- _storeInLTMButton = new QPushButton("Store query (LTM)", this);
-
- hlayout1->addWidget(_lineEdit);
- hlayout1->addWidget(_openFileBrowserButton);
-
- hlayout2->addWidget(_storeOnDiskButton);
- hlayout2->addWidget(_storeInLTMButton);
-
- vlayout->addItem(hlayout1);
- vlayout->addItem(hlayout2);
-
- this->setLayout(vlayout);
- // Private connections.
-
- // Public connections.
- connect(_openFileBrowserButton, &QPushButton::pressed, this, &This::openFileBrowser);
- connect(_storeInLTMButton, &QPushButton::pressed, this, &This::storeInLTM);
- connect(_storeOnDiskButton, &QPushButton::pressed, this, &This::storeOnDisk);
- }
-
- void MemoryControlWidget::openFileBrowser()
- {
- QFileDialog dialog;
- dialog.setFileMode(QFileDialog::DirectoryOnly);
- //dialog.setOption(QFileDialog::DontUseNativeDialog, true);
- dialog.setOption(QFileDialog::ShowDirsOnly, false);
- dialog.exec();
- QString open = dialog.directory().path();
- _lineEdit->setText(open);
- }
-
- QLineEdit* MemoryControlWidget::pathInputBox()
- {
- return _lineEdit;
- }
-
- 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 c6b156db313b01180a7f5884959ffc21b0a73f9d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.h
+++ /dev/null
@@ -1,55 +0,0 @@
-#pragma once
-
-#include <QWidget>
-
-class QPushButton;
-class QLineEdit;
-class QFileDialog;
-
-namespace armarx::armem::gui
-{
-
- class MemoryControlWidget : public QWidget
- {
- Q_OBJECT
- using This = MemoryControlWidget;
-
- public:
-
- MemoryControlWidget();
-
- QLineEdit* pathInputBox();
- QString getEnteredPath();
-
- QPushButton* openFileBrowserButton();
-
- QPushButton* storeInLTMButton();
- QPushButton* storeOnDiskButton();
-
- public slots:
-
- void openFileBrowser();
-
- signals:
-
- void storeInLTM();
- void storeOnDisk();
-
- private slots:
-
-
- signals:
-
-
- private:
-
- QLineEdit* _lineEdit;
-
- QPushButton* _openFileBrowserButton;
-
- QPushButton* _storeInLTMButton;
- QPushButton* _storeOnDiskButton;
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
index 4edb2b417bc9b40fde95643519701b4962aee114..c56cc1077ef852c2a92d53cb3ae24f65a3974ccd 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,10 @@ 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 +81,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::startQueries);
+ connect(updateWidget, &armem::gui::PeriodicUpdateWidget::update, this, &This::startQueries);
+ connect(processQueryResultTimer, &QTimer::timeout, this, &This::processQueryResults);
- connect(this, &This::connected, this, &This::updateMemories);
- connect(updateWidget, &armem::gui::PeriodicUpdateWidget::update, this, &This::updateMemories);
+ 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 +109,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,6 +130,7 @@ namespace armarx::armem::gui
emit initialized();
}
+
void MemoryViewer::onConnect(ManagedIceObject& component)
{
if (!mnsName.empty())
@@ -135,6 +152,7 @@ namespace armarx::armem::gui
emit connected();
}
+
void MemoryViewer::onDisconnect(ManagedIceObject&)
{
updateWidget->stopTimer();
@@ -142,6 +160,7 @@ namespace armarx::armem::gui
emit disconnected();
}
+
const armem::wm::Memory* MemoryViewer::getSingleMemoryData(const std::string& memoryName)
{
auto it = memoryData.find(memoryName);
@@ -164,9 +183,10 @@ namespace armarx::armem::gui
}
}
+
void MemoryViewer::storeInLTM()
{
- TIMING_START(MemoryStore);
+ TIMING_START(MemoryStore)
for (auto& [name, reader] : memoryReaders)
{
@@ -175,162 +195,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()
- {
- int errorCnt = 0;
-
- memoryReaders.clear();
- memoryData.clear();
+ void MemoryViewer::startQueries()
+ {
memoryReaders = mns.getAllReaders(true);
+ startDueQueries(runningQueries, memoryReaders);
+ }
+
- bool dataChanged = false;
+ void MemoryViewer::processQueryResults()
+ {
+ const std::map<std::string, client::QueryResult> results = collectQueryResults(runningQueries, memoryReaders);
- QString qs = memoryControlWidget->getEnteredPath();
- std::string utf8_text = qs.toUtf8().constData();
+ int errorCount = 0;
+ applyQueryResults(results, &errorCount);
- std::filesystem::path p(utf8_text);
+ emit memoryDataChanged();
+ updateStatusLabel(errorCount);
+ }
- armem::client::QueryInput input = memoryGroup->queryWidget()->queryInput();
- // first check if the local file system should be queried
- if (memoryGroup->queryWidget()->alsoQueryLocalDisk())
+ void MemoryViewer::updateStatusLabel(int errorCount)
+ {
+ // Code to output status label information
+ if (statusLabel and errorCount > 0)
{
- if (std::filesystem::is_directory(p))
- {
- 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);
+ auto now = std::chrono::system_clock::now();
+ auto in_time_t = std::chrono::system_clock::to_time_t(now);
- input.addQueryTargetToAll(armem::query::data::QueryTarget::LTM); // We use LTM as query target for the disk
+ 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.";
- armem::d_ltm::query_proc::MemoryQueryProcessor d_ltm_processor;
- dMem = d_ltm_processor.process(input.toIce(), dMem);
+ statusLabel->setText(QString::fromStdString(ss.str()));
+ }
+ }
- wm::Memory converted = dMem.convert();
- memoryData[k] = std::move(converted);
- }
- }
- }
- else
+
+ 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();
+
+ for (auto& [name, reader] : readers)
+ {
+ if (queries.count(name) == 0)
{
- ARMARX_WARNING << "Could not import a memory from '" << utf8_text << "'. Skipping import.";
+ queries[name] = reader.memoryPrx->begin_query(input.toIce());
}
-
- dataChanged = true; // in any case we update the view (even if there was no result)
}
- else
+ }
+
+
+ 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;
- for (auto& [name, reader] : memoryReaders)
+ if (queryPromise->isCompleted())
{
- TIMING_START(MemoryQuery);
+ if (auto jt = memoryReaders.find(name); jt != readers.end())
{
- armem::client::QueryResult result = reader.query(input);
- if (result)
+ try
{
- if (result.memory.hasData())
- {
- if (const auto& it = memoryData.find(name); it != memoryData.end())
- {
- // TODO is this necessary (or dead code?)
- result.memory.append(it->second);
-
- if (!result.memory.hasData())
- {
- ARMARX_ERROR << "A memory which had data before lost data. This indicates that there is something wrong.";
- }
- }
-
- memoryData[name] = std::move(result.memory);
- }
- else
- {
- ARMARX_INFO << "The memory " << name << " has no data after querying.";
- }
- dataChanged = true; // in any case we update the view (even if there was no result)
+ results[name] = client::QueryResult::fromIce(jt->second.memoryPrx->end_query(queryPromise));
}
- else
+ catch (const Ice::ConnectionRefusedException&)
{
- ARMARX_WARNING << "A query for memory '" << name << "' produced an error: " << result.errorMessage;
- errorCnt++;
+ // Server is gone (MNS did not now about it yet) => Skip result.
}
}
- TIMING_END_STREAM(MemoryQuery, ARMARX_VERBOSE);
+ // else: Server is gone (MNS new about it) => Skip result.
- if (debugObserver)
- {
- debugObserver->setDebugDatafield(Logging::tag.tagName, "Memory Query [ms]", new Variant(MemoryQuery.toMilliSecondsDouble()));
- }
+ // Promise is completed => Clean up in any case.
+ it = runningQueries.erase(it);
+ }
+ else
+ {
+ ++it; // Uncompleted => Keep.
}
}
- // Code to output status label information
- std::stringstream ss;
- auto now = std::chrono::system_clock::now();
- auto in_time_t = std::chrono::system_clock::to_time_t(now);
- ss << "Last update: " << std::put_time(std::localtime(&in_time_t), "%Y-%m-%d %X");
-
- if (dataChanged)
+ TIMING_END_STREAM(tCollectQueryResults, ARMARX_VERBOSE)
+ if (debugObserver)
{
- emit memoryDataChanged();
+ debugObserver->begin_setDebugChannel(Logging::tag.tagName,
+ {
+ { "t Collect Query Results [ms]", new Variant(tCollectQueryResults.toMilliSecondsDouble()) },
+ { "# Collected Query Results", new Variant(static_cast<int>(results.size())) },
+ });
}
- else
+
+ return results;
+ }
+
+
+ void MemoryViewer::applyQueryResults(
+ const std::map<std::string, client::QueryResult>& results,
+ int* outErrorCount)
+ {
+ TIMING_START(tProcessQueryResults)
+ for (const auto& [name, result] : results)
{
- ss << "- No query result. ";
+ if (result.success)
+ {
+ memoryData[name] = std::move(result.memory);
+ }
+ else
+ {
+ ARMARX_WARNING << "Querying memory server '" << name << "' produced an error: \n" << result.errorMessage;
+ if (outErrorCount)
+ {
+ outErrorCount++;
+ }
+ }
}
- if (errorCnt > 0)
+ // Drop all entries in memoryData which are not in memoryReaders anymore.
+ for (auto it = memoryData.begin(); it != memoryData.end();)
{
- ss << "The query produced " << errorCnt << " errors! Please check log.";
+ if (memoryReaders.count(it->first) == 0)
+ {
+ it = memoryData.erase(it);
+ }
+ else
+ {
+ ++it;
+ }
}
- if (statusLabel)
+ TIMING_END_STREAM(tProcessQueryResults, ARMARX_VERBOSE)
+ if (debugObserver)
{
- statusLabel->setText(ss.str().c_str());
+ debugObserver->begin_setDebugChannel(Logging::tag.tagName,
+ {
+ { "t Process Query Results [ms]", new Variant(tProcessQueryResults.toMilliSecondsDouble()) },
+ { "# Processed Query Results", new Variant(static_cast<int>(results.size())) },
+ });
}
}
@@ -362,7 +403,7 @@ namespace armarx::armem::gui
{
try
{
- snapshot = &data->getEntitySnapshot(id);
+ snapshot = &data->getSnapshot(id);
}
catch (const armem::error::ArMemError& e)
{
@@ -384,6 +425,7 @@ namespace armarx::armem::gui
}
}
+
void MemoryViewer::resolveMemoryID(const MemoryID& id)
{
// ARMARX_IMPORTANT << "Resolving memory ID: " << id;
@@ -403,18 +445,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
{
@@ -422,13 +463,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
{
@@ -452,26 +492,20 @@ 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)
{
@@ -479,9 +513,11 @@ namespace armarx::armem::gui
}
}
+
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();
@@ -493,12 +529,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/InstanceView.cpp b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
index 2b4ab121dabc7d52188f1d10b8adb501f2c72d32..8fda3cc4bafd3458cc76f1829466afc8f84abeb9 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
@@ -104,7 +104,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 +113,7 @@ namespace armarx::armem::gui::instance
catch (const armem::error::ArMemError& e)
{
showErrorMessage(e.what());
- return;
- };
+ }
if (instance)
{
update(*instance, aronType);
@@ -146,7 +145,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 +174,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 +190,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 +208,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 +225,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;
+ }
}
@@ -258,14 +268,15 @@ namespace armarx::armem::gui::instance
{
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());
+ });
+ }
}
break;
default:
@@ -276,17 +287,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()))
- {
- menu.addAction(action);
- }
- if (QAction* action = makeActionResolveMemoryID(id.value()))
+ if (std::optional<MemoryID> id = getElementMemoryID(path.value()))
{
- menu.addAction(action);
+ if (QAction* action = makeActionCopyMemoryID(id.value()))
+ {
+ menu.addAction(action);
+ }
+ if (QAction* action = makeActionResolveMemoryID(id.value()))
+ {
+ menu.addAction(action);
+ }
}
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
index 26f9d3be5fccfdce4fdb17a9ddbe430c2f334433..76a3988db8112f9b9437172710e05a900bb95f8f 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
@@ -5,9 +5,12 @@
#include <QWidget>
#include <QGroupBox>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.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 +27,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 +71,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);
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/TypedDataDisplayVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
index 827d14a599af43e6067fd1b3fa18bb136dfcbed8..eb1cda5fd7ffd8b4b06f7286d7292e3a058cdc35 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,20 +98,45 @@ 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)
+ {
+ 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)
{
- value << DataDisplayVisitor::getValue(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)
{
- value << DataDisplayVisitor::getValue(data);
+ processEigenQuaternion(data);
return false;
}
@@ -122,26 +160,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..87220babb8b64d78d7db6bfb1fe7cce41b070fd6 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;
@@ -49,7 +54,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..c548192cb7a98a390f708c8bd7789e51e63089cc 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
@@ -10,6 +10,28 @@
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_prefix(const std::string& string, const std::string& prefix)
+ {
+ return simox::alg::starts_with(string, prefix)
+ ? string.substr(prefix.size(), std::string::npos)
+ : string;
+ }
+ 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,15 +41,25 @@ 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)
{
- std::string begin = "Object<";
- std::string end = ">";
- n = n.substr(begin.size(), n.size() - begin.size() - end.size());
+ 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
+ {
+ n = s::replace_all(n, "Type", "");
+ }
+
+
+ n = remove_prefix(n, "Aron");
+ n = remove_wrap(n, "Object<", ">");
if (true)
{
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 9d5cc9ea6064bb6977892623649518791dcaffec..6b38efdddb30e67d1abfb3190d88508dff72258a 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
@@ -29,27 +29,14 @@ namespace armarx::armem::gui
return targets;
}
- void SnapshotSelectorWidget::localDiskStateChanged()
+ void SnapshotSelectorWidget::queryTargetStateChanged()
{
- if (_LocalDiskMemoryQueryTargetCheckBox->isChecked())
+ if (!_WMQueryTargetCheckBox->isChecked() && !_LTMQueryTargetCheckBox->isChecked())
{
- _WMQueryTargetCheckBox->setChecked(false);
- _LTMQueryTargetCheckBox->setChecked(false);
- _WMQueryTargetCheckBox->setEnabled(false);
- _LTMQueryTargetCheckBox->setEnabled(false);
- }
- else
- {
- _WMQueryTargetCheckBox->setEnabled(true);
- _LTMQueryTargetCheckBox->setEnabled(true);
+ _WMQueryTargetCheckBox->setChecked(true);
}
}
- bool SnapshotSelectorWidget::queryTargetContainsLocalDisk() const
- {
- return _LocalDiskMemoryQueryTargetCheckBox->isChecked();
- }
-
SnapshotSelectorWidget::SnapshotSelectorWidget()
{
_pageLayout = new QVBoxLayout();
@@ -72,13 +59,12 @@ namespace armarx::armem::gui
auto queryTargetLayout = new QHBoxLayout();
_WMQueryTargetCheckBox = new QCheckBox("WM");
_LTMQueryTargetCheckBox = new QCheckBox("LTM");
- _LocalDiskMemoryQueryTargetCheckBox = new QCheckBox("Local Disk");
- connect(_LocalDiskMemoryQueryTargetCheckBox, &QCheckBox::stateChanged, this, &This::localDiskStateChanged);
+ connect(_WMQueryTargetCheckBox, &QCheckBox::stateChanged, this, &This::queryTargetStateChanged);
+ connect(_LTMQueryTargetCheckBox, &QCheckBox::stateChanged, this, &This::queryTargetStateChanged);
queryTargetLayout->addWidget(_WMQueryTargetCheckBox);
queryTargetLayout->addWidget(_LTMQueryTargetCheckBox);
- 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 a0b69eb1b6f362e64880ab400c0bea29bcdc73fc..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:
@@ -51,7 +50,7 @@ namespace armarx::armem::gui
void hideAllForms();
void showSelectedFormForQuery(QString selected);
- void localDiskStateChanged();
+ void queryTargetStateChanged();
signals:
void queryOutdated();
@@ -67,7 +66,6 @@ namespace armarx::armem::gui
QComboBox* _queryComboBox;
QCheckBox* _WMQueryTargetCheckBox;
QCheckBox* _LTMQueryTargetCheckBox;
- 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..976d5bf9abb8a7d7586a0501568e6c8240c1e45c 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
@@ -16,7 +16,7 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/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..27d1cd851dd4500a2d3c6373a2b7c1cd2c5eb3e5 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -13,7 +13,7 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
@@ -242,32 +242,19 @@ namespace armarx::armem::articulated_object
.getCoreSegment(properties.coreInstanceSegmentName);
// clang-format on
- for (const auto& [_, providerSegment] : coreSegment.providerSegments())
+ std::optional<wm::EntityInstance> instance;
+ coreSegment.forEachInstance([&instance](const wm::EntityInstance & i)
{
-
- const auto entities = simox::alg::get_values(providerSegment.entities());
-
- if (entities.empty())
- {
- ARMARX_WARNING << "No entity found";
- continue;
- }
-
- const auto entitySnapshots = simox::alg::get_values(entities.front().history());
-
- if (entitySnapshots.empty())
- {
- ARMARX_WARNING << "No entity snapshots found";
- continue;
- }
-
- // TODO(fabian.reister): check if 0 available
- const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
-
- return robot::convertRobotState(instance);
+ instance = i;
+ });
+ if (instance.has_value())
+ {
+ return robot::convertRobotState(instance.value());
+ }
+ else
+ {
+ return std::nullopt;
}
-
- return std::nullopt;
}
@@ -275,67 +262,43 @@ namespace armarx::armem::articulated_object
Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
{
// clang-format off
- const armem::wm::CoreSegment& coreSegment = memory
- .getCoreSegment(properties.coreClassSegmentName);
+ const armem::wm::CoreSegment& coreSegment = memory.getCoreSegment(properties.coreClassSegmentName);
// clang-format on
- for (const auto& [_, providerSegment] : coreSegment.providerSegments())
+ std::optional<wm::EntityInstance> instance;
+ coreSegment.forEachInstance([&instance](const wm::EntityInstance & i)
{
- const auto entities = simox::alg::get_values(providerSegment.entities());
-
- if (entities.empty())
- {
- ARMARX_WARNING << "No entity found";
- continue;
- }
-
- const auto entitySnapshots = simox::alg::get_values(entities.front().history());
-
- if (entitySnapshots.empty())
- {
- ARMARX_WARNING << "No entity snapshots found";
- continue;
- }
-
- if (entitySnapshots.front().hasInstance(0))
- {
- const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
- return convertRobotDescription(instance);
- }
+ instance = i;
+ });
+ if (instance.has_value())
+ {
+ return convertRobotDescription(instance.value());
+ }
+ else
+ {
+ return std::nullopt;
}
-
- return std::nullopt;
}
+
std::vector<robot::RobotDescription>
Reader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
{
- std::vector<robot::RobotDescription> descriptions;
-
const armem::wm::CoreSegment& coreSegment =
memory.getCoreSegment(properties.coreClassSegmentName);
- for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
+ std::vector<robot::RobotDescription> descriptions;
+ coreSegment.forEachEntity([&descriptions](const wm::Entity & entity)
{
- for (const auto& [name, entity] : providerSegment.entities())
+ if (not entity.empty())
{
- if (entity.empty())
- {
- ARMARX_WARNING << "No entity found";
- continue;
- }
-
- const auto entitySnapshots = simox::alg::get_values(entity.history());
- const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
-
- const auto robotDescription = convertRobotDescription(instance);
-
+ const auto robotDescription = convertRobotDescription(entity.getFirstSnapshot().getInstance(0));
if (robotDescription)
{
descriptions.push_back(*robotDescription);
}
}
- }
+ });
return descriptions;
}
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
index 237bc31cc8827f0fb9f80284bf0c639c23d093ec..d1cb1e0082d6ef95491530518e7f52839601d771 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) :
@@ -86,7 +82,6 @@ namespace armarx::armem::articulated_object
void Writer::updateKnownObject(const armem::MemoryID& snapshotId)
{
-
arondto::RobotDescription aronArticulatedObjectDescription;
// aronArticulatedObjectDescription.fromAron(snapshotId.ent);
@@ -269,59 +264,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.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/robot_conversions.cpp b/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
index 291a7ef1f65546af0c698736bd099756830490b1..8b7829a2f0c3f6dadd54bf2ce624c9fcbe793efd 100644
--- a/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
@@ -4,7 +4,7 @@
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-#include "RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h"
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index 4fed4d1cb277ab316a318cbc13b4f32222fd8939..4ae48e00f0755b8a24b7baab6f48df14e5f70632 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -10,6 +10,7 @@
#include <ArmarXCore/core/PackagePath.h>
+
namespace armarx::armem::robot
{
struct RobotDescription
@@ -45,4 +46,4 @@ namespace armarx::armem::robot
using RobotDescriptions = std::vector<RobotDescription>;
using RobotStates = std::vector<RobotState>;
-} // namespace armarx::armem::robot
\ No newline at end of file
+} // 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..85370141f880a72b26039c0af0c765f929e6a902 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -10,7 +10,7 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
@@ -214,30 +214,31 @@ namespace armarx::armem::robot_state
const armem::wm::ProviderSegment& providerSegment = memory
.getCoreSegment(properties.proprioceptionCoreSegment)
.getProviderSegment(name);
- // clang-format on
- const auto entities = simox::alg::get_values(providerSegment.entities());
// TODO entitiesToRobotState()
- if (entities.empty())
+ if (providerSegment.empty())
{
ARMARX_WARNING << "No entity found";
return std::nullopt;
}
- const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+ // clang-format on
- if (entitySnapshots.empty())
+ const armem::wm::EntityInstance* instance = nullptr;
+ providerSegment.forEachInstance([&instance](const wm::EntityInstance & i)
+ {
+ instance = &i;
+ return false; // break
+ });
+ if (!instance)
{
ARMARX_WARNING << "No entity snapshots found";
return std::nullopt;
}
- // TODO(fabian.reister): check if 0 available
- const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
-
// Here, we access the RobotUnit streaming data stored in the proprioception segment.
- return robot::convertRobotState(instance);
+ return robot::convertRobotState(*instance);
}
// FIXME remove this, use armem/util/util.h
@@ -271,24 +272,19 @@ namespace armarx::armem::robot_state
.getCoreSegment(properties.proprioceptionCoreSegment);
// clang-format on
- for (const auto &[_, providerSegment] : coreSegment.providerSegments())
+ coreSegment.forEachEntity([&jointMap](const wm::Entity & entity)
{
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- for (const auto &[name, entity] : providerSegment.entities())
+ const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
+ if (not jointState)
{
- const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
-
- const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
-
- if (not jointState)
- {
- // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
- continue;
- }
-
- jointMap.emplace(jointState->name, jointState->position);
+ // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+ return;
}
- }
+
+ jointMap.emplace(jointState->name, jointState->position);
+ });
if (jointMap.empty())
{
@@ -307,26 +303,19 @@ namespace armarx::armem::robot_state
.getCoreSegment(properties.descriptionCoreSegment)
.getProviderSegment(name); // TODO(fabian.reister): all
// clang-format on
- const auto entities = simox::alg::get_values(providerSegment.entities());
- if (entities.empty())
+ const armem::wm::EntityInstance* instance = nullptr;
+ providerSegment.forEachInstance([&instance](const wm::EntityInstance & i)
{
- ARMARX_WARNING << "No entity found";
- return std::nullopt;
- }
-
- const auto entitySnapshots = simox::alg::get_values(entities.front().history());
-
- if (entitySnapshots.empty())
+ instance = &i;
+ });
+ if (!instance)
{
ARMARX_WARNING << "No entity snapshots found";
return std::nullopt;
}
- // TODO(fabian.reister): check if 0 available
- const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
-
- return robot::convertRobotDescription(instance);
+ return robot::convertRobotDescription(*instance);
}
} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/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 6f7dc4c14db6da83691331063ff273cd5ddc4158..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->container());
- 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->container());
-
- 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..02a98ef640f4cf4b2dc0f6166c18fd3c5ccbc5bc 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,8 +25,6 @@ namespace armarx::armem::server::robot_state::proprioception
void Segment::onConnect(RobotUnitInterfacePrx robotUnitPrx)
{
- Base::onConnect();
-
this->robotUnit = robotUnitPrx;
std::string providerSegmentName = "Robot";
@@ -59,22 +43,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 +75,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 +128,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..c5282fb54aba97b583f15f2df1876c0dde3e4ead 100644
--- a/source/RobotAPI/libraries/aron/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
@@ -151,6 +151,7 @@ set(LIB_HEADERS
navigator/data/primitive/Bool.h
navigator/data/AllNavigators.h
navigator/data/NavigatorFactory.h
+ navigator/data/forward_declarations.h
navigator/type/Navigator.h
navigator/type/detail/NavigatorBase.h
@@ -181,6 +182,7 @@ set(LIB_HEADERS
navigator/type/primitive/Time.h
navigator/type/AllNavigators.h
navigator/type/NavigatorFactory.h
+ navigator/type/forward_declarations.h
navigator/visitors/DataVisitor.h
navigator/visitors/TypedDataVisitor.h
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp
index c431b5b49ea2547f262da4fd657be6aa6fed1fac..6c95ec35b8eb3c7ce42bf6cbc74c0c56adc68829 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.");
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h b/source/RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h
new file mode 100644
index 0000000000000000000000000000000000000000..d76939c317aafd2acc4eb797d38d1ba1ff96da26
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h
@@ -0,0 +1,22 @@
+#pragma once
+
+#include <memory>
+
+
+namespace armarx::aron::datanavigator
+{
+ class Navigator;
+ using NavigatorPtr = std::shared_ptr<Navigator>;
+
+ using DictNavigatorPtr = std::shared_ptr<class DictNavigator>;
+ using ListNavigatorPtr = std::shared_ptr<class ListNavigator>;
+ using NDArrayNavigatorPtr = std::shared_ptr<class NDArrayNavigator>;
+
+ using IntNavigatorPtr = std::shared_ptr<class IntNavigator>;
+ using LongNavigatorPtr = std::shared_ptr<class LongNavigator>;
+ using FloatNavigatorPtr = std::shared_ptr<class FloatNavigator>;
+ using DoubleNavigatorPtr = std::shared_ptr<class DoubleNavigator>;
+ using StringNavigatorPtr = std::shared_ptr<class StringNavigator>;
+ using BoolNavigatorPtr = std::shared_ptr<class BoolNavigator>;
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h b/source/RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h
index 51ea5a762037da7b7e4a3808c42d36a468d9c6b1..82702de998bb7e6b0972afd9e96ae027b433eae3 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h
@@ -12,7 +12,6 @@
#include "ndarray/OpenCVMat.h"
#include "ndarray/Orientation.h"
#include "ndarray/PCLPointCloud.h"
-#include "ndarray/PCLPointCloud.h"
#include "ndarray/Pose.h"
#include "ndarray/Position.h"
#include "enum/IntEnum.h"
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h b/source/RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h
new file mode 100644
index 0000000000000000000000000000000000000000..af5d62e7c0decbc296cea942fa1983b0e0cb5924
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include <memory>
+
+
+namespace armarx::aron::typenavigator
+{
+ using NavigatorPtr = std::shared_ptr<class Navigator>;
+
+ using DictNavigatorPtr = std::shared_ptr<class DictNavigator>;
+ using ListNavigatorPtr = std::shared_ptr<class ListNavigator>;
+ using ObjectNavigatorPtr = std::shared_ptr<class ObjectNavigator>;
+ using PoseNavigatorPtr = std::shared_ptr<class PoseNavigator>;
+ using TupleNavigatorPtr = std::shared_ptr<class TupleNavigator>;
+ using NDArrayNavigatorPtr = std::shared_ptr<class NDArrayNavigator>;
+ using EigenMatrixNavigatorPtr = std::shared_ptr<class EigenMatrixNavigator>;
+ using EigenQuaternionNavigatorPtr = std::shared_ptr<class EigenQuaternionNavigator>;
+ using IVTCByteImageNavigatorPtr = std::shared_ptr<class IVTCByteImageNavigator>;
+ using OpenCVMatNavigatorPtr = std::shared_ptr<class OpenCVMatNavigator>;
+ using OrientationNavigatorPtr = std::shared_ptr<class OrientationNavigator>;
+ using PCLPointCloudNavigatorPtr = std::shared_ptr<class PCLPointCloudNavigator>;
+ using PositionNavigatorPtr = std::shared_ptr<class PositionNavigator>;
+ using IntEnumNavigatorPtr = std::shared_ptr<class IntEnumNavigator>;
+
+ using IntNavigatorPtr = std::shared_ptr<class IntNavigator>;
+ using LongNavigatorPtr = std::shared_ptr<class LongNavigator>;
+ using FloatNavigatorPtr = std::shared_ptr<class FloatNavigator>;
+ using DoubleNavigatorPtr = std::shared_ptr<class DoubleNavigator>;
+ using StringNavigatorPtr = std::shared_ptr<class StringNavigator>;
+ using BoolNavigatorPtr = std::shared_ptr<class BoolNavigator>;
+ using TimeNavigatorPtr = std::shared_ptr<class TimeNavigator>;
+
+}
+