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 fda00f26806d70a922b9487a008099117150d9e4..b647c6937e1f63cfc061cdd7a1845357fcd001ed 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(); + } }