armem_robot_state: fixing getRobotJointStates

source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp

@@ -203,6 +203,37 @@ namespace armarx::armem::robot_state
         return getRobotJointState(qResult.memory, description.name);
     }
 
+    RobotReader::JointTrajectory
+    RobotReader::queryJointStates(const robot::RobotDescription& description,
+                                  const armem::Time& begin,
+                                  const armem::Time& end) const
+    {
+        armem::client::query::Builder qb;
+
+        ARMARX_DEBUG << "Querying robot joint states for robot: `" << description
+                     << "` on time interval [" << begin << "," << end << "]";
+
+        // clang-format off
+        qb
+        .coreSegments().withName(properties.proprioceptionCoreSegment)
+        .providerSegments().withName(description.name) // agent
+        .entities().all() // TODO
+        .snapshots().timeRange(begin, end);
+        // clang-format on
+
+        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+        if (not qResult.success) /* c++20 [[unlikely]] */
+        {
+            ARMARX_WARNING << qResult.errorMessage;
+            return {};
+        }
+
+        return getRobotJointStates(qResult.memory, description.name);
+    }
+
 
     std::optional<robot::PlatformState>
     RobotReader::queryPlatformState(const robot::RobotDescription& description,
@@ -275,6 +306,7 @@ namespace armarx::armem::robot_state
                 instance = &i;
                 return false; // break
             });
+
         if (!instance)
         {
             ARMARX_WARNING << "No entity snapshots found";
@@ -348,6 +380,46 @@ namespace armarx::armem::robot_state
         return jointMap;
     }
 
+    RobotReader::JointTrajectory
+    RobotReader::getRobotJointStates(const armarx::armem::wm::Memory& memory,
+                                     const std::string& name) const
+    {
+
+        RobotReader::JointTrajectory jointTrajectory;
+
+        // clang-format off
+        const armem::wm::CoreSegment& coreSegment = memory
+                .getCoreSegment(properties.proprioceptionCoreSegment);
+        // clang-format on
+
+        coreSegment.forEachEntity(
+            [&jointTrajectory](const wm::Entity& entity)
+            {
+                entity.forEachSnapshot(
+                    [&](const auto& snapshot)
+                    {
+                        if (not snapshot.hasInstance(0))
+                        {
+                            return;
+                        }
+
+                        const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+                        const auto proprioception =
+                            tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+                        ARMARX_CHECK(proprioception.has_value());
+
+                        const armarx::armem::prop::arondto::Joints& joints = proprioception->joints;
+
+                        jointTrajectory.emplace(entityInstance.id().timestamp, joints.position);
+                    });
+            });
+
+        ARMARX_INFO << "Joint trajectory with " << jointTrajectory.size() << " elements";
+
+        return jointTrajectory;
+    }
+
 
     // force torque for left and right
     std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>

source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h

@@ -66,6 +66,12 @@ namespace armarx::armem::robot_state
         queryJointState(const robot::RobotDescription& description,
                         const armem::Time& timestamp) const;
 
+        using JointTrajectory = std::map<armem::Time, robot::RobotState::JointMap>;
+        
+        JointTrajectory
+        queryJointStates(const robot::RobotDescription& description,
+                        const armem::Time& begin, const armem::Time& end) const;
+
         std::optional<robot::RobotState::Pose>
         queryGlobalPose(const robot::RobotDescription& description,
                         const armem::Time& timestamp) const;
@@ -100,6 +106,10 @@ namespace armarx::armem::robot_state
         std::optional<robot::RobotState::JointMap>
         getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
 
+        JointTrajectory
+        getRobotJointStates(const armarx::armem::wm::Memory& memory,
+                                    const std::string& name) const;
+
         std::optional<robot::PlatformState>
         getRobotPlatformState(const armarx::armem::wm::Memory& memory,
                               const std::string& name) const;

source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp

@@ -78,7 +78,7 @@ namespace armarx::armem::server::robot_state::proprioception
             {
                 std::lock_guard lock{dataMutex};
                 queueSize = dataQueue.size();
-                if (dataQueue.size() >= properties.memoryBatchSize)
+                if (!dataQueue.empty())
                 {
                     std::swap(batch, dataQueue);
                 }

source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h

@@ -84,7 +84,6 @@ namespace armarx::armem::server::robot_state::proprioception
 
         struct Properties
         {
-            unsigned int memoryBatchSize = 50;
             armem::MemoryID robotUnitProviderID;
         };
         Properties properties;