[BUG] {Unable to get imu data with queues callback}

[BearrGod]

Hello every one. I'm using an OAKD-W and i'm planning on getting my imu data in a callback manner for a faster data consumption.
For that purpose i readapted the example here imu_gyroscope_accelerometer.cpp following the instructions here Queue add callback

Minimal Reproducible Example

#include <iostream>
#include <opencv4/opencv2/highgui.hpp>
// Includes common necessary includes for development using depthai library
#include <depthai/depthai.hpp>
bool firstTs = false;
auto baseTs = std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration>();

void imu_cb_dai(std::shared_ptr<dai::ADatatype> callback)
{
    using namespace std;
    using namespace std::chrono;
    if(dynamic_cast<dai::IMUData*>(callback.get()) != nullptr)
    {
        auto imuData = std::shared_ptr<dai::IMUData>(static_cast<dai::IMUData*>(callback.get())) ;
        if(imuData)
        {
            auto imuPackets = imuData->packets;
            for(auto& imuPacket : imuPackets) {
                auto& acceleroValues = imuPacket.acceleroMeter;
                auto& gyroValues = imuPacket.gyroscope;

                auto acceleroTs1 = acceleroValues.getTimestampDevice();
                auto gyroTs1 = gyroValues.getTimestampDevice();
                if(!firstTs) {
                    baseTs = std::min(acceleroTs1, gyroTs1);
                    firstTs = true;
                }

                auto acceleroTs = acceleroTs1 - baseTs;
                auto gyroTs = gyroTs1 - baseTs;

                printf("Accelerometer timestamp: %ld ms\n", static_cast<long>(duration_cast<milliseconds>(acceleroTs).count()));
                // printf("Accelerometer [m/s^2]: x: %.3f y: %.3f z: %.3f \n", acceleroValues.x, acceleroValues.y, acceleroValues.z);
                // printf("Gyroscope timestamp: %ld ms\n", static_cast<long>(duration_cast<milliseconds>(gyroTs).count()));
                // printf("Gyroscope [rad/s]: x: %.3f y: %.3f z: %.3f \n", gyroValues.x, gyroValues.y, gyroValues.z);
            }
        }
        else 
            std::cout<<"Imu ptr returned null  \n" ;
    }
} 


int main() {

    // Create pipeline
    dai::Pipeline pipeline;

    // Define sources and outputs
    auto imu = pipeline.create<dai::node::IMU>();
    auto xlinkOut = pipeline.create<dai::node::XLinkOut>();

    xlinkOut->setStreamName("imu");

    // enable ACCELEROMETER_RAW at 500 hz rate
    imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_RAW, 500);
    // enable GYROSCOPE_RAW at 400 hz rate
    imu->enableIMUSensor(dai::IMUSensor::GYROSCOPE_RAW, 400);
    // it's recommended to set both setBatchReportThreshold and setMaxBatchReports to 20 when integrating in a pipeline with a lot of input/output connections
    // above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
    imu->setBatchReportThreshold(1);
    // maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
    // if lower or equal to batchReportThreshold then the sending is always blocking on device
    // useful to reduce device's CPU load  and number of lost packets, if CPU load is high on device side due to multiple nodes
    imu->setMaxBatchReports(10);

    // Link plugins IMU -> XLINK
    imu->out.link(xlinkOut->input);

    // Pipeline is defined, now we can connect to the device
    dai::Device d(pipeline);

    auto imuQueue = d.getOutputQueue("imu", 50, false);
    baseTs = std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration>();
    imuQueue->addCallback(imu_cb_dai) ; 
    

    while(true) {
        int key = cv::waitKey(1);
        if(key == 'q') {
            return 0;
        }
    }

    return 0;
}

Result
After compiling and executing i get this error.

Accelerometer timestamp: 0 ms
Accelerometer [m/s^2]: x: 9.979 y: -0.335 z: 0.057 
Gyroscope timestamp: 1 ms
Gyroscope [rad/s]: x: 0.002 y: 0.000 z: 0.000 
munmap_chunk(): invalid pointer
Aborted (core dumped)

I don't know what i'm doing wrong here.

[moratom]

Hi @BearrGod !

The issue you face is that the code does a "double free"
as both shared pointers that you created attempt to free/destruct the same underlying raw pointer.

Here is the corrected code:

#include <iostream>
#include <memory>
#include <opencv4/opencv2/highgui.hpp>
// Includes common necessary includes for development using depthai library
#include <depthai/depthai.hpp>
bool firstTs = false;
auto baseTs = std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration>();

void imu_cb_dai(std::shared_ptr<dai::ADatatype> callback) {
    using namespace std;
    using namespace std::chrono;
    auto imuData = std::dynamic_pointer_cast<dai::IMUData>(callback);
    if(imuData) {
        auto imuPackets = imuData->packets;
        for(auto& imuPacket : imuPackets) {
            auto& acceleroValues = imuPacket.acceleroMeter;
            auto& gyroValues = imuPacket.gyroscope;

            auto acceleroTs1 = acceleroValues.getTimestampDevice();
            auto gyroTs1 = gyroValues.getTimestampDevice();
            if(!firstTs) {
                baseTs = std::min(acceleroTs1, gyroTs1);
                firstTs = true;
            }

            auto acceleroTs = acceleroTs1 - baseTs;
            auto gyroTs = gyroTs1 - baseTs;

            printf("Accelerometer timestamp: %ld ms\n", static_cast<long>(duration_cast<milliseconds>(acceleroTs).count()));
            // printf("Accelerometer [m/s^2]: x: %.3f y: %.3f z: %.3f \n", acceleroValues.x, acceleroValues.y, acceleroValues.z);
            // printf("Gyroscope timestamp: %ld ms\n", static_cast<long>(duration_cast<milliseconds>(gyroTs).count()));
            // printf("Gyroscope [rad/s]: x: %.3f y: %.3f z: %.3f \n", gyroValues.x, gyroValues.y, gyroValues.z);
        }
    } else
        std::cout << "Imu ptr returned null  \n";
}

int main() {
    // Create pipeline
    dai::Pipeline pipeline;

    // Define sources and outputs
    auto imu = pipeline.create<dai::node::IMU>();
    auto xlinkOut = pipeline.create<dai::node::XLinkOut>();

    xlinkOut->setStreamName("imu");

    // enable ACCELEROMETER_RAW at 500 hz rate
    imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_RAW, 500);
    // enable GYROSCOPE_RAW at 400 hz rate
    imu->enableIMUSensor(dai::IMUSensor::GYROSCOPE_RAW, 400);
    // it's recommended to set both setBatchReportThreshold and setMaxBatchReports to 20 when integrating in a pipeline with a lot of input/output connections
    // above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
    imu->setBatchReportThreshold(1);
    // maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
    // if lower or equal to batchReportThreshold then the sending is always blocking on device
    // useful to reduce device's CPU load  and number of lost packets, if CPU load is high on device side due to multiple nodes
    imu->setMaxBatchReports(10);

    // Link plugins IMU -> XLINK
    imu->out.link(xlinkOut->input);

    // Pipeline is defined, now we can connect to the device
    dai::Device d(pipeline);

    auto imuQueue = d.getOutputQueue("imu", 50, false);
    baseTs = std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration>();
    imuQueue->addCallback(imu_cb_dai);

    while(true) {
        int key = cv::waitKey(1);
        if(key == 'q') {
            return 0;
        }
    }

    return 0;
}

[BearrGod]

Hi @moratom . Thanks a lot for your help. I fixed my issue.