ArticulatedObjectLocalizer now working properly

scenarios/ArMemObjectMemory/ArMemObjectMemory.scx

@@ -9,5 +9,6 @@
 	<application name="RobotStateComponent" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
 	<application name="RobotToArVizApp" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
 	<application name="ObjectPoseClientExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+	<application name="ArticulatedObjectLocalizerExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
 </scenario>
 

scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg

+# ==================================================================
+# ArticulatedObjectLocalizerExample properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages:  List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+#  Attributes:
+#  - Default:            Default value not mapped.
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName:  Application name
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.EnableProfiling:  enable profiler which is used for logging performance events
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ArticulatedObjectLocalizerExample.EnableProfiling = false
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.MinimumLoggingLevel:  Local logging level only for this component
+#  Attributes:
+#  - Default:            Undefined
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.ArticulatedObjectLocalizerExample.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.ObjectName:  Name of IceGrid well-known object
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ArticulatedObjectLocalizerExample.ObjectName = ""
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment:  Name of the memory core segment to use for object classes.
+#  Attributes:
+#  - Default:            ArticulatedObjectClass
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment = ArticulatedObjectClass
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.MemoryName:  
+#  Attributes:
+#  - Default:            Object
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.MemoryName = Object
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName:  
+#  Attributes:
+#  - Default:            ArmarXObjects
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName = ArmarXObjects
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemEnabled:  Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+#  Attributes:
+#  - Default:            true
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemName:  Name of the Memory Name System (MNS) component.
+#  Attributes:
+#  - Default:            MemoryNameSystem
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.pub.DebugObserver:  Name of the `DebugObserver` topic to publish data to.
+#  Attributes:
+#  - Default:            DebugObserver
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.pub.DebugObserver = DebugObserver
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.sub.MemoryListener:  Name of the `MemoryListener` topic to subscribe to.
+#  Attributes:
+#  - Default:            MemoryUpdates
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.sub.MemoryListener = MemoryUpdates
+
+
+# ArmarX.CachePath:  Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+#  Attributes:
+#  - Default:            mongo/.cache
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config:  Comma-separated list of configuration files 
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath:  Semicolon-separated search list for data files
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages:  List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+#  Attributes:
+#  - Default:            Default value not mapped.
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig:  Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+#  Attributes:
+#  - Default:            ./config/dependencies.cfg
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging:  Turn logging off in whole application
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling:  Enable profiling of CPU load produced by this application
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries:  Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup:  The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout:  Redirect std::cout and std::cerr to ArmarXLog
+#  Attributes:
+#  - Default:            true
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout:  The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+#  Attributes:
+#  - Default:            3000
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay:  The startup will be delayed by this number of seconds (useful for debugging)
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash:  If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize:  Size of the ArmarX ThreadPool that is always running.
+#  Attributes:
+#  - Default:            1
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix:  Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer:  Enable using a global Timeserver (e.g. from ArmarXSimulator)
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity:  Global logging level for whole application
+#  Attributes:
+#  - Default:            Info
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+

scenarios/ArMemObjectMemory/config/ObjectMemory.cfg

@@ -7,7 +7,7 @@
 #  - Default:            Default value not mapped.
 #  - Case sensitivity:   yes
 #  - Required:           no
-# ArmarX.AdditionalPackages = Default value not mapped.
+ArmarX.AdditionalPackages = ArmarXObjects
 
 
 # ArmarX.ApplicationName:  Application name
@@ -174,14 +174,13 @@
 # ArmarX.ObjectMemory.mem.articulated.cls.CoreSegmentName = ArticulatedObjectClass
 
 
-# ArmarX.ObjectMemory.mem.articulated.cls.DiscardSnapshotsWhileAttached:  If true, no new snapshots are stored while an object is attached to a robot node.
-# If false, new snapshots are stored, but the attachment is kept in the new snapshots.
+# ArmarX.ObjectMemory.mem.articulated.cls.LoadFromObjectsPackage:  If true, load the objects from the objects package on startup.
 #  Attributes:
 #  - Default:            true
 #  - Case sensitivity:   yes
 #  - Required:           no
 #  - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.articulated.cls.DiscardSnapshotsWhileAttached = true
+# ArmarX.ObjectMemory.mem.articulated.cls.LoadFromObjectsPackage = true
 
 
 # ArmarX.ObjectMemory.mem.articulated.cls.MaxHistorySize:  Maximal size of object poses history (-1 for infinite).
