Merge branch '160-articulated_object-reader-fixing-faulty-memory-subscription' into 'master'

Resolve "articulated_object reader: fixing faulty memory subscription"

Closes #160

See merge request !518

source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp

@@ -22,6 +22,7 @@
 #include "ArticulatedObjectLocalizerExample.h"
 
 #include <memory>
+#include <optional>
 
 #include <Eigen/Geometry>
 
@@ -38,6 +39,9 @@
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/services/tasks/PeriodicTask.h>
 #include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/time/Duration.h>
+#include <ArmarXCore/core/time/ScopedStopWatch.h>
+#include <ArmarXCore/core/time/StopWatch.h>
 
 #include <RobotAPI/libraries/armem/core/Time.h>
 #include <RobotAPI/libraries/armem_objects/types.h>
@@ -162,13 +166,32 @@ namespace armarx::articulated_object
             }
         }
 
-        const auto state = articulatedObjectReaderPlugin->get().queryState(
-            articulatedObject->getType() + "/" + articulatedObject->getName(),
-            Clock::Now(),
-            p.obj.readProviderName);
+        const armem::robot_state::RobotState state = [this]()
+        {
+            // Query state from memory
+            {
+                const std::optional<armem::robot_state::RobotState> state =
+                    articulatedObjectReaderPlugin->get().queryState(
+                        articulatedObject->getType() + "/" + articulatedObject->getName(),
+                        Clock::Now(),
+                        p.obj.readProviderName);
+
+                if (state)
+                {
+                    return state.value();
+                }
+            }
+
+            // The object does not exist in the memory (yet). Therefore, we create an initial state.
+            armem::robot_state::RobotState state{.timestamp = Clock::Now(),
+                                                 .globalPose = Eigen::Isometry3f::Identity(),
+                                                 .jointMap = articulatedObject->getJointValues(),
+                                                 .proprioception = std::nullopt};
+
+            return state;
+        }();
 
-        ARMARX_CHECK(state.has_value());
-        articulatedObject->setGlobalPose(state->globalPose.matrix());
+        articulatedObject->setGlobalPose(state.globalPose.matrix());
 
         ARMARX_DEBUG << "Reporting articulated objects";
 
@@ -189,8 +212,14 @@ namespace armarx::articulated_object
         // articulatedObject->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
         articulatedObject->setJointValues(jointValues);
 
-        auto& articulatedObjectWriter = articulatedObjectWriterPlugin->get();
-        articulatedObjectWriter.storeArticulatedObject(articulatedObject, now);
+        {
+            core::time::ScopedStopWatch sw(
+                [this](const armarx::Duration& dur)
+                { ARMARX_INFO << "Storing object took " << dur << "."; });
+
+            auto& articulatedObjectWriter = articulatedObjectWriterPlugin->get();
+            articulatedObjectWriter.storeArticulatedObject(articulatedObject, now);
+        }
     }
 
 } // namespace armarx::articulated_object

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

@@ -352,7 +352,7 @@ namespace armarx::armem::articulated_object
         }
         catch (...)
         {
-            ARMARX_FATAL << "Failed to obtain robot state";
+            ARMARX_VERBOSE << "Failed to obtain robot state";
             return std::nullopt;
         }
     }

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

@@ -68,46 +68,18 @@ namespace armarx::armem::articulated_object
 
         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!
-
-        updateKnownObjects();
-        memoryNameSystem.subscribe(id, this, &Writer::updateKnownObjects);
-    }
-
-    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!";
-        updateKnownObjects();
-    }
-
-    void
-    Writer::updateKnownObjects()
-    {
-        knownObjects = queryDescriptions(Time::Now());
-
-        ARMARX_INFO << "Known articulated objects " << simox::alg::get_keys(knownObjects);
-    }
 
     std::optional<armem::MemoryID>
     Writer::storeOrGetClass(const ArticulatedObject& obj) const
     {
         ARMARX_TRACE;
 
+        // key: name of object: RobotDescription::name
+        const std::unordered_map<std::string, MemoryID> knownObjects = queryDescriptions(Time::Now());
+        ARMARX_VERBOSE << "Known articulated objects " << simox::alg::get_keys(knownObjects);
+
         const auto objectId = knownObjects.find(obj.description.name);
 
         // check if exists
@@ -172,9 +144,6 @@ namespace armarx::armem::articulated_object
             return std::nullopt;
         }
 
-        // update cache (TODO: likely remove this)
-        knownObjects[obj.description.name] = updateResult.snapshotID;
-
         return updateResult.snapshotID;
     }
 
@@ -276,7 +245,7 @@ namespace armarx::armem::articulated_object
         if (not classId)
         {
             ARMARX_WARNING << "Could not get class id for object " << obj.description.name << "! "
-                           << "Known classes are " << simox::alg::get_keys(knownObjects);
+                           << "Known classes are " << simox::alg::get_keys(queryDescriptions(Time::Now()));
             return false;
         }
 
@@ -333,7 +302,7 @@ namespace armarx::armem::articulated_object
     }
 
     std::unordered_map<std::string, armem::MemoryID>
-    Writer::queryDescriptions(const armem::Time& timestamp)
+    Writer::queryDescriptions(const armem::Time& timestamp) const
     {
         // Query all entities from provider.
         armem::client::query::Builder qb;
@@ -343,7 +312,7 @@ namespace armarx::armem::articulated_object
         .coreSegments().withName(properties.coreClassSegmentName)
         .providerSegments().all()
         .entities().all()
-        .snapshots().latest(); // TODO beforeTime(timestamp);
+        .snapshots().beforeTime(timestamp);
         // clang-format on
 
         const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());

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

@@ -59,14 +59,9 @@ namespace armarx::armem::articulated_object
     private:
         std::optional<armem::MemoryID> storeOrGetClass(const ArticulatedObject& obj) const;
 
-        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);
+        queryDescriptions(const armem::Time& timestamp) const;
         std::optional<robot_state::description::RobotDescription>
         getRobotDescription(const armarx::armem::wm::Memory& memory) const;
         std::unordered_map<std::string, armem::MemoryID>
@@ -89,9 +84,6 @@ namespace armarx::armem::articulated_object
 
         armem::client::Reader memoryReader;
         mutable std::mutex memoryReaderMutex;
-
-        // key: name of object: RobotDescription::name
-        mutable std::unordered_map<std::string, MemoryID> knownObjects;
     };