From 89940f64fca760be7f6f9178eb27b8122a5b08e6 Mon Sep 17 00:00:00 2001
From: armar6-demo <armar6-demo@kit.edu>
Date: Thu, 17 Nov 2022 15:50:45 +0100
Subject: [PATCH] local changes on A-6 demo account
---
.../common/localization/TransformHelper.cpp | 54 ++++++++++++++-----
1 file changed, 41 insertions(+), 13 deletions(-)
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 fda00f268..b647c6937 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -49,10 +49,20 @@ namespace armarx::armem::common::robot_state::localization
localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
const armem::Time sanitizedTimestamp = _obtainTimestamp(localizationCoreSegment, query.header.timestamp);
+
+ if (sanitizedTimestamp.isInvalid())
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "No timestamp available.";
+ return {.transform = {.header = query.header},
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Error in TF loookup: '" + query.header.parentFrame +
+ " -> " + query.header.frame +
+ "'. No memory data in time range."};
+ }
+
auto header = query.header;
// ARMARX_INFO << header.timestamp << "vs" << sanitizedTimestamp;
-
header.timestamp = sanitizedTimestamp;
if (transforms.empty())
@@ -181,24 +191,42 @@ namespace armarx::armem::common::robot_state::localization
armarx::core::time::DateTime
TransformHelper::_obtainTimestamp(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment, const armem::Time& timestamp)
{
-
// first we check which the newest timestamp is
- int64_t timeSinceEpochUs = 0;
+ std::optional<int64_t> timeSinceEpochUs = std::nullopt;
- localizationCoreSegment.forEachEntity([&timeSinceEpochUs, ×tamp](const auto& entity){
+ localizationCoreSegment.forEachEntity([&timeSinceEpochUs, ×tamp](const auto& entity)
+ {
auto snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
- const armem::wm::EntityInstance& item = snapshot->getInstance(0);
- const auto tf = _convertEntityToTransform(item);
-
- const auto& dataTs = tf.header.timestamp;
-
- timeSinceEpochUs = std::max(timeSinceEpochUs, dataTs.toMicroSecondsSinceEpoch());
+ if (snapshot)
+ {
+ // This caused a segfault when the snapshot does not exist.
+ const armem::wm::EntityInstance& item = snapshot->getInstance(0);
+ const auto tf = _convertEntityToTransform(item);
+
+ const auto& dataTs = tf.header.timestamp;
+
+ if (timeSinceEpochUs.has_value())
+ {
+ timeSinceEpochUs = std::max(timeSinceEpochUs.value(), dataTs.toMicroSecondsSinceEpoch());
+ }
+ else
+ {
+ timeSinceEpochUs = dataTs.toMicroSecondsSinceEpoch();
+ }
+ }
});
- // then we ensure that the timestamp is not more recent than the query timestamp
- timeSinceEpochUs = std::min(timeSinceEpochUs, timestamp.toMicroSecondsSinceEpoch());
+ if (timeSinceEpochUs.has_value())
+ {
+ // then we ensure that the timestamp is not more recent than the query timestamp
+ timeSinceEpochUs = std::min(timeSinceEpochUs.value(), timestamp.toMicroSecondsSinceEpoch());
- return armarx::core::time::DateTime(armarx::core::time::Duration::MicroSeconds(timeSinceEpochUs));
+ return armarx::core::time::DateTime(armarx::core::time::Duration::MicroSeconds(timeSinceEpochUs.value()));
+ }
+ else
+ {
+ return DateTime::Invalid();
+ }
}
--
GitLab