armem_objects: improving reader and writer for articulated objects

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());
 

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

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()
             };
         }