Laser scans memory: 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