diff --git a/scenarios/ArMemObjectMemory/config/ArticulatedObjectExampleMemoryWriterClient.cfg b/scenarios/ArMemObjectMemory/config/ArticulatedObjectExampleMemoryWriterClient.cfg
index 340a7f1e6fafa4118860e3d6a8ace505b9b9bf1d..bbfc7a0ca3a8eddd35f8c447efb327bbbe260c48 100644
--- a/scenarios/ArMemObjectMemory/config/ArticulatedObjectExampleMemoryWriterClient.cfg
+++ b/scenarios/ArMemObjectMemory/config/ArticulatedObjectExampleMemoryWriterClient.cfg
@@ -44,35 +44,35 @@
# ArmarX.ArticulatedObjectExampleMemoryWriterClient.ObjectName = ""
-# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ArMemMemoryNameSystem: Ice object name of the `MemoryNameSystem` component.
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.ArMemMemoryNameSystem: Ice object name of the `MemoryNameSystem` component.
# Attributes:
# - Default: ArMemMemoryNameSystem
# - Case sensitivity: yes
# - Required: no
-ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ArMemMemoryNameSystem = MemoryNameSystem
+ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.ArMemMemoryNameSystem = MemoryNameSystem
-# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.CoreSegment: Name of the memory core segment to use.
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.CoreSegment: Name of the memory core segment to use for object classes.
# Attributes:
-# - Default: ArticulatedObjectInstance
+# - Default: ArticulatedObjectClass
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.CoreSegment = ArticulatedObjectInstance
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.CoreSegment = ArticulatedObjectClass
-# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.MemoryName:
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.MemoryName:
# Attributes:
-# - Default: ObjectMemory
+# - Default: Object
# - Case sensitivity: yes
# - Required: no
-ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.MemoryName = Object
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.MemoryName = Object
-# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ProviderName:
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.ProviderName:
# Attributes:
# - Case sensitivity: yes
# - Required: yes
-ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ProviderName = ExampleProvider
+ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.ProviderName = ExampleProvider
# ArmarX.ArticulatedObjectExampleMemoryWriterClient.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
@@ -225,3 +225,18 @@ ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.Prov
# ArmarX.Verbosity = Info
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ArMemMemoryNameSystem:
+# Attributes:
+ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ArMemMemoryNameSystem = MemoryNameSystem
+
+
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.MemoryName:
+# Attributes:
+ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.MemoryName = Object
+
+
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ProviderName:
+# Attributes:
+ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ProviderName = ExampleProvider
+
+
diff --git a/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.cpp b/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.cpp
index 553a24c85e79c7353c09893df04212e17c2c1fb8..4d45a76b229a11ac6570fb519ff3b2eb0cadd048 100644
--- a/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.cpp
+++ b/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.cpp
@@ -69,7 +69,6 @@ namespace armarx::articulated_object
{
const std::string xml =
"./ArmarXObjects/Environment/mobile-kitchen/dishwasher-only/dishwasher.xml";
- const std::string name = "dishwasher";
return VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(xml), VirtualRobot::RobotIO::eStructure);
}
@@ -85,6 +84,7 @@ namespace armarx::articulated_object
.name = obj.getName(),
.xml = PackagePath(armarx::ArmarXDataPath::getProject({"ArmarXObjects"}, obj.getFilename()), obj.getFilename())
},
+ .instance = "", // TODO(fabian.reister):
.config = {
.timestamp = timestamp,
.globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
index ed69e4c80c5bc975c1acd46ffb7b57e4418915d7..e1f73815a610f18139205f8a1a0c776a3ef797f7 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -21,6 +21,7 @@ namespace armarx::armem::articulated_object
ArticulatedObject obj
{
.description = description,
+ .instance = "", // TODO(fabian.reister):
.config = {}, // will be populated by synchronize
.timestamp = timestamp
};
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
index 3fe33b6c6bc69f61a4721a014cb2618a5f59d905..170afc2113293244c8dcc90245227db8f3744549 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -30,12 +30,12 @@ namespace armarx::armem::articulated_object
struct Properties
{
- std::string memoryName = "ObjectMemory";
+ std::string memoryName = "Object";
std::string coreSegmentName = "ArticulatedObjectInstance";
std::string providerName;
} properties;
- const std::string propertyPrefix = "mem.obj.articulated.read.";
+ const std::string propertyPrefix = "mem.obj.articulated.";
armem::client::Reader memoryReader;
std::mutex memoryWriterMutex;
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
index ea8545699d4dbc85a3a135ca3efb269d2a0f755c..cffcbdbb32fb0ad70d983174596de9fa799ccd5c 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -1,9 +1,13 @@
#include "Writer.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
+#include <RobotAPI/libraries/armem_objects/aron/RobotDescription.aron.generated.h>
#include <mutex>
#include <RobotAPI/libraries/armem_objects/aron/Robot.aron.generated.h>
+#include <optional>
namespace armarx::armem::articulated_object
@@ -18,12 +22,14 @@ namespace armarx::armem::articulated_object
const std::string prefix = getPropertyPrefix();
- def->optional(properties.coreSegmentName,
- prefix + "CoreSegment",
- "Name of the memory core segment to use.");
-
def->optional(properties.memoryName, prefix + "MemoryName");
+ def->optional(properties.coreInstanceSegmentName,
+ prefix + "CoreSegment",
+ "Name of the memory core segment to use for object instances.");
+ def->optional(properties.coreClassSegmentName,
+ prefix + "CoreSegment",
+ "Name of the memory core segment to use for object classes.");
def->required(properties.providerName, prefix + "ProviderName");
}
@@ -41,16 +47,111 @@ namespace armarx::armem::articulated_object
ARMARX_IMPORTANT << "Writer: Connected to memory '" << properties.memoryName;
memoryWriter.setWritingMemory(result.proxy);
+ memoryReader.setReadingMemory(result.proxy);
+
+ const auto resultCoreClassSegment = memoryWriter.addSegment(properties.coreClassSegmentName, properties.providerName);
+
+ const auto resultCoreInstanceSegmentName =
+ memoryWriter.addSegment(properties.coreInstanceSegmentName, properties.providerName);
+
+ armem::MemoryID refId = armem::MemoryID(resultCoreClassSegment.segmentID);
+
+ armem::MemoryID id;
+ id.setCoreSegmentID(refId); // listen to all provider segments!
+
+ memoryReader.subscribe(id, this, &Writer::updateKnownObjects);
}
- bool Writer::store(const ArticulatedObject& obj)
+ void Writer::updateKnownObject(const armem::MemoryID& snapshotId)
+ {
+
+ arondto::RobotDescription aronArticulatedObjectDescription;
+ // aronArticulatedObjectDescription.fromAron(snapshotId.ent);
+
+ // TODO(fabian.reister): implement
+ }
+
+ void Writer::updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
+ {
+ ARMARX_INFO << "New objects available!";
+
+ std::for_each(snapshotIDs.begin(), snapshotIDs.end(), [&](const auto & snapshotID)
+ {
+ updateKnownObject(snapshotID);
+ });
+ }
+
+ std::optional<armem::MemoryID> Writer::storeOrGetClass(const ArticulatedObject& obj)
+ {
+ const auto objectId = knownObjects.find(obj.description.name);
+
+ // check if exists
+ if (objectId != knownObjects.end())
+ {
+ return objectId->second;
+ }
+
+ // otherwise create
+ return storeClass(obj);
+ }
+
+ std::optional<armem::MemoryID> Writer::storeClass(const ArticulatedObject& obj)
+ {
+ std::lock_guard g{memoryWriterMutex};
+
+ ARMARX_DEBUG << "Trying to create core segment + provider segment";
+
+ // TODO(fabian.reister): variable provider segment
+ const auto result = memoryWriter.addSegment(properties.coreClassSegmentName, properties.providerName);
+
+ if (not result.success)
+ {
+ ARMARX_ERROR << "Creating core segment failed. Reason: " << result.errorMessage;
+ return std::nullopt;
+ }
+
+ const auto& timestamp = obj.timestamp;
+
+ const auto providerId = armem::MemoryID(result.segmentID);
+ const auto entityID =
+ providerId
+ .withEntityName(obj.description.name)
+ .withTimestamp(timestamp);
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+ arondto::RobotDescription aronArticulatedObjectDescription;
+ toAron(aronArticulatedObjectDescription, obj.description);
+
+ update.instancesData = {aronArticulatedObjectDescription.toAron()};
+ update.timeCreated = timestamp;
+
+ ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
+ armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
+
+ ARMARX_DEBUG << updateResult;
+
+ if (not updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ return std::nullopt;
+ }
+
+ // update cache (TODO: likely remove this)
+ knownObjects[obj.description.name] = updateResult.snapshotID;
+
+ return updateResult.snapshotID;
+ }
+
+ bool Writer::storeInstance(const ArticulatedObject& obj)
{
std::lock_guard g{memoryWriterMutex};
ARMARX_DEBUG << "Trying to create core segment + provider segment";
const auto result =
- memoryWriter.addSegment(properties.coreSegmentName, properties.providerName);
+ memoryWriter.addSegment(properties.coreInstanceSegmentName, properties.providerName);
if (not result.success)
{
@@ -88,6 +189,20 @@ namespace armarx::armem::articulated_object
return updateResult.success;
}
+ bool Writer::store(const ArticulatedObject& obj)
+ {
+ const std::optional<armem::MemoryID> classId = storeOrGetClass(obj);
+
+ if (not classId)
+ {
+ ARMARX_WARNING << "Could not get class id!";
+ return false;
+ }
+
+ // TODO(fabian.reister): integrate memory link
+ return storeInstance(obj);
+ }
+
const std::string& Writer::getPropertyPrefix() const
{
return propertyPrefix;
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
index e396b1c4c9090e2cf2223d219b2be0fac0a26d3d..74ec9a1de548eba30b470271080c0721eb43fb7d 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
@@ -3,6 +3,7 @@
#include <mutex>
#include "RobotAPI/libraries/armem/client/MemoryConnector.h"
+#include "RobotAPI/libraries/armem/client/Reader.h"
#include "RobotAPI/libraries/armem/client/Writer.h"
#include "interfaces.h"
@@ -19,28 +20,42 @@ namespace armarx::armem::articulated_object
Writer(ManagedIceObject& component);
virtual ~Writer() = default;
- bool store(const ArticulatedObject& obj) override;
-
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
void connect();
+
+ bool store(const ArticulatedObject& obj) override;
+
+ bool storeInstance(const ArticulatedObject& obj);
+ std::optional<armem::MemoryID> storeClass(const ArticulatedObject& obj);
+
const std::string& getPropertyPrefix() const override;
private:
+ std::optional<armem::MemoryID> storeOrGetClass(const ArticulatedObject& obj);
+
+ void updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs);
+ void updateKnownObject(const armem::MemoryID& snapshotId);
struct Properties
{
- std::string memoryName = "ObjectMemory";
- std::string coreSegmentName = "ArticulatedObjectInstance";
+ std::string memoryName = "Object";
+ std::string coreInstanceSegmentName = "ArticulatedObjectInstance";
+ std::string coreClassSegmentName = "ArticulatedObjectClass";
std::string providerName;
} properties;
- const std::string propertyPrefix = "mem.obj.articulated.write.";
+ const std::string propertyPrefix = "mem.obj.articulated.";
armem::client::Writer memoryWriter;
std::mutex memoryWriterMutex;
+ armem::client::Reader memoryReader;
+ std::mutex memoryReaderMutex;
+
+ // key: name of object: RobotDescription::name
+ std::map<std::string, MemoryID> knownObjects;
};
diff --git a/source/RobotAPI/libraries/armem_objects/types.h b/source/RobotAPI/libraries/armem_objects/types.h
index 0db042762ef9e95da2508be38da71688a722a49f..a127850fe89fde6a635e360aba51e4a13f7d95b7 100644
--- a/source/RobotAPI/libraries/armem_objects/types.h
+++ b/source/RobotAPI/libraries/armem_objects/types.h
@@ -20,6 +20,7 @@ namespace armarx::armem
std::string name;
PackagePath xml;
+
};
@@ -34,6 +35,8 @@ namespace armarx::armem
struct Robot
{
RobotDescription description;
+ std::string instance;
+
RobotState config;
IceUtil::Time timestamp;