@@ -192,22 +191,20 @@
 # ArmarX.ObjectMemory.mem.articulated.cls.MaxHistorySize = -1
 
 
-# ArmarX.ObjectMemory.mem.articulated.inst.CoreSegmentName:  Name of the object instance core segment.
+# ArmarX.ObjectMemory.mem.articulated.cls.ObjectsPackage:  Name of the objects package to load from.
 #  Attributes:
-#  - Default:            ArticulatedObjectInstance
+#  - Default:            ArmarXObjects
 #  - Case sensitivity:   yes
 #  - Required:           no
-# ArmarX.ObjectMemory.mem.articulated.inst.CoreSegmentName = ArticulatedObjectInstance
+# ArmarX.ObjectMemory.mem.articulated.cls.ObjectsPackage = ArmarXObjects
 
 
-# ArmarX.ObjectMemory.mem.articulated.inst.DiscardSnapshotsWhileAttached:  If true, no new snapshots are stored while an object is attached to a robot node.
-# If false, new snapshots are stored, but the attachment is kept in the new snapshots.
+# ArmarX.ObjectMemory.mem.articulated.inst.CoreSegmentName:  Name of the object instance core segment.
 #  Attributes:
-#  - Default:            true
+#  - Default:            ArticulatedObjectInstance
 #  - Case sensitivity:   yes
 #  - Required:           no
-#  - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.articulated.inst.DiscardSnapshotsWhileAttached = true
+# ArmarX.ObjectMemory.mem.articulated.inst.CoreSegmentName = ArticulatedObjectInstance
 
 
 # ArmarX.ObjectMemory.mem.articulated.inst.MaxHistorySize:  Maximal size of object poses history (-1 for infinite).
@@ -278,12 +275,12 @@
 # ArmarX.ObjectMemory.mem.cls.MaxHistorySize = -1
 
 
-# ArmarX.ObjectMemory.mem.cls.ObjectsPackgage:  Name of the objects package to load from.
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackage:  Name of the objects package to load from.
 #  Attributes:
 #  - Default:            ArmarXObjects
 #  - Case sensitivity:   yes
 #  - Required:           no
-# ArmarX.ObjectMemory.mem.cls.ObjectsPackgage = ArmarXObjects
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackage = ArmarXObjects
 
 
 # ArmarX.ObjectMemory.mem.inst.CoreSegmentName:  Name of the object instance core segment.

source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp

@@ -20,6 +20,8 @@
  */
 
 #include "ArticulatedObjectLocalizerExample.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "RobotAPI/libraries/armem_objects/types.h"
 
 #include <memory>
 
