Added queue.getFps() function and tests for this function

bindings/python/src/MessageQueueBindings.cpp

@@ -81,6 +81,7 @@ void MessageQueueBindings::bind(pybind11::module& m, void* pCallstack) {
         .def("getMaxSize", &MessageQueue::getMaxSize, DOC(dai, MessageQueue, getMaxSize))
         .def("getSize", &MessageQueue::getSize, DOC(dai, MessageQueue, getSize))
         .def("isFull", &MessageQueue::isFull, DOC(dai, MessageQueue, isFull))
+        .def("getFps", &MessageQueue::getFps, DOC(dai, MessageQueue, getFps))
         .def("addCallback", addCallbackLambda, py::arg("callback"), DOC(dai, MessageQueue, addCallback))
         .def("removeCallback", &MessageQueue::removeCallback, py::arg("callbackId"), DOC(dai, MessageQueue, removeCallback))
         .def("has", static_cast<bool (MessageQueue::*)()>(&MessageQueue::has), DOC(dai, MessageQueue, has))

include/depthai/pipeline/MessageQueue.hpp

@@ -3,6 +3,8 @@
 // std
 #include <memory>
 #include <vector>
+#include <deque>
+#include <chrono>
 
 // project
 #include "depthai/pipeline/datatype/ADatatype.hpp"
@@ -26,10 +28,12 @@ class MessageQueue : public std::enable_shared_from_this<MessageQueue> {
 
    private:
     static constexpr auto CLOSED_QUEUE_MESSAGE = "MessageQueue was closed";
+    static constexpr size_t FPS_QUEUE_MAX_SIZE = 10;
     LockingQueue<std::shared_ptr<ADatatype>> queue;
     std::string name;
     std::mutex callbacksMtx;
     std::unordered_map<CallbackId, std::function<void(std::string, std::shared_ptr<ADatatype>)>> callbacks;
+    std::deque<std::chrono::steady_clock::time_point> fpsQueue;
     CallbackId uniqueCallbackId{0};
     void callCallbacks(std::shared_ptr<ADatatype> msg);
 
@@ -128,6 +132,13 @@ class MessageQueue : public std::enable_shared_from_this<MessageQueue> {
      */
     unsigned int isFull() const;
 
+    /**
+     * Gets current FPS of the queue
+     *
+     * @returns Current FPS
+     */
+    float getFps();
+
     /**
      * Adds a callback on message received
      *

src/pipeline/MessageQueue.cpp

@@ -2,6 +2,7 @@
 
 // std
 #include <chrono>
+#include <deque>
 #include <iostream>
 
 // project
@@ -70,6 +71,37 @@ unsigned int MessageQueue::isFull() const {
     return queue.isFull();
 }
 
+float MessageQueue::getFps() {
+    std::unique_lock<std::mutex> lock(callbacksMtx);
+
+    // Get current time
+    auto now = std::chrono::steady_clock::now();
+    auto threshold = now - std::chrono::seconds(2);
+
+    // Remove timestamps older than 2 seconds
+    while (!fpsQueue.empty() && fpsQueue.front() < threshold) {
+        fpsQueue.pop_front();
+    }
+
+    // If fewer than 2 timestamps are in queue, not enough data to compute FPS
+    if(fpsQueue.size() < 2) {
+        return 0.0;
+    }
+
+    auto oldest = fpsQueue.front();
+    auto newest = fpsQueue.back();
+    auto diff = std::chrono::duration<float>(newest - oldest).count();  // seconds
+
+    // If diff is extremely small, avoid dividing by zero
+    if(diff <= 0.0) {
+        return 0.0;
+    }
+    // Using (N - 1) frames over 'diff' seconds
+    // or (N) messages over 'diff' seconds—both approaches are common.
+    // This calculates how many frames we got over that time window.
+    return (fpsQueue.size() - 1) / diff;
+}
+
 int MessageQueue::addCallback(std::function<void(std::string, std::shared_ptr<ADatatype>)> callback) {
     // Lock first
     std::unique_lock<std::mutex> lock(callbacksMtx);
@@ -111,6 +143,15 @@ void MessageQueue::send(const std::shared_ptr<ADatatype>& msg) {
     callCallbacks(msg);
     auto queueNotClosed = queue.push(msg);
     if(!queueNotClosed) throw QueueException(CLOSED_QUEUE_MESSAGE);
+
+    // Record the timestamp for FPS calculation
+    {
+        auto now = std::chrono::steady_clock::now();
+        fpsQueue.push_back(now);
+        if(fpsQueue.size() > FPS_QUEUE_MAX_SIZE) {
+            fpsQueue.pop_front();
+        }
+    }
 }
 
 bool MessageQueue::send(const std::shared_ptr<ADatatype>& msg, std::chrono::milliseconds timeout) {
@@ -119,6 +160,13 @@ bool MessageQueue::send(const std::shared_ptr<ADatatype>& msg, std::chrono::mill
     if(queue.isDestroyed()) {
         throw QueueException(CLOSED_QUEUE_MESSAGE);
     }
+    {
+        auto now = std::chrono::steady_clock::now();
+        fpsQueue.push_back(now);
+        if(fpsQueue.size() > FPS_QUEUE_MAX_SIZE) {
+            fpsQueue.pop_front();
+        }
+    }
     return queue.tryWaitAndPush(msg, timeout);
 }
 

tests/src/onhost_tests/message_queue_test.cpp

@@ -395,3 +395,36 @@ TEST_CASE("Multi callbacks", "[MessageQueue]") {
     REQUIRE(callbackCount1 == 1);
     REQUIRE(callbackCount2 == 1);
 }
+
+TEST_CASE("MessageQueue - FPS Calculation", "[MessageQueue]") {
+    MessageQueue queue(10);
+
+    // Ensure FPS starts at 0
+    REQUIRE(queue.getFps() == 0.0);
+
+    // Send 10 messages with a small delay
+    constexpr int NUM_MESSAGES = 10;
+    constexpr int DELAY_MS = 50;  // 50ms delay between messages
+
+    for(int i = 0; i < NUM_MESSAGES; ++i) {
+        auto msg = std::make_shared<ADatatype>();
+        queue.send(msg);
+        std::this_thread::sleep_for(std::chrono::milliseconds(DELAY_MS));
+    }
+
+    // Compute expected FPS: 10 messages over ~450ms -> ~22.2 FPS
+    double fps = queue.getFps();
+    REQUIRE(fps > 15.0);  // Should be around 20 FPS
+    REQUIRE(fps < 30.0);  // Upper bound check
+
+    // Send one more message, verify FPS updates
+    auto msg = std::make_shared<ADatatype>();
+    queue.send(msg);
+    fps = queue.getFps();
+    REQUIRE(fps > 15.0);  // Should be consistent
+
+    // Ensure FPS decreases when there are gaps
+    std::this_thread::sleep_for(std::chrono::milliseconds(500));
+    fps = queue.getFps();
+    REQUIRE(fps < 10.0);  // Should drop after a pause
+}