added some logs for queue sizes and delay in receiving laser scanner data

source/RobotComponents/components/CartographerMappingAndLocalization/CartographerMappingAndLocalization.cpp

@@ -57,6 +57,7 @@
 #include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time.h>
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
 #include <ArmarXCore/core/time/CycleUtil.h>
 #include <ArmarXCore/util/CPPUtility/trace.h>
@@ -66,6 +67,7 @@
 
 #include "RobotAPI/libraries/armem_robot/types.h"
 #include "RobotAPI/libraries/armem_vision/types.h"
+#include "RobotAPI/libraries/core/Pose.h"
 #include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
 #include <RobotAPI/interface/core/RobotLocalization.h>
 #include <RobotAPI/interface/core/RobotState.h>
@@ -765,6 +767,15 @@ namespace armarx::cartographer
 
         ARMARX_VERBOSE << name << " points " << scan.size();
 
+        const armarx::DateTime timestampReference = armarx::Duration::MilliSeconds(timestamp->timestamp);
+        const armarx::DateTime timestampArrival = armarx::Clock::Now();
+
+        const armarx::Duration timediff = timestampArrival - timestampReference;
+        if(timediff.toMilliSeconds() > 100)
+        {
+            ARMARX_WARNING << "There is a significant delay (receiving laserscanner data) of " << timediff.toMilliSeconds() << "ms from sensor " << name;
+        }
+
         std::lock_guard g{inputMtx};
 
         if (not receivingData.load())

source/RobotComponents/libraries/cartographer/ApproximateTimeQueue.cpp

@@ -171,6 +171,16 @@ namespace armarx::cartographer
 
         ARMARX_DEBUG << "Processing approximate time data " << timestamp;
 
+        std::map<std::string, std::size_t> queueSizes;
+        queueSizes["odometry"] = odomQueue.size();
+
+        for(const auto& [sensorName, sensorQueue] : laserQueues)
+        {
+            queueSizes["laser_" + sensorName] = sensorQueue.size();
+        }
+
+        ARMARX_INFO << deactivateSpam(1) << "queue sizes: " << queueSizes;
+
         lastProcessedTimestamp = timestamp;
 
         try

source/RobotComponents/libraries/cartographer/InterpolatingTimeQueue.h

@@ -80,6 +80,13 @@ namespace armarx::cartographer
 
             return interpolate(posePre, poseNext, t);
         }
+
+
+        std::size_t size() const noexcept
+        {
+            std::shared_lock g{TimeQueue<MessageType>::mtx};
+            return TimeQueue<MessageType>::queue.size();
+        }
     };
 
 } // namespace armarx::cartographer

source/RobotComponents/libraries/cartographer/MessageQueue.h

@@ -144,6 +144,8 @@ namespace armarx
                         messageQueue.pop();
                     }
                 }
+
+                ARMARX_INFO << deactivateSpam(1) << "Message queue size is " << queueSize;
             }
             cv.notify_all(); // for run() and waitUntilProcessed()
         }

source/RobotComponents/libraries/cartographer/TimeQueue.h

@@ -152,6 +152,13 @@ namespace armarx::cartographer
             queue.clear();
         }
 
+        std::size_t 
+        size() const noexcept
+        {
+            std::lock_guard g{mtx};
+            return queue.size();
+        }
+
         using QueueType = std::deque<MessageType>;
         using QueueIterator = typename QueueType::iterator;
         using QueueConstIterator = typename QueueType::const_iterator;