From c6d0a42f96f2842a85fd64bc12a0c0adb58d61e5 Mon Sep 17 00:00:00 2001
From: Patrick Hegemann <335495-patrickhegemann@users.noreply.gitlab.com>
Date: Wed, 9 Feb 2022 11:41:34 +0100
Subject: [PATCH] Get snapshot before or at timestamp in TransformHelper
---
.../common/localization/TransformHelper.cpp | 7 ++++++-
1 file changed, 6 insertions(+), 1 deletion(-)
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
index dbf23a237..a595d5502 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -5,6 +5,8 @@
#include <SimoxUtility/math/pose/interpolate.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
@@ -249,7 +251,10 @@ namespace armarx::armem::common::robot_state::localization
// }
std::vector<::armarx::armem::robot_state::Transform> transforms;
- transforms.push_back(_convertEntityToTransform(entity.getLatestSnapshot().getInstance(0)));
+
+ auto snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
+ ARMARX_CHECK(snapshot) << "No snapshot found before or at time " << timestamp;
+ transforms.push_back(_convertEntityToTransform(snapshot->getInstance(0)));
// ARMARX_DEBUG << "obtaining transform";
if (transforms.size() > 1)
--
GitLab