From a0f1426cb478690aabd92683a02b3bab74100bc0 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Fri, 1 Dec 2023 12:29:06 +0100
Subject: [PATCH] Object memory discards object poses with too old timestamps,
instead of spamming warnings
---
.../armem_objects/server/instance/Segment.cpp | 166 ++++++++++--------
1 file changed, 90 insertions(+), 76 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index d6635696a..62dec875e 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -226,107 +226,121 @@ namespace armarx::armem::server::obj::instance
}
}
- if (!discard)
+ if (discard)
{
- // Update the entity.
- stats.numUpdated++;
+ continue;
+ }
- VirtualRobot::RobotPtr robot =
- robots.get(provided.robotName, provided.providerName);
+ // Update the entity.
+ stats.numUpdated++;
- if (not robot)
- {
- ARMARX_INFO << deactivateSpam(1) << "Failed to retrieve robot `"
- << provided.robotName << "`.";
- }
+ VirtualRobot::RobotPtr robot = robots.get(provided.robotName, provided.providerName);
+ if (not robot)
+ {
+ ARMARX_INFO << deactivateSpam(1) << "Failed to retrieve robot `"
+ << provided.robotName << "`.";
+ }
- // robot may be null!
+ // robot may be null!
- // Update the robot to obtain correct local -> global transformation
- if (robot and robotSyncTimestamp != timestamp)
- {
- ARMARX_CHECK(robots.reader->synchronizeRobot(*robot, timestamp))
- << "Failed to synchronize robot to timestamp " << timestamp << ". This is "
- << (Clock::Now() - timestamp).toSecondsDouble() << " seconds in the past.";
+ bool synchronized = false;
- robotSyncTimestamp = timestamp;
+ // Update the robot to obtain correct local -> global transformation
+ if (robot and robotSyncTimestamp != timestamp)
+ {
+ synchronized = robots.reader->synchronizeRobot(*robot, timestamp);
+ if (not synchronized)
+ {
+ ARMARX_VERBOSE << "Failed to synchronize robot to timestamp " << timestamp
+ << " of provided object pose (" << provided.objectID
+ << "). This is " << (Clock::Now() - timestamp).toSecondsDouble()
+ << " seconds in the past.";
+ }
+ robotSyncTimestamp = timestamp;
- // Apply calibration offset after synchronizing the robot.
+ // Apply calibration offset after synchronizing the robot.
+ {
+ if (calibration.offset != 0 and robot->hasRobotNode(calibration.robotNode))
{
- if (calibration.offset != 0 and robot->hasRobotNode(calibration.robotNode))
- {
- VirtualRobot::RobotNodePtr robotNode =
- robot->getRobotNode(calibration.robotNode);
+ VirtualRobot::RobotNodePtr robotNode =
+ robot->getRobotNode(calibration.robotNode);
- float value = robotNode->getJointValue();
- float newValue = value + calibration.offset;
+ float value = robotNode->getJointValue();
+ float newValue = value + calibration.offset;
- /*
+ /*
* If newValue is out of the joint's limits, then the calibration would be ignored.
* In that case, we expand the joint value limits of the local robot model to allow
* for the calibrated value.
* As this is just for perception (and not for controlling the robot), this should be fine^TM.
*/
- VirtualRobot::RobotNode::JointLimits limits =
- robotNode->getJointLimits();
- bool limitsChanged = false;
- if (newValue < limits.low)
- {
- limits.low = newValue;
- limitsChanged = true;
- }
- if (newValue > limits.high)
- {
- limits.high = newValue;
- limitsChanged = true;
- }
- if (limitsChanged)
- {
- robotNode->setJointLimits(limits.low, limits.high);
- }
-
- robotNode->setJointValue(newValue);
+ VirtualRobot::RobotNode::JointLimits limits = robotNode->getJointLimits();
+ bool limitsChanged = false;
+ if (newValue < limits.low)
+ {
+ limits.low = newValue;
+ limitsChanged = true;
}
+ if (newValue > limits.high)
+ {
+ limits.high = newValue;
+ limitsChanged = true;
+ }
+ if (limitsChanged)
+ {
+ robotNode->setJointLimits(limits.low, limits.high);
+ }
+
+ robotNode->setJointValue(newValue);
}
}
+ }
- objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
- if (provided.objectPoseFrame.empty())
- {
- objpose::data::ProvidedObjectPose copy = provided;
- copy.objectPoseFrame = armarx::GlobalFrame;
- newPose.fromProvidedPose(copy, robot); // robot == nullptr is OK.
- }
- else
- {
- newPose.fromProvidedPose(provided, robot); // robot == nullptr is OK.
- }
+ if (not synchronized)
+ {
+ /*
+ * We know nothing about the current robot configuration. So the behvaiour of the
+ * following code is the same as if we have no robot model at all.
+ */
+ robot = nullptr;
+ }
- if (previousPose && previousPose->attachment)
- {
- // Keep current attachment.
- ARMARX_CHECK(!p.discardSnapshotsWhileAttached);
- newPose.attachment = previousPose->attachment;
- }
+ objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
+ if (provided.objectPoseFrame.empty())
+ {
+ objpose::data::ProvidedObjectPose copy = provided;
+ copy.objectPoseFrame = armarx::GlobalFrame;
+ newPose.fromProvidedPose(copy, robot); // robot == nullptr is OK.
+ }
+ else
+ {
+ newPose.fromProvidedPose(provided, robot); // robot == nullptr is OK.
+ }
- if (newPose.objectID.dataset().empty())
- {
- // Try to find the data set.
- const std::string dataset =
- classNameToDatasetCache.get(newPose.objectID.className());
- if (!dataset.empty())
- {
- newPose.objectID = {
- dataset, newPose.objectID.className(), newPose.objectID.instanceName()};
- }
- }
- if (!provided.localOOBB)
+ if (previousPose && previousPose->attachment)
+ {
+ // Keep current attachment.
+ ARMARX_CHECK(!p.discardSnapshotsWhileAttached);
+ newPose.attachment = previousPose->attachment;
+ }
+
+ if (newPose.objectID.dataset().empty())
+ {
+ // Try to find the data set.
+ const std::string dataset =
+ classNameToDatasetCache.get(newPose.objectID.className());
+ if (!dataset.empty())
{
- // Try to load oobb from disk.
- newPose.localOOBB = getObjectOOBB(newPose.objectID);
+ newPose.objectID = {
+ dataset, newPose.objectID.className(), newPose.objectID.instanceName()};
}
}
+ if (!provided.localOOBB)
+ {
+ // Try to load oobb from disk.
+ newPose.localOOBB = getObjectOOBB(newPose.objectID);
+ }
}
commitObjectPoses(newObjectPoses, providerName);
--
GitLab