From 004842368f42153ff5c966e218105f4fe56cc69a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Fri, 10 Feb 2023 18:46:39 +0100
Subject: [PATCH] Synchronize robot in regular intervals
---
.../armem_laser_scans/server/Visu.cpp | 44 ++++++++++++-------
.../libraries/armem_laser_scans/server/Visu.h | 4 +-
2 files changed, 30 insertions(+), 18 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp
index bb28ee2fe..2387e7319 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp
+++ b/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
diff --git a/source/RobotAPI/libraries/armem_laser_scans/server/Visu.h b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.h
index 23b040637..d6c79dd12 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/server/Visu.h
+++ b/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);
--
GitLab