Log queue size to debug observer

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

@@ -67,10 +67,14 @@ namespace armarx::armem::server::robot_state::proprioception
     {
         while (isRunning())
         {
-            // if (debugObserver)
-            // {
-            //     debugObserver->setDebugObserverDatafield("RobotStateWriter | Queue Size", dataBuffer.size());
-            // }
+            if (debugObserver)
+            {
+                // This locks the queue, but I did not find an interface to lock the queue,
+                // get the size and wait_pull().
+                size_t dataBufferSize = dataBuffer.size();
+                debugObserver->setDebugObserverDatafield("RobotStateWriter | Queue Size",
+                                                         static_cast<long>(dataBufferSize));
+            }
 
             RobotUnitData robotUnitData;
             if (const auto status = dataBuffer.wait_pull(robotUnitData);