diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp index 93ab1366c59de95f1fd025f0daf5d4f4e0125907..ca502d961f9e9ae8a6014d9e2e53668f73f0688b 100644 --- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp +++ b/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; }