@@ -46,7 +48,8 @@
 namespace armarx::articulated_object
 {
     ArticulatedObjectLocalizerExample::ArticulatedObjectLocalizerExample() :
-        articulatedObjectWriter(new ::armarx::armem::articulated_object::Writer(*this)) {}
+        articulatedObjectWriter(new ::armarx::armem::articulated_object::Writer(*this)),
+        articulatedObjectReader(new ::armarx::armem::articulated_object::Reader(*this)) {}
 
     armarx::PropertyDefinitionsPtr ArticulatedObjectLocalizerExample::createPropertyDefinitions()
     {
@@ -58,6 +61,7 @@ namespace armarx::articulated_object
         // defs->optional(memoryName, "mem.MemoryName", "Name of the memory to use.");
 
         articulatedObjectWriter->registerPropertyDefinitions(defs);
+        articulatedObjectReader->registerPropertyDefinitions(defs);
 
         return defs;
     }
@@ -72,6 +76,7 @@ namespace armarx::articulated_object
     void ArticulatedObjectLocalizerExample::onConnectComponent()
     {
         articulatedObjectWriter->connect();
+        articulatedObjectReader->connect();
 
         task = new RunningTask<ArticulatedObjectLocalizerExample>(this, &ArticulatedObjectLocalizerExample::run);
         task->start();
@@ -84,17 +89,31 @@ namespace armarx::articulated_object
 
     void ArticulatedObjectLocalizerExample::onExitComponent() {}
 
-    VirtualRobot::RobotPtr createDishwasher()
+    VirtualRobot::RobotPtr ArticulatedObjectLocalizerExample::createDishwasher()
     {
-        const std::string xml =
-            "./ArmarXObjects/Environment/mobile-kitchen/dishwasher-only/dishwasher.xml";
+        const std::string dishwasherName = "CupboardWithDishwasher";
 
-        return VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(xml), VirtualRobot::RobotIO::eStructure);
+        const auto descriptions = articulatedObjectReader->queryDescriptions(IceUtil::Time::now());
+
+        ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
+
+        const auto it = std::find_if(descriptions.begin(), descriptions.end(), [&](const armem::articulated_object::ArticulatedObjectDescription & desc) -> bool
+        {
+            return desc.name == dishwasherName;
+        });
+
+        if (it == descriptions.end())
+        {
+            ARMARX_WARNING << "Articulated object " << dishwasherName << " not (yet) available";
+            return nullptr;
+        }
+
+        return VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(it->xml.serialize().path), VirtualRobot::RobotIO::eStructure);
     }
 
     armem::articulated_object::ArticulatedObject convert(const VirtualRobot::Robot& obj, const armem::Time& timestamp)
     {
-        ARMARX_INFO << "Filename is " << obj.getFilename();
+        ARMARX_DEBUG << "Filename is " << obj.getFilename();
 
         return
             armem::articulated_object::ArticulatedObject
@@ -117,7 +136,8 @@ namespace armarx::articulated_object
     {
         ARMARX_IMPORTANT << "Running example.";
 
-        std::shared_ptr<VirtualRobot::Robot> dishwasher = createDishwasher();
+        std::shared_ptr<VirtualRobot::Robot> dishwasher;
+
 
         CycleUtil cycle(IceUtil::Time::milliSeconds(100));
         IceUtil::Time start = TimeUtil::GetTime();
@@ -126,7 +146,19 @@ namespace armarx::articulated_object
 
         while (not task->isStopped())
         {
-            ARMARX_INFO << "Reporting articulated objects";
+
+            if (dishwasher == nullptr)
+            {
+                dishwasher = createDishwasher();
+            }
+
+            if (dishwasher == nullptr) // still
+            {
+                c.waitForCycleDuration();
+                continue;
+            }
+
+            ARMARX_DEBUG << "Reporting articulated objects";
 
             const IceUtil::Time now = TimeUtil::GetTime();
             const float t     = float((now - start).toSecondsDouble());

source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h

@@ -15,6 +15,8 @@
 #include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
 
 #include <RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h>
+#include <VirtualRobot/VirtualRobot.h>
 
 namespace armarx::articulated_object
 {
@@ -54,11 +56,15 @@ namespace armarx::articulated_object
 
     private:
 
+        VirtualRobot::RobotPtr createDishwasher();
+
+
         armarx::RunningTask<ArticulatedObjectLocalizerExample>::pointer_type task;
 
         armarx::DebugObserverInterfacePrx debugObserver;
 
         std::unique_ptr<::armarx::armem::articulated_object::Writer> articulatedObjectWriter;
+        std::unique_ptr<::armarx::armem::articulated_object::Reader> articulatedObjectReader;
 
     };
 

source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp

@@ -8,6 +8,7 @@
 
 #include "RobotAPI/libraries/armem/core/Time.h"
 #include "RobotAPI/libraries/armem/client/query/Builder.h"
+#include "RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h"
 #include "RobotAPI/libraries/armem_robot/robot_conversions.h"
 #include "RobotAPI/libraries/armem_robot/aron_conversions.h"
 #include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
@@ -20,6 +21,82 @@ namespace armarx::armem::articulated_object
 
     Reader::Reader(armem::ClientReaderComponentPluginUser& component) : component(component) {}
 
+    void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+    {
+        ARMARX_DEBUG << "Reader: registerPropertyDefinitions";
+
+        const std::string prefix = propertyPrefix;
+
+        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->optional(properties.providerName, prefix + "ProviderName");
+    }
+
+
+    void Reader::connect()
+    {
+        // Wait for the memory to become available and add it as dependency.
+        ARMARX_IMPORTANT << "Reader: Waiting for memory '" << properties.memoryName << "' ...";
+        auto result = component.useMemory(properties.memoryName);
+        if (not result.success)
+        {
+            ARMARX_ERROR << result.errorMessage;
+            return;
+        }
+
+        ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName;
+
+        memoryReader.setReadingMemory(result.proxy);
+
+        armem::MemoryID id = armem::MemoryID();
+        id.memoryName = properties.memoryName;
+        id.coreSegmentName = properties.coreClassSegmentName;
+        // listen to all provider segments!
+
+        memoryReader.subscribe(id, this, &Reader::updateKnownObjects);
+    }
+
+    void Reader::updateKnownObject(const armem::MemoryID& snapshotId)
+    {
+        // const std::string& nameWithDataset = snapshotId.providerSegmentName;
+
+        // arondto::RobotDescription aronArticulatedObjectDescription;
+        // aronArticulatedObjectDescription.fromAron(snapshotId.);
+
+        // TODO(fabian.reister): implement
+    }
+
+    void Reader::updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
+    {
+        ARMARX_INFO << "New objects available!";
+
+        // // Query all entities from provider.
+        // armem::client::query::Builder qb;
+
+        // // clang-format off
+        // qb
+        // .coreSegments().withName(properties.coreClassSegmentName)
+        // .providerSegments().all() // TODO(fabian.reister): think about this: which authority is trustworthy?
+        // .entities().withName(name)
+        // .snapshots().atTime(timestamp);
+        // // clang-format on
+
+        // const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+
+        // std::for_each(snapshotIDs.begin(), snapshotIDs.end(), [&](const auto & snapshotID)
+        // {
+        //     updateKnownObject(snapshotID);
+        // });
+    }
+
+
     std::optional<ArticulatedObject> Reader::get(const std::string& name, const armem::Time& timestamp)
     {
         const auto description = queryDescription(name, timestamp);
@@ -63,6 +140,31 @@ namespace armarx::armem::articulated_object
         obj.config = std::move(*state);
     }
 
+    std::vector<robot::RobotDescription> Reader::queryDescriptions(const armem::Time& timestamp)
+    {
+        // Query all entities from provider.
+        armem::client::query::Builder qb;
+
+        // clang-format off
+        qb
+        .coreSegments().withName(properties.coreClassSegmentName)
+        .providerSegments().all()
+        .entities().all()
+        .snapshots().latest(); // TODO beforeTime(timestamp);
+        // clang-format on
+
+        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+        if (not qResult.success) /* c++20 [[unlikely]] */
+        {
+            return {};
+        }
+
+        return getRobotDescriptions(qResult.memory);
+    }
+
     std::optional<robot::RobotDescription> Reader::queryDescription(const std::string& name, const armem::Time& timestamp)
     {
         // Query all entities from provider.
@@ -176,4 +278,36 @@ namespace armarx::armem::articulated_object
         return robot::convertRobotDescription(instance);
     }
 
+    std::vector<robot::RobotDescription> Reader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
+    {
+        std::vector<robot::RobotDescription> descriptions;
+
+        const armem::wm::CoreSegment& coreSegment = memory.getCoreSegment(properties.coreClassSegmentName);
+
+        for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
+        {
+            for (const auto& [name, entity] : providerSegment.entities())
+            {
+                if (entity.empty())
+                {
+                    ARMARX_WARNING << "No entity found";
+                    continue;
+                }
+
+                const auto entitySnapshots = simox::alg::get_values(entity.history());
+                const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+
+                const auto robotDescription = robot::convertRobotDescription(instance);
+
+                if (robotDescription)
+                {
+                    descriptions.push_back(*robotDescription);
+                }
+            }
+        }
+
+        return descriptions;
+    }
+
+
 } // namespace armarx::armem::articulated_object
\ No newline at end of file

source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h

@@ -33,12 +33,14 @@ namespace armarx::armem::articulated_object
 {
     class Reader:
         virtual public ReaderInterface
-    // virtual public ::armarx::armem::MemoryConnector
     {
     public:
         Reader(armem::ClientReaderComponentPluginUser& component);
         virtual ~Reader() = default;
 
+        void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+        void connect();
+
         void synchronize(ArticulatedObject& obj, const armem::Time& timestamp) override;
 
         std::optional<ArticulatedObject> get(const std::string& name, const armem::Time& timestamp) override;
@@ -47,21 +49,25 @@ namespace armarx::armem::articulated_object
         std::optional<robot::RobotState> queryState(const robot::RobotDescription& description, const armem::Time& timestamp);
         std::optional<robot::RobotDescription> queryDescription(const std::string& name, const armem::Time& timestamp);
 
+        std::vector<robot::RobotDescription> queryDescriptions(const armem::Time& timestamp);
 
         // TODO(fabian.reister): register property defs
 
     protected:
         std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory) const;
         std::optional<robot::RobotDescription> getRobotDescription(const armarx::armem::wm::Memory& memory) const;
+        std::vector<robot::RobotDescription> getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
 
     private:
+        void updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs);
+        void updateKnownObject(const armem::MemoryID& snapshotId);
 
         struct Properties
         {
             std::string memoryName               = "Object";
             std::string coreInstanceSegmentName  = "ArticulatedObjectInstance";
             std::string coreClassSegmentName     = "ArticulatedObjectClass";
-            std::string providerName;
+            std::string providerName             = "ArmarXObjects";
         } properties;
 
         const std::string propertyPrefix = "mem.obj.articulated.";

source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp

 #include "Writer.h"
 
+#include <IceUtil/Time.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
 #include <mutex>
 #include <optional>
 
@@ -11,6 +13,7 @@
 #include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
 #include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
 #include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
 
 
 namespace armarx::armem::articulated_object
@@ -21,8 +24,6 @@ namespace armarx::armem::articulated_object
     {
         ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
 
-        // MemoryConnector::registerPropertyDefinitions(def);
-
         const std::string prefix = propertyPrefix;
 
         def->optional(properties.memoryName, prefix + "MemoryName");
@@ -33,7 +34,7 @@ namespace armarx::armem::articulated_object
         def->optional(properties.coreClassSegmentName,
                       prefix + "CoreSegment",
                       "Name of the memory core segment to use for object classes.");
-        def->required(properties.providerName, prefix + "ProviderName");
+        def->optional(properties.providerName, prefix + "ProviderName");
     }
 
     void Writer::connect()
@@ -62,6 +63,7 @@ namespace armarx::armem::articulated_object
         armem::MemoryID id;
         id.setCoreSegmentID(refId); // listen to all provider segments!
 
+        updateKnownObjects();
         memoryReader.subscribe(id, this, &Writer::updateKnownObjects);
     }
 
@@ -77,11 +79,12 @@ namespace armarx::armem::articulated_object
     void Writer::updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
     {
         ARMARX_INFO << "New objects available!";
+        updateKnownObjects();
+    }
 
-        std::for_each(snapshotIDs.begin(), snapshotIDs.end(), [&](const auto & snapshotID)
-        {
-            updateKnownObject(snapshotID);
-        });
+    void Writer::updateKnownObjects()
+    {
+        knownObjects = queryDescriptions(IceUtil::Time::now());
     }
 
     std::optional<armem::MemoryID> Writer::storeOrGetClass(const ArticulatedObject& obj)
@@ -95,7 +98,12 @@ namespace armarx::armem::articulated_object
         }
 
         // otherwise create
