Hello. I'm planning on using data queues callback with my imu data queue just like here : https://docs.luxonis.com/projects/api/en/latest/samples/host_side/queue_add_callback/ . I did the relevant adaptations to the minimal example provided by depthai-core but i seam to be doing something wrong. Here is my code :
#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;
}