Messages are now cached in a queue before being processed. This reduces the execution time of the callback thread which will hopefully alleviate the weird sampling frequency drops.

Additionally the memory leak of in Node.cpp was fixed

Author: raffael0

CMakeLists.txt

@@ -24,6 +24,18 @@ endif()
 # Define a library for the main project
 add_library(${PROJECT_NAME}_lib STATIC ${sources})
 
+include(FetchContent)
+
+FetchContent_Declare(
+        readerwriterqueue
+        GIT_REPOSITORY    https://github.com/cameron314/readerwriterqueue
+        GIT_TAG           v1.0.7
+)
+
+FetchContent_MakeAvailable(readerwriterqueue)
+
+target_link_libraries(${PROJECT_NAME}_lib PUBLIC readerwriterqueue)
+
 # Create an executable that links to this library
 add_executable(${PROJECT_NAME} main.cpp)
 target_link_libraries(${PROJECT_NAME} PRIVATE ${PROJECT_NAME}_lib)

include/can/CANDriverKvaser.h

@@ -10,8 +10,11 @@
 #include <functional>
 #include <string>
 #include <canlib.h>
-#include "common.h"
+#include <readerwriterqueue.h>
 #include "utility/Config.h"
+#include "CanKvaserReceiveThread.h"
+#include "CommonKvaser.h"
+
 
 class CANDriverKvaser : public CANDriver
 {
@@ -21,6 +24,9 @@ class CANDriverKvaser : public CANDriver
 		std::map<uint32_t, CANParams> arbitrationParamsMap = std::map<uint32_t, CANParams>();
 		std::map<uint32_t, CANParams> dataParamsMap = std::map<uint32_t, CANParams>();
 
+        std::shared_ptr<::moodycamel::ReaderWriterQueue<std::unique_ptr<RawKvaserMessage>>> receivedMessagesQueue;
+        std::unique_ptr<CanKvaserReceiveThread> receiveThread;
+
         uint64_t blockingTimeout;
 
         static void OnCANCallback(int handle, void *driver, unsigned int event);

include/can/CanKvaserReceiveThread.h

@@ -0,0 +1,38 @@
+//
+// Created by raffael on 24.08.25.
+//
+
+#ifndef CANRECEIVETHREAD_H
+#define CANRECEIVETHREAD_H
+
+#ifndef NO_CANLIB
+#include "CommonKvaser.h"
+#include <readerwriterqueue.h>
+#include <functional>
+#include <atomic>
+#include "CANDriver.h"
+
+
+class CanKvaserReceiveThread {
+public:
+    explicit CanKvaserReceiveThread(
+                           canRecvCallback_t onRecvCallback);
+
+    ~CanKvaserReceiveThread();
+
+    void stop();
+    void pushMessage(std::unique_ptr<RawKvaserMessage> msg);
+    [[nodiscard]] bool isRunning() const;
+
+private:
+    std::atomic_bool running{false};
+    std::atomic_int32_t messagesInQueue{0};
+    std::shared_ptr<moodycamel::ReaderWriterQueue<std::unique_ptr<RawKvaserMessage>> > queue;
+    canRecvCallback_t onRecvCallback;
+    void receiveLoop();
+};
+
+
+#endif
+
+#endif //CANRECEIVETHREAD_H

include/can/CommonKvaser.h

@@ -0,0 +1,20 @@
+//
+// Created by raffael on 28.08.25.
+//
+
+#ifndef COMMONKVASER_H
+#define COMMONKVASER_H
+#include <array>
+#include <cstdint>
+
+#include "CANDriver.h"
+struct RawKvaserMessage {
+    uint32_t busChannelID;
+    uint32_t messageID;
+    uint8_t  data[64];
+    uint8_t  dlc;
+    uint64_t timestamp;
+    CANDriver *driver;
+};
+
+#endif //COMMONKVASER_H

src/can/CANDriverKvaser.cpp

@@ -8,7 +8,7 @@
 
 CANDriverKvaser::CANDriverKvaser(canRecvCallback_t onRecvCallback,
 								 std::function<void(std::string *)> onErrorCallback, std::vector<uint32_t> &canBusChannelIDs, Config &config) :
-	CANDriver(onRecvCallback, onErrorCallback)
+	CANDriver(std::move(onRecvCallback), std::move(onErrorCallback))
 {
     //arbitration bus parameters
     int32_t bitrate = config["/CAN/BUS/ARBITRATION/bitrate"];
@@ -29,6 +29,8 @@ CANDriverKvaser::CANDriverKvaser(canRecvCallback_t onRecvCallback,
 
     nlohmann::json busExtra = config["/CAN/BUS_EXTRA"];
 
+    receiveThread = std::make_unique<CanKvaserReceiveThread>(onRecvCallback);
+
     canStatus stat;
     for (auto &channelID : canBusChannelIDs)
     {
@@ -167,15 +169,15 @@ void CANDriverKvaser::OnCANCallback(int handle, void *driver, unsigned int event
                 std::string errorMsg = "canNOTIFY_ERROR: " + canDriver->CANError(stat);
                 canDriver->onErrorCallback(&errorMsg);
             }
-            break;   
+            break;
         }
         case canNOTIFY_STATUS:
         {
             uint64_t statFlags = 0;
             stat = canReadStatus(handle, &statFlags);
             Debug::print("canNOTIFY_STATUS changed");
             if(statFlags & canSTAT_OVERRUN) {
-                Debug::print("canNOTIFY_STATUS: buffer overflow");
+                Debug::warning("canNOTIFY_STATUS: buffer overflow");
             }
             break;
         }
@@ -208,11 +210,14 @@ void CANDriverKvaser::OnCANCallback(int handle, void *driver, unsigned int event
                     try
                     {
                         // Copy the received data into a new buffer for the thread
-                        auto threadData = std::make_unique<uint8_t[]>(dlc);
-                        std::copy_n(data, dlc, threadData.get());
-                        std::thread([canDriver, canBusChannelID, id, dlc, softwareTime, threadData = std::move(threadData)]() {
-                            canDriver->onRecvCallback(canBusChannelID, id, threadData.get(), dlc, softwareTime, canDriver);
-                        }).detach();
+                        std::unique_ptr<RawKvaserMessage> threadData = std::make_unique<RawKvaserMessage>();
+                        std::ranges::copy(data, threadData->data);
+                        threadData->dlc = dlc;
+                        threadData->busChannelID = canBusChannelID;
+                        threadData->messageID = id;
+                        threadData->timestamp = softwareTime;
+                        threadData->driver = canDriver;
+                        canDriver->receiveThread->pushMessage(std::move(threadData));
                     }
                     catch(const std::exception& e)
                     {
@@ -236,7 +241,7 @@ void CANDriverKvaser::OnCANCallback(int handle, void *driver, unsigned int event
                     }
                     Debug::print("\t\tCAN Status Flags: 0x%016x", statFlags);
                 }
-                
+
                 stat = canRead(handle, &id, data, &dlc, &flags, &timestamp);
             }
             // stat is either canERR_NOMSG or any different error code
@@ -246,7 +251,7 @@ void CANDriverKvaser::OnCANCallback(int handle, void *driver, unsigned int event
             }
             break;
         }
-        
+
         default:
             //TODO: MP since this thread is managed by the canlib, should we really throw exceptions?
             throw std::runtime_error("Callback got called with neither ERROR nor RX, gigantic UFF");

src/can/CanKvaserReceiveThread.cpp

@@ -0,0 +1,44 @@
+//
+// Created by raffael on 24.08.25.
+//
+
+#include "can/CanKvaserReceiveThread.h"
+
+#include <utility>
+
+CanKvaserReceiveThread::CanKvaserReceiveThread(canRecvCallback_t onRecvCallback): queue(std::make_shared<moodycamel::ReaderWriterQueue<std::unique_ptr<RawKvaserMessage>>>(100)),
+    onRecvCallback(std::move(onRecvCallback)) {
+
+    std::thread([this]() { this->receiveLoop(); }).detach();
+}
+
+CanKvaserReceiveThread::~CanKvaserReceiveThread() {
+    stop();
+}
+
+void CanKvaserReceiveThread::stop() {
+    running.store(false);
+}
+
+void CanKvaserReceiveThread::pushMessage(std::unique_ptr<RawKvaserMessage> msg) {
+    queue->enqueue(std::move(msg));
+    ++messagesInQueue;
+}
+
+bool CanKvaserReceiveThread::isRunning() const {
+    return running.load();
+}
+
+void CanKvaserReceiveThread::receiveLoop(){
+    while (running.load()) {
+        std::unique_ptr<RawKvaserMessage> msg;
+        if (queue->try_dequeue(msg)) {
+            onRecvCallback(msg->busChannelID, msg->messageID, msg->data, msg->dlc, msg->timestamp, msg->driver);
+        }
+        --messagesInQueue;
+        if (messagesInQueue>40) {
+            Debug::warning("CanKvaserReceiveThread::receiveLoop: High message load, messages in queue: %d", messagesInQueue.load());
+        }
+    }
+}
+

src/can/Node.cpp

@@ -249,6 +249,7 @@ std::map<std::string, std::tuple<double, uint64_t>> Node::GetLatestSensorData()
     /* if (nodeID==8)
     Debug::print("NodeID %d, %zd sensor data transmissions", nodeID, uint64_t(count));*/
     count = 0;
+    delete[] copy;
     return sensorData;
 }