-        return storeClass(obj);
+        if (properties.allowClassCreation)
+        {
+            return storeClass(obj);
+        }
+
+        return std::nullopt;
     }
 
     std::optional<armem::MemoryID> Writer::storeClass(const ArticulatedObject& obj)
@@ -180,7 +188,7 @@ namespace armarx::armem::articulated_object
 
         if (not descriptionId)
         {
-            ARMARX_ERROR << "Could not get class for object " << obj.description.name;
+            ARMARX_WARNING << "Could not get class for object " << obj.description.name;
             return false;
         }
 
@@ -197,7 +205,7 @@ namespace armarx::armem::articulated_object
 
         if (not updateResult.success)
         {
-            ARMARX_ERROR << updateResult.errorMessage;
+            ARMARX_WARNING << updateResult.errorMessage;
         }
 
         return updateResult.success;
@@ -209,7 +217,8 @@ namespace armarx::armem::articulated_object
 
         if (not classId)
         {
-            ARMARX_WARNING << "Could not get class id!";
+            ARMARX_WARNING << "Could not get class id for object " << obj.description.name << "! "
+                           << "Known classes are " << simox::alg::get_keys(knownObjects);
             return false;
         }
 
@@ -222,4 +231,96 @@ namespace armarx::armem::articulated_object
     //     return propertyPrefix;
     // }
 
