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 6e954e5542a9cdbfbd909266e94811d8dd724390..39604fa3dabac64b806d63001bcb2866e2b421ed 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 = + [×tamp, + 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 b3bececb885a0f47bd04fb54185dd797a356ade3..c5b08d065c2a3cbb607f3e007973d2925a394882 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;