From 0d1f46d32ec0d2c608a90f0a91b13d4d37136442 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 16 Jan 2024 15:36:00 +0100
Subject: [PATCH] armem_objects: improving reader and writer for articulated
objects
---
.../client/articulated_object/Reader.cpp | 34 +++++++++++++------
.../client/articulated_object/Writer.cpp | 2 ++
.../common/localization/TransformHelper.cpp | 8 ++---
3 files changed, 30 insertions(+), 14 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
index 4c9ec304a..28a4198b5 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -126,7 +126,7 @@ namespace armarx::armem::articulated_object
.config = {}, // will be populated by `synchronize()`
.timestamp = timestamp};
- synchronize(obj, timestamp, providerName);
+ ARMARX_CHECK(synchronize(obj, timestamp, providerName));
return obj;
}
@@ -225,18 +225,32 @@ namespace armarx::armem::articulated_object
const armem::Time& timestamp,
const std::optional<std::string>& providerName)
{
- // TODO(fabian.reister): how to deal with multiple providers?
-
// Query all entities from provider.
armem::client::query::Builder qb;
- // clang-format off
- qb
- .coreSegments().withName(objects::constants::CoreInstanceSegmentName)
- .providerSegments().all() // withName(properties.providerName) // agent
- .entities().withName(instanceName)
- .snapshots().latest();
- // clang-format on
+
+ if(not providerName.has_value())
+ {
+ // clang-format off
+ qb
+ .coreSegments().withName(objects::constants::CoreInstanceSegmentName)
+ .providerSegments().all() // no specific provider
+ .entities().withName(instanceName)
+ // .snapshots().beforeOrAtTime(timestamp);
+ .snapshots().latest();
+ // clang-format on
+ }else
+ {
+ // clang-format off
+ qb
+ .coreSegments().withName(objects::constants::CoreInstanceSegmentName)
+ .providerSegments().withName(providerName.value()) // agent
+ .entities().withName(instanceName)
+ // .snapshots().beforeOrAtTime(timestamp);
+ .snapshots().latest();
+ // clang-format on
+ }
+
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
index b87554869..c507fd905 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -115,6 +115,8 @@ namespace armarx::armem::articulated_object
return objectId->second;
}
+ ARMARX_INFO << simox::alg::get_keys(knownObjects);
+
throw LocalException("articulated object class " + obj.description.name + " not found");
// otherwise create
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 c163ff7f6..252dd7869 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -71,9 +71,9 @@ namespace armarx::armem::common::robot_state::localization
ARMARX_INFO << deactivateSpam(1) << "No transform available.";
return {.transform = {.header = query.header},
.status = TransformResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Error in TF loookup: '" + query.header.parentFrame +
+ .errorMessage = "Error in TF lookup: '" + query.header.parentFrame +
" -> " + query.header.frame +
- "'. No memory data in time range."};
+ "'. No memory data in time range. Reference time " + sanitizedTimestamp.value().toTimeString()};
}
const Eigen::Affine3f transform = std::accumulate(
@@ -114,8 +114,8 @@ namespace armarx::armem::common::robot_state::localization
.header = query.header,
.transforms = {},
.status = TransformChainResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Error in TF loookup: '" + query.header.parentFrame + " -> " +
- query.header.frame + "'. No memory data in time range."
+ .errorMessage = "Error in TF lookup: '" + query.header.parentFrame + " -> " +
+ query.header.frame + "'. No memory data in time range. Reference time " + query.header.timestamp.toTimeString()
};
}
--
GitLab