+
+    // TODO this is a duplicate
+    std::optional<robot::RobotDescription> Writer::getRobotDescription(const armarx::armem::wm::Memory& memory) const
+    {
+        // clang-format off
+        const armem::wm::ProviderSegment& providerSegment = memory
+                .getCoreSegment(properties.coreClassSegmentName)
+                .getProviderSegment(properties.providerName); // TODO(fabian.reister): all
+        // clang-format on
+        const auto entities = simox::alg::get_values(providerSegment.entities());
+
+        if (entities.empty())
+        {
+            ARMARX_WARNING << "No entity found";
+            return std::nullopt;
+        }
+
+        const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+
+        if (entitySnapshots.empty())
+        {
+            ARMARX_WARNING << "No entity snapshots found";
+            return std::nullopt;
+        }
+
+        // TODO(fabian.reister): check if 0 available
+        const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+
+        return robot::convertRobotDescription(instance);
+    }
+
+    std::unordered_map<std::string, armem::MemoryID>Writer::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
+    {
+        std::unordered_map<std::string, armem::MemoryID> descriptions;
+
+        const armem::wm::CoreSegment& coreSegment = memory.getCoreSegment(properties.coreClassSegmentName);
+
+        for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
+        {
+            for (const auto& [name, entity] : providerSegment.entities())
+            {
+                if (entity.empty())
+                {
+                    ARMARX_WARNING << "No entity found";
+                    continue;
+                }
+
+                const auto entitySnapshots = simox::alg::get_values(entity.history());
+                const armem::wm::EntitySnapshot& sn = entitySnapshots.front();
+                const armem::wm::EntityInstance& instance = sn.getInstance(0);
+
+                const auto robotDescription = robot::convertRobotDescription(instance);
+
+                if (robotDescription)
+                {
+                    const armem::MemoryID snapshotID(sn.id());
+                    descriptions.insert({robotDescription->name, snapshotID});
+                }
+            }
+
+        }
+
+        return descriptions;
+    }
+
+
+    std::unordered_map<std::string, armem::MemoryID> Writer::queryDescriptions(const armem::Time& timestamp)
+    {
+        // Query all entities from provider.
+        armem::client::query::Builder qb;
+
+        // clang-format off
+        qb
+        .coreSegments().withName(properties.coreClassSegmentName)
+        .providerSegments().all()
+        .entities().all()
+        .snapshots().latest(); // TODO beforeTime(timestamp);
+        // clang-format on
+
+        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+        if (not qResult.success) /* c++20 [[unlikely]] */
+        {
+            return {};
+        }
+
+        return getRobotDescriptions(qResult.memory);
+    }
+
+
 } // namespace armarx::armem::articulated_object
