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 4c9ec304ab9195abfd49fb5dcd2cadc18211fc88..28a4198b54d9c743cfaff031d238fff42084168d 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 b8755486996f7092e1687bcefc55f302566d62d2..c507fd90594bb6b300080f737d6263a5025417dc 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 c163ff7f68ca6fae8b4bccdf6ecfce72187f10fa..252dd7869ec119acfb5c8bcc38a7bc0a5e3d9a6f 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() }; }