From 60cc7e21f6ce1840e346049e2123ebc29b1c15b6 Mon Sep 17 00:00:00 2001
From: Timo Birr <timo.birr@kit.edu>
Date: Mon, 28 Oct 2024 09:31:38 -0700
Subject: [PATCH] updated familar object visu

---
 .../server/familiar_object_instance/Visu.cpp  | 24 ++++++++++++++++---
 .../server/familiar_object_instance/Visu.h    |  1 +
 2 files changed, 22 insertions(+), 3 deletions(-)

diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp
index 6e954e554..39604fa3d 100644
--- a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp
@@ -7,6 +7,7 @@
 #include <SimoxUtility/math/rescale.h>
 
 #include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
+#include <ArmarXCore/core/time/Clock.h>
 #include <ArmarXCore/core/time/TimeUtil.h>
 #include <ArmarXCore/core/time/ice_conversions.h>
 
@@ -36,7 +37,7 @@ namespace armarx::armem::server::obj::familiar_object_instance
                        "If true, use the pose confidence as alpha (if < 1.0).");
         defs->optional(oobbs, prefix + "oobbs", "Enable showing oriented bounding boxes.");
         defs->optional(sizePixel, prefix + "sizePixel", "Pixel size of point cloud.");
-        
+
         defs->optional(visualizePointCloud, prefix + "visualizePointCloud", "");
         defs->optional(visualizePose, prefix + "visualizePose", "");
         defs->optional(visualizeBoundingBox, prefix + "visualizeBoundingBox", "");
@@ -51,6 +52,17 @@ namespace armarx::armem::server::obj::familiar_object_instance
         const std::map<std::string, std::vector<armarx::armem::arondto::FamiliarObjectInstance>>&
             familiarObjectsByProvider)
     {
+        const auto timestamp = armarx::Clock::Now();
+
+        const auto isWithinTimeFrame =
+            [&timestamp,
+             this](const armarx::armem::arondto::FamiliarObjectInstance& instance) -> bool
+        {
+            const auto dt = timestamp - instance.timestamp;
+            ARMARX_DEBUG << VAROUT(dt);
+
+            return dt.toSecondsDouble() < visualizationDuration;
+        };
 
         std::vector<viz::Layer> layers;
 
@@ -62,6 +74,11 @@ namespace armarx::armem::server::obj::familiar_object_instance
 
             for (const auto& familiarObject : familiarObjects)
             {
+                if (not isWithinTimeFrame(familiarObject))
+                {
+                    continue;
+                }
+
                 armarx::ObjectID objectId;
                 fromAron(familiarObject.objectID, objectId);
 
@@ -92,9 +109,10 @@ namespace armarx::armem::server::obj::familiar_object_instance
                         Eigen::Isometry3f obj_T_bb = Eigen::Isometry3f::Identity();
                         obj_T_bb.translation() = familiarObject.bounding_box.center;
 
-                        if(familiarObject.boundingBoxOrientation.has_value())
+                        if (familiarObject.boundingBoxOrientation.has_value())
                         {
-                            obj_T_bb.linear() = familiarObject.boundingBoxOrientation->toRotationMatrix();
+                            obj_T_bb.linear() =
+                                familiarObject.boundingBoxOrientation->toRotationMatrix();
                         }
 
                         const Eigen::Isometry3f global_T_bb = global_T_obj * obj_T_bb;
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.h
index b3bececb8..c5b08d065 100644
--- a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.h
@@ -57,6 +57,7 @@ namespace armarx::armem::server::obj::familiar_object_instance
         bool objectFrames = false;
         float objectFramesScale = 1.0;
 
+        float visualizationDuration = 20; //[s]
 
 
         SimpleRunningTask<>::pointer_type updateTask;
-- 
GitLab