fixing query

source/RobotAPI/libraries/armem/util/util.h

@@ -22,6 +22,7 @@
 
 #pragma once
 
+#include "ArmarXCore/core/logging/Logging.h"
 #include <vector>
 #include <optional>
 
@@ -29,14 +30,15 @@
 #include <RobotAPI/libraries/armem/core/EntityInstance.h>
 #include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h>
 
-namespace armarx::armem {
+namespace armarx::armem
+{
 
     /**
      * @brief Tries to cast a armem::EntityInstance to AronClass
-     * 
+     *
      * @tparam AronClass class name. Needs to be derived from armarx::aron::cppcodegenerator::AronCppClass
-     * @param item 
-     * @return std::optional<AronClass> 
+     * @param item
+     * @return std::optional<AronClass>
      */
     template <typename AronClass>
     std::optional<AronClass> tryCast(const EntityInstance& item)
@@ -58,10 +60,10 @@ namespace armarx::armem {
 
     /**
      * @brief Returns all entities that can be cast to AronClass
-     * 
+     *
      * @tparam AronClass class name. Needs to be derived from armarx::aron::cppcodegenerator::AronCppClass
      * @param entities collection of entities
-     * @return std::vector<AronClass> 
+     * @return std::vector<AronClass>
      */
     template <typename AronClass>
     std::vector<AronClass>
@@ -93,8 +95,8 @@ namespace armarx::armem {
     }
 
     /**
-     * @brief filter + transform for entities. 
-     * 
+     * @brief filter + transform for entities.
+     *
      * Can be used instead of
      *
      *      std::vector<Bar> ret;
@@ -102,26 +104,38 @@ namespace armarx::armem {
      *      const auto allOf = allOfType<Foo>(entities);
      *      std::transform(allOf.begin(), allOf.end(), std::back_inserter(ret), pred)
      *
-     * This function has the benefit that the transform function will be applied directly. 
-     * No intermediate vector has to be created (e.g. "allOf" in the example above). 
-     * 
+     * This function has the benefit that the transform function will be applied directly.
+     * No intermediate vector has to be created (e.g. "allOf" in the example above).
+     *
      * @tparam AronClass class name. Needs to be derived from armarx::aron::cppcodegenerator::AronCppClass
      * @param entities collection of entities
      * @param pred binary predicate function, applied to all entity instances
-     * @return vector of "pred"-transformed elements that can be cast to AronClass 
+     * @return vector of "pred"-transformed elements that can be cast to AronClass
      */
     template <typename AronClass>
     auto transformAllOfType(const std::map<std::string, Entity>& entities,
-                            auto pred) -> std::vector<decltype(pred(AronClass()))> 
+                            auto pred) -> std::vector<decltype(pred(AronClass()))>
     {
         static_assert(std::is_base_of<armarx::aron::cppcodegenerator::AronCppClass,
                       AronClass>::value);
 
         std::vector<decltype(pred(AronClass()))> outV;
 
+        if (entities.empty())
+        {
+            ARMARX_WARNING << "No entities!";
+        }
+
         // loop over all entities and their snapshots
         for (const auto &[s, entity] : entities)
         {
+            if (entity.history.empty())
+            {
+                ARMARX_WARNING << "Empty history for " << s;
+            }
+
+            ARMARX_INFO << "History size: " << entity.history.size();
+
             for (const auto &[ss, entitySnapshot] : entity.history)
             {
                 for (const auto& entityInstance : entitySnapshot.instances)

source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp

@@ -10,19 +10,19 @@
 #include <RobotAPI/libraries/armem/core/EntityInstance.h>
 #include <RobotAPI/libraries/armem/core/EntitySnapshot.h>
 #include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
 #include <RobotAPI/libraries/armem_robot_mapping/aron_conversions.h>
 #include <RobotAPI/libraries/armem_robot_mapping/types.h>
 #include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h>
-#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
 
 namespace armarx::armem
 {
 
-    MappingDataReader::MappingDataReader(ManagedIceObject& component) : armarx::armem::MemoryConnector(component) {}
+    MappingDataReader::MappingDataReader(ManagedIceObject& component)
+        : armarx::armem::MemoryConnector(component) {}
 
     MappingDataReader::~MappingDataReader() = default;
 
-
     void MappingDataReader::registerPropertyDefinitions(
         armarx::PropertyDefinitionsPtr& def)
     {
@@ -60,34 +60,42 @@ namespace armarx::armem
     {
         armem::client::query::Builder qb;
 
-        // clang-format off
-        auto qbProviderSegments = qb
-                                  .coreSegments().withName(properties.mappingMemoryName)
-                                  .providerSegments().withName(query.agent);
-        // clang-format on
+        ARMARX_INFO << "Query for agent: " << query.agent
+                    << " memory name: " << properties.mappingMemoryName;
 
-        auto& qbEntities = [&]() -> armarx::armem::client::query::EntitySelector &
+        if (query.sensorList.empty()) // all sensors
         {
-            if (query.sensorList.empty())
-            {
-                return qbProviderSegments.entities().all();
-            }
-
-            return qbProviderSegments.entities().withNames(query.sensorList);
-        }();
-
-        qbEntities.snapshots().timeRange(
-            IceUtil::Time::microSeconds(query.timeRange.min),
-            IceUtil::Time::microSeconds(query.timeRange.max));
+            // clang-format off
+            qb
+            .coreSegments().withName(properties.mappingMemoryName)
+            .providerSegments().withName(query.agent)
+            .entities().all()
+            .snapshots().timeRange(
+                IceUtil::Time::microSeconds(query.timeRange.min),
+                IceUtil::Time::microSeconds(query.timeRange.max));
+            // clang-format on
+        }
+        else
+        {
+            // clang-format off
+            qb
+            .coreSegments().withName(properties.mappingMemoryName)
+            .providerSegments().withName(query.agent)
+            .entities().withNames(query.sensorList)
+            .snapshots().timeRange(
+                IceUtil::Time::microSeconds(query.timeRange.min),
+                IceUtil::Time::microSeconds(query.timeRange.max));
+            // clang-format on
+        }
 
         return qb;
-    }
 
+    }
 
     MappingDataReader::Result
     MappingDataReader::queryData(const Query& query) const
     {
-        auto qb = buildQuery(query);
+        const auto qb = buildQuery(query);
 
         ARMARX_IMPORTANT << "[MappingDataReader] query ... ";
 
@@ -98,6 +106,8 @@ namespace armarx::armem
 
         if (not qResult.success)
         {
+            ARMARX_WARNING << "Failed to query data from memory: "
+                           << qResult.errorMessage;
             return {.laserScans = {},
                     .status = Result::Status::Error,
                     .errorMessage = qResult.errorMessage};
@@ -123,4 +133,4 @@ namespace armarx::armem
                 .errorMessage = ""};
     }
 
-} // namespace armarx::armem
\ No newline at end of file
+} // namespace armarx::armem