Only store the latest instance for each object ID

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

@@ -970,29 +970,54 @@ namespace armarx::armem::server::obj::instance
 
     armarx::objects::Scene Segment::getSceneSnapshot() const
     {
-        armarx::objects::Scene scene;
-        segmentPtr->forEachEntity([&scene](wm::Entity & entity)
+        using armarx::objects::SceneObject;
+
+        // We only store the latest version of each objectID.
+
+        struct StampedSceneObject
         {
-            try
+            SceneObject object;
+            Time timestamp;
+        };
+
+        std::map<ObjectID, StampedSceneObject> objects;
+        segmentPtr->forEachEntity([&objects](wm::Entity & entity)
+        {
+            const wm::EntityInstance* entityInstance = entity.findLatestInstance();
+            if (entityInstance)
             {
-                const wm::EntityInstance& entityInstance = entity.getLatestSnapshot().getInstance(0);
-                std::optional<arondto::ObjectInstance> objectInstance = tryCast<arondto::ObjectInstance>(entityInstance);
+                std::optional<arondto::ObjectInstance> objectInstance = tryCast<arondto::ObjectInstance>(*entityInstance);
                 if (objectInstance)
                 {
-                    armarx::objects::SceneObject& 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);
+                    const ObjectID objectID = ObjectID::FromString(objectInstance->classID.entityName);
+
+                    auto it = objects.find(objectID);
+                    if (it == objects.end() or objectInstance->pose.timestamp > it->second.timestamp)
+                    {
+                        StampedSceneObject& stamped = objects[objectID];
+                        stamped.timestamp = objectInstance->pose.timestamp;
+
+                        SceneObject& object = stamped.object;
+                        object.className = objectID.getClassID().str();
+                        object.instanceName = objectID.instanceName();
+                        object.collection = "";
+
+                        object.position = simox::math::position(objectInstance->pose.objectPoseGlobal);
+                        object.orientation = simox::math::orientation(objectInstance->pose.objectPoseGlobal);
+
+                        object.isStatic = objectInstance->pose.isStatic;
+                        object.jointValues = objectInstance->pose.objectJointValues;
+                    }
                 }
             }
-            catch (const armem::error::ArMemError& e)
-            {
-                ARMARX_WARNING_S << e.what();
-            }
         });
 
+        armarx::objects::Scene scene;
+        for (const auto& [id, stamped] : objects)
+        {
+            ARMARX_IMPORTANT << "OBJECT ID: " << id;
+            scene.objects.emplace_back(std::move(stamped.object));
+        }
         return scene;
     }