diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index b6d997f6882daa1b9ee95d97fb4502ae181aa9ab..0c5d82c8da19676c0ab3b544ea381ed7ae5f8776 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -86,8 +86,6 @@ namespace armarx::armem::server::obj
{
workingMemory.name() = defaultMemoryName;
- instance::SegmentAdapter::init();
-
const auto initSegmentWithCatch = [&](const std::string & segmentName, const auto&& fn)
{
try
@@ -108,11 +106,15 @@ namespace armarx::armem::server::obj
}
};
+ // Class segment needs to be initialized before instance segment,
+ // as the instance segment may refer to and search through the class segment.
initSegmentWithCatch("class", [&]()
{
classSegment.init();
});
+ instance::SegmentAdapter::init();
+
initSegmentWithCatch("articulated object class", [&]()
{
articulatedObjectClassSegment.init();
@@ -185,28 +187,34 @@ namespace armarx::armem::server::obj
{
using namespace armarx::RemoteGui::Client;
- tab.instance.setup(*this);
- tab.clazz.setup(classSegment);
+ tab.reset(new RemoteGuiTab);
+
+ tab->instance.setup(*this);
+ tab->clazz.setup(classSegment);
HBoxLayout segments =
{
- tab.instance.group,
- tab.clazz.group
+ tab->instance.group,
+ tab->clazz.group
};
VBoxLayout root =
{
segments,
VSpacer()
};
- RemoteGui_createTab(Component::getName(), root, &tab);
+ RemoteGui_createTab(Component::getName(), root, tab.get());
}
+
void ObjectMemory::RemoteGui_update()
{
- // Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
- tab.instance.update(*this);
- tab.clazz.update(classSegment);
+ tab->instance.update(*this);
+ tab->clazz.update(classSegment);
+
+ if (tab->clazz.data.rebuild)
+ {
+ createRemoteGuiTab();
+ }
}
}
-
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index 2dfd668d1e43df914ff7b2f02205e699ad2ea5e9..a513016a3da9f0376a092768e4a977974493ca32 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
@@ -122,7 +122,7 @@ namespace armarx::armem::server::obj
instance::SegmentAdapter::RemoteGui instance;
clazz::Segment::RemoteGui clazz;
};
- RemoteGuiTab tab;
+ std::unique_ptr<RemoteGuiTab> tab;
};
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index 8e86d96dc398d707ab4e8629e4b971519db8dc65..2e026dd211cf6906d8b54b468ad6eebfff12141b 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -55,6 +55,12 @@ namespace armarx
}
+ bool ObjectFinder::isDatasetDirValid(const path& path) const
+ {
+ return path.filename() != "Scenes" and std::filesystem::is_directory(path);
+ }
+
+
std::optional<ObjectInfo> ObjectFinder::findObject(const std::string& dataset, const std::string& name) const
{
init();
@@ -67,8 +73,8 @@ namespace armarx
return ObjectInfo(packageName, packageDataDir, dataset, name);
}
// Search for object in datasets.
- const auto& datasets = getDatasets();
- for (const path& dataset : datasets)
+ const std::vector<std::string>& datasets = getDatasets();
+ for (const std::string& dataset : datasets)
{
if (fs::is_directory(_rootDirAbs() / dataset / name))
{
@@ -106,7 +112,7 @@ namespace armarx
{
// init(); // Done by called methods.
std::vector<std::string> datasets;
- for (const auto& dir : getDatasetDirectories())
+ for (const path& dir : getDatasetDirectories())
{
datasets.push_back(dir.filename());
}
@@ -123,9 +129,9 @@ namespace armarx
const bool local = false;
std::vector<path> dirs = simox::fs::list_directory(_rootDirAbs(), local);
std::vector<path> datasetDirs;
- for (const auto& p : dirs)
+ for (const path& p : dirs)
{
- if (std::filesystem::is_directory(p))
+ if (isDatasetDirValid(p))
{
datasetDirs.push_back(p);
}
@@ -144,7 +150,7 @@ namespace armarx
std::vector<ObjectInfo> objects;
for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
{
- if (fs::is_directory(_rootDirAbs() / datasetDir))
+ if (isDatasetDirValid(_rootDirAbs() / datasetDir))
{
std::vector<ObjectInfo> dataset = findAllObjectsOfDataset(datasetDir, checkPaths);
for (const auto& o : dataset)
@@ -169,7 +175,7 @@ namespace armarx
std::vector<armem::articulated_object::ArticulatedObjectDescription> objects;
for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
{
- if (fs::is_directory(_rootDirAbs() / datasetDir))
+ if (isDatasetDirValid(_rootDirAbs() / datasetDir))
{
const auto dataset = findAllArticulatedObjectsOfDataset(datasetDir, checkPaths);
objects.insert(objects.end(), dataset.begin(), dataset.end());
@@ -221,7 +227,9 @@ namespace armarx
return objects;
}
- std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>> ObjectFinder::findAllArticulatedObjectsByDataset(bool checkPaths) const
+
+ std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>>
+ ObjectFinder::findAllArticulatedObjectsByDataset(bool checkPaths) const
{
init();
if (!_ready())
@@ -234,7 +242,7 @@ namespace armarx
std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>> datasets;
for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
{
- if (fs::is_directory(_rootDirAbs() / datasetDir))
+ if (isDatasetDirValid(_rootDirAbs() / datasetDir))
{
const auto dataset = findAllArticulatedObjectsOfDataset(datasetDir, checkPaths);
datasets[datasetDir] = dataset;
@@ -244,7 +252,8 @@ namespace armarx
}
- std::vector<armem::articulated_object::ArticulatedObjectDescription> ObjectFinder::findAllArticulatedObjectsOfDataset(const std::string& dataset, bool checkPaths) const
+ std::vector<armem::articulated_object::ArticulatedObjectDescription>
+ ObjectFinder::findAllArticulatedObjectsOfDataset(const std::string& dataset, bool checkPaths) const
{
init();
if (!_ready())
@@ -252,7 +261,7 @@ namespace armarx
return {};
}
path datasetDir = _rootDirAbs() / dataset;
- if (!fs::is_directory(datasetDir))
+ if (!isDatasetDirValid(datasetDir))
{
ARMARX_WARNING << "Expected dataset directory for dataset '" << dataset << "': \n"
<< datasetDir;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
index 2b342f3ac1151d0589bbc4b0212c3aed2fde798d..0bef0667120ee99e7d65f575f1c9b61b446ffe6f 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
@@ -89,14 +89,18 @@ namespace armarx
private:
+
void init() const;
+ bool isDatasetDirValid(const std::filesystem::path& path) const;
path _rootDirAbs() const;
path _rootDirRel() const;
bool _ready() const;
+
private:
+
/// Name of package containing the object models (ArmarXObjects by default).
mutable std::string packageName;
@@ -105,5 +109,6 @@ namespace armarx
* Empty if package could not be found.
*/
mutable path packageDataDir;
+
};
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
index adb8260199706797be7b36b65e4fd504ad2e8a63..f2d76a501ecf8e1f53dcf9bf80a2e8a0f66d7536 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
@@ -57,6 +57,11 @@ namespace armarx
return _className == rhs._className && _dataset == rhs._dataset;
}
+ ObjectID ObjectID::withInstanceName(const std::string& instanceName) const
+ {
+ return ObjectID(_dataset, _className, instanceName);
+ }
+
bool ObjectID::operator==(const ObjectID& rhs) const
{
return _className == rhs._className
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
index 73e2c6ca5f8d272fd90257fd5aedfd839266174d..389b203a6ecc67f2f6cd074ce103c5b52687a2be 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
@@ -43,6 +43,8 @@ namespace armarx
/// Indicates whether dataset and class name are equal.
bool equalClass(const ObjectID& rhs) const;
+ ObjectID withInstanceName(const std::string& instanceName) const;
+
/// Indicates whether dataset, class name and instance name are equal.
bool operator==(const ObjectID& rhs) const;
inline bool operator!=(const ObjectID& rhs) const
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index c4ad28ab1f6880f59adb4ca7eda5e4e8a8a65007..9ebc8f2abc95547925b756254e0725d3c1dc2ce0 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -18,6 +18,7 @@ set(LIB_FILES
core/Time.cpp
core/ice_conversions.cpp
core/aron_conversions.cpp
+ core/json_conversions.cpp
core/base/detail/MemoryItem.cpp
core/base/detail/MaxHistorySize.cpp
@@ -40,8 +41,9 @@ set(LIB_FILES
core/workingmemory/EntitySnapshot.cpp
core/workingmemory/Memory.cpp
core/workingmemory/ProviderSegment.cpp
- core/workingmemory/Visitor.cpp
core/workingmemory/ice_conversions.cpp
+ core/workingmemory/visitor/Visitor.cpp
+ core/workingmemory/visitor/FunctionalVisitor.cpp
core/longtermmemory/CoreSegment.cpp
core/longtermmemory/Entity.cpp
@@ -111,6 +113,7 @@ set(LIB_HEADERS
core/SuccessHeader.h
core/Time.h
core/aron_conversions.h
+ core/json_conversions.h
core/ice_conversions.h
core/ice_conversions_templates.h
@@ -137,8 +140,10 @@ set(LIB_HEADERS
core/workingmemory/EntitySnapshot.h
core/workingmemory/Memory.h
core/workingmemory/ProviderSegment.h
- core/workingmemory/Visitor.h
core/workingmemory/ice_conversions.h
+ core/workingmemory/visitor.h
+ core/workingmemory/visitor/Visitor.h
+ core/workingmemory/visitor/FunctionalVisitor.h
core/longtermmemory/CoreSegment.h
core/longtermmemory/Entity.h
diff --git a/source/RobotAPI/libraries/armem/core/json_conversions.cpp b/source/RobotAPI/libraries/armem/core/json_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e4da9d89b5e04351e5e528132406a962470364e3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/json_conversions.cpp
@@ -0,0 +1,26 @@
+#include "json_conversions.h"
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+
+void armarx::armem::to_json(nlohmann::json& j, const MemoryID& bo)
+{
+ j["memoryName"] = bo.memoryName;
+ j["coreSegmentName"] = bo.coreSegmentName;
+ j["providerSegmentName"] = bo.providerSegmentName;
+ j["entityName"] = bo.entityName;
+ j["timestamp_usec"] = bo.timestamp.toMicroSeconds();
+ j["timestamp_datetime"] = toDateTimeMilliSeconds(bo.timestamp);
+ j["instanceIndex"] = bo.instanceIndex;
+}
+
+void armarx::armem::from_json(const nlohmann::json& j, MemoryID& bo)
+{
+ j.at("memoryName").get_to(bo.memoryName);
+ j.at("coreSegmentName").get_to(bo.coreSegmentName);
+ j.at("providerSegmentName").get_to(bo.providerSegmentName);
+ j.at("entityName").get_to(bo.entityName);
+ bo.timestamp = Time::microSeconds(j.at("timestamp_usec").get<int64_t>());
+ // j.at("timestamp_datetime").get_to(toDateTimeMilliSeconds(bo.timestamp));
+ j.at("instanceIndex").get_to(bo.instanceIndex);
+}
diff --git a/source/RobotAPI/libraries/armem/core/json_conversions.h b/source/RobotAPI/libraries/armem/core/json_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..86e508c228fe57063216c2f1c6fa7710066992bc
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/json_conversions.h
@@ -0,0 +1,12 @@
+#pragma once
+
+#include <SimoxUtility/json/json.hpp>
+
+
+namespace armarx::armem
+{
+ class MemoryID;
+
+ void to_json(nlohmann::json& j, const MemoryID& bo);
+ void from_json(const nlohmann::json& j, MemoryID& bo);
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/visitor.h b/source/RobotAPI/libraries/armem/core/workingmemory/visitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..815ee1b34908dc5916e7d909e709bc735409f63c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/visitor.h
@@ -0,0 +1,4 @@
+#pragma once
+
+#include "visitor/FunctionalVisitor.h"
+#include "visitor/Visitor.h"
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..53016fc6c83d9ef6e24a2e75942a12bced7d0eaa
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.cpp
@@ -0,0 +1,23 @@
+#include "FunctionalVisitor.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+
+
+namespace armarx::armem::wm
+{
+
+ FunctionalVisitor::FunctionalVisitor()
+ {
+ }
+
+
+ FunctionalVisitor::~FunctionalVisitor()
+ {
+ }
+
+
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..1c1b46e138c7eb2d713ba15b76ba77a2588ea0ce
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h
@@ -0,0 +1,101 @@
+#pragma once
+
+#include <functional>
+
+#include "Visitor.h"
+
+
+namespace armarx::armem::wm
+{
+
+ /**
+ * @brief A `Visitor` which can be parametrized by `std::function`
+ * instead of inheriting and overriding.
+ */
+ class FunctionalVisitor : public Visitor
+ {
+ public:
+
+ FunctionalVisitor();
+ virtual ~FunctionalVisitor() override;
+
+
+ bool visitEnter(Memory& memory) override
+ {
+ return memoryFn ? memoryFn(memory) : Visitor::visitEnter(memory);
+ }
+ bool visitEnter(CoreSegment& coreSegment) override
+ {
+ return coreSegmentFn ? coreSegmentFn(coreSegment) : Visitor::visitEnter(coreSegment);
+ }
+ bool visitEnter(ProviderSegment& providerSegment) override
+ {
+ return providerSegmentFn ? providerSegmentFn(providerSegment) : Visitor::visitEnter(providerSegment);
+ }
+ bool visitEnter(Entity& entity) override
+ {
+ return entityFn ? entityFn(entity) : Visitor::visitEnter(entity);
+ }
+ bool visitEnter(EntitySnapshot& snapshot) override
+ {
+ return snapshotFn ? snapshotFn(snapshot) : Visitor::visitEnter(snapshot);
+ }
+
+ bool visit(EntityInstance& instance) override
+ {
+ return instanceFn ? instanceFn(instance) : Visitor::visit(instance);
+ }
+
+
+
+ // Const versions
+
+
+ bool visitEnter(const Memory& memory) override
+ {
+ return memoryFn ? memoryConstFn(memory) : Visitor::visitEnter(memory);
+ }
+ bool visitEnter(const CoreSegment& coreSegment) override
+ {
+ return coreSegmentFn ? coreSegmentConstFn(coreSegment) : Visitor::visitEnter(coreSegment);
+ }
+ bool visitEnter(const ProviderSegment& providerSegment) override
+ {
+ return providerSegmentFn ? providerSegmentConstFn(providerSegment) : Visitor::visitEnter(providerSegment);
+ }
+ bool visitEnter(const Entity& entity) override
+ {
+ return entityFn ? entityConstFn(entity) : Visitor::visitEnter(entity);
+ }
+ bool visitEnter(const EntitySnapshot& snapshot) override
+ {
+ return memoryFn ? snapshotConstFn(snapshot) : Visitor::visitEnter(snapshot);
+ }
+
+ bool visit(const EntityInstance& instance) override
+ {
+ return instanceFn ? instanceConstFn(instance) : Visitor::visit(instance);
+ }
+
+
+ std::function<bool(Memory& memory)> memoryFn;
+ std::function<bool(const Memory& memory)> memoryConstFn;
+
+ std::function<bool(CoreSegment& coreSegment)> coreSegmentFn;
+ std::function<bool(const CoreSegment& coreSegment)> coreSegmentConstFn;
+
+ std::function<bool(ProviderSegment& providerSegment)> providerSegmentFn;
+ std::function<bool(const ProviderSegment& providerSegment)> providerSegmentConstFn;
+
+ std::function<bool(Entity& entity)> entityFn;
+ std::function<bool(const Entity& entity)> entityConstFn;
+
+ std::function<bool(EntitySnapshot& snapshot)> snapshotFn;
+ std::function<bool(const EntitySnapshot& snapshot)> snapshotConstFn;
+
+ std::function<bool(EntityInstance& instance)> instanceFn;
+ std::function<bool(const EntityInstance& instance)> instanceConstFn;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/Visitor.cpp
similarity index 100%
rename from source/RobotAPI/libraries/armem/core/workingmemory/Visitor.cpp
rename to source/RobotAPI/libraries/armem/core/workingmemory/visitor/Visitor.cpp
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.h b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/Visitor.h
similarity index 98%
rename from source/RobotAPI/libraries/armem/core/workingmemory/Visitor.h
rename to source/RobotAPI/libraries/armem/core/workingmemory/visitor/Visitor.h
index c0a3854a242c030113eeb46409c172563660294a..fa789fcf0b6876ec5d9fa0ff1a3944b4e3bf3fd3 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/Visitor.h
@@ -12,7 +12,7 @@ namespace armarx::armem::wm
/**
- * @brief A visitor for the hierarchical Memory data structure.
+ * @brief A visitor for the hierarchical memory data structure.
*/
class Visitor
{
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index 8c0f625f022e3efeb7cf7763841a25e6469b6ac8..51296814ff03036d197a96683b15827ef9af5fe5 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -20,6 +20,8 @@ armarx_add_library(
aron_conversions.h
aron_forward_declarations.h
+ SceneSnapshot.h
+
server/class/FloorVis.h
server/class/Segment.h
@@ -46,6 +48,8 @@ armarx_add_library(
SOURCES
aron_conversions.cpp
+ SceneSnapshot.cpp
+
server/class/FloorVis.cpp
server/class/Segment.cpp
diff --git a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d8ae0a10b97825f924d7e1605dc4687dcce9bfee
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp
@@ -0,0 +1,53 @@
+#include "SceneSnapshot.h"
+
+#include <SimoxUtility/json.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+
+// #include <RobotAPI/libraries/armem/core/json_conversions.h>
+
+
+namespace armarx::armem::obj
+{
+
+ ObjectID SceneSnapshot::Object::getClassID() const
+ {
+ return ObjectID(className);
+ }
+
+}
+
+
+void armarx::armem::obj::to_json(nlohmann::json& j, const SceneSnapshot::Object& rhs)
+{
+ // j["instanceID"] = rhs.instanceID;
+ j["class"] = rhs.className;
+ j["collection"] = rhs.collection;
+ j["position"] = rhs.position;
+ j["orientation"] = rhs.orientation;
+}
+
+
+void armarx::armem::obj::from_json(const nlohmann::json& j, SceneSnapshot::Object& rhs)
+{
+ // j.at("instanceID").get_to(rhs.instanceID);
+ j.at("class").get_to(rhs.className);
+ j.at("collection").get_to(rhs.collection);
+ j.at("position").get_to(rhs.position);
+ j.at("orientation").get_to(rhs.orientation);
+}
+
+
+
+void armarx::armem::obj::to_json(nlohmann::json& j, const SceneSnapshot& rhs)
+{
+ j["objects"] = rhs.objects;
+}
+
+
+void armarx::armem::obj::from_json(const nlohmann::json& j, SceneSnapshot& rhs)
+{
+ j.at("objects").get_to(rhs.objects);
+}
+
+
diff --git a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.h b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.h
new file mode 100644
index 0000000000000000000000000000000000000000..2baa2cd3c323794aa4112bbc5df8c3353073dc61
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.h
@@ -0,0 +1,57 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::ObjectMemory
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <SimoxUtility/json/json.hpp>
+
+#include <Eigen/Geometry>
+
+
+namespace armarx
+{
+ class ObjectID;
+}
+namespace armarx::armem::obj
+{
+
+ struct SceneSnapshot
+ {
+ struct Object
+ {
+ std::string className;
+ std::string collection;
+
+ Eigen::Vector3f position = Eigen::Vector3f::Zero();
+ Eigen::Quaternionf orientation = Eigen::Quaternionf::Identity();
+
+ ObjectID getClassID() const;
+ };
+ std::vector<Object> objects;
+ };
+
+ void to_json(nlohmann::json& j, const SceneSnapshot::Object& rhs);
+ void from_json(const nlohmann::json& j, SceneSnapshot::Object& rhs);
+
+ void to_json(nlohmann::json& j, const SceneSnapshot& rhs);
+ void from_json(const nlohmann::json& j, SceneSnapshot& rhs);
+
+}
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
index 422b8e6be1fa3b50fe0ffc154ce2612528c3a578..aa4afc7b071fb30d36640fef9fd498b12b25f08f 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
@@ -9,7 +9,7 @@
#include "RobotAPI/libraries/aron/common/aron_conversions.h"
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
diff --git a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
index 338bc804b12c0569542e57ed283741e4145893a5..d1daccd0b3db38814b94a2e7624b53aede655204 100644
--- a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
@@ -9,7 +9,7 @@
#include "RobotAPI/libraries/aron/common/aron_conversions.h"
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
index 93989e18d6c5cd39d0f878cd2caea652fadb41b3..323a4df0d37cbf5d8177e763dddb9b58091bf4de 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -79,7 +79,10 @@ namespace armarx::armem::server::obj::clazz
std::vector<ObjectInfo> infos = objectFinder.findAllObjects(checkPaths);
const MemoryID providerID = coreSegment->id().withProviderSegmentName(objectFinder.getPackageName());
- coreSegment->addProviderSegment(providerID.providerSegmentName);
+ if (not coreSegment->hasProviderSegment(providerID.providerSegmentName))
+ {
+ coreSegment->addProviderSegment(providerID.providerSegmentName);
+ }
ARMARX_INFO << "Loading up to " << infos.size() << " object classes from '"
<< objectFinder.getPackageName() << "' ...";
@@ -225,12 +228,16 @@ namespace armarx::armem::server::obj::clazz
{
using namespace armarx::RemoteGui::Client;
+ reloadButton.setLabel("Reload");
+
maxHistorySize.setValue(std::max(1, int(segment.p.maxHistorySize)));
maxHistorySize.setRange(1, 1e6);
infiniteHistory.setValue(segment.p.maxHistorySize == -1);
GridLayout grid;
int row = 0;
+ grid.add(reloadButton, {row, 0}, {1, 2});
+ row++;
grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
row++;
grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
@@ -243,6 +250,12 @@ namespace armarx::armem::server::obj::clazz
void Segment::RemoteGui::Data::update(Segment& segment)
{
+ if (reloadButton.wasClicked())
+ {
+ std::scoped_lock lock(segment.memoryMutex);
+ segment.loadByObjectFinder();
+ rebuild = true;
+ }
if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
{
std::scoped_lock lock(segment.memoryMutex);
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
index 7356e6a1d9e490840f7c19b5e41e27506ddf2ecf..88eaedbefb6529b873dfe6033e6a2857672c0586 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
@@ -76,11 +76,14 @@ namespace armarx::armem::server::obj::clazz
{
armarx::RemoteGui::Client::GroupBox group;
+ armarx::RemoteGui::Client::Button reloadButton;
armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
armarx::RemoteGui::Client::CheckBox infiniteHistory;
void setup(const Segment& segment);
void update(Segment& segment);
+
+ bool rebuild = false;
};
Data data;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index aa2125dada86d895e7fe063cbdc20501623576b7..eb81a73dc3fa4b949dbdf880a9023b51d02d7320 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -1,12 +1,17 @@
#include "Segment.h"
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/SceneSnapshot.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
@@ -17,10 +22,17 @@
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+
+#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/json.h>
+
+#include <Eigen/Geometry>
#include <sstream>
+
namespace armarx::armem::server::obj::instance
{
@@ -37,6 +49,7 @@ namespace armarx::armem::server::obj::instance
{
try
{
+ objectInfo->setLogError(false); // Don't log missing files
return objectInfo->loadOOBB();
}
catch (const std::ios_base::failure& e)
@@ -59,6 +72,7 @@ namespace armarx::armem::server::obj::instance
});
}
+
void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
@@ -67,15 +81,30 @@ namespace armarx::armem::server::obj::instance
"If true, no new snapshots are stored while an object is attached to a robot node.\n"
"If false, new snapshots are stored, but the attachment is kept in the new snapshots.");
+ defs->optional(p.sceneSnapshotsPackage, prefix + "scene.Package",
+ "ArmarX package containing the scene snapshots.\n"
+ "Scene snapshots are expected to be located in Package/data/Package/Scenes/*.json.");
+ defs->optional(p.sceneSnapshotToLoad, prefix + "scene.SnapshotToLoad",
+ "Scene snapshot to load on startup (e.g. 'Scene_2021-06-24_20-20-03').\n"
+ "You can also specify paths relative to 'Package/Scenes/'.");
+
decay.defineProperties(defs, prefix + "decay.");
}
+
void Segment::init()
{
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, arondto::ObjectInstance::toInitialAronType());
coreSegment->setMaxHistorySize(p.maxHistorySize);
+
+
+ if (not p.sceneSnapshotToLoad.empty())
+ {
+ bool lockMemory = false;
+ commitSceneSnapshotFromFilename(p.sceneSnapshotToLoad, lockMemory);
+ }
}
@@ -156,42 +185,45 @@ namespace armarx::armem::server::obj::instance
}
}
- commitObjectPoses(providerName, newObjectPoses);
+ commitObjectPoses(newObjectPoses, providerName);
return stats;
}
- void Segment::commitObjectPoses(const std::string& providerName, const ObjectPoseSeq& objectPoses)
+
+ void Segment::commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName)
{
ARMARX_CHECK_NOT_NULL(coreSegment);
+ Time now = TimeUtil::GetTime();
- // Update memory.
- const MemoryID providerSegmentID = coreSegment->id().withProviderSegmentName(providerName);
- if (!coreSegment->hasProviderSegment(providerSegmentID.providerSegmentName))
- {
- coreSegment->addProviderSegment(providerSegmentID.providerSegmentName);
- }
+ const MemoryID coreSegmentID = coreSegment->id();
Commit commit;
for (const objpose::ObjectPose& pose : objectPoses)
{
EntityUpdate& update = commit.updates.emplace_back();
+
+ const MemoryID providerID = coreSegmentID.withProviderSegmentName(
+ providerName.empty() ? pose.providerName : providerName);
+
+ update.entityID = providerID.withEntityName(pose.objectID.str());
+ update.timeArrived = now;
+ update.timeCreated = pose.timestamp;
+ update.confidence = pose.confidence;
+
+ arondto::ObjectInstance dto;
+ toAron(dto, pose);
+ // Search for object class.
+ if (auto instance = findClassInstance(pose.objectID))
{
- update.entityID = providerSegmentID.withEntityName(pose.objectID.str());
- update.timeArrived = TimeUtil::GetTime();
- update.timeCreated = pose.timestamp;
- update.confidence = pose.confidence;
-
- arondto::ObjectInstance dto;
- toAron(dto, pose);
- // Search for object class.
- if (auto instance = findClassInstance(pose.objectID))
- {
- toAron(dto.classID, instance->id());
- }
- update.instancesData.push_back(dto.toAron());
+ toAron(dto.classID, instance->id());
}
-
+ else
+ {
+ toAron(dto.classID, MemoryID());
+ }
+ toAron(dto.sourceID, MemoryID());
+ update.instancesData.push_back(dto.toAron());
}
iceMemory.commit(commit);
}
@@ -203,12 +235,14 @@ namespace armarx::armem::server::obj::instance
return *coreSegment;
}
+
const wm::CoreSegment& Segment::getCoreSegment() const
{
ARMARX_CHECK_NOT_NULL(coreSegment);
return *coreSegment;
}
+
objpose::ObjectPoseSeq Segment::getObjectPoses(IceUtil::Time now)
{
ObjectPoseSeq objectPoses = getLatestObjectPoses();
@@ -217,6 +251,7 @@ namespace armarx::armem::server::obj::instance
}
+
objpose::ObjectPoseSeq Segment::getObjectPosesByProvider(
const std::string& providerName,
IceUtil::Time now)
@@ -227,6 +262,7 @@ namespace armarx::armem::server::obj::instance
return filterObjectPoses(objectPoses);
}
+
armem::wm::Entity* Segment::findObjectEntity(const ObjectID& objectID, const std::string& providerName)
{
ARMARX_CHECK_NOT_NULL(coreSegment);
@@ -329,12 +365,14 @@ namespace armarx::armem::server::obj::instance
objectPose.updateAttached(agent);
}
+
objpose::ObjectPoseSeq Segment::getLatestObjectPoses() const
{
ARMARX_CHECK_NOT_NULL(coreSegment);
return getLatestObjectPoses(*coreSegment);
}
+
objpose::ObjectPoseSeq Segment::getLatestObjectPoses(const armem::wm::CoreSegment& coreSeg)
{
ObjectPoseSeq result;
@@ -342,6 +380,7 @@ namespace armarx::armem::server::obj::instance
return result;
}
+
objpose::ObjectPoseSeq Segment::getLatestObjectPoses(const armem::wm::ProviderSegment& provSeg)
{
ObjectPoseSeq result;
@@ -349,6 +388,7 @@ namespace armarx::armem::server::obj::instance
return result;
}
+
objpose::ObjectPose Segment::getLatestObjectPose(const armem::wm::Entity& entity)
{
ObjectPose result;
@@ -356,6 +396,7 @@ namespace armarx::armem::server::obj::instance
return result;
}
+
void Segment::getLatestObjectPoses(const armem::wm::CoreSegment& coreSeg, ObjectPoseSeq& out)
{
for (const auto& [_, provSegment] : coreSeg)
@@ -364,6 +405,7 @@ namespace armarx::armem::server::obj::instance
}
}
+
void Segment::getLatestObjectPoses(const armem::wm::ProviderSegment& provSegment, ObjectPoseSeq& out)
{
for (const auto& [_, entity] : provSegment)
@@ -376,6 +418,7 @@ namespace armarx::armem::server::obj::instance
}
}
+
void Segment::getLatestObjectPose(const armem::wm::Entity& entity, ObjectPose& out)
{
for (const armem::wm::EntityInstance& instance : entity.getLatestSnapshot())
@@ -402,11 +445,13 @@ namespace armarx::armem::server::obj::instance
return data;
}
+
std::optional<simox::OrientedBoxf> Segment::getObjectOOBB(const ObjectID& id)
{
return oobbCache.get(id);
}
+
objpose::ProviderInfo Segment::getProviderInfo(const std::string& providerName)
{
try
@@ -427,7 +472,6 @@ namespace armarx::armem::server::obj::instance
}
-
objpose::AttachObjectToRobotNodeOutput
Segment::attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input)
{
@@ -517,6 +561,7 @@ namespace armarx::armem::server::obj::instance
return output;
}
+
objpose::DetachObjectFromRobotNodeOutput Segment::detachObjectFromRobotNode(
const objpose::DetachObjectFromRobotNodeInput& input)
{
@@ -581,6 +626,7 @@ namespace armarx::armem::server::obj::instance
virtual bool visitEnter(armem::wm::Entity& entity) override;
};
+
bool DetachVisitor::visitEnter(armem::wm::Entity& entity)
{
const arondto::ObjectInstance data = owner.getLatestInstanceData(entity);
@@ -590,7 +636,6 @@ namespace armarx::armem::server::obj::instance
// Store non-attached pose in new snapshot.
owner.storeDetachedSnapshot(entity, data, now, commitAttachedPose);
}
-
return false; // Stop descending.
}
@@ -613,6 +658,7 @@ namespace armarx::armem::server::obj::instance
return output;
}
+
void Segment::storeDetachedSnapshot(
armem::wm::Entity& entity,
const arondto::ObjectInstance& data,
@@ -648,14 +694,17 @@ namespace armarx::armem::server::obj::instance
}
- std::optional<wm::EntityInstance> Segment::findClassInstance(const ObjectID& objectID)
+ std::optional<wm::EntityInstance> Segment::findClassInstance(const ObjectID& objectID) const
{
const ObjectID classID = { objectID.dataset(), objectID.className() };
try
{
for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment("Class"))
{
- return provSeg.getEntity(classID.str()).getLatestSnapshot().getInstance(0);
+ if (provSeg.hasEntity(classID.str()))
+ {
+ return provSeg.getEntity(classID.str()).getLatestSnapshot().getInstance(0);
+ }
}
return std::nullopt;
}
@@ -667,6 +716,181 @@ namespace armarx::armem::server::obj::instance
}
+ void Segment::storeScene(const std::string& filename, const armem::obj::SceneSnapshot& scene)
+ {
+ if (const std::optional<std::filesystem::path> path = resolveSceneFilename(filename))
+ {
+ const nlohmann::json j = scene;
+ ARMARX_INFO << "Storing scene snapshot at: \n" << path.value() << "\n\n" << j.dump(2);
+ try
+ {
+ nlohmann::write_json(path.value(), j, 2);
+ }
+ catch (const std::ios_base::failure& e)
+ {
+ ARMARX_WARNING << "Storing scene snapshot failed: \n" << e.what();
+ }
+ }
+ }
+
+
+ std::optional<armem::obj::SceneSnapshot> Segment::loadScene(const std::string& filename)
+ {
+ if (const std::optional<std::filesystem::path> path = resolveSceneFilename(filename))
+ {
+ return loadScene(path.value());
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+ std::optional<armem::obj::SceneSnapshot> Segment::loadScene(const std::filesystem::path& path)
+ {
+ ARMARX_INFO << "Loading scene snapshot from: \n" << path;
+ nlohmann::json j;
+ try
+ {
+ j = nlohmann::read_json(path);
+ }
+ catch (const std::ios_base::failure& e)
+ {
+ ARMARX_WARNING << "Loading scene snapshot failed: \n" << e.what();
+ return std::nullopt;
+ }
+
+ armem::obj::SceneSnapshot snapshot = j;
+ return snapshot;
+ }
+
+
+ const std::string Segment::timestampPlaceholder = "%TIMESTAMP";
+
+
+ std::optional<std::filesystem::path> Segment::resolveSceneFilename(const std::string& _filename)
+ {
+ std::string filename = _filename;
+
+ filename = simox::alg::replace_all(filename, timestampPlaceholder,
+ Time::now().toString("%Y-%m-%d_%H-%M-%S"));
+ if (not simox::alg::ends_with(filename, ".json"))
+ {
+ filename += ".json";
+ }
+
+ if (!finder)
+ {
+ finder.reset(new CMakePackageFinder(p.sceneSnapshotsPackage));
+ if (not finder->packageFound())
+ {
+ ARMARX_WARNING << "Could not find CMake package " << p.sceneSnapshotsPackage << ".";
+ }
+ }
+ if (finder->packageFound())
+ {
+ std::filesystem::path dataDir = finder->getDataDir();
+ std::filesystem::path path = dataDir / p.sceneSnapshotsPackage / "Scenes" / filename;
+ return path;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+ armem::obj::SceneSnapshot Segment::getSceneSnapshot() const
+ {
+ armem::obj::SceneSnapshot scene;
+
+ wm::FunctionalVisitor visitor;
+ visitor.entityFn = [&scene](wm::Entity & entity)
+ {
+ try
+ {
+ const wm::EntityInstance& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ std::optional<arondto::ObjectInstance> objectInstance = tryCast<arondto::ObjectInstance>(entityInstance);
+ if (objectInstance)
+ {
+ armem::obj::SceneSnapshot::Object& object = scene.objects.emplace_back();
+ // object.instanceID = entityInstance.id();
+ object.className = ObjectID(objectInstance->classID.entityName).getClassID().str();
+ object.collection = "";
+ object.position = simox::math::position(objectInstance->pose.objectPoseGlobal);
+ object.orientation = simox::math::orientation(objectInstance->pose.objectPoseGlobal);
+ }
+ }
+ catch (const armem::error::ArMemError& e)
+ {
+ ARMARX_WARNING_S << e.what();
+ }
+ return false;
+ };
+
+ visitor.applyTo(*coreSegment);
+ return scene;
+ }
+
+
+ void Segment::commitSceneSnapshot(const armem::obj::SceneSnapshot& scene, const std::string& sceneName)
+ {
+ const Time now = TimeUtil::GetTime();
+ std::map<ObjectID, int> idCounters;
+
+ objpose::ObjectPoseSeq objectPoses;
+
+ for (const auto& object : scene.objects)
+ {
+ const ObjectID classID = object.getClassID();
+
+ objpose::ObjectPose& pose = objectPoses.emplace_back();
+
+ pose.providerName = sceneName;
+ pose.objectType = objpose::ObjectTypeEnum::KnownObject;
+ pose.objectID = classID.withInstanceName(std::to_string(idCounters[classID]++));
+
+ pose.objectPoseGlobal = simox::math::pose(object.position, object.orientation);
+ pose.objectPoseRobot = pose.objectPoseGlobal;
+ pose.objectPoseOriginal = pose.objectPoseGlobal;
+ pose.objectPoseOriginalFrame = armarx::GlobalFrame;
+
+ pose.robotConfig = {};
+ pose.robotPose = Eigen::Matrix4f::Identity();
+
+ pose.confidence = 1.0;
+ pose.localOOBB = getObjectOOBB(classID);
+ pose.timestamp = now;
+ }
+
+ commitObjectPoses(objectPoses);
+ }
+
+
+ void Segment::commitSceneSnapshotFromFilename(const std::string& filename, bool lockMemory)
+ {
+ ARMARX_INFO << "Loading scene snapshot '" << filename << "' ...";
+ if (auto path = resolveSceneFilename(filename))
+ {
+ if (const auto snapshot = loadScene(path.value()))
+ {
+ std::filesystem::path filename = path->filename();
+ filename.replace_extension(); // Removes extension
+
+ if (lockMemory)
+ {
+ std::scoped_lock lock(memoryMutex);
+ commitSceneSnapshot(snapshot.value(), filename.string());
+ }
+ else
+ {
+ commitSceneSnapshot(snapshot.value(), filename.string());
+ }
+ }
+ }
+ }
+
void Segment::RemoteGui::setup(const Segment& data)
{
@@ -677,8 +901,21 @@ namespace armarx::armem::server::obj::instance
infiniteHistory.setValue(data.p.maxHistorySize == -1);
discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
+ storeLoadLine.setValue("Scene_" + timestampPlaceholder);
+ loadButton.setLabel("Load Scene");
+ storeButton.setLabel("Store Scene");
+
+ HBoxLayout storeLoadLineLayout({Label(data.p.sceneSnapshotsPackage + "/Scenes/"), storeLoadLine, Label(".json")});
+ HBoxLayout storeLoadButtonsLayout({loadButton, storeButton});
+
GridLayout grid;
int row = 0;
+
+ grid.add(storeLoadLineLayout, {row, 0}, {1, 2});
+ row++;
+ grid.add(storeLoadButtonsLayout, {row, 0}, {1, 2});
+ row++;
+
grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
row++;
grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
@@ -690,8 +927,25 @@ namespace armarx::armem::server::obj::instance
group.addChild(grid);
}
+
void Segment::RemoteGui::update(Segment& data)
{
+ if (loadButton.wasClicked())
+ {
+ bool lockMemory = true;
+ data.commitSceneSnapshotFromFilename(storeLoadLine.getValue(), lockMemory);
+ }
+
+ if (storeButton.wasClicked())
+ {
+ armem::obj::SceneSnapshot scene;
+ {
+ std::scoped_lock lock(data.memoryMutex);
+ scene = data.getSceneSnapshot();
+ }
+ data.storeScene(storeLoadLine.getValue(), scene);
+ }
+
if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
|| discardSnapshotsWhileAttached.hasValueChanged())
{
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index 5b7c268bb28530b82906bc07cee31431d6738f5c..0ac63b410d8d5593ac3c6b9be1fb9ea1cc58eb4d 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -1,8 +1,9 @@
#pragma once
+#include <filesystem>
#include <map>
-#include <string>
#include <optional>
+#include <string>
#include <SimoxUtility/caching/CacheMap.h>
#include <SimoxUtility/shapes/OrientedBox.h>
@@ -23,6 +24,11 @@
#include "Decay.h"
+namespace armarx::armem::obj
+{
+ struct SceneSnapshot;
+}
+
namespace armarx::armem::server::obj::instance
{
@@ -53,7 +59,7 @@ namespace armarx::armem::server::obj::instance
const std::string& providerName,
const objpose::data::ProvidedObjectPoseSeq& providedPoses,
std::optional<Time> discardUpdatesUntil = std::nullopt);
- void commitObjectPoses(const std::string& providerName, const ObjectPoseSeq& objectPoses);
+ void commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName = "");
wm::CoreSegment& getCoreSegment();
@@ -129,12 +135,24 @@ namespace armarx::armem::server::obj::instance
bool commitAttachedPose);
- std::optional<wm::EntityInstance> findClassInstance(const ObjectID& objectID);
+ std::optional<wm::EntityInstance> findClassInstance(const ObjectID& objectID) const;
friend struct DetachVisitor;
+ private:
+
+ void storeScene(const std::string& filename, const armem::obj::SceneSnapshot& scene);
+ std::optional<armem::obj::SceneSnapshot> loadScene(const std::string& filename);
+ std::optional<armem::obj::SceneSnapshot> loadScene(const std::filesystem::path& path);
+ std::optional<std::filesystem::path> resolveSceneFilename(const std::string& filename);
+
+ armem::obj::SceneSnapshot getSceneSnapshot() const;
+ void commitSceneSnapshot(const armem::obj::SceneSnapshot& scene, const std::string& sceneName);
+ void commitSceneSnapshotFromFilename(const std::string& filename, bool lockMemory);
+
+
public:
RobotStateComponentInterfacePrx robotStateComponent;
@@ -162,6 +180,10 @@ namespace armarx::armem::server::obj::instance
// -1 would be infinite, but this can let the RAM grow quickly.
long maxHistorySize = 25;
bool discardSnapshotsWhileAttached = true;
+
+ /// Package containing the scene snapshots
+ std::string sceneSnapshotsPackage = "ArmarXObjects";
+ std::string sceneSnapshotToLoad = "";
};
Properties p;
@@ -172,6 +194,9 @@ namespace armarx::armem::server::obj::instance
/// Class name -> dataset name.
simox::caching::CacheMap<std::string, std::string> classNameToDatasetCache;
+ std::unique_ptr<CMakePackageFinder> finder;
+
+ static const std::string timestampPlaceholder;
public:
@@ -179,6 +204,10 @@ namespace armarx::armem::server::obj::instance
{
armarx::RemoteGui::Client::GroupBox group;
+ armarx::RemoteGui::Client::LineEdit storeLoadLine;
+ armarx::RemoteGui::Client::Button storeButton;
+ armarx::RemoteGui::Client::Button loadButton;
+
armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
armarx::RemoteGui::Client::CheckBox infiniteHistory;
armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index 3773c087bf70fa68981be86696b01c6fae7645d6..46a7a4fba84f5c81799d6e7330359b4e2ad7a52f 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -484,9 +484,9 @@ namespace armarx::armem::server::obj::instance
std::scoped_lock lock(adapter.visuMutex);
this->visu.update(adapter.visu);
}
+ this->segment.update(adapter.segment);
{
std::scoped_lock lock(adapter.memoryMutex);
- this->segment.update(adapter.segment);
this->decay.update(adapter.segment.decay);
}
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index c4dc03c66eadfa840923bf32c5fc03533749f55b..5a299161453d2480fb82601d74e563e5227f8a10 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -18,7 +18,7 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
index 3137127aa112528fdf39351b9bfc52e4c4ece3b8..a9975f431c0a22b2a3367f42c7311d7f894f0b2d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -26,7 +26,7 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
index 5ec63c12d9c3f6485839ff138b1ebe409dd1adc8..7f95e43878aee62619f8c4997db25460e11ed8cd 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -10,7 +10,7 @@
#include "RobotAPI/libraries/aron/common/aron_conversions.h"
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>