From ec201f05e53bee0be243adac203247183dbb21a9 Mon Sep 17 00:00:00 2001
From: Fabian Peller-Konrad <fabian.peller-konrad@kit.edu>
Date: Fri, 25 Feb 2022 15:16:22 +0100
Subject: [PATCH] improved ltm storage
---
.../server/ExampleMemory/ExampleMemory.cpp | 3 +-
.../SkillProviderExample.cpp | 51 +++-
.../SkillProviderExample.h | 13 +-
.../SkillManagerMonitorWidget.ui | 4 +-
.../SkillManagerMonitorWidgetController.cpp | 43 ++-
.../SkillManagerMonitorWidgetController.h | 8 +-
.../skills/SkillManagerInterface.ice | 2 +
.../skills/SkillProviderInterface.ice | 3 +-
.../libraries/armem/server/CMakeLists.txt | 33 ++-
.../armem/server/MemoryToIceAdapter.cpp | 1 +
.../server/ltm/base/converter/Converter.h | 7 +-
.../ltm/base/converter/image/Converter.h | 4 +-
.../base/converter/image/png/PngConverter.h | 4 +-
.../converter/{dict => object}/Converter.cpp | 4 +-
.../converter/{dict => object}/Converter.h | 8 +-
.../{dict => object}/bson/BsonConverter.cpp | 2 +-
.../{dict => object}/bson/BsonConverter.h | 6 +-
.../{dict => object}/json/JsonConverter.cpp | 2 +-
.../{dict => object}/json/JsonConverter.h | 10 +-
.../ltm/base/detail/BufferedMemoryBase.h | 6 +-
.../armem/server/ltm/base/detail/MemoryBase.h | 51 ++--
.../server/ltm/base/detail/MemoryItem.cpp | 8 +-
.../armem/server/ltm/base/detail/MemoryItem.h | 40 ++-
.../server/ltm/base/extractor/Extractor.h | 5 +-
.../extractor/imageExtractor/ImageExtractor.h | 6 +-
.../armem/server/ltm/base/filter/Filter.h | 4 +
.../filter/equalityFilter/EqualityFilter.cpp | 52 ++++
.../filter/equalityFilter/EqualityFilter.h | 29 ++
.../filter/frequencyFilter/FrequencyFilter.h | 2 +
.../armem/server/ltm/disk/CoreSegment.cpp | 103 ++++---
.../armem/server/ltm/disk/CoreSegment.h | 3 +-
.../armem/server/ltm/disk/Entity.cpp | 261 ++++++++++++++----
.../libraries/armem/server/ltm/disk/Entity.h | 3 +-
.../armem/server/ltm/disk/EntitySnapshot.cpp | 224 ++++++++-------
.../armem/server/ltm/disk/EntitySnapshot.h | 2 +-
.../armem/server/ltm/disk/Memory.cpp | 122 ++++----
.../libraries/armem/server/ltm/disk/Memory.h | 6 +-
.../armem/server/ltm/disk/ProviderSegment.cpp | 103 ++++---
.../armem/server/ltm/disk/ProviderSegment.h | 2 +-
.../armem/server/ltm/disk/detail/Data.h | 232 ++++++++++++++--
.../server/ltm/disk/detail/DiskStorage.cpp | 75 +++--
.../server/ltm/disk/detail/DiskStorage.h | 44 ++-
.../libraries/armem/server/plugins/Plugin.cpp | 3 -
.../armem/server/test/ArMemLTMBenchmark.cpp | 4 +-
.../armem_gui/disk/ControlWidget.cpp | 9 +-
.../client/instance/ObjectReader.cpp | 3 +
.../client/instance/ObjectReader.h | 70 ++++-
.../aron/core/data/variant/complex/NDArray.h | 9 +
.../aron/core/data/variant/container/Dict.h | 10 +
.../aron/core/data/variant/container/List.h | 9 +
.../aron/core/data/variant/primitive/Bool.h | 9 +
.../aron/core/data/variant/primitive/Double.h | 9 +
.../aron/core/data/variant/primitive/Float.h | 9 +
.../aron/core/data/variant/primitive/Int.h | 9 +
.../aron/core/data/variant/primitive/Long.h | 9 +
.../aron/core/data/variant/primitive/String.h | 9 +
.../RobotAPI/libraries/skills/CMakeLists.txt | 2 +
.../manager/SkillManagerComponentPlugin.cpp | 10 +
.../manager/SkillManagerComponentPlugin.h | 2 +-
.../libraries/skills/provider/Skill.cpp | 57 ++--
.../libraries/skills/provider/Skill.h | 41 +--
.../skills/provider/SkillParameterization.cpp | 10 +-
.../skills/provider/SkillParameterization.h | 5 +-
.../provider/SkillProviderComponentPlugin.cpp | 79 ++++--
.../provider/SkillProviderComponentPlugin.h | 13 +-
.../skills/provider/SkillStatusUpdate.cpp | 4 +-
.../skills/provider/SkillStatusUpdate.h | 13 +-
.../skills/provider/SpecializedSkill.h | 44 ++-
.../detail/SkillImplementationWrapper.cpp | 93 +++----
.../detail/SkillImplementationWrapper.h | 26 +-
.../helper/LambdaSkillImplementation.cpp | 2 +-
.../helper/LambdaSkillImplementation.h | 2 +-
72 files changed, 1513 insertions(+), 652 deletions(-)
rename source/RobotAPI/libraries/armem/server/ltm/base/converter/{dict => object}/Converter.cpp (55%)
rename source/RobotAPI/libraries/armem/server/ltm/base/converter/{dict => object}/Converter.h (72%)
rename source/RobotAPI/libraries/armem/server/ltm/base/converter/{dict => object}/bson/BsonConverter.cpp (95%)
rename source/RobotAPI/libraries/armem/server/ltm/base/converter/{dict => object}/bson/BsonConverter.h (74%)
rename source/RobotAPI/libraries/armem/server/ltm/base/converter/{dict => object}/json/JsonConverter.cpp (92%)
rename source/RobotAPI/libraries/armem/server/ltm/base/converter/{dict => object}/json/JsonConverter.h (59%)
create mode 100644 source/RobotAPI/libraries/armem/server/ltm/base/filter/equalityFilter/EqualityFilter.cpp
create mode 100644 source/RobotAPI/libraries/armem/server/ltm/base/filter/equalityFilter/EqualityFilter.h
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
index 98788a3a6..4adee0b0f 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
@@ -72,7 +72,8 @@ namespace armarx
for (const std::string& name : p.core.defaultCoreSegments)
{
- workingMemory().addCoreSegment(name);
+ auto& c = workingMemory().addCoreSegment(name);
+ c.setMaxHistorySize(100);
}
}
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
index 1ddbdb5a9..ab6944e97 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
@@ -19,7 +19,7 @@ namespace armarx::skills::provider
armarx::skills::Example::HelloWorldAcceptedType::ToAronType()
})
{}
- Skill::Status HelloWorldSkill::_execute(const aron::data::DictPtr& d, const CallbackT&)
+ Skill::Status HelloWorldSkill::execute(const aron::data::DictPtr& d, const CallbackT&)
{
ARMARX_IMPORTANT << "Hi, from the Hello World Skill.\n" <<
"I received the following data: \n" <<
@@ -28,6 +28,42 @@ namespace armarx::skills::provider
return Skill::Status::Succeeded;
}
+ ChainingSkill::ChainingSkill() :
+ Skill(SkillDescription{
+ "ChainingSkill",
+ "This skill calls the HelloWorld skill three times.",
+ {},
+ 3000,
+ nullptr
+ })
+ {}
+ Skill::Status ChainingSkill::execute(const aron::data::DictPtr& d, const CallbackT&)
+ {
+ armarx::skills::Example::HelloWorldAcceptedType exec1;
+ armarx::skills::Example::HelloWorldAcceptedType exec2;
+ armarx::skills::Example::HelloWorldAcceptedType exec3;
+
+ exec1.some_text = "Hello from the ChainingSkill 1";
+ exec2.some_text = "Hello from the ChainingSkill 2";
+ exec3.some_text = "Hello from the ChainingSkill 3";
+
+ manager::dto::SkillExecutionInfo exec;
+ exec.providerName = "SkillProviderExample";
+ exec.skillName = "HelloWorld";
+ exec.waitUntilSkillFinished = false;
+
+ exec.params = exec1.toAron()->toAronDictDTO();
+ ownerManager->executeSkill(exec);
+
+ exec.params = exec2.toAron()->toAronDictDTO();
+ ownerManager->executeSkill(exec);
+
+ exec.params = exec3.toAron()->toAronDictDTO();
+ ownerManager->executeSkill(exec);
+
+ return Skill::Status::Succeeded;
+ }
+
TimeoutSkill::TimeoutSkill() :
Skill(SkillDescription{
"Timeout",
@@ -37,7 +73,7 @@ namespace armarx::skills::provider
nullptr
})
{}
- Skill::Status TimeoutSkill::_execute(const aron::data::DictPtr& d, const CallbackT&)
+ Skill::Status TimeoutSkill::execute(const aron::data::DictPtr& d, const CallbackT&)
{
int i = 0;
while (!timeoutReached)
@@ -64,7 +100,7 @@ namespace armarx::skills::provider
nullptr
})
{}
- Skill::Status CallbackSkill::_execute(const aron::data::DictPtr& d, const CallbackT& callback)
+ Skill::Status CallbackSkill::execute(const aron::data::DictPtr& d, const CallbackT& callback)
{
ARMARX_IMPORTANT << "Logging three updates via the callback";
auto up1 = std::make_shared<aron::data::Dict>();
@@ -101,7 +137,7 @@ namespace armarx::skills::provider
void SkillProviderExample::onInitComponent()
{
// Add example skill
- addSkill(std::make_shared<HelloWorldSkill>());
+ addSkill(std::make_unique<HelloWorldSkill>());
// Add another lambda example skill
{
@@ -114,10 +150,13 @@ namespace armarx::skills::provider
}
// Add another example skill
- addSkill(std::make_shared<CallbackSkill>());
+ addSkill(std::make_unique<CallbackSkill>());
// Add timeout skill
- addSkill(std::make_shared<TimeoutSkill>());
+ addSkill(std::make_unique<TimeoutSkill>());
+
+ // chaining
+ addSkill(std::make_unique<ChainingSkill>());
}
void SkillProviderExample::onConnectComponent()
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
index 2cd96b5eb..57853d642 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
@@ -36,7 +36,14 @@ namespace armarx::skills::provider
{
public:
HelloWorldSkill();
- Status _execute(const aron::data::DictPtr&, const CallbackT&) final;
+ Status execute(const aron::data::DictPtr&, const CallbackT&) final;
+ };
+
+ class ChainingSkill : public Skill
+ {
+ public:
+ ChainingSkill();
+ Status execute(const aron::data::DictPtr&, const CallbackT&) final;
};
@@ -44,7 +51,7 @@ namespace armarx::skills::provider
{
public:
TimeoutSkill();
- Status _execute(const aron::data::DictPtr&, const CallbackT&) final;
+ Status execute(const aron::data::DictPtr&, const CallbackT&) final;
};
@@ -52,7 +59,7 @@ namespace armarx::skills::provider
{
public:
CallbackSkill();
- Status _execute(const aron::data::DictPtr&, const CallbackT&) final;
+ Status execute(const aron::data::DictPtr&, const CallbackT&) final;
};
/**
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
index 15678facb..59089db20 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
@@ -70,7 +70,7 @@
<item row="6" column="0">
<widget class="QPushButton" name="pushButtonStopSkill">
<property name="text">
- <string>Stop</string>
+ <string>Stop current skill</string>
</property>
</widget>
</item>
@@ -108,7 +108,7 @@
<item row="6" column="1">
<widget class="QPushButton" name="pushButtonExecuteSkill">
<property name="text">
- <string>Execute</string>
+ <string>Request Execution</string>
</property>
</widget>
</item>
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
index 4a458d690..3b57b3012 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
@@ -148,6 +148,15 @@ namespace armarx
void SkillManagerMonitorWidgetController::refreshSkills()
{
+ static std::map<skills::provider::dto::Execution::Status, std::string> ExecutionStatus2String = {
+ {skills::provider::dto::Execution::Status::Aborted, "Aborted"},
+ {skills::provider::dto::Execution::Status::Failed, "Failed"},
+ {skills::provider::dto::Execution::Status::Idle, "Not yet started"},
+ {skills::provider::dto::Execution::Status::Running, "Running"},
+ {skills::provider::dto::Execution::Status::Scheduled, "Scheduled"},
+ {skills::provider::dto::Execution::Status::Succeeded, "Succeeded"}
+ };
+
if (!connected)
return;
@@ -178,6 +187,7 @@ namespace armarx
SkillProviderData providerData;
providerData.providerName = providerName;
providerData.skillDescriptions = provider->getSkills();
+ providerData.skillProviderPrx = provider;
skills.insert(std::make_pair(providerName, providerData));
newProviders.push_back(providerName);
@@ -197,18 +207,35 @@ namespace armarx
// add new providers
for (const auto& [providerName, provider] : skills)
{
- if (std::find(newProviders.begin(), newProviders.end(), providerName) != newProviders.end())
+ if (auto it = std::find(newProviders.begin(), newProviders.end(), providerName); it != newProviders.end())
{
- auto it = new QTreeWidgetItem(widget.treeWidgetSkills);
- it->setText(0, QString::fromStdString(providerName));
+ auto item = new QTreeWidgetItem(widget.treeWidgetSkills);
+ item->setText(0, QString::fromStdString(providerName));
for (const auto& [name, sk] : provider.skillDescriptions)
{
- auto itsk = new QTreeWidgetItem(it);
- it->addChild(itsk);
+ auto itsk = new QTreeWidgetItem(item);
+ item->addChild(itsk);
itsk->setText(0, QString::fromStdString(name));
}
}
}
+
+ // update status
+ for (int i = 0; i < widget.treeWidgetSkills->topLevelItemCount(); ++i)
+ {
+ QTreeWidgetItem* item = widget.treeWidgetSkills->topLevelItem(i);
+ auto providerName = item->text(0).toStdString();
+ for (int j = 0; j < item->childCount(); ++j)
+ {
+ QTreeWidgetItem* skillItem = item->child(j);
+ auto skillName = skillItem->text(0).toStdString();
+
+ auto& providerPrx = skills.at(providerName).skillProviderPrx;
+ auto statusUpdate = providerPrx->getSkillExecutionStatus(skillName);
+
+ skillItem->setText(2, QString::fromStdString(ExecutionStatus2String.at(statusUpdate.status)));
+ }
+ }
}
void SkillManagerMonitorWidgetController::executeSkill()
@@ -227,11 +254,14 @@ namespace armarx
auto data = getConfigAsAron();
skills::manager::dto::SkillExecutionInfo exInfo;
+ exInfo.waitUntilSkillFinished = false;
exInfo.providerName = selectedSkill.providerName;
exInfo.skillName = selectedSkill.skillName;
exInfo.params = aron::data::Dict::ToAronDictDTO(data);
ARMARX_INFO << "Executing skill from GUI: " << selectedSkill.providerName << "/" << selectedSkill.skillName;
+ // Note that we execute the skill in a seperate thread so that the GUI thread does not freeze.
+ //executions.emplace_back([&](){ manager->executeSkill(exInfo); });
manager->executeSkill(exInfo);
}
@@ -248,9 +278,8 @@ namespace armarx
return;
}
- // TODO
ARMARX_INFO << "Stopping skill from GUI: " << selectedSkill.providerName << "/" << selectedSkill.skillName;
- //observer->abortSkill(skills.selectedSkill);
+ manager->abortSkill(selectedSkill.providerName, selectedSkill.skillName);
}
void SkillManagerMonitorWidgetController::skillSelectionChanged(QTreeWidgetItem* current, QTreeWidgetItem*)
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
index 9ba0d8a37..debeaedff 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
@@ -22,6 +22,8 @@
#pragma once
#include <stack>
+#include <vector>
+#include <thread>
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
@@ -95,7 +97,7 @@ namespace armarx
* Widget Form
*/
Ui::SkillManagerMonitorWidget widget;
- QPointer<SimpleConfigDialog> dialog;
+ QPointer<SimpleConfigDialog> dialog;
std::string observerName = "SkillManager";
skills::manager::dti::SkillManagerInterfacePrx manager = nullptr;
@@ -110,6 +112,7 @@ namespace armarx
{
std::string providerName;
skills::provider::dto::SkillDescriptionMap skillDescriptions;
+ skills::provider::dti::SkillProviderInterfacePrx skillProviderPrx;
};
// Data taken from observer
@@ -125,6 +128,9 @@ namespace armarx
// others
std::atomic_bool connected = false;
QTimer* refreshSkillsResultTimer;
+
+ // skillExecutions
+ std::vector<std::thread> executions;
};
}
diff --git a/source/RobotAPI/interface/skills/SkillManagerInterface.ice b/source/RobotAPI/interface/skills/SkillManagerInterface.ice
index ea2ffed63..9f6bd7314 100644
--- a/source/RobotAPI/interface/skills/SkillManagerInterface.ice
+++ b/source/RobotAPI/interface/skills/SkillManagerInterface.ice
@@ -39,6 +39,7 @@ module armarx
string providerName;
string skillName;
aron::data::dto::Dict params;
+ bool waitUntilSkillFinished;
};
struct ProviderInfo
@@ -57,6 +58,7 @@ module armarx
void removeProvider(string providerName);
provider::dti::SkillProviderMap getSkillProviders();
void executeSkill(dto::SkillExecutionInfo skillExecutionInfo);
+ void abortSkill(string providerName, string skillName);
};
}
}
diff --git a/source/RobotAPI/interface/skills/SkillProviderInterface.ice b/source/RobotAPI/interface/skills/SkillProviderInterface.ice
index ff147cc4a..c6eec5736 100644
--- a/source/RobotAPI/interface/skills/SkillProviderInterface.ice
+++ b/source/RobotAPI/interface/skills/SkillProviderInterface.ice
@@ -68,6 +68,7 @@ module armarx
string skillName;
aron::data::dto::Dict params;
callback::dti::SkillProviderCallbackInterface* callbackInterface; // use nullptr if you do not want to have callbacks
+ bool waitUntilSkillFinished;
};
// The status enum of a skill
@@ -108,7 +109,7 @@ module armarx
dto::SkillDescriptionMap getSkills();
dto::SkillStatusUpdate getSkillExecutionStatus(string name);
void executeSkill(dto::SkillExecutionInfo executionInfo);
- dto::SkillStatusUpdate abortSkill(string skill);
+ void abortSkill(string skill, bool waitUntilSkillFinished);
};
dictionary<string, SkillProviderInterface*> SkillProviderMap;
diff --git a/source/RobotAPI/libraries/armem/server/CMakeLists.txt b/source/RobotAPI/libraries/armem/server/CMakeLists.txt
index 782e86c28..34109f1d8 100644
--- a/source/RobotAPI/libraries/armem/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/server/CMakeLists.txt
@@ -7,11 +7,19 @@ SET(INSTALL_SCRIPT_MSG
"Please use the installation script in RobotAPI/etc/mongocxx to install libmongocxx and libbsoncxx."
)
-find_package(libmongocxx QUIET)
-armarx_build_if(libmongocxx_FOUND "libmongocxx not available. ${INSTALL_SCRIPT_MSG}")
+
+
+# MongoLTM
+#find_package(libmongocxx QUIET)
+#armarx_build_if(libmongocxx_FOUND "libmongocxx not available. ${INSTALL_SCRIPT_MSG}")
+
+# DiskLTM
+find_package(ZLIB QUIET)
+armarx_build_if(ZLIB_FOUND "zlib not available.")
+
+# LTM Encoding stuff
find_package(libbsoncxx QUIET)
armarx_build_if(libbsoncxx_FOUND "libbsoncxx not available. ${INSTALL_SCRIPT_MSG}")
-
armarx_build_if(OpenCV_FOUND "OpenCV not available")
set(LIBS
@@ -21,11 +29,12 @@ set(LIBS
aron
RobotAPI::armem
- # Needed for LTM
+ # LTM
RobotAPI::aron::converter::json
RobotAPI::aron::converter::opencv
- ${LIBMONGOCXX_LIBRARIES}
+ #${LIBMONGOCXX_LIBRARIES}
${LIBBSONCXX_LIBRARIES}
+ ${ZLIB_LIBRARIES}
)
set(LIB_FILES
@@ -44,14 +53,15 @@ set(LIB_FILES
ltm/base/filter/Filter.cpp
ltm/base/filter/frequencyFilter/FrequencyFilter.cpp
+ ltm/base/filter/equalityFilter/EqualityFilter.cpp
ltm/base/extractor/Extractor.cpp
ltm/base/extractor/imageExtractor/ImageExtractor.cpp
ltm/base/converter/Converter.cpp
- ltm/base/converter/dict/Converter.cpp
- ltm/base/converter/dict/json/JsonConverter.cpp
- ltm/base/converter/dict/bson/BsonConverter.cpp
+ ltm/base/converter/object/Converter.cpp
+ ltm/base/converter/object/json/JsonConverter.cpp
+ ltm/base/converter/object/bson/BsonConverter.cpp
ltm/base/converter/image/Converter.cpp
ltm/base/converter/image/png/PngConverter.cpp
@@ -116,14 +126,15 @@ set(LIB_HEADERS
ltm/base/filter/Filter.h
ltm/base/filter/frequencyFilter/FrequencyFilter.h
+ ltm/base/filter/equalityFilter/EqualityFilter.h
ltm/base/extractor/Extractor.h
ltm/base/extractor/imageExtractor/ImageExtractor.h
ltm/base/converter/Converter.h
- ltm/base/converter/dict/Converter.h
- ltm/base/converter/dict/json/JsonConverter.h
- ltm/base/converter/dict/bson/BsonConverter.h
+ ltm/base/converter/object/Converter.h
+ ltm/base/converter/object/json/JsonConverter.h
+ ltm/base/converter/object/bson/BsonConverter.h
ltm/base/converter/image/Converter.h
ltm/base/converter/image/png/PngConverter.h
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index 1060a616f..d5318d3f7 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -221,6 +221,7 @@ namespace armarx::armem::server
e->addSnapshot(snapshot);
}
+ // store memory
longtermMemory->store(m);
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/Converter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/Converter.h
index 1e01564f3..f6eb1f71e 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/Converter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/Converter.h
@@ -17,9 +17,10 @@ namespace armarx::armem::server::ltm
Binary
};
- Converter(const ConverterType t, const std::string& s):
+ Converter(const ConverterType t, const std::string& s, const aron::type::Descriptor c):
type(t),
- suffix(s)
+ suffix(s),
+ convertsType(c)
{}
virtual ~Converter() = default;
@@ -28,5 +29,7 @@ namespace armarx::armem::server::ltm
const ConverterType type;
const std::string suffix;
+ const aron::type::Descriptor convertsType;
+ bool enabled = false;
};
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.h
index 0dd01ad84..78ba28f4a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.h
@@ -14,7 +14,9 @@ namespace armarx::armem::server::ltm
class ImageConverter : public Converter
{
public:
- using Converter::Converter;
+ ImageConverter(const ConverterType t, const std::string& s):
+ Converter(t, s, aron::type::Descriptor::eImage)
+ {}
virtual ~ImageConverter() = default;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.h
index fb1321bc6..69e80f988 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.h
@@ -10,7 +10,9 @@ namespace armarx::armem::server::ltm::converter::image
public:
PngConverter() :
ImageConverter(ConverterType::Str, ".png")
- {}
+ {
+ enabled = true; // enabled by default
+ }
protected:
std::vector<unsigned char> _convert(const aron::data::NDArrayPtr& data) final;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/Converter.cpp
similarity index 55%
rename from source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.cpp
rename to source/RobotAPI/libraries/armem/server/ltm/base/converter/object/Converter.cpp
index ea89e7dbc..26b9ffac7 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/Converter.cpp
@@ -3,13 +3,13 @@
namespace armarx::armem::server::ltm
{
- std::vector<unsigned char> DictConverter::convert(const aron::data::VariantPtr& data)
+ std::vector<unsigned char> ObjectConverter::convert(const aron::data::VariantPtr& data)
{
auto d = aron::data::Dict::DynamicCastAndCheck(data);
return _convert(d);
}
- aron::data::VariantPtr DictConverter::convert(const std::vector<unsigned char>& data)
+ aron::data::VariantPtr ObjectConverter::convert(const std::vector<unsigned char>& data)
{
auto d = _convert(data);
return d;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/Converter.h
similarity index 72%
rename from source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.h
rename to source/RobotAPI/libraries/armem/server/ltm/base/converter/object/Converter.h
index d2965ce0e..3ae9dcfa2 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/Converter.h
@@ -11,12 +11,14 @@
namespace armarx::armem::server::ltm
{
- class DictConverter : public Converter
+ class ObjectConverter : public Converter
{
public:
- using Converter::Converter;
+ ObjectConverter(const ConverterType t, const std::string& s):
+ Converter(t, s, aron::type::Descriptor::eObject)
+ {}
- virtual ~DictConverter() = default;
+ virtual ~ObjectConverter() = default;
std::vector<unsigned char> convert(const aron::data::VariantPtr& data) final;
aron::data::VariantPtr convert(const std::vector<unsigned char>& data) final;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/bson/BsonConverter.cpp
similarity index 95%
rename from source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.cpp
rename to source/RobotAPI/libraries/armem/server/ltm/base/converter/object/bson/BsonConverter.cpp
index f183f002f..d483958fa 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/bson/BsonConverter.cpp
@@ -5,7 +5,7 @@
#include <bsoncxx/builder/stream/document.hpp>
#include <bsoncxx/builder/stream/array.hpp>
-namespace armarx::armem::server::ltm::converter::dict
+namespace armarx::armem::server::ltm::converter::object
{
namespace bsoncxxbuilder = bsoncxx::builder::stream;
namespace bsoncxxdoc = bsoncxx::document;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/bson/BsonConverter.h
similarity index 74%
rename from source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.h
rename to source/RobotAPI/libraries/armem/server/ltm/base/converter/object/bson/BsonConverter.h
index e00211110..2dbb293ab 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/bson/BsonConverter.h
@@ -6,16 +6,16 @@
// ArmarX
#include "../json/JsonConverter.h"
-namespace armarx::armem::server::ltm::converter::dict
+namespace armarx::armem::server::ltm::converter::object
{
class BsonConverter;
using BsonConverterPtr = std::shared_ptr<BsonConverter>;
- class BsonConverter : public DictConverter
+ class BsonConverter : public ObjectConverter
{
public:
BsonConverter() :
- DictConverter(ConverterType::Binary, ".bson")
+ ObjectConverter(ConverterType::Binary, ".bson")
{}
protected:
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.cpp
similarity index 92%
rename from source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.cpp
rename to source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.cpp
index 1847b8752..55fe9fddb 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.cpp
@@ -2,7 +2,7 @@
#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
-namespace armarx::armem::server::ltm::converter::dict
+namespace armarx::armem::server::ltm::converter::object
{
std::vector<unsigned char> JsonConverter::_convert(const aron::data::DictPtr& data)
{
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.h
similarity index 59%
rename from source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.h
rename to source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.h
index 8ef4cbc27..98be34661 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.h
@@ -6,14 +6,16 @@
// Simox
#include <SimoxUtility/json.h>
-namespace armarx::armem::server::ltm::converter::dict
+namespace armarx::armem::server::ltm::converter::object
{
- class JsonConverter : public DictConverter
+ class JsonConverter : public ObjectConverter
{
public:
JsonConverter() :
- DictConverter(ConverterType::Str, ".json")
- {}
+ ObjectConverter(ConverterType::Str, ".json")
+ {
+ enabled = true; // always true!
+ }
protected:
std::vector<unsigned char> _convert(const aron::data::DictPtr& data) final;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/BufferedMemoryBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/BufferedMemoryBase.h
index 541e2107b..b351a2784 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/BufferedMemoryBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/BufferedMemoryBase.h
@@ -35,11 +35,11 @@ namespace armarx::armem::server::ltm
void directlyStore(const armem::wm::Memory& memory)
{
TIMING_START(LTM_Memory_DirectlyStore);
- for (auto& f : this->filters->memFilters)
+ for (auto& f : this->processors->memFilters)
{
if (!f->accept(memory))
{
- ARMARX_WARNING << deactivateSpam() << "Ignoring to put a Memory into the LTM because it got filtered.";
+ ARMARX_WARNING << deactivateSpam() << "Ignoring to commit a Memory into the LTM because the full commit got filtered.";
return;
}
}
@@ -69,7 +69,7 @@ namespace armarx::armem::server::ltm
{
Base::createPropertyDefinitions(defs, prefix);
- defs->optional(storeFrequency, prefix + ".buffer.storeFreq", "Frequency to store the buffer to the LTM in Hz.");
+ defs->optional(storeFrequency, prefix + ".buffer.storeFreq", "Frequency to store the buffer to the LTM in Hz.").setMin(1).setMax(1000);
}
protected:
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h
index 3879f6092..ce25bee41 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h
@@ -19,8 +19,6 @@
#include <RobotAPI/libraries/armem/core/operations.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-// The default filter
-#include "../converter/dict/Converter.h"
namespace armarx::armem::server::ltm
{
@@ -85,11 +83,11 @@ namespace armarx::armem::server::ltm
void store(const armem::wm::Memory& memory)
{
TIMING_START(LTM_Memory_Append);
- for (auto& f : filters->memFilters)
+ for (auto& f : processors->memFilters)
{
- if (!f->accept(memory))
+ if (f->enabled && !f->accept(memory))
{
- ARMARX_WARNING << deactivateSpam() << "Ignoring to put a Memory into the LTM because it got filtered.";
+ ARMARX_INFO << deactivateSpam() << "Ignoring to put a Memory into the LTM because it got filtered.";
return;
}
}
@@ -105,27 +103,6 @@ namespace armarx::armem::server::ltm
this->store(memory);
}
- void init()
- {
- // setup the filters
- if (mFreqFilterEnabled)
- {
- auto mFreqFilter = std::make_unique<filter::MemoryFrequencyFilter>();
- mFreqFilter->waitingTimeInMs = mFreqFilterWaitingTime;
- filters->memFilters.push_back(std::move(mFreqFilter));
- }
-
- if (snapFreqFilterEnabled)
- {
- auto snapFreqFilter = std::make_unique<filter::SnapshotFrequencyFilter>();
- snapFreqFilter->waitingTimeInMs = snapFreqFilterEnabled;
- filters->snapFilters.push_back(std::move(snapFreqFilter));
- }
-
- // setup converters
- filters->converters[aron::type::Descriptor::eObject] = std::make_unique<converter::dict::JsonConverter>();
- }
-
/// iterate over all core segments of this ltm
virtual bool forEachCoreSegment(std::function<void(CoreSegmentT&)>&& func) const = 0;
@@ -135,10 +112,19 @@ namespace armarx::armem::server::ltm
/// parameters
virtual void createPropertyDefinitions(PropertyDefinitionsPtr& defs, const std::string& prefix)
{
- defs->optional(mFreqFilterEnabled, prefix + "memFreqFilter.Enabled");
- defs->optional(mFreqFilterWaitingTime, prefix + "memFreqFilter.WaitingTime", "Waiting time in MS after each LTM update.");
- defs->optional(snapFreqFilterEnabled, prefix + "memFreqFilter.Enabled");
- defs->optional(mFreqFilterWaitingTime, prefix + "memSnapFilter.WaitingTime", "Waiting time in MS after each Entity update.");
+ // filters
+ defs->optional(processors->memFreqFilter.enabled, prefix + "memFreqFilter.Enabled");
+ defs->optional(processors->memFreqFilter.waitingTimeInMs, prefix + "memFreqFilter.WaitingTime", "Waiting time in MS after each LTM update.");
+ defs->optional(processors->snapFreqFilter.enabled, prefix + "snapFreqFilter.Enabled");
+ defs->optional(processors->snapFreqFilter.waitingTimeInMs, prefix + "snapFreqFilter.WaitingTime", "Waiting time in MS after each Entity update.");
+ defs->optional(processors->snapEqFilter.enabled, prefix + "snapEqFilter.Enabled");
+ defs->optional(processors->snapEqFilter.maxWaitingTimeInMs, prefix + "snapEqFilter.MaxWaitingTime", "Max Waiting time in MS after each Entity update.");
+
+ // extractors
+ defs->optional(processors->imageExtractor.enabled, prefix + "imageExtractor.Enabled");
+
+ // converters
+ defs->optional(processors->pngConverter.enabled, prefix + "pngConverter.Enabled");
}
/// get level name
@@ -158,10 +144,5 @@ namespace armarx::armem::server::ltm
protected:
mutable std::recursive_mutex ltm_mutex;
- private:
- bool mFreqFilterEnabled = false;
- long mFreqFilterWaitingTime = -1;
- bool snapFreqFilterEnabled = false;
- long snapFreqFilterWaitingTime = -1;
};
} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp
index bc60bb5d2..6f38d6ea2 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp
@@ -8,15 +8,15 @@
namespace armarx::armem::server::ltm
{
MemoryItem::MemoryItem(const MemoryID& id) :
- MemoryItem(id, std::make_shared<FilterCollection>())
+ processors(std::make_shared<Processors>()),
+ _id(id)
{
}
- MemoryItem::MemoryItem(const MemoryID& id, const std::shared_ptr<FilterCollection>& p) :
- filters(p),
+ MemoryItem::MemoryItem(const MemoryID& id, const std::shared_ptr<Processors>& p) :
+ processors(p),
_id(id)
{
- ARMARX_CHECK_NOT_NULL(p); // There must be a filter stack.
}
void MemoryItem::setMemoryID(const MemoryID& id)
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h
index d8d9a5ced..27b91fdb9 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h
@@ -6,8 +6,9 @@
#include <string>
#include "../filter/frequencyFilter/FrequencyFilter.h"
+#include "../filter/equalityFilter/EqualityFilter.h"
#include "../extractor/imageExtractor/ImageExtractor.h"
-#include "../converter/dict/json/JsonConverter.h"
+#include "../converter/object/json/JsonConverter.h"
#include "../converter/image/png/PngConverter.h"
#include <RobotAPI/libraries/armem/core/MemoryID.h>
@@ -15,27 +16,44 @@
namespace armarx::armem::server::ltm
{
/// all necessary classes to filter and convert an entry of the ltm to some other format(s)
- struct FilterCollection
+ struct Processors
{
- // Memory Filters
- std::vector<std::unique_ptr<MemoryFilter>> memFilters;
+ // Unique Memory Filters
+ std::vector<MemoryFilter*> memFilters;
+ filter::MemoryFrequencyFilter memFreqFilter;
- // Snapshot filters
- std::vector<std::unique_ptr<SnapshotFilter>> snapFilters;
+ // Unique Snapshot filters
+ std::vector<SnapshotFilter*> snapFilters;
+ filter::SnapshotFrequencyFilter snapFreqFilter;
+ filter::SnapshotEqualityFilter snapEqFilter;
// Extractors
- std::map<aron::data::Descriptor, std::unique_ptr<Extractor>> extractors;
+ std::vector<Extractor*> extractors;
+ extractor::ImageExtractor imageExtractor;
// Converters
- std::map<aron::type::Descriptor, std::unique_ptr<Converter>> converters;
+ std::map<aron::type::Descriptor, Converter*> converters;
+ converter::object::JsonConverter jsonConverter;
+ converter::image::PngConverter pngConverter;
+
+ Processors()
+ {
+ // setup containers
+ memFilters.push_back(&memFreqFilter);
+ snapFilters.push_back(&snapFreqFilter);
+ snapFilters.push_back(&snapEqFilter);
+ extractors.push_back(&imageExtractor);
+ converters.insert({jsonConverter.convertsType, &jsonConverter});
+ converters.insert({pngConverter.convertsType, &pngConverter});
+ }
};
/// @brief Interface functions for the longterm memory classes
class MemoryItem
{
public:
- MemoryItem(const MemoryID&);
- MemoryItem(const MemoryID&, const std::shared_ptr<FilterCollection>&);
+ MemoryItem(const MemoryID&); // only used by memory
+ MemoryItem(const MemoryID&, const std::shared_ptr<Processors>&); // used by all other segments
virtual ~MemoryItem() = default;
MemoryID id() const;
@@ -44,7 +62,7 @@ namespace armarx::armem::server::ltm
virtual void setMemoryID(const MemoryID&);
protected:
- std::shared_ptr<FilterCollection> filters;
+ std::shared_ptr<Processors> processors;
private:
MemoryID _id;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/Extractor.h b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/Extractor.h
index 10647ccbe..8fbedc484 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/Extractor.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/Extractor.h
@@ -18,10 +18,13 @@ namespace armarx::armem::server::ltm
std::map<std::string, aron::data::VariantPtr> extraction;
};
- Extractor() = default;
+ Extractor(const aron::type::Descriptor t) : extractsType(t) {};
virtual ~Extractor() = default;
virtual Extraction extract(aron::data::DictPtr& data) = 0;
virtual aron::data::DictPtr merge(Extraction& encoding) = 0;
+
+ const aron::type::Descriptor extractsType;
+ bool enabled = false;
};
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.h b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.h
index 08161d3fc..73a923579 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.h
@@ -19,7 +19,11 @@ namespace armarx::armem::server::ltm::extractor
class ImageExtractor : public Extractor
{
public:
- ImageExtractor() = default;
+ ImageExtractor() :
+ Extractor(aron::type::Descriptor::eImage)
+ {
+ enabled = true;
+ };
virtual Extraction extract(aron::data::DictPtr& data) override;
virtual aron::data::DictPtr merge(Extraction& encoding) override;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/filter/Filter.h b/source/RobotAPI/libraries/armem/server/ltm/base/filter/Filter.h
index 6d64ca98c..e75969bfa 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/filter/Filter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/filter/Filter.h
@@ -16,6 +16,8 @@ namespace armarx::armem::server::ltm
virtual ~MemoryFilter() = default;
virtual bool accept(const armem::wm::Memory& e) = 0;
+
+ bool enabled = false;
};
class SnapshotFilter
@@ -25,5 +27,7 @@ namespace armarx::armem::server::ltm
virtual ~SnapshotFilter() = default;
virtual bool accept(const armem::wm::EntitySnapshot& e) = 0;
+
+ bool enabled = false;
};
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/filter/equalityFilter/EqualityFilter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/filter/equalityFilter/EqualityFilter.cpp
new file mode 100644
index 000000000..ec52015e2
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/filter/equalityFilter/EqualityFilter.cpp
@@ -0,0 +1,52 @@
+#include "EqualityFilter.h"
+
+#include <IceUtil/Time.h>
+
+namespace armarx::armem::server::ltm::filter
+{
+ bool SnapshotEqualityFilter::accept(const armem::wm::EntitySnapshot& e)
+ {
+ auto entityID = e.id().getEntityID();
+ auto genMs = e.time().toMilliSeconds();
+
+ long lastMs = 0;
+ std::vector<aron::data::DictPtr> lastData;
+ if (timestampLastCommitInMs.count(entityID) > 0)
+ {
+ lastData = dataLastCommit.at(entityID);
+ lastMs = timestampLastCommitInMs.at(entityID);
+ }
+
+ auto timePassedSinceLastStored = genMs - lastMs;
+ if (maxWaitingTimeInMs < 0 || timePassedSinceLastStored > maxWaitingTimeInMs)
+ {
+ bool accept = false;
+ std::vector<aron::data::DictPtr> genData;
+ for (unsigned int i = 0; i != e.size(); ++i)
+ {
+ const auto& d = e.getInstance(i).data();
+ genData.push_back(d);
+
+ if (lastMs == 0 || e.size() != lastData.size()) // nothing stored yet or we cannot compare
+ {
+ accept = true;
+ break;
+ }
+
+ const auto& el = lastData.at(i);
+ if ((!d and el) || (d and !el) || (d && el && !(*d == *el))) // data unequal?
+ {
+ accept = true;
+ break;
+ }
+ }
+
+ if (!accept) return false;
+
+ dataLastCommit[entityID] = genData;
+ timestampLastCommitInMs[entityID] = genMs;
+ return true;
+ }
+ return false;
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/filter/equalityFilter/EqualityFilter.h b/source/RobotAPI/libraries/armem/server/ltm/base/filter/equalityFilter/EqualityFilter.h
new file mode 100644
index 000000000..1ad630b4f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/filter/equalityFilter/EqualityFilter.h
@@ -0,0 +1,29 @@
+#pragma once
+
+#include <vector>
+#include <map>
+
+// Base Class
+#include "../Filter.h"
+
+// Aron
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx::armem::server::ltm::filter
+{
+ class SnapshotEqualityFilter :
+ public SnapshotFilter
+ {
+ public:
+ SnapshotEqualityFilter() = default;
+
+ virtual bool accept(const armem::wm::EntitySnapshot& e) override;
+
+ public:
+ int maxWaitingTimeInMs = -1;
+
+ private:
+ std::map<MemoryID, std::vector<aron::data::DictPtr>> dataLastCommit;
+ std::map<MemoryID, long> timestampLastCommitInMs;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.h b/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.h
index 100ccf625..f8427b2b7 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.h
@@ -1,5 +1,7 @@
#pragma once
+#include <map>
+
// Base Class
#include "../Filter.h"
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp
index c22b406d8..216f5ed50 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp
@@ -8,72 +8,62 @@
namespace armarx::armem::server::ltm::disk
{
- namespace
- {
- MemoryID getMemoryIDFromPath(const std::filesystem::path& p)
- {
- util::ensureFolderExists(p);
-
- MemoryID m;
- m.memoryName = p.parent_path().filename();
- m.coreSegmentName = p.filename();
- return m;
- }
- }
-
- CoreSegment::CoreSegment(const std::filesystem::path& p, const std::shared_ptr<FilterCollection>& pipe) :
- CoreSegmentBase(getMemoryIDFromPath(p), pipe),
- DiskMemoryItem(p.parent_path())
+ CoreSegment::CoreSegment(const std::filesystem::path& p, const MemoryID& parentID, const std::string& escapedSegmentName, const std::shared_ptr<Processors>& filters) :
+ CoreSegmentBase(parentID.withCoreSegmentName(UnescapeSegmentName(escapedSegmentName)), filters),
+ DiskMemoryItem(p)
{
}
bool CoreSegment::forEachProviderSegment(std::function<void(ProviderSegment&)>&& func) const
{
- if (!checkPath(id().coreSegmentName))
+ if (!checkPathAndSegmentName(id().coreSegmentName))
{
return false;
}
- std::filesystem::path p = path;
- p = p / id().coreSegmentName;
- util::ensureFolderExists(p, false);
-
- for (const auto& subdir : std::filesystem::directory_iterator(p))
+ std::filesystem::path csPath = std::filesystem::path(parentPath) / EscapeSegmentName(id().coreSegmentName);
+ for (const auto& subdir : std::filesystem::directory_iterator(csPath))
{
std::filesystem::path subdirPath = subdir.path();
- ProviderSegment c(subdirPath, filters);
+ ProviderSegment c(csPath, id(), subdirPath.filename(), processors);
func(c);
}
return true;
}
- std::shared_ptr<ProviderSegment> CoreSegment::findProviderSegment(const std::string& n) const
+ std::shared_ptr<ProviderSegment> CoreSegment::findProviderSegment(const std::string& providerSegmentName) const
{
- if (!checkPath(id().coreSegmentName))
+ if (!checkPathAndSegmentName(id().coreSegmentName))
{
- return {};
+ return nullptr;
}
- std::filesystem::path p = path;
- p = p / id().coreSegmentName;
- util::ensureFolderExists(p, false);
+ std::filesystem::path csPath = std::filesystem::path(parentPath) / EscapeSegmentName(id().coreSegmentName);
+ std::filesystem::path provPath = csPath / EscapeSegmentName(providerSegmentName);
+ if (!filesystem::util::checkIfFolderExists(provPath))
+ {
+ return nullptr;
+ }
- std::filesystem::path subpath = p / n;
- util::ensureFolderExists(subpath, false);
- auto c = std::make_shared<ProviderSegment>(subpath, filters);
+ auto c = std::make_shared<ProviderSegment>(csPath, id(), provPath.filename(), processors);
return c;
}
std::string CoreSegment::getExpectedFolderName() const
{
- return name();
+ return EscapeSegmentName(id().coreSegmentName);
}
void CoreSegment::_loadAllReferences(armem::wm::CoreSegment& e)
{
+ if (!checkPathAndSegmentName(id().coreSegmentName))
+ {
+ return;
+ }
+
e.id() = id();
- forEachProviderSegment([&e](ProviderSegment& x) {
+ forEachProviderSegment([&e](auto& x) {
armem::wm::ProviderSegment s;
x.loadAllReferences(s);
e.addProviderSegment(s);
@@ -82,21 +72,50 @@ namespace armarx::armem::server::ltm::disk
void CoreSegment::_resolve(armem::wm::CoreSegment& c)
{
- c.forEachProviderSegment([this](armem::wm::ProviderSegment& e)
+ if (!checkPathAndSegmentName(id().coreSegmentName))
+ {
+ return;
+ }
+
+ std::filesystem::path csPath = std::filesystem::path(parentPath) / EscapeSegmentName(id().coreSegmentName);
+ c.forEachProviderSegment([&](auto& e)
{
- util::ensureFolderExists(std::filesystem::path(path) / id().coreSegmentName / e.id().providerSegmentName, false);
- ProviderSegment c((std::filesystem::path(path) / id().coreSegmentName / e.id().providerSegmentName), filters);
- c.resolve(e);
+ std::filesystem::path provPath = csPath / EscapeSegmentName(e.id().providerSegmentName);
+ if (filesystem::util::checkIfFolderExists(provPath))
+ {
+ ProviderSegment c(csPath, id(), provPath.filename(), processors);
+ c.resolve(e);
+ }
+ else
+ {
+ ARMARX_WARNING << "Could not find the provider segment folder for segment '" << e.id().str() << "'.";
+ }
});
}
void CoreSegment::_store(const armem::wm::CoreSegment& c)
{
- c.forEachProviderSegment([this](const auto& provSegment)
+ if (id().coreSegmentName.empty())
{
- util::ensureFolderExists(std::filesystem::path(path) / id().coreSegmentName / provSegment.id().providerSegmentName);
- ProviderSegment c((std::filesystem::path(path) / id().coreSegmentName / provSegment.id().providerSegmentName), filters);
- c.store(provSegment);
+ ARMARX_WARNING << "During storage of segment '" << c.id().str() << "' I noticed that the corresponding LTM has no id set. " <<
+ "I set the id of the LTM to the same name, however this should not happen!";
+ id().coreSegmentName = c.id().coreSegmentName;
+ }
+
+ std::filesystem::path csPath = std::filesystem::path(parentPath) / EscapeSegmentName(id().coreSegmentName);
+ if (!filesystem::util::checkIfFolderExists(csPath))
+ {
+ ARMARX_WARNING << "The segment folder for segment '"+id().str()+"'was not created. I will create the folder by myself, however it seems like there is a bug in the ltm pipeline.";
+ filesystem::util::ensureFolderExists(csPath, true);
+ }
+
+ c.forEachProviderSegment([&](const auto& prov)
+ {
+ std::filesystem::path provPath = csPath / EscapeSegmentName(prov.id().providerSegmentName);
+ filesystem::util::ensureFolderExists(provPath, true);
+
+ ProviderSegment c(csPath, id(), provPath.filename(), processors);
+ c.store(prov);
});
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h
index bb16074bc..7aac18270 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h
@@ -16,7 +16,7 @@ namespace armarx::armem::server::ltm::disk
{
public:
- CoreSegment(const std::filesystem::path&, const std::shared_ptr<FilterCollection>& p);
+ CoreSegment(const std::filesystem::path& parentPath, const MemoryID& parentID, const std::string& escapedSegmentName, const std::shared_ptr<Processors>& p);
bool forEachProviderSegment(std::function<void(ProviderSegment&)>&& func) const override;
@@ -28,7 +28,6 @@ namespace armarx::armem::server::ltm::disk
void _store(const armem::wm::CoreSegment&) override;
std::string getExpectedFolderName() const override;
-
};
} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp
index 3854c834c..83841c65e 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp
@@ -12,48 +12,36 @@
namespace armarx::armem::server::ltm::disk
{
-
- namespace
- {
- MemoryID getMemoryIDFromPath(const std::filesystem::path& p)
- {
- util::ensureFolderExists(p);
-
- MemoryID m;
- m.memoryName = p.parent_path().parent_path().parent_path().filename();
- m.coreSegmentName = p.parent_path().parent_path().filename();
- m.providerSegmentName = p.parent_path().filename();
- m.entityName = p.filename();
- return m;
- }
- }
-
- Entity::Entity(const std::filesystem::path& p, const std::shared_ptr<FilterCollection>& pipe) :
- EntityBase(getMemoryIDFromPath(p), pipe),
- DiskMemoryItem(p.parent_path())
+ Entity::Entity(const std::filesystem::path& p, const MemoryID& parentID, const std::string& escapedSegmentName, const std::shared_ptr<Processors>& filters) :
+ EntityBase(parentID.withEntityName(UnescapeSegmentName(escapedSegmentName)), filters),
+ DiskMemoryItem(p)
{
}
std::string Entity::getExpectedFolderName() const
{
- return name();
+ return EscapeSegmentName(id().entityName);
}
bool Entity::forEachSnapshot(std::function<void(EntitySnapshot&)>&& func) const
{
- if (!checkPath(id().entityName))
+ if (!checkPathAndSegmentName(id().entityName))
{
return false;
}
- std::filesystem::path p = path;
- p = p / id().entityName;
- util::ensureFolderExists(p, false);
-
- for (const auto& subdir : std::filesystem::directory_iterator(p))
+ std::filesystem::path ePath = std::filesystem::path(parentPath) / EscapeSegmentName(id().entityName);
+ for (const auto& subdir : std::filesystem::directory_iterator(ePath))
{
std::filesystem::path subdirPath = subdir.path();
- EntitySnapshot c(subdirPath, filters);
+ if (!util::isNumber(subdirPath.filename()))
+ {
+ ARMARX_WARNING << "Found a non-timestamp folder inside an entity '" << id().str() << "' with name '" << subdirPath.filename() << "'. " <<
+ "Ignoring this folder, however this is a bad situation.";
+ continue;
+ }
+
+ EntitySnapshot c(ePath, id(), Time::microSeconds(std::stol(subdirPath.filename())), processors);
func(c);
}
return true;
@@ -61,104 +49,255 @@ namespace armarx::armem::server::ltm::disk
bool Entity::forEachSnapshotInIndexRange(long first, long last, std::function<void(EntitySnapshot&)>&& func) const
{
- return true;
+ throw LocalException("NOT IMPLEMENTED YET BECAUSE THE DIRECTORY ITERATOR IS UNSORTED!");
}
bool Entity::forEachSnapshotInTimeRange(const Time& min, const Time& max, std::function<void(EntitySnapshot&)>&& func) const
{
- return true;
+ auto f = [&](EntitySnapshot& e) {
+ auto ts = e.id().timestamp;
+ if (ts >= min && ts <= max)
+ {
+ func(e);
+ }
+ };
+
+ return forEachSnapshot(std::move(f));
}
bool Entity::forEachSnapshotBeforeOrAt(const Time& time, std::function<void(EntitySnapshot&)>&& func) const
{
- return true;
+ auto f = [&](EntitySnapshot& e) {
+ auto ts = e.id().timestamp;
+ if (ts <= time)
+ {
+ func(e);
+ }
+ };
+
+ return forEachSnapshot(std::move(f));
}
bool Entity::forEachSnapshotBefore(const Time& time, std::function<void(EntitySnapshot&)>&& func) const
{
- return true;
+ auto f = [&](EntitySnapshot& e) {
+ auto ts = e.id().timestamp;
+ if (ts < time)
+ {
+ func(e);
+ }
+ };
+
+ return forEachSnapshot(std::move(f));
}
std::shared_ptr<EntitySnapshot> Entity::findSnapshot(const Time& n) const
{
- if (!checkPath(id().entityName))
+ if (!checkPathAndSegmentName(id().entityName))
{
return {};
}
- std::filesystem::path p = path;
- p = p / id().entityName;
- util::ensureFolderExists(p, false);
+ std::filesystem::path ePath = std::filesystem::path(parentPath) / EscapeSegmentName(id().entityName);
+ std::filesystem::path tsPath = ePath / std::to_string(n.toMicroSeconds());
+ if (!filesystem::util::checkIfFolderExists(tsPath))
+ {
+ return nullptr;
+ }
- std::filesystem::path subpath = p / std::to_string(n.toMicroSeconds());
- util::ensureFolderExists(subpath, false);
- auto c = std::make_shared<EntitySnapshot>(subpath, filters);
- return c;
+ return std::make_shared<EntitySnapshot>(ePath, id(), n, processors);
}
std::shared_ptr<EntitySnapshot> Entity::findLatestSnapshot() const
{
- return {};
+ Time bestMatch = Time::microSeconds(-1);
+ auto f = [&](EntitySnapshot& e) {
+ auto ts = e.id().timestamp;
+ if (ts > bestMatch)
+ {
+ bestMatch = ts;
+ }
+ };
+
+ forEachSnapshot(std::move(f));
+
+ if (bestMatch == Time::microSeconds(-1))
+ {
+ return nullptr;
+ }
+
+ std::filesystem::path ePath = std::filesystem::path(parentPath) / id().entityName;
+ return std::make_shared<EntitySnapshot>(ePath, id(), bestMatch, processors);
}
std::shared_ptr<EntitySnapshot> Entity::findLatestSnapshotBefore(const Time& time) const
{
- return {};
+ Time bestMatch = Time::microSeconds(-1);
+ auto f = [&](EntitySnapshot& e) {
+ auto ts = e.id().timestamp;
+ if (ts < time && ts > bestMatch)
+ {
+ bestMatch = ts;
+ }
+ };
+
+ forEachSnapshot(std::move(f));
+
+ if (bestMatch == Time::microSeconds(-1))
+ {
+ return nullptr;
+ }
+
+ std::filesystem::path ePath = std::filesystem::path(parentPath) / id().entityName;
+ return std::make_shared<EntitySnapshot>(ePath, id(), bestMatch, processors);
}
std::shared_ptr<EntitySnapshot> Entity::findLatestSnapshotBeforeOrAt(const Time& time) const
{
- return {};
+ Time bestMatch = Time::microSeconds(-1);
+ auto f = [&](EntitySnapshot& e) {
+ auto ts = e.id().timestamp;
+ if (ts <= time && ts > bestMatch)
+ {
+ bestMatch = ts;
+ }
+ };
+
+ forEachSnapshot(std::move(f));
+
+ if (bestMatch == Time::microSeconds(-1))
+ {
+ return nullptr;
+ }
+
+ std::filesystem::path ePath = std::filesystem::path(parentPath) / id().entityName;
+ return std::make_shared<EntitySnapshot>(ePath, id(), bestMatch, processors);
}
std::shared_ptr<EntitySnapshot> Entity::findFirstSnapshotAfter(const Time& time) const
{
- return {};
+ Time bestMatch = Time::microSeconds(std::numeric_limits<long>::max());
+ auto f = [&](EntitySnapshot& e) {
+ auto ts = e.id().timestamp;
+ if (ts > time && ts < bestMatch)
+ {
+ bestMatch = ts;
+ }
+ };
+
+ forEachSnapshot(std::move(f));
+
+ if (bestMatch == Time::microSeconds(std::numeric_limits<long>::max()))
+ {
+ return nullptr;
+ }
+
+ std::filesystem::path ePath = std::filesystem::path(parentPath) / id().entityName;
+ return std::make_shared<EntitySnapshot>(ePath, id(), bestMatch, processors);
}
std::shared_ptr<EntitySnapshot> Entity::findFirstSnapshotAfterOrAt(const Time& time) const
{
- return {};
+ Time bestMatch = Time::microSeconds(std::numeric_limits<long>::max());
+ auto f = [&](EntitySnapshot& e) {
+ auto ts = e.id().timestamp;
+ if (ts >= time && ts < bestMatch)
+ {
+ bestMatch = ts;
+ }
+ };
+
+ forEachSnapshot(std::move(f));
+
+ if (bestMatch == Time::microSeconds(std::numeric_limits<long>::max()))
+ {
+ return nullptr;
+ }
+
+ std::filesystem::path ePath = std::filesystem::path(parentPath) / id().entityName;
+ return std::make_shared<EntitySnapshot>(ePath, id(), bestMatch, processors);
}
void Entity::_loadAllReferences(armem::wm::Entity& e)
{
+ if (!checkPathAndSegmentName(id().entityName))
+ {
+ return;
+ }
+
e.id() = id();
- forEachSnapshot([&e](EntitySnapshotBase& x) {
- armem::wm::EntitySnapshot s;
- x.loadAllReferences(s);
- e.addSnapshot(s);
+ forEachSnapshot([&e](auto& x) {
+ if (!e.hasSnapshot(x.id().timestamp)) // we only load the references if the snapshot is not existant
+ {
+ armem::wm::EntitySnapshot s;
+ x.loadAllReferences(s);
+ e.addSnapshot(s);
+ }
});
}
void Entity::_resolve(armem::wm::Entity& p)
{
- p.forEachSnapshot([this](armem::wm::EntitySnapshot& e)
+ if (!checkPathAndSegmentName(id().entityName))
+ {
+ return;
+ }
+
+ std::filesystem::path ePath = std::filesystem::path(parentPath) / EscapeSegmentName(id().entityName);
+ p.forEachSnapshot([&](auto& e)
{
- util::ensureFolderExists(std::filesystem::path(path) / id().entityName / std::to_string(e.id().timestamp.toMicroSeconds()), false);
- EntitySnapshot c((std::filesystem::path(path) / id().entityName / std::to_string(e.id().timestamp.toMicroSeconds())), filters);
- c.resolve(e);
+ std::filesystem::path tsPath = ePath / e.id().timestampStr();
+ if (filesystem::util::checkIfFolderExists(tsPath))
+ {
+ EntitySnapshot c(ePath, id(), e.id().timestamp, processors);
+ c.resolve(e);
+ }
+ else
+ {
+ ARMARX_WARNING << "Could not find the snapshot segment folder for segment '" << e.id().str() << "'.";
+ }
});
}
- void Entity::_store(const armem::wm::Entity& entity)
+ void Entity::_store(const armem::wm::Entity& c)
{
- entity.forEachSnapshot([this](armem::wm::EntitySnapshot& e)
+ if (id().entityName.empty())
+ {
+ ARMARX_WARNING << "During storage of segment '" << c.id().str() << "' I noticed that the corresponding LTM has no id set. " <<
+ "I set the id of the LTM to the same name, however this should not happen!";
+ id().entityName = c.id().entityName;
+ }
+
+ std::filesystem::path ePath = std::filesystem::path(parentPath) / EscapeSegmentName(id().entityName);
+ if (!filesystem::util::checkIfFolderExists(ePath))
+ {
+ ARMARX_WARNING << "The segment folder for segment '"+id().str()+"'was not created. I will create the folder by myself, however it seems like there is a bug in the ltm pipeline.";
+ filesystem::util::ensureFolderExists(ePath, true);
+ }
+
+ c.forEachSnapshot([&](const auto& snap)
{
- for (auto& f : filters->snapFilters)
+ std::filesystem::path tsPath = ePath / snap.id().timestampStr();
+ if (filesystem::util::checkIfFolderExists(tsPath))
{
- if (!f->accept(e))
+ ARMARX_INFO << deactivateSpam() << "Ignoring to put an EntitiySnapshot into the LTM because the timestamp already existed (we assume snapshots are const and do not change outside the ltm).";
+ return;
+ }
+
+ for (auto& f : processors->snapFilters)
+ {
+ if (f->enabled && !f->accept(snap))
{
- ARMARX_WARNING << deactivateSpam() << "Ignoring to put an EntitiySnapshot into the LTM because it got filtered.";
+ ARMARX_INFO << deactivateSpam() << "Ignoring to put an EntitiySnapshot into the LTM because it got filtered.";
return;
}
}
- util::ensureFolderExists(std::filesystem::path(path) / id().entityName / std::to_string(e.id().timestamp.toMicroSeconds()));
- EntitySnapshot c((std::filesystem::path(path) / id().entityName / std::to_string(e.id().timestamp.toMicroSeconds())), filters);
- c.store(e);
+ filesystem::util::ensureFolderExists(tsPath);
+
+ EntitySnapshot c(ePath, id(), snap.id().timestamp, processors);
+ c.store(snap);
});
}
-
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h
index e6926cc8c..a32e3566b 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h
@@ -16,7 +16,7 @@ namespace armarx::armem::server::ltm::disk
public DiskMemoryItem
{
public:
- Entity(const std::filesystem::path&, const std::shared_ptr<FilterCollection>& p);
+ Entity(const std::filesystem::path& parentPath, const MemoryID& parentID, const std::string& escapedSegmentName, const std::shared_ptr<Processors>& p);
bool forEachSnapshot(std::function<void(EntitySnapshot&)>&& func) const override;
bool forEachSnapshotInIndexRange(long first, long last, std::function<void(EntitySnapshot&)>&& func) const override;
@@ -37,7 +37,6 @@ namespace armarx::armem::server::ltm::disk
void _store(const armem::wm::Entity&) override;
std::string getExpectedFolderName() const override;
-
};
} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.cpp
index 7e37722aa..53e4301a5 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.cpp
@@ -15,23 +15,15 @@ namespace armarx::armem::server::ltm::disk
namespace
{
- MemoryID getMemoryIDFromPath(const std::filesystem::path& p)
- {
- util::ensureFolderExists(p);
-
- MemoryID m;
- m.memoryName = p.parent_path().parent_path().parent_path().parent_path().filename();
- m.coreSegmentName = p.parent_path().parent_path().parent_path().filename();
- m.providerSegmentName = p.parent_path().parent_path().filename();
- m.entityName = p.parent_path().filename();
- m.timestamp = IceUtil::Time::microSeconds(std::stol(p.filename()));
- return m;
- }
-
void writeDataToFile(const std::filesystem::path& path, const std::vector<unsigned char>& data)
{
std::ofstream dataofs;
dataofs.open(path);
+ if (!dataofs)
+ {
+ ARMARX_ERROR << "Could not write data to file '" << path.string() << "'. Skipping this file.";
+ return;
+ }
dataofs.write(reinterpret_cast<const char*>(data.data()), data.size());
dataofs.close();
}
@@ -44,29 +36,30 @@ namespace armarx::armem::server::ltm::disk
}
}
- EntitySnapshot::EntitySnapshot(const std::filesystem::path& p, const std::shared_ptr<FilterCollection>& pipe) :
- EntitySnapshotBase(getMemoryIDFromPath(p), pipe),
- DiskMemoryItem(p.parent_path())
+ EntitySnapshot::EntitySnapshot(const std::filesystem::path& p, const MemoryID& parentID, const Time& ts, const std::shared_ptr<Processors>& filters) :
+ EntitySnapshotBase(parentID.withTimestamp(ts), filters),
+ DiskMemoryItem(p)
{
-
}
std::string EntitySnapshot::getExpectedFolderName() const
{
- return name();
+ return id().timestampStr();
}
void EntitySnapshot::_loadAllReferences(armem::wm::EntitySnapshot& e) const
{
- std::filesystem::path p = path;
- p = p / id().timestampStr();
- util::ensureFolderExists(p, false);
+ if (!checkPathAndSegmentName(id().timestampStr()))
+ {
+ return;
+ }
e.id() = id();
+ std::filesystem::path tsPath = std::filesystem::path(parentPath) / id().timestampStr();
for (unsigned int i = 0; i < 1000; ++i) // 1000 is max size for instances in a single timestamp
{
- if (!util::checkIfFolderExists(p / std::to_string(i)))
+ if (!filesystem::util::checkIfFolderExists(tsPath / std::to_string(i)))
{
break;
}
@@ -78,113 +71,154 @@ namespace armarx::armem::server::ltm::disk
void EntitySnapshot::_resolve(armem::wm::EntitySnapshot& e) const
{
- ARMARX_IMPORTANT << __PRETTY_FUNCTION__;
- auto& dictConverter = filters->converters.at(aron::type::Descriptor::eObject);
+ if (!checkPathAndSegmentName(id().timestampStr()))
+ {
+ return;
+ }
+
+ auto& dictConverter = processors->converters.at(aron::type::Descriptor::eObject);
// Get data from disk
- std::filesystem::path p = path;
- p = p / id().timestampStr();
- util::ensureFolderExists(p, false);
+ std::filesystem::path tsPath = std::filesystem::path(parentPath) / id().timestampStr();
for (unsigned int i = 0; i < e.size(); ++i)
{
- util::ensureFolderExists(p / std::to_string(i), false);
-
- std::filesystem::path data = p / std::to_string(i) / (constantes::DATA_FILENAME + dictConverter->suffix);
- std::filesystem::path metadata = p / std::to_string(i) / (constantes::METADATA_FILENAME + dictConverter->suffix);
-
- util::ensureFileExists(data);
-
- auto& ins = e.getInstance(i);
+ std::filesystem::path iPath = tsPath / std::to_string(i);
+ if (filesystem::util::checkIfFolderExists(iPath))
+ {
+ std::filesystem::path data = iPath / (constantes::DATA_FILENAME + dictConverter->suffix);
+ std::filesystem::path metadata = iPath / (constantes::METADATA_FILENAME + dictConverter->suffix);
- auto datafilecontent = readDataFromFile(data);
- auto dataaron = dictConverter->convert(datafilecontent);
- auto datadict = aron::data::Dict::DynamicCastAndCheck(dataaron);
+ auto& ins = e.getInstance(i);
+ aron::data::DictPtr datadict = nullptr;
+ aron::data::DictPtr metadatadict = nullptr;
- // check for special members
- for (const auto& [key, m] : datadict->getElements())
- {
- for (auto& [t, f] : filters->converters)
+ if (filesystem::util::checkIfFileExists(data))
{
- if (t == aron::type::Descriptor::eObject)
- {
- continue;
- }
+ auto datafilecontent = readDataFromFile(data);
+ auto dataaron = dictConverter->convert(datafilecontent);
+ datadict = aron::data::Dict::DynamicCastAndCheck(dataaron);
- std::filesystem::path member = p / std::to_string(i) / (key + f->suffix);
-
- if (std::filesystem::exists(member) && std::filesystem::is_regular_file(member))
+ // check for special members
+ for (const auto& [key, m] : datadict->getElements())
{
- auto memberfilecontent = readDataFromFile(member);
- auto memberaron = f->convert(memberfilecontent);
- datadict->setElement(key, memberaron);
+ for (auto& [t, f] : processors->converters)
+ {
+ if (t == aron::type::Descriptor::eObject)
+ {
+ continue;
+ }
+
+ std::filesystem::path member = iPath / (key + f->suffix);
+
+ if (std::filesystem::exists(member) && std::filesystem::is_regular_file(member))
+ {
+ auto memberfilecontent = readDataFromFile(member);
+ auto memberaron = f->convert(memberfilecontent);
+ datadict->setElement(key, memberaron);
+ }
+ }
}
}
- }
+ else
+ {
+ ARMARX_ERROR << "Could not find the data file '" << data.string() << "'. Continuing without data.";
+ }
- auto metadatafilecontent = readDataFromFile(metadata);
- auto metadataaron = dictConverter->convert(metadatafilecontent);
- auto metadatadict = aron::data::Dict::DynamicCastAndCheck(metadataaron);
+ if (filesystem::util::checkIfFileExists(metadata))
+ {
+ auto metadatafilecontent = readDataFromFile(metadata);
+ auto metadataaron = dictConverter->convert(metadatafilecontent);
+ metadatadict = aron::data::Dict::DynamicCastAndCheck(metadataaron);
+ }
+ else
+ {
+ ARMARX_ERROR << "Could not find the metadata file '" << metadata.string() << "'. Continuing without metadata.";
+ }
- from_aron(metadatadict, datadict, ins);
+ from_aron(metadatadict, datadict, ins);
+ }
+ else
+ {
+ ARMARX_WARNING << "Could not find the index segment folder for segment '" << e.id().str() << "'.";
+ }
}
}
void EntitySnapshot::_store(const armem::wm::EntitySnapshot& e) const
{
- ARMARX_IMPORTANT << __PRETTY_FUNCTION__;
- auto& dictConverter = filters->converters.at(aron::type::Descriptor::eObject);
+ if (id().timestampStr().empty())
+ {
+ ARMARX_WARNING << "During storage of segment '" << e.id().str() << "' I noticed that the corresponding LTM has no id set. " <<
+ "I set the id of the LTM to the same name, however this should not happen!";
+ id().timestamp = e.id().timestamp;
+ }
- std::filesystem::path p = path;
- p = p / id().timestampStr();
- util::ensureFolderExists(p);
+ auto& dictConverter = processors->converters.at(aron::type::Descriptor::eObject);
- for (unsigned int i = 0; i < e.size(); ++i)
+ std::filesystem::path tsPath = std::filesystem::path(parentPath) / id().timestampStr();
+ if (!filesystem::util::checkIfFolderExists(tsPath))
{
- std::filesystem::path instancePath = p / std::to_string(i);
- util::ensureFolderExists(instancePath);
-
- std::filesystem::path dataPath = instancePath / (constantes::DATA_FILENAME + dictConverter->suffix);
- std::filesystem::path metadataPath = instancePath / (constantes::METADATA_FILENAME + dictConverter->suffix);
+ ARMARX_WARNING << "The segment folder for segment '"+id().str()+"'was not created. I will create the folder by myself, however it seems like there is a bug in the ltm pipeline.";
+ filesystem::util::ensureFolderExists(tsPath, true);
+ }
- if (util::checkIfFileExists(dataPath) or util::checkIfFileExists(metadataPath))
+ for (unsigned int i = 0; i < e.size(); ++i)
+ {
+ std::filesystem::path iPath = tsPath / std::to_string(i);
+ if (!filesystem::util::checkIfFolderExists(iPath))
{
- continue; // we ignore if file already exists
- }
+ filesystem::util::ensureFolderExists(iPath);
- auto& ins = e.getInstance(i);
+ std::filesystem::path dataPath = iPath / (constantes::DATA_FILENAME + dictConverter->suffix);
+ std::filesystem::path metadataPath = iPath / (constantes::METADATA_FILENAME + dictConverter->suffix);
- // data
- auto dataAron = std::make_shared<aron::data::Dict>();
- auto metadataAron = std::make_shared<aron::data::Dict>();
- to_aron(metadataAron, dataAron, ins);
+ auto& ins = e.getInstance(i);
- // check special members for special extractions
- for (auto& [t, x] : filters->extractors)
- {
- if (auto it = filters->converters.find(aron::data::defaultconversion::Data2TypeDescriptor.at(t)); it != filters->converters.end())
+ // data
+ auto dataAron = std::make_shared<aron::data::Dict>();
+ auto metadataAron = std::make_shared<aron::data::Dict>();
+ to_aron(metadataAron, dataAron, ins);
+
+ // check special members for special extractions
+ for (auto& x : processors->extractors)
{
- auto& conv = it->second;
- auto dataExt = x->extract(dataAron);
+ if (!x->enabled) continue;
+
+ auto t = x->extractsType;
- for (const auto& [memberName, var] : dataExt.extraction)
+ Converter* conv = nullptr; // find suitable converter
+ for (const auto& [ct, c] : processors->converters)
{
- ARMARX_CHECK_NOT_NULL(var);
- std::filesystem::path extPath = instancePath / (memberName + conv->suffix);
- auto extVec = conv->convert(var);
- writeDataToFile(extPath, extVec);
+ if (!c->enabled) continue;
+ if (t != ct) continue;
+ conv = c;
}
- dataAron = dataExt.dataWithoutExtraction;
+ if (conv)
+ {
+ auto dataExt = x->extract(dataAron);
+
+ for (const auto& [memberName, var] : dataExt.extraction)
+ {
+ ARMARX_CHECK_NOT_NULL(var);
+ std::filesystem::path extPath = iPath / (memberName + conv->suffix);
+ auto extVec = conv->convert(var);
+ writeDataToFile(extPath, extVec);
+ }
+
+ dataAron = dataExt.dataWithoutExtraction;
+ }
+ // else we could not convert the extracted data so it makes no sense to extract it at all...
}
- // else we could not convert the extracted data so it makes no sense to extract it at all...
- }
- // convert dict and metadata
- auto dataVec = dictConverter->convert(dataAron);
- auto metadataVec = dictConverter->convert(metadataAron);
- writeDataToFile(dataPath, dataVec);
- writeDataToFile(metadataPath, metadataVec);
+ // convert dict and metadata
+ auto dataVec = dictConverter->convert(dataAron);
+ auto metadataVec = dictConverter->convert(metadataAron);
+ writeDataToFile(dataPath, dataVec);
+ writeDataToFile(metadataPath, metadataVec);
+ }
+ // Ignore if the full index already exists
}
}
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.h b/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.h
index 2c5eb55cb..cc63d289a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.h
@@ -14,7 +14,7 @@ namespace armarx::armem::server::ltm::disk
public DiskMemoryItem
{
public:
- EntitySnapshot(const std::filesystem::path&, const std::shared_ptr<FilterCollection>& p);
+ EntitySnapshot(const std::filesystem::path& parentPath, const MemoryID& parentID, const Time& ts, const std::shared_ptr<Processors>& p);
protected:
void _loadAllReferences(armem::wm::EntitySnapshot&) const override;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp
index 190699393..f313df5ce 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp
@@ -5,52 +5,22 @@
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-// ArmarX
-#include "../base/filter/frequencyFilter/FrequencyFilter.h"
-#include "../base/extractor/imageExtractor/ImageExtractor.h"
-#include "../base/converter/dict/json/JsonConverter.h"
-#include "../base/converter/dict/bson/BsonConverter.h"
-#include "../base/converter/image/png/PngConverter.h"
-
namespace armarx::armem::server::ltm::disk
{
- namespace
- {
- MemoryID getMemoryIDFromPath(const std::filesystem::path& p)
- {
- ARMARX_CHECK(!p.empty());
-
- MemoryID m;
- m.memoryName = p.filename();
- return m;
- }
-
- std::filesystem::path getDefaultStoragePath()
- {
- /*std::string armarx_home = std::string(getenv("HOME")) + "/.armarx";
- if (getenv("ARMARX_DEFAULTS_DIR"))
- {
- armarx_home = getenv("ARMARX_DEFAULTS_DIR");
- }
- path = armarx_home + "/armem/disk/data/db";*/
- return "/tmp/MemoryExport/Test";
- }
- }
-
void Memory::createPropertyDefinitions(PropertyDefinitionsPtr& properties, const std::string& prefix)
{
Base::createPropertyDefinitions(properties, prefix);
- properties->optional(path, prefix + "storagepath", "The path to the memory storage.");
+ properties->optional(relativeParentPath, prefix + "storagepath", "The path to the memory storage (the memory will be stored in a seperate subfolder).");
}
Memory::Memory() :
- Memory(getDefaultStoragePath())
+ Memory(std::filesystem::path("/tmp/MemoryExport"), "Test")
{
}
- Memory::Memory(const std::filesystem::path& p) :
- Base(getMemoryIDFromPath(p)),
- DiskMemoryItem(p.parent_path())
+ Memory::Memory(const std::filesystem::path& p, const std::string& memoryName /* UNESCAPED */) :
+ Base(MemoryID(memoryName, "")),
+ DiskMemoryItem(p)
{
}
@@ -65,43 +35,41 @@ namespace armarx::armem::server::ltm::disk
std::string Memory::getExpectedFolderName() const
{
- return id().memoryName;
+ return EscapeSegmentName(id().memoryName);
}
bool Memory::forEachCoreSegment(std::function<void(CoreSegment&)>&& func) const
{
- if (!checkPath(id().memoryName))
+ if (!checkPathAndSegmentFolderName(id().memoryName))
{
return false;
}
- std::filesystem::path p = path;
- p = p / id().memoryName;
- util::ensureFolderExists(p, false);
-
- for (const auto& subdir : std::filesystem::directory_iterator(p))
+ std::filesystem::path mPath = std::filesystem::path(relativeParentPath) / EscapeSegmentName(id().memoryName);
+ for (const auto& subdir : std::filesystem::directory_iterator(mPath))
{
std::filesystem::path subdirPath = subdir.path();
- CoreSegment c(subdirPath, filters);
+ CoreSegment c(mPath, id(), subdirPath.filename(), processors);
func(c);
}
return true;
}
- std::shared_ptr<CoreSegment> Memory::findCoreSegment(const std::string& n) const
+ std::shared_ptr<CoreSegment> Memory::findCoreSegment(const std::string& coreSegmentName) const
{
- if (!checkPath(id().memoryName))
+ if (!checkPathAndSegmentFolderName(id().memoryName))
{
- return {};
+ return nullptr;
}
- std::filesystem::path p = path;
- p = p / id().memoryName;
- util::ensureFolderExists(p, false);
+ std::filesystem::path mPath = std::filesystem::path(relativeParentPath) / EscapeSegmentName(id().memoryName);
+ std::filesystem::path csPath = mPath / EscapeSegmentName(coreSegmentName);
+ if (!filesystem::util::checkIfFolderInFilesystemExists(csPath))
+ {
+ return nullptr;
+ }
- std::filesystem::path subpath = p / n;
- util::ensureFolderExists(subpath, false);
- auto c = std::make_shared<CoreSegment>(subpath, filters);
+ auto c = std::make_shared<CoreSegment>(mPath, id(), csPath.filename(), processors);
return c;
}
@@ -109,7 +77,7 @@ namespace armarx::armem::server::ltm::disk
{
m.id() = id();
- forEachCoreSegment([&m](CoreSegment& x) {
+ forEachCoreSegment([&m](auto& x) {
armem::wm::CoreSegment s;
x.loadAllReferences(s);
m.addCoreSegment(s);
@@ -118,42 +86,50 @@ namespace armarx::armem::server::ltm::disk
void Memory::_resolve(armem::wm::Memory& m)
{
- if (!checkPath(id().memoryName))
+ std::lock_guard l(ltm_mutex); // we cannot load a memory multiple times simultaneously
+ if (!checkPathAndSegmentFolderName(id().memoryName))
{
+ // Memory folder does not exist. Memory has not been exported yet?
return;
}
- std::lock_guard l(ltm_mutex);
- m.forEachCoreSegment([this](armem::wm::CoreSegment& e)
+ std::filesystem::path mPath = std::filesystem::path(relativeParentPath) / EscapeSegmentName(id().memoryName);
+ m.forEachCoreSegment([&](auto& e)
{
- util::ensureFolderExists(std::filesystem::path(path) / id().memoryName / e.id().coreSegmentName, false);
- CoreSegment c((std::filesystem::path(path) / id().memoryName / e.id().coreSegmentName), filters);
- c.resolve(e);
+ std::filesystem::path csPath = mPath / EscapeSegmentName(e.id().coreSegmentName);
+ if (filesystem::util::checkIfFolderInFilesystemExists(csPath))
+ {
+ CoreSegment c(mPath, id(), csPath.filename(), processors);
+ c.resolve(e);
+ }
+ else
+ {
+ ARMARX_WARNING << "Could not find the core segment folder for segment '" << e.id().str() << "'.";
+ }
});
}
void Memory::_directlyStore(const armem::wm::Memory& memory)
{
- if (!checkPath(id().memoryName))
+ std::lock_guard l(ltm_mutex); // we cannot store a memory multiple times simultaneously
+
+ if (id().memoryName.empty())
{
- return;
+ ARMARX_WARNING << "During storage of memory '" << memory.id().str() << "' I noticed that the corresponding LTM has no id set. " <<
+ "I set the id of the LTM to the same name, however this should not happen!";
+ id().memoryName = memory.id().memoryName;
}
- std::lock_guard l(ltm_mutex);
+ std::filesystem::path mPath = getPathToMemory(EscapeSegmentName(id().memoryName));
+ util::ensureFolderExists(mPath, true); // create if not exists
- memory.forEachCoreSegment([this](const auto& core)
+ memory.forEachCoreSegment([&](const auto& core)
{
- util::ensureFolderExists(std::filesystem::path(path) / id().memoryName / core.id().coreSegmentName);
- CoreSegment c((std::filesystem::path(path) / id().memoryName / core.id().coreSegmentName), filters);
+ std::filesystem::path relCsPath = (EscapeSegmentName(core.id().coreSegmentName) + "/");
+ minizip::util::ensureFolderInZipExists(mPath, relCsPath, true); // create subfolder
+
+ CoreSegment c(mPath, id(), core.id().coreSegmentName, processors);
c.store(core);
});
}
-
- void Memory::setPath(const std::string& ps)
- {
- std::filesystem::path p = ps;
- DiskMemoryItem::setPath(p.parent_path());
- setMemoryID(getMemoryIDFromPath(p));
- }
-
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h
index 44971a7ef..76a3c67de 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h
@@ -21,16 +21,14 @@ namespace armarx::armem::server::ltm::disk
using Base = BufferedMemoryBase<CoreSegment>;
Memory();
- Memory(const std::filesystem::path&);
+ Memory(const std::filesystem::path&, const std::string&);
void setMemoryID(const MemoryID& id) override;
void createPropertyDefinitions(PropertyDefinitionsPtr& properties, const std::string& prefix) override;
bool forEachCoreSegment(std::function<void(CoreSegment&)>&& func) const override;
- std::shared_ptr<CoreSegment> findCoreSegment(const std::string&) const override;
-
- void setPath(const std::string&) override;
+ std::shared_ptr<CoreSegment> findCoreSegment(const std::string& coreSegmentName) const override;
protected:
void _loadAllReferences(armem::wm::Memory&) override;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp
index 4d4fdb8f0..22c1e3542 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp
@@ -10,73 +10,62 @@
namespace armarx::armem::server::ltm::disk
{
- namespace
- {
- MemoryID getMemoryIDFromPath(const std::filesystem::path& p)
- {
- util::ensureFolderExists(p);
-
- MemoryID m;
- m.memoryName = p.parent_path().parent_path().filename();
- m.coreSegmentName = p.parent_path().filename();
- m.providerSegmentName = p.filename();
- return m;
- }
- }
-
- ProviderSegment::ProviderSegment(const std::filesystem::path& p, const std::shared_ptr<FilterCollection>& pipe) :
- ProviderSegmentBase(getMemoryIDFromPath(p), pipe),
- DiskMemoryItem(p.parent_path())
+ ProviderSegment::ProviderSegment(const std::filesystem::path& p, const MemoryID& parentID, const std::string& escapedSegmentName, const std::shared_ptr<Processors>& filters) :
+ ProviderSegmentBase(parentID.withProviderSegmentName(UnescapeSegmentName(escapedSegmentName)), filters),
+ DiskMemoryItem(p)
{
}
bool ProviderSegment::forEachEntity(std::function<void(Entity&)>&& func) const
{
- if (!checkPath(id().providerSegmentName))
+ if (!checkPathAndSegmentName(id().providerSegmentName))
{
return false;
}
- std::filesystem::path p = path;
- p = p / id().providerSegmentName;
- util::ensureFolderExists(p, false);
-
- for (const auto& subdir : std::filesystem::directory_iterator(p))
+ std::filesystem::path provPath = std::filesystem::path(parentPath) / EscapeSegmentName(id().providerSegmentName);
+ for (const auto& subdir : std::filesystem::directory_iterator(provPath))
{
std::filesystem::path subdirPath = subdir.path();
- Entity c(subdirPath, filters);
+ Entity c(provPath, id(), subdirPath.filename(), processors);
func(c);
}
return true;
}
- std::shared_ptr<Entity> ProviderSegment::findEntity(const std::string& n) const
+ std::shared_ptr<Entity> ProviderSegment::findEntity(const std::string& entityName) const
{
- if (!checkPath(id().providerSegmentName))
+ if (!checkPathAndSegmentName(id().providerSegmentName))
{
return {};
}
- std::filesystem::path p = path;
- p = p / id().providerSegmentName;
- util::ensureFolderExists(p, false);
+ std::filesystem::path provPath = std::filesystem::path(parentPath) / EscapeSegmentName(id().providerSegmentName);
+ std::filesystem::path ePath = provPath / EscapeSegmentName(entityName);
+ if (!filesystem::util::checkIfFolderExists(ePath))
+ {
+ return nullptr;
+ }
- std::filesystem::path subpath = p / n;
- util::ensureFolderExists(subpath, false);
- auto c = std::make_shared<Entity>(subpath, filters);
+ auto c = std::make_shared<Entity>(provPath, id(), ePath.filename(), processors);
return c;
}
std::string ProviderSegment::getExpectedFolderName() const
{
- return name();
+ return EscapeSegmentName(id().providerSegmentName);
}
void ProviderSegment::_loadAllReferences(armem::wm::ProviderSegment& e)
{
+ if (!checkPathAndSegmentName(id().providerSegmentName))
+ {
+ return;
+ }
+
e.id() = id();
- forEachEntity([&e](Entity& x) {
+ forEachEntity([&e](auto& x) {
armem::wm::Entity s;
x.loadAllReferences(s);
e.addEntity(s);
@@ -85,20 +74,50 @@ namespace armarx::armem::server::ltm::disk
void ProviderSegment::_resolve(armem::wm::ProviderSegment& p)
{
- p.forEachEntity([this](armem::wm::Entity& e)
+ if (!checkPathAndSegmentName(id().providerSegmentName))
{
- util::ensureFolderExists(std::filesystem::path(path) / id().providerSegmentName / e.id().entityName, false);
- Entity c((std::filesystem::path(path) / id().providerSegmentName / e.id().entityName), filters);
- c.resolve(e);
+ return;
+ }
+
+ std::filesystem::path provPath = std::filesystem::path(parentPath) / EscapeSegmentName(id().providerSegmentName);
+ p.forEachEntity([&](auto& e)
+ {
+ std::filesystem::path ePath = provPath / EscapeSegmentName(e.id().entityName);
+ if (filesystem::util::checkIfFolderExists(ePath))
+ {
+ Entity c(provPath, id(), ePath.filename(), processors);
+ c.resolve(e);
+ }
+ else
+ {
+ ARMARX_WARNING << "Could not find the entity segment folder for segment '" << e.id().str() << "'.";
+ }
});
}
- void ProviderSegment::_store(const armem::wm::ProviderSegment& providerSegment)
+ void ProviderSegment::_store(const armem::wm::ProviderSegment& p)
{
- providerSegment.forEachEntity([this](const auto& entity)
+ if (id().providerSegmentName.empty())
+ {
+ ARMARX_WARNING << "During storage of segment '" << p.id().str() << "' I noticed that the corresponding LTM has no id set. " <<
+ "I set the id of the LTM to the same name, however this should not happen!";
+ id().providerSegmentName = p.id().providerSegmentName;
+ }
+
+ std::filesystem::path provPath = std::filesystem::path(parentPath) / EscapeSegmentName(id().providerSegmentName);
+ if (!filesystem::util::checkIfFolderExists(provPath))
{
- util::ensureFolderExists(std::filesystem::path(path) / id().providerSegmentName / entity.id().entityName);
- Entity c((std::filesystem::path(path) / id().providerSegmentName / entity.id().entityName), filters);
+ ARMARX_WARNING << "The segment folder for segment '"+id().str()+"'was not created. I will create the folder by myself, however it seems like there is a bug in the ltm pipeline.";
+ filesystem::util::ensureFolderExists(provPath, true);
+ }
+
+
+ p.forEachEntity([&](const auto& entity)
+ {
+ std::filesystem::path ePath = provPath / EscapeSegmentName(entity.id().entityName);
+ filesystem::util::ensureFolderExists(ePath, true);
+
+ Entity c(provPath, id(), ePath.filename(), processors);
c.store(entity);
});
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h
index 1d754d868..3e173405c 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h
@@ -15,7 +15,7 @@ namespace armarx::armem::server::ltm::disk
public DiskMemoryItem
{
public:
- ProviderSegment(const std::filesystem::path&, const std::shared_ptr<FilterCollection>& p);
+ ProviderSegment(const std::filesystem::path& parentPath, const MemoryID& parentID, const std::string& escapedSegmentName, const std::shared_ptr<Processors>& p);
bool forEachEntity(std::function<void(Entity&)>&& func) const override;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.h b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.h
index b5b2fbf41..13ac180c3 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.h
@@ -1,6 +1,8 @@
#pragma once
#include <filesystem>
+#include <minizip/zip.h>
+#include <minizip/unzip.h>
#include "../../../../core/error.h"
@@ -13,6 +15,188 @@ namespace armarx::armem::server::ltm::disk
const std::string METADATA_FILENAME = "metadata.aron";
}
+ namespace filesystem::util
+ {
+ inline bool checkIfFolderInFilesystemExists(const std::filesystem::path& p)
+ {
+ return std::filesystem::exists(p) and std::filesystem::is_directory(p);
+ }
+
+ inline void ensureFolderInFilesystemExists(const std::filesystem::path& p, bool createIfNotExistent = true)
+ {
+ if (!std::filesystem::exists(p))
+ {
+ if (createIfNotExistent)
+ {
+ std::filesystem::create_directories(p);
+ }
+ }
+ if (!std::filesystem::is_directory(p))
+ {
+ throw error::ArMemError("Could not find folder: " + p.string());
+ }
+ }
+
+ inline bool checkIfFileInFilesystemExists(const std::filesystem::path& p)
+ {
+ return std::filesystem::exists(p) and std::filesystem::is_regular_file(p);
+ }
+
+ inline void ensureFileInFilesystemExists(const std::filesystem::path& p)
+ {
+ if (!std::filesystem::exists(p) || !std::filesystem::is_regular_file(p))
+ {
+ // not found
+ throw error::ArMemError("Could not find file: " + p.string());
+ }
+ }
+ }
+
+ namespace minizip::util
+ {
+ const std::string MINIZIP_SUFFIX = ".zip";
+ const int MINIZIP_CASE_SENSITIVITY = 1;
+
+ inline bool checkZipFile(const std::filesystem::path& pathToZip)
+ {
+ bool ret = false;
+ zipFile z = NULL;
+ if (std::filesystem::exists(pathToZip))
+ {
+ if (std::filesystem::is_regular_file(pathToZip) && pathToZip.extension() == MINIZIP_SUFFIX)
+ {
+ z = zipOpen(pathToZip.string().c_str(), APPEND_STATUS_ADDINZIP);
+ ret = (bool) z;
+ zipClose(z, NULL);
+ }
+ }
+ return ret;
+ }
+
+ inline zipFile ensureZipFile(const std::filesystem::path& pathToZip, bool createIfNotExistent = true)
+ {
+ int mode = APPEND_STATUS_CREATE;
+ if (!checkZipFile(pathToZip))
+ {
+ if (createIfNotExistent)
+ {
+ mode = APPEND_STATUS_CREATE;
+ }
+ else
+ {
+ throw error::ArMemError("Could not find zip file: " + pathToZip.string());
+ }
+ }
+
+ zipFile z = zipOpen(pathToZip.string().c_str(), mode);
+ if (!z)
+ {
+ throw error::ArMemError("Unknown error occured during opening zip file: " + pathToZip.string());
+ }
+ return z;
+ }
+
+ inline unzFile getUnzFile(const std::filesystem::path& pathToZip)
+ {
+ unzFile z = NULL;
+ if (std::filesystem::exists(pathToZip))
+ {
+ if (std::filesystem::is_regular_file(pathToZip) && pathToZip.extension() == MINIZIP_SUFFIX)
+ {
+ z = unzOpen(pathToZip.string().c_str());
+ }
+ else
+ {
+ throw error::ArMemError("Could not replace existing file: " + pathToZip.string());
+ }
+ }
+
+ if (!z)
+ {
+ throw error::ArMemError("Unknown error occured during opening unz file: " + pathToZip.string());
+ }
+ return z;
+ }
+
+ inline bool checkIfElementInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p)
+ {
+ auto zf = ensureZipFile(pathToZip);
+ auto uzf = getUnzFile(pathToZip);
+
+ bool ret = (unzLocateFile(uzf, p.string().c_str(), MINIZIP_CASE_SENSITIVITY) == UNZ_OK);
+
+ unzClose(uzf);
+ zipClose(zf, NULL);
+
+ return ret;
+ }
+
+ inline void ensureElementInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p, bool createIfNotExistent)
+ {
+ auto zf = ensureZipFile(pathToZip);
+ auto uzf = getUnzFile(pathToZip);
+
+ if (auto r = unzLocateFile(uzf, p.string().c_str(), MINIZIP_CASE_SENSITIVITY); r != UNZ_OK)
+ {
+ if (createIfNotExistent)
+ {
+ zip_fileinfo zfi;
+ zipOpenNewFileInZip(zf, p.string().c_str(), &zfi, NULL, 0, NULL, 0, NULL, Z_DEFLATED, Z_DEFAULT_COMPRESSION);
+ zipCloseFileInZip(zf);
+ }
+ else
+ {
+ throw error::ArMemError("Could not find element '" + p.string() + "' in zip file: " + pathToZip.string());
+ }
+ }
+ // else folder exists
+
+ unzClose(uzf);
+ zipClose(zf, NULL);
+ }
+
+ inline void ensureFolderInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p, bool createIfNotExistent = true)
+ {
+ if (!p.filename().empty())
+ {
+ throw error::ArMemError("The specified path is not a folder (it needs tailing /): " + p.string());
+ }
+
+ ensureElementInZipExists(pathToZip, p, createIfNotExistent);
+ }
+
+ inline void ensureFileInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p)
+ {
+ if (p.filename().empty())
+ {
+ throw error::ArMemError("The specified path is not a file (it needs a filename): " + p.string());
+ }
+
+ ensureElementInZipExists(pathToZip, p, true);
+ }
+
+ inline bool checkIfFolderInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p)
+ {
+ if (!p.filename().empty())
+ {
+ throw error::ArMemError("The specified path is not a folder (it needs tailing /): " + p.string());
+ }
+
+ return checkIfFolderInZipExists(pathToZip, p);
+ }
+
+ inline bool checkIfFileInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p)
+ {
+ if (p.filename().empty())
+ {
+ throw error::ArMemError("The specified path is not a file (it needs a filename): " + p.string());
+ }
+
+ return checkIfFolderInZipExists(pathToZip, p);
+ }
+ }
+
+
namespace util
{
// check whether a string is a number (e.g. timestamp folders)
@@ -28,38 +212,54 @@ namespace armarx::armem::server::ltm::disk
return true;
}
- inline bool checkIfFolderExists(const std::filesystem::path& p)
+ inline bool checkIfFolderExists(const std::filesystem::path& mPath, const std::filesystem::path& p)
{
- return std::filesystem::exists(p) and std::filesystem::is_directory(p);
+ if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+ {
+ return minizip::util::checkIfFolderInZipExists(mPath, p);
+ }
+
+ return filesystem::util::checkIfFolderInFilesystemExists(mPath / p);
}
- inline void ensureFolderExists(const std::filesystem::path& p, bool createIfNotExistent = true)
+ inline void ensureBasePathExists(const std::filesystem::path& mPath, bool createIfNotExistent = true)
{
- if (!std::filesystem::exists(p))
+ if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
{
- if (createIfNotExistent)
- {
- std::filesystem::create_directories(p);
- }
+ minizip::util::ensureZipFile(mPath, createIfNotExistent);
+ return;
}
- if (!std::filesystem::is_directory(p))
+ return filesystem::util::ensureFolderInFilesystemExists(mPath, createIfNotExistent);
+ }
+
+ inline void ensureFolderExists(const std::filesystem::path& mPath, const std::filesystem::path& p, bool createIfNotExistent = true)
+ {
+ if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
{
- throw error::ArMemError("Could not find folder: " + p.string());
+ return minizip::util::ensureFolderInZipExists(mPath, p, createIfNotExistent);
}
+
+ return filesystem::util::ensureFolderInFilesystemExists(mPath / p, createIfNotExistent);
}
- inline bool checkIfFileExists(const std::filesystem::path& p)
+ inline bool checkIfFileExists(const std::filesystem::path& mPath, const std::filesystem::path& p)
{
- return std::filesystem::exists(p) and std::filesystem::is_regular_file(p);
+ if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+ {
+ return minizip::util::checkIfFileInZipExists(mPath, p);
+ }
+
+ return filesystem::util::checkIfFileInFilesystemExists(mPath / p);
}
- inline void ensureFileExists(const std::filesystem::path& p)
+ inline void ensureFileExists(const std::filesystem::path& mPath, const std::filesystem::path& p)
{
- if (!std::filesystem::exists(p) || !std::filesystem::is_regular_file(p))
+ if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
{
- // not found
- throw error::ArMemError("Could not find file: " + p.string());
+ return minizip::util::ensureFileInZipExists(mPath, p);
}
+
+ return filesystem::util::ensureFileInFilesystemExists(mPath / p);
}
}
} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.cpp
index b30b7cde3..cfb714f26 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.cpp
@@ -1,46 +1,83 @@
// Header
#include "DiskStorage.h"
+// Simox
+#include <SimoxUtility/algorithm/string.h>
+
// ArmarX
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/LocalException.h>
namespace armarx::armem::server::ltm::disk
{
- DiskMemoryItem::DiskMemoryItem(const std::filesystem::path& p) :
- path(p.string())
+ //const std::string DiskMemoryItem::Prefix = "_";
+ //const std::string DiskMemoryItem::PrefixEscaped = "__";
+ const std::map<std::string, std::string> DiskMemoryItem::EscapeTable = {
+ {"/", "|"}
+ };
+
+ DiskMemoryItem::DiskMemoryItem(const std::filesystem::path& p, const MemoryEncodingMode mode) :
+ relativeParentPath(p),
+ mode(mode)
{
}
- bool DiskMemoryItem::checkPath(const std::string& suffix) const
+ bool DiskMemoryItem::checkPathAndSegmentFolderName(const std::string& segmentName) const
{
- std::filesystem::path memoryP(path);
- memoryP = memoryP / suffix;
+ const std::string expectedFolderName = getExpectedFolderName();
+ const std::string error_id = "LTM_PathError_" + expectedFolderName;
+ const std::string escaped = EscapeSegmentName(segmentName);
+ std::filesystem::path relativePath = relativeParentPath / escaped;
- // Check connection:
- if (!std::filesystem::exists(path))
+ if (escaped != expectedFolderName)
{
- std::filesystem::create_directories(path);
- return true;
- }
- else if (std::string expectedFolderName = getExpectedFolderName(); !std::filesystem::is_directory(path) || memoryP.filename() != expectedFolderName)
- {
- ARMARX_WARNING << deactivateSpam("LTM_PathError_" + expectedFolderName)
- << "The entered path is not valid. Please use a path leading to a memory folder with name: " << expectedFolderName << ". The used path was: " << getPath()
+ ARMARX_WARNING << deactivateSpam()
+ << "The entered path is not valid. Please use a path leading to a memory folder with name: " << expectedFolderName << "."
<< "\n\n";
return false;
}
- return true;
+ switch (mode)
+ {
+ case MemoryEncodingMode::FILESYSTEM:
+ {
+ std::filesystem::path fullPath = absoluteMemoryPath / relativePath;
+ if (!filesystem::util::checkIfFolderExists(fullPath))
+ {
+ return false;
+ }
+ return true;
+ }
+ case MemoryEncodingMode::MINIZIP:
+ {
+ if (!minizip::util::checkIfFolderInZipExists(absoluteMemoryPath, relativePath))
+ {
+ return false;
+ }
+ return true;
+ }
+ }
}
- std::string DiskMemoryItem::getPath() const
+ std::string DiskMemoryItem::EscapeSegmentName(const std::string& segmentName)
{
- return path;
+ std::string ret = segmentName;
+ //simox::alg::replace_all(ret, Prefix, PrefixEscaped);
+ for (const auto& [s, r] : EscapeTable)
+ {
+ ret = simox::alg::replace_all(ret, s, r);
+ }
+ return ret;
}
- void DiskMemoryItem::setPath(const std::string& ps)
+ std::string DiskMemoryItem::UnescapeSegmentName(const std::string& escapedName)
{
- path = ps;
+ std::string ret = escapedName;
+ for (const auto& [s, r] : EscapeTable) // Here we assume that noone uses the replaced char usually in the segment name... TODO
+ {
+ ret = simox::alg::replace_all(ret, r, s);
+ }
+ return ret;
}
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.h b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.h
index 01142b930..1df90e20c 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.h
@@ -1,6 +1,11 @@
#pragma once
#include <filesystem>
+#include <map>
+#include <string>
+
+// minizip
+#include <minizip/zip.h>
#include "Data.h"
@@ -9,18 +14,45 @@ namespace armarx::armem::server::ltm::disk
class DiskMemoryItem
{
public:
+ enum class MemoryEncodingMode
+ {
+ FILESYSTEM = 0,
+ MINIZIP = 1
+ };
+
DiskMemoryItem() = default;
- DiskMemoryItem(const std::filesystem::path&);
+ DiskMemoryItem(const std::filesystem::path& parentPath, const MemoryEncodingMode mode = MemoryEncodingMode::FILESYSTEM);
virtual ~DiskMemoryItem() = default;
- virtual void setPath(const std::string&);
- std::string getPath() const;
-
protected:
- bool checkPath(const std::string&) const;
+ bool checkPathAndSegmentFolderName(const std::string& segmentName) const;
virtual std::string getExpectedFolderName() const = 0;
+ static std::string EscapeSegmentName(const std::string& segmentName);
+ static std::string UnescapeSegmentName(const std::string& escapedName);
+
+ std::filesystem::path getPathToMemory(const std::string& escapedMemoryName)
+ {
+ switch (mode)
+ {
+ case MemoryEncodingMode::FILESYSTEM:
+ {
+ return memoryParentPath / escapedMemoryName / "/";
+ }
+ case MemoryEncodingMode::MINIZIP:
+ {
+ return memoryParentPath / (escapedMemoryName + minizip::util::MINIZIP_SUFFIX);
+ }
+ }
+ }
+
protected:
- std::string path;
+ //static const std::string Prefix;
+ //static const std::string PrefixEscaped;
+ static const std::map<std::string, std::string> EscapeTable;
+
+ std::filesystem::path memoryParentPath;
+ std::filesystem::path relativeParentPath;
+ MemoryEncodingMode mode;
};
} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
index 28d09f3e1..4356aed6f 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
@@ -47,9 +47,6 @@ namespace armarx::armem::server::plugins
{
Component& parent = this->parent<Component>();
- // init ltm stuff (such as propterty definition dependent filters)
- longtermMemory.init();
-
// register to MNS
if (clientPlugin->isMemoryNameSystemEnabled() and clientPlugin->getMemoryNameSystemClient())
{
diff --git a/source/RobotAPI/libraries/armem/server/test/ArMemLTMBenchmark.cpp b/source/RobotAPI/libraries/armem/server/test/ArMemLTMBenchmark.cpp
index b4965373d..8aded460e 100644
--- a/source/RobotAPI/libraries/armem/server/test/ArMemLTMBenchmark.cpp
+++ b/source/RobotAPI/libraries/armem/server/test/ArMemLTMBenchmark.cpp
@@ -68,9 +68,7 @@ namespace ArMemLTMBenchmark
void storeElementNTimes(const std::string& memoryName, const aron::data::DictPtr& dict, int waitingTimeMs, int n)
{
- armem::server::ltm::disk::Memory ltm;
- ltm.setPath(storagePath / memoryName);
- ltm.setMemoryID(ltm.id().withMemoryName(memoryName));
+ armem::server::ltm::disk::Memory ltm(storagePath, memoryName);
armem::wm::Memory wm(memoryName);
auto& core = wm.addCoreSegment("CoreS");
diff --git a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
index 215ee34da..ff966676a 100644
--- a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
@@ -100,10 +100,7 @@ namespace armarx::armem::gui::disk
}
else
{
- std::filesystem::create_directories(path / name);
-
- armem::server::ltm::disk::Memory memory((path / name));
- memory.init();
+ armem::server::ltm::disk::Memory memory(path, name);
memory.directlyStore(data);
numStored++;
@@ -174,9 +171,7 @@ namespace armarx::armem::gui::disk
auto loadMemory = [&](const std::filesystem::path& p){
if (std::filesystem::is_directory(p))
{
- armem::server::ltm::disk::Memory ltm(p);
- ltm.init();
-
+ armem::server::ltm::disk::Memory ltm(p.parent_path(), p.filename());
armem::wm::Memory memory = ltm.loadAllAndResolve(); // load list of references
memoryData[memory.name()] = std::move(memory);
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
index 49dbdcf61..861ff14b9 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
@@ -55,6 +55,7 @@ namespace armarx::armem::obj::instance
}
}
+ /// get the latest object from an memory and cast it to an ObjectInstance
std::optional<armarx::armem::arondto::ObjectInstance> Reader::queryObject(const armem::wm::Memory& memory, const armem::Time& timestamp)
{
// clang-format off
@@ -73,6 +74,7 @@ namespace armarx::armem::obj::instance
return armem::arondto::ObjectInstance::FromAron(instance->data());
}
+ /// Query an object with full name entityName (e.g. Kitchen/green-cup/0)
std::optional<armarx::armem::arondto::ObjectInstance> Reader::queryObjectByEntityID(const std::string& entityName, const armem::Time& timestamp)
{
// Query all entities from all provider.
@@ -98,6 +100,7 @@ namespace armarx::armem::obj::instance
return queryObject(qResult.memory, timestamp);
}
+ /// Query an object by objectId (e.g. Kitchen/green-cup in Entity Kitchen/green-cup/0)
std::optional<armarx::armem::arondto::ObjectInstance> Reader::queryObjectByObjectID(const std::string& objectId, const armem::Time& timestamp)
{
// Query all entities from all provider.
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
index 8db6a1cab..45a342307 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
@@ -49,9 +49,77 @@ namespace armarx::armem::obj::instance
std::optional<armem::arondto::ObjectInstance> queryObjectByEntityID(const std::string& entityName, const armem::Time&);
std::optional<armem::arondto::ObjectInstance> queryObjectByObjectID(const std::string& objectId, const armem::Time&);
+ static std::string GetObjectId(const std::string& s)
+ {
+ auto split = simox::alg::split(s, "/");
+ if (simox::alg::starts_with(s, "/"))
+ {
+ split.insert(split.begin(), ""); // sanitize
+ }
- private:
+ for (auto& e : split)
+ {
+ e = simox::alg::replace_all(e, "/", "");
+ }
+
+ if (IsEntityId(s)) return (split[0] + "/" + split[1]);
+ if (IsObjectId(s)) return s;
+
+ ARMARX_ERROR << "Unknown structure for object id '" << s << "'.";
+ return "";
+ }
+
+ static std::string GetObjectClassName(const std::string& s)
+ {
+ auto split = simox::alg::split(s, "/");
+ if (simox::alg::starts_with(s, "/"))
+ {
+ split.insert(split.begin(), ""); // sanitize
+ }
+
+ for (auto& e : split)
+ {
+ e = simox::alg::replace_all(e, "/", "");
+ }
+ if (IsEntityId(s)) return split[1];
+ if (IsObjectId(s)) return split[1];
+
+ ARMARX_ERROR << "Unknown structure for object id '" << s << "'.";
+ return "";
+ }
+
+ static bool IsEntityId(const std::string& s)
+ {
+ auto split = simox::alg::split(s, "/");
+ if (simox::alg::starts_with(s, "/"))
+ {
+ split.insert(split.begin(), ""); // sanitize
+ }
+
+ if (split.size() != 3)
+ {
+ return false;
+ }
+ return true;
+ }
+
+ static bool IsObjectId(const std::string& s)
+ {
+ auto split = simox::alg::split(s, "/");
+ if (simox::alg::starts_with(s, "/"))
+ {
+ split.insert(split.begin(), ""); // sanitize
+ }
+
+ if (split.size() != 2)
+ {
+ return false;
+ }
+ return true;
+ }
+
+ private:
struct Properties
{
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h
index 31677060a..593298a7d 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h
@@ -97,3 +97,12 @@ namespace armarx::aron::data
virtual bool fullfillsType(const type::VariantPtr&) const override;
};
}
+
+namespace armarx::aron
+{
+ template<typename... _Args>
+ aron::data::NDArrayPtr make_ndarray(_Args&&... args)
+ {
+ return std::make_shared<aron::data::NDArray>(args...);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
index f022d0264..6087ddfac 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
@@ -26,6 +26,7 @@
// STD/STL
#include <memory>
#include <map>
+#include <utility>
// Base class
#include "../detail/ContainerVariant.h"
@@ -98,3 +99,12 @@ namespace armarx::aron::data
std::map<std::string, VariantPtr> childrenNavigators;
};
}
+
+namespace armarx::aron
+{
+ template<typename... _Args>
+ aron::data::DictPtr make_dict(_Args&&... args)
+ {
+ return std::make_shared<aron::data::Dict>(args...);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/List.h b/source/RobotAPI/libraries/aron/core/data/variant/container/List.h
index b766ce5aa..4ab656bfa 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/List.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/List.h
@@ -97,3 +97,12 @@ namespace armarx::aron::data
std::vector<VariantPtr> childrenNavigators;
};
}
+
+namespace armarx::aron
+{
+ template<typename... _Args>
+ aron::data::ListPtr make_list(_Args&&... args)
+ {
+ return std::make_shared<aron::data::List>(args...);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h
index 65cfc31a7..5e07b8a8e 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h
@@ -68,3 +68,12 @@ namespace armarx::aron::data
bool fullfillsType(const type::VariantPtr&) const override;
};
}
+
+namespace armarx::aron
+{
+ template<typename... _Args>
+ aron::data::BoolPtr make_bool(_Args&&... args)
+ {
+ return std::make_shared<aron::data::Bool>(args...);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h
index b02571f1d..fb7a6880a 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h
@@ -68,3 +68,12 @@ namespace armarx::aron::data
bool fullfillsType(const type::VariantPtr&) const override;
};
}
+
+namespace armarx::aron
+{
+ template<typename... _Args>
+ aron::data::DoublePtr make_double(_Args&&... args)
+ {
+ return std::make_shared<aron::data::Double>(args...);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h
index d7b44e567..2d0cecb7e 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h
@@ -68,3 +68,12 @@ namespace armarx::aron::data
bool fullfillsType(const type::VariantPtr&) const override;
};
}
+
+namespace armarx::aron
+{
+ template<typename... _Args>
+ aron::data::FloatPtr make_float(_Args&&... args)
+ {
+ return std::make_shared<aron::data::Float>(args...);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h
index 9a803a035..5ee4a270f 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h
@@ -69,3 +69,12 @@ namespace armarx::aron::data
bool fullfillsType(const type::VariantPtr&) const override;
};
}
+
+namespace armarx::aron
+{
+ template<typename... _Args>
+ aron::data::IntPtr make_int(_Args&&... args)
+ {
+ return std::make_shared<aron::data::Int>(args...);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h
index 82b9481be..0cbcbdb66 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h
@@ -69,3 +69,12 @@ namespace armarx::aron::data
bool fullfillsType(const type::VariantPtr&) const override;
};
}
+
+namespace armarx::aron
+{
+ template<typename... _Args>
+ aron::data::LongPtr make_long(_Args&&... args)
+ {
+ return std::make_shared<aron::data::Long>(args...);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h
index 33e9ef6ea..21c23698c 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h
@@ -68,3 +68,12 @@ namespace armarx::aron::data
bool fullfillsType(const type::VariantPtr&) const override;
};
}
+
+namespace armarx::aron
+{
+ template<typename... _Args>
+ aron::data::StringPtr make_string(_Args&&... args)
+ {
+ return std::make_shared<aron::data::String>(args...);
+ }
+}
diff --git a/source/RobotAPI/libraries/skills/CMakeLists.txt b/source/RobotAPI/libraries/skills/CMakeLists.txt
index 416121d84..260f78956 100644
--- a/source/RobotAPI/libraries/skills/CMakeLists.txt
+++ b/source/RobotAPI/libraries/skills/CMakeLists.txt
@@ -18,6 +18,7 @@ armarx_add_library(
./provider/SpecializedSkill.cpp
./provider/SkillDescription.cpp
./provider/SkillStatusUpdate.cpp
+ ./provider/SkillParameterization.cpp
./provider/helper/LambdaSkillImplementation.cpp
./provider/detail/SkillImplementationWrapper.cpp
HEADERS
@@ -27,6 +28,7 @@ armarx_add_library(
./provider/SpecializedSkill.h
./provider/SkillDescription.h
./provider/SkillStatusUpdate.h
+ ./provider/SkillParameterization.h
./provider/helper/LambdaSkillImplementation.h
./provider/detail/SkillImplementationWrapper.h
)
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
index 514f11ad7..268c2cf4d 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
@@ -57,6 +57,8 @@ namespace armarx
exInfo.skillName = info.skillName;
exInfo.callbackInterface = myPrx;
exInfo.params = info.params;
+ exInfo.waitUntilSkillFinished = info.waitUntilSkillFinished;
+
it->second->executeSkill(exInfo);
}
else
@@ -65,6 +67,14 @@ namespace armarx
}
}
+ void SkillManagerComponentPluginUser::abortSkill(const std::string& providerName, const std::string& skillName, const Ice::Current ¤t)
+ {
+ if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
+ {
+ it->second->abortSkill(skillName, false);
+ }
+ }
+
void SkillManagerComponentPluginUser::updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& statusUpdate, const Ice::Current&)
{
(void) statusUpdate;
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
index 6fd40f3eb..5c59564f6 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
@@ -36,7 +36,7 @@ namespace armarx
skills::provider::dti::SkillProviderMap getSkillProviders(const Ice::Current ¤t) override;
void executeSkill(const skills::manager::dto::SkillExecutionInfo& info, const Ice::Current ¤t) override;
void updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& update, const Ice::Current ¤t) override;
-
+ void abortSkill(const std::string& providerName, const std::string& skillName, const Ice::Current ¤t) override;
private:
armarx::plugins::SkillManagerComponentPlugin* plugin = nullptr;
diff --git a/source/RobotAPI/libraries/skills/provider/Skill.cpp b/source/RobotAPI/libraries/skills/provider/Skill.cpp
index 7a26a5bf9..aaeaabbe3 100644
--- a/source/RobotAPI/libraries/skills/provider/Skill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/Skill.cpp
@@ -17,7 +17,7 @@ namespace armarx
return std::thread{ [&](){
if (description.timeoutMs <= 0) return;
long skillStartedAt = IceUtil::Time::now().toMilliSeconds();
- while(running)
+ while(running and !stopRequested())
{
auto now = IceUtil::Time::now().toMilliSeconds();
if ((now - skillStartedAt) >= description.timeoutMs) notifyTimeoutReached();
@@ -30,52 +30,67 @@ namespace armarx
void Skill::notifyTimeoutReached()
{
timeoutReached = true;
- _notifyTimeoutReached();
}
void Skill::notifyStopped()
{
stopped = true;
- _notifyStopped();
}
void Skill::reset()
{
+ started = IceUtil::Time::milliSeconds(-1);
+ exited = IceUtil::Time::milliSeconds(-1);
+
+ running = false;
stopped = false;
timeoutReached = false;
-
- //provider = nullptr;
- //manager = nullptr;
-
- _reset();
}
- void Skill::_notifyTimeoutReached() { }
- void Skill::_notifyStopped() { }
- void Skill::_reset() { }
-
- Skill::Status Skill::execute(const aron::data::DictPtr& params, const CallbackT& callback)
+ void Skill::init(const aron::data::DictPtr& params)
{
+ (void) params;
+
+ ARMARX_IMPORTANT << "Initializing Skill '" << description.skillName << "'";
+
+ // always called before execute (should not take longer than 100ms)
running = true;
- started = IceUtil::Time::now().toMilliSeconds();
+ started = IceUtil::Time::now();
+ timeoutCheck = installTimeoutCondition();
+ }
- auto timeoutCheck = installTimeoutCondition();
+ void Skill::exit(const aron::data::DictPtr& params)
+ {
+ (void) params;
- auto ret = _execute(params, callback);
+ ARMARX_IMPORTANT << "Exiting Skill '" << description.skillName << "'";
- started = 0;
+ // always called after execute (should not take longer than 100ms)
running = false;
- timeoutCheck.join(); // safely wait for the timeoutcheck to return
- return ret;
+ timeoutCheck.join();
+ exited = IceUtil::Time::now();
}
- Skill::Status Skill::_execute(const aron::data::DictPtr& params, const CallbackT& callback)
+ Skill::Status Skill::execute(const aron::data::DictPtr& params, const CallbackT& callback)
{
(void) params;
- ARMARX_WARNING_S << "You have to override this method!";
+ ARMARX_IMPORTANT << "Executing Skill '" << description.skillName << "'";
return Status::Succeeded;
}
+ Skill::Status Skill::initExecuteExit(const aron::data::DictPtr& params, const CallbackT& callback)
+ {
+ this->reset();
+ this->init(params);
+ auto ret = this->execute(params);
+ this->exit(params);
+ return ret;
+ }
+
+ bool Skill::stopRequested() const
+ {
+ return (stopped || timeoutReached);
+ }
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/Skill.h b/source/RobotAPI/libraries/skills/provider/Skill.h
index 324ccb24e..d4fe0e5cf 100644
--- a/source/RobotAPI/libraries/skills/provider/Skill.h
+++ b/source/RobotAPI/libraries/skills/provider/Skill.h
@@ -36,25 +36,26 @@ namespace armarx
Skill(const SkillDescription&);
virtual ~Skill() = default;
- /// Execute Skill
- Status execute(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; });
+ /// Override this method with the actual implementation.
+ virtual void init(const aron::data::DictPtr& params);
- /// Reset all parameters before starting a skill.
- void reset();
-
- /// Set the notification bools
- void notifyTimeoutReached();
- void notifyStopped();
+ /// Override this method with the actual implementation.
+ virtual void exit(const aron::data::DictPtr& params);
/// Override this method with the actual implementation. The callback is for status updates to the calling instance
- virtual Status _execute(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; });
+ virtual Status execute(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; });
+
+ /// Do all methods at once.
+ Status initExecuteExit(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; });
- /// Override if you have special members that needs to be resetted
- virtual void _reset();
+ /// Override if you have special members that needs to be resetted. It is called before the skill ititializes
+ virtual void reset();
/// Override these methods if you want to do something special when notification comes
- virtual void _notifyTimeoutReached();
- virtual void _notifyStopped();
+ virtual void notifyTimeoutReached();
+ virtual void notifyStopped();
+
+ virtual bool stopRequested() const;
private:
/// install a condition as a seperate thread
@@ -64,17 +65,21 @@ namespace armarx
const SkillDescription description;
/// running params
- std::atomic_bool running = false;
- long started = 0;
+ IceUtil::Time started = IceUtil::Time::milliSeconds(-1);
+ IceUtil::Time exited = IceUtil::Time::milliSeconds(-1);
- /// proxies that called the skills
- //provider::dti::SkillProviderInterfacePrx provider = nullptr;
- //manager::dti::SkillManagerInterfacePrx manager = nullptr;
+ /// proxies that called the skills. Will be set from provider and is const afterwards
+ provider::dti::SkillProviderInterfacePrx ownerProvider = nullptr;
+ manager::dti::SkillManagerInterfacePrx ownerManager = nullptr;
protected:
/// Use conditions during runtime this way
+ std::atomic_bool running = true;
std::atomic_bool stopped = false;
std::atomic_bool timeoutReached = false;
+
+ private:
+ std::thread timeoutCheck;
};
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp b/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp
index 222c860e4..2dc791fe3 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp
@@ -4,14 +4,6 @@ namespace armarx
{
namespace skills
{
- provider::dto::SkillParameterization SkillParameterization::toIce() const
- {
- provider::dto::SkillParameterization ret;
- if (params)
- {
- ret.params = params->toAronDictPtr();
- }
- return ret;
- }
+
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillParameterization.h b/source/RobotAPI/libraries/skills/provider/SkillParameterization.h
index b42c0e927..a6ce6ed4a 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillParameterization.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillParameterization.h
@@ -12,9 +12,8 @@ namespace armarx
{
struct SkillParameterization
{
- aron::data::DictPtr params;
-
- provider::dto::SkillParameterization toIce() const;
+ aron::data::DictPtr usedInputParams = nullptr;
+ callback::dti::SkillProviderCallbackInterfacePrx usedCallbackInterface = nullptr;
};
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
index c471f140c..638de458a 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
@@ -22,11 +22,24 @@ namespace armarx::plugins
auto& p = parent<SkillProviderComponentPluginUser>();
std::string providerName = p.getName();
+ // update skill ownership
+ for (auto& [skillName, impl] : p.skillImplementations)
+ {
+ impl.skill->ownerProvider = myPrx;
+ impl.skill->ownerManager = ownedBySkillManager;
+
+ impl.statusUpdate.providerName = p.getName();
+ impl.statusUpdate.skillName = skillName;
+ }
+
+ // register to manager
skills::manager::dto::ProviderInfo i;
i.provider = myPrx;
i.providerName = providerName;
i.providedSkills = p.getSkills();
- skillManager->addProvider(i);
+ ownedBySkillManager->addProvider(i);
+
+ p.connected = true;
}
void SkillProviderComponentPlugin::preOnDisconnectComponent()
@@ -34,13 +47,13 @@ namespace armarx::plugins
auto& p = parent<SkillProviderComponentPluginUser>();
std::string providerName = p.getName();
- skillManager->removeProvider(providerName);
+ ownedBySkillManager->removeProvider(providerName);
}
void SkillProviderComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
{
std::string prefix = "skill.";
- properties->component(skillManager, "SkillMemory", prefix + "SkillManager", "The name of the SkillManager (or SkillMemory) proxy.");
+ properties->component(ownedBySkillManager, "SkillMemory", prefix + "SkillManager", "The name of the SkillManager (or SkillMemory) proxy this provider belongs to.");
}
}
@@ -52,14 +65,20 @@ namespace armarx
addPlugin(plugin);
}
- void SkillProviderComponentPluginUser::addSkill(const std::shared_ptr<skills::Skill>& skill)
+ void SkillProviderComponentPluginUser::addSkill(std::unique_ptr<skills::Skill>&& skill)
{
if (!skill)
{
return;
}
- std::lock_guard l(skillsMutex);
+ if (connected)
+ {
+ ARMARX_WARNING << "The SkillProvider already registered to a manager. The skill '" + skill->description.skillName + "' therefore cannot be added anymore.";
+ return;
+ }
+
+ std::unique_lock l(skillsMutex);
std::string skillName = skill->description.skillName;
if (skillImplementations.find(skillName) != skillImplementations.end())
@@ -69,19 +88,18 @@ namespace armarx
}
ARMARX_INFO << "Adding skill " << skillName;
- skills::detail::SkillImplementationWrapper s(skill);
- skillImplementations.insert({skillName, s});
+ skillImplementations.emplace(skillName, std::move(skill));
}
void SkillProviderComponentPluginUser::addSkill(const skills::helper::LambdaSkill::FunT& f, const skills::SkillDescription& desc)
{
- auto lambda = std::make_shared<skills::helper::LambdaSkill>(f, desc);
- addSkill(lambda);
+ auto lambda = std::make_unique<skills::helper::LambdaSkill>(f, desc);
+ addSkill(std::move(lambda));
}
skills::provider::dto::SkillDescriptionMap SkillProviderComponentPluginUser::getSkills(const Ice::Current ¤t)
{
- std::lock_guard l(skillsMutex);
+ std::shared_lock l(skillsMutex);
skills::provider::dto::SkillDescriptionMap skillDesciptions;
for (const auto& [key, skillWrapper] : skillImplementations)
{
@@ -92,51 +110,54 @@ namespace armarx
skills::provider::dto::SkillStatusUpdate SkillProviderComponentPluginUser::getSkillExecutionStatus(const std::string& skill, const Ice::Current ¤t)
{
- std::lock_guard l(skillsMutex);
+ std::shared_lock l(skillsMutex);
auto& skillWrapper = skillImplementations.at(skill);
- std::lock_guard l2(skillWrapper.skillStatusMutex);
+ std::shared_lock l2(skillWrapper.skillStatusMutex);
return skillWrapper.statusUpdate.toIce();
}
+ // Please not that this method waits until the skill can be scheduled!
void SkillProviderComponentPluginUser::executeSkill(const skills::provider::dto::SkillExecutionInfo& info, const Ice::Current ¤t)
{
- std::lock_guard l(skillsMutex);
+ std::shared_lock l(skillsMutex);
std::string skillName = info.skillName;
ARMARX_CHECK_EXPRESSION(skillImplementations.count(skillName) > 0);
auto& wrapper = skillImplementations.at(skillName);
+
+ skills::SkillParameterization usedParameterization;
+ usedParameterization.usedCallbackInterface = info.callbackInterface;
+ usedParameterization.usedInputParams = aron::data::Dict::FromAronDictDTO(info.params);
+
+ // We have to wait until the last task is finished
if (wrapper.task.joinable())
{
wrapper.task.join();
}
- // update input params
- wrapper.reset();
- wrapper.statusUpdate.usedCallbackInterface = info.callbackInterface;
- wrapper.statusUpdate.skillName = info.skillName;
- wrapper.statusUpdate.providerName = getName();
- wrapper.statusUpdate.usedParameterization = aron::data::Dict::FromAronDictDTO(info.params);
+ // recreate thread and execute skill. A skill can only be executed once
+ wrapper.task = std::thread{ [&] { wrapper.execute(usedParameterization);}};
+ std::this_thread::sleep_for(std::chrono::milliseconds(50)); // somehow we need to wait, otherwise it chrashes
+
- // recreate thread and execute skill.
- wrapper.task = std::thread{ [&] { wrapper.execute();}};
+ if (info.waitUntilSkillFinished && wrapper.task.joinable())
+ {
+ // wait until task is finished. We hold the shared lock for the whole time.
+ wrapper.task.join();
+ }
}
- skills::provider::dto::SkillStatusUpdate SkillProviderComponentPluginUser::abortSkill(const std::string& skillName, const Ice::Current ¤t)
+ void SkillProviderComponentPluginUser::abortSkill(const std::string& skillName, bool waitUntilSkillFinished, const Ice::Current ¤t)
{
- std::lock_guard l(skillsMutex);
+ std::shared_lock l(skillsMutex);
ARMARX_CHECK_EXPRESSION(skillImplementations.count(skillName) > 0);
auto& wrapper = skillImplementations.at(skillName);
-
- std::lock_guard l2(wrapper.skillStatusMutex);
-
- if (wrapper.skill->running)
+ if (waitUntilSkillFinished && !wrapper.skill->stopRequested() && wrapper.task.joinable())
{
wrapper.skill->notifyStopped();
wrapper.task.join();
}
-
- return wrapper.statusUpdate.toIce();
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
index 7e370346c..58895250b 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <mutex>
+#include <shared_mutex>
#include <queue>
#include <thread>
#include <functional>
@@ -33,7 +33,7 @@ namespace armarx::plugins
void preOnDisconnectComponent() override;
private:
- skills::manager::dti::SkillManagerInterfacePrx skillManager;
+ skills::manager::dti::SkillManagerInterfacePrx ownedBySkillManager;
skills::provider::dti::SkillProviderInterfacePrx myPrx;
};
}
@@ -50,17 +50,20 @@ namespace armarx
skills::provider::dto::SkillDescriptionMap getSkills(const Ice::Current ¤t = Ice::Current()) override;
skills::provider::dto::SkillStatusUpdate getSkillExecutionStatus(const std::string& skill, const Ice::Current ¤t = Ice::Current()) override;
void executeSkill(const skills::provider::dto::SkillExecutionInfo& executionInfo, const Ice::Current ¤t = Ice::Current()) override;
- skills::provider::dto::SkillStatusUpdate abortSkill(const std::string &skill, const Ice::Current ¤t = Ice::Current()) override;
+ void abortSkill(const std::string &skill, bool waitUntilSkillFinished, const Ice::Current ¤t = Ice::Current()) override;
protected:
void addSkill(const skills::helper::LambdaSkill::FunT&, const skills::SkillDescription&);
- void addSkill(const std::shared_ptr<skills::Skill>&);
+ void addSkill(std::unique_ptr<skills::Skill>&&);
private:
armarx::plugins::SkillProviderComponentPlugin* plugin = nullptr;
protected:
- mutable std::mutex skillsMutex;
+ mutable std::shared_mutex skillsMutex;
+
+ public:
+ bool connected = false;
std::map<std::string, skills::detail::SkillImplementationWrapper> skillImplementations;
};
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp b/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp
index cfc9f6fd2..eb98bb0b0 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp
@@ -11,8 +11,8 @@ namespace armarx
ret.skillName = skillName;
ret.data = aron::data::Dict::ToAronDictDTO(data);
ret.status = status;
- ret.usedCallbackInterface = usedCallbackInterface;
- ret.usedParams = aron::data::Dict::ToAronDictDTO(usedParameterization);
+ ret.usedCallbackInterface = usedParameterization.usedCallbackInterface;
+ ret.usedParams = aron::data::Dict::ToAronDictDTO(usedParameterization.usedInputParams);
return ret;
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.h b/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.h
index 5385c6f0d..2b69c7e5d 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.h
@@ -6,18 +6,19 @@
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/interface/skills/SkillProviderInterface.h>
+#include "SkillParameterization.h"
+
namespace armarx
{
namespace skills
{
struct SkillStatusUpdate
{
- std::string providerName;
- std::string skillName;
- aron::data::DictPtr usedParameterization;
- callback::dti::SkillProviderCallbackInterfacePrx usedCallbackInterface;
- provider::dto::Execution::Status status;
- aron::data::DictPtr data;
+ std::string providerName = "";
+ std::string skillName = "";
+ provider::dto::Execution::Status status = provider::dto::Execution::Status::Idle;
+ aron::data::DictPtr data = nullptr;
+ SkillParameterization usedParameterization;
provider::dto::SkillStatusUpdate toIce() const;
};
diff --git a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
index fec98231a..19bc9dc5c 100644
--- a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
@@ -25,22 +25,56 @@ namespace armarx
}
/// Override this method with the actual implementation. The callback is for status updates to the calling instance
- virtual Status _execute(const AronT& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; })
+ virtual Status execute(const AronT& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; })
{
(void) params;
-
- ARMARX_WARNING_S << "You have to override this method!";
return Status::Succeeded;
}
+ virtual void init(const AronT& params)
+ {
+ (void) params;
+ }
+
+ virtual void exit(const AronT& params)
+ {
+ (void) params;
+ }
+
+ Status initExecuteExit(const AronT& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; })
+ {
+ return Skill::initExecuteExit(params.toAron(), callback);
+ }
+
+ /// Do not use anymore
+ Status execute(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; }) final
+ {
+ Skill::execute(params, callback);
+ AronT p;
+ p.fromAron(params);
+
+ return execute(p, callback);
+ }
+
/// Do not use anymore
- Status _execute(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; }) final
+ void init(const aron::data::DictPtr& params) final
{
+ Skill::init(params);
AronT p;
//ARMARX_IMPORTANT << aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(params).dump(2);
p.fromAron(params);
- return _execute(p, callback);
+ return init(p);
+ }
+
+ /// Do not use anymore
+ void exit(const aron::data::DictPtr& params) final
+ {
+ Skill::exit(params);
+ AronT p;
+ p.fromAron(params);
+
+ exit(p);
}
};
}
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
index 9f9c573b8..c6add6570 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
@@ -4,25 +4,40 @@ namespace armarx
{
namespace skills::detail
{
- void SkillImplementationWrapper::reset()
+ void SkillImplementationWrapper::execute(const skills::SkillParameterization parameterization)
{
- ARMARX_CHECK_NOT_NULL(skill);
- std::lock_guard l(skillStatusMutex);
- statusUpdate.status = skills::provider::dto::Execution::Status::Idle;
- statusUpdate.data = nullptr;
- skill->reset();
- }
-
- void SkillImplementationWrapper::execute()
- {
- ARMARX_CHECK_NOT_NULL(skill);
- std::lock_guard l(executingMutex);
+ std::unique_lock l(executingMutex);
const std::string skillName = skill->description.skillName;
ARMARX_INFO_S << "Executing skill: " << skillName;
- // get params and setup variables
- auto& aron_params = statusUpdate.usedParameterization;
+ // reset Skill
+ {
+ std::unique_lock l2(skillStatusMutex); // skill is not updating
+ statusUpdate.status = skills::provider::dto::Execution::Status::Idle;
+ statusUpdate.data = nullptr;
+ skill->reset();
+ }
+
+ // set params and setup variables
+ {
+ std::lock_guard l(skillStatusMutex);
+ statusUpdate.usedParameterization = parameterization;
+ }
+
+ auto& aron_params = parameterization.usedInputParams;
+ auto updateStatus = [&](const skills::provider::dto::Execution::Status status, const aron::data::DictPtr& data = nullptr){
+ std::lock_guard l(skillStatusMutex);
+ statusUpdate.status = status;
+ statusUpdate.data = data;
+
+ auto& callbackInterface = statusUpdate.usedParameterization.usedCallbackInterface;
+
+ if (callbackInterface)
+ {
+ callbackInterface->updateStatusForSkill(statusUpdate.toIce());
+ }
+ };
try
{
@@ -33,28 +48,16 @@ namespace armarx
}
// set scheduled
- {
- std::lock_guard l(skillStatusMutex);
- statusUpdate.status = skills::provider::dto::Execution::Status::Scheduled;
- updateStatus();
- }
-
+ updateStatus(skills::provider::dto::Execution::Status::Scheduled);
// execute
- {
- std::lock_guard l(skillStatusMutex);
- statusUpdate.status = skills::provider::dto::Execution::Status::Running;
- updateStatus();
- }
+ updateStatus(skills::provider::dto::Execution::Status::Running);
- auto execution_callback = [&](const aron::data::DictPtr& update)
- {
- statusUpdate.data = update;
- updateStatus();
- };
- auto result = skill->execute(aron_params, execution_callback);
+ skill->init(aron_params);
+ auto ret = skill->execute(aron_params, [&](const aron::data::DictPtr& update) { updateStatus(statusUpdate.status, update); });
+ skill->exit(aron_params);
- switch (result)
+ switch (ret)
{
case skills::Skill::Status::Failed:
throw armarx::LocalException("The Skill '" + skillName + "' failed during execution.");
@@ -64,36 +67,20 @@ namespace armarx
break;
case skills::Skill::Status::Stopped:
{
- std::lock_guard l(skillStatusMutex);
- statusUpdate.status = skills::provider::dto::Execution::Status::Aborted;
- updateStatus();
+ updateStatus(skills::provider::dto::Execution::Status::Aborted);
break;
}
- default:
+ case skills::Skill::Status::Succeeded:
{
- std::lock_guard l(skillStatusMutex);
- statusUpdate.status = skills::provider::dto::Execution::Status::Succeeded;
- updateStatus();
+ updateStatus(skills::provider::dto::Execution::Status::Succeeded);
+ break;
}
}
}
catch (const std::exception& ex)
{
ARMARX_WARNING_S << "Skill " << skillName << " died with exception:\n" << ex.what();
-
- std::lock_guard l(skillStatusMutex);
- statusUpdate.status = skills::provider::dto::Execution::Status::Failed;
- updateStatus();
- }
- }
-
- void SkillImplementationWrapper::updateStatus() const
- {
- auto& callbackInterface = statusUpdate.usedCallbackInterface;
-
- if (callbackInterface)
- {
- callbackInterface->updateStatusForSkill(statusUpdate.toIce());
+ updateStatus(skills::provider::dto::Execution::Status::Failed);
}
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
index 35910a968..1c70a14e5 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
@@ -1,5 +1,7 @@
#pragma once
+#include <shared_mutex>
+
#include "../SkillDescription.h"
#include "../SkillStatusUpdate.h"
#include "../Skill.h"
@@ -16,32 +18,26 @@ namespace armarx
{
public:
// fixed values. Do not change after skill instantiation
- const std::shared_ptr<Skill> skill;
+ const std::unique_ptr<Skill> skill;
// Current execution status. Changes during execution
// The status also holds the used parameterization
- mutable std::mutex skillStatusMutex;
+ // skillName and providerName are const after registering the skill in a provider
+ mutable std::shared_mutex skillStatusMutex;
SkillStatusUpdate statusUpdate;
// Task information. task is recreated every time the skill restarts
- mutable std::mutex executingMutex;
+ mutable std::shared_mutex executingMutex;
std::thread task;
- SkillImplementationWrapper(const std::shared_ptr<skills::Skill> skill) :
- skill(skill)
+ SkillImplementationWrapper(std::unique_ptr<skills::Skill>&& skill) :
+ skill(std::move(skill))
{
- reset();
+ ARMARX_CHECK_NOT_NULL(this->skill);
}
- SkillImplementationWrapper(const SkillImplementationWrapper& s) :
- SkillImplementationWrapper(s.skill)
- {}
-
- void execute();
- void reset();
-
- protected:
- void updateStatus() const;
+ // execute a skill. The parameterization is copied
+ void execute(const skills::SkillParameterization);
};
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.cpp b/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.cpp
index 59d51cd16..9b761b403 100644
--- a/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.cpp
+++ b/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.cpp
@@ -5,7 +5,7 @@ namespace armarx
namespace skills::helper
{
- Skill::Status LambdaSkill::_execute(const aron::data::DictPtr& data, const CallbackT& callback)
+ Skill::Status LambdaSkill::execute(const aron::data::DictPtr& data, const CallbackT& callback)
{
(void) callback;
bool res = fun(data);
diff --git a/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.h b/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.h
index 27e1ab0ae..63f537d4c 100644
--- a/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.h
+++ b/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.h
@@ -18,7 +18,7 @@ namespace armarx
{};
protected:
- Skill::Status _execute(const aron::data::DictPtr& data, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; }) override;
+ Skill::Status execute(const aron::data::DictPtr& data, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; }) override;
private:
FunT fun;
--
GitLab