\ No newline at end of file

source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h

@@ -56,14 +56,23 @@ namespace armarx::armem::articulated_object
         std::optional<armem::MemoryID> storeOrGetClass(const ArticulatedObject& obj);
 
         void updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs);
+        void updateKnownObjects();
         void updateKnownObject(const armem::MemoryID& snapshotId);
 
+        // TODO duplicate
+        std::unordered_map<std::string, armem::MemoryID> queryDescriptions(const armem::Time& timestamp);
+        std::optional<robot::RobotDescription> getRobotDescription(const armarx::armem::wm::Memory& memory) const;
+        std::unordered_map<std::string, armem::MemoryID> getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
+
+
         struct Properties
         {
             std::string memoryName              = "Object";
             std::string coreInstanceSegmentName = "ArticulatedObjectInstance";
             std::string coreClassSegmentName    = "ArticulatedObjectClass";
-            std::string providerName;
+            std::string providerName            = "ArmarXObjects";
+
+            bool allowClassCreation             = false;
         } properties;
 
         const std::string propertyPrefix = "mem.obj.articulated.";
@@ -75,7 +84,7 @@ namespace armarx::armem::articulated_object
         std::mutex memoryReaderMutex;
 
         // key: name of object: RobotDescription::name
-        std::map<std::string, MemoryID> knownObjects;
+        std::unordered_map<std::string, MemoryID> knownObjects;
 
         armem::ClientComponentPluginUser& component;
     };

source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp

@@ -93,7 +93,7 @@ namespace armarx::armem::server::obj::articulated_object_class
 
                 arondto::RobotDescription aronRobotDescription;
                 toAron(aronRobotDescription, desc);
-                aronRobotDescription.timestamp = now;
+                // TODO toAron(aronRobotDescription.timestamp, now);
 
                 update.instancesData = { aronRobotDescription.toAron()};
             }