Synchronize robot in regular intervals

source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp

@@ -45,6 +45,9 @@ namespace armarx::armem::server::laser_scans
             p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
         defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
         defs->optional(p.uniformColor, prefix + "uniformColor", "If enabled, points will be drawn in red.");
+        defs->optional(p.maxRobotAgeMs,
+                       prefix + "maxRobotAgeMs",
+                       "Maximum age of robot state before a new one is retrieved in milliseconds.");
     }
 
 
@@ -295,29 +298,36 @@ namespace armarx::armem::server::laser_scans
     VirtualRobot::RobotPtr
     Visu::getSynchronizedRobot(const std::string& name, const DateTime& timestamp)
     {
-        if (robots.count(name) > 0)
+        if (robots.count(name) == 0)
         {
-            return robots.at(name);
-        }
-
-        ARMARX_CHECK_NOT_NULL(virtualRobotReader);
-        const auto robot = virtualRobotReader->getRobot(name, timestamp, 
-        VirtualRobot::RobotIO::RobotDescription::eStructure);
+            ARMARX_CHECK_NOT_NULL(virtualRobotReader);
+            const auto robot = virtualRobotReader->getRobot(
+                name, timestamp, VirtualRobot::RobotIO::RobotDescription::eStructure);
 
-        if(robot)
-        {
-            robots[name] = robot;
-        }else
-        {
-            return nullptr;
+            if (robot)
+            {
+                robots[name] = {robot, DateTime::Invalid()};
+            }
+            else
+            {
+                return nullptr;
+            }
         }
 
-        if(not virtualRobotReader->synchronizeRobot(*robot, timestamp))
+        auto& entry = robots.at(name);
+        if (entry.second.isInvalid() ||
+            (timestamp - entry.second) > Duration::MilliSeconds(p.maxRobotAgeMs))
         {
-            ARMARX_VERBOSE << "Faield to synchronize robot `" << name << "`";
+            if (virtualRobotReader->synchronizeRobot(*entry.first, timestamp))
+            {
+                entry.second = timestamp;
+            }
+            else
+            {
+                ARMARX_VERBOSE << "Faield to synchronize robot `" << name << "`";
+            }
         }
-
-        return robots.at(name);
+        return entry.first;
     }
 
 } // namespace armarx::armem::server::laser_scans

source/RobotAPI/libraries/armem_laser_scans/server/Visu.h

@@ -27,6 +27,7 @@
 
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/core/time.h>
 #include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
 
 #include "RobotAPI/libraries/armem/server/wm/memory_definitions.h"
@@ -70,6 +71,7 @@ namespace armarx::armem::server::laser_scans
             bool enabled = true;
             bool uniformColor = false;
             float frequencyHz = 5;
+            int maxRobotAgeMs = 100;
         } p;
 
 
@@ -77,7 +79,7 @@ namespace armarx::armem::server::laser_scans
 
         armem::robot_state::VirtualRobotReader* virtualRobotReader;
 
-        std::map<std::string, VirtualRobot::RobotPtr> robots;
+        std::map<std::string, std::pair<VirtualRobot::RobotPtr, DateTime>> robots;
 
         VirtualRobot::RobotPtr getSynchronizedRobot(const std::string& name,
                                                     const DateTime& timestamp);