luxonis / depthai-core

DepthAI C++ Library
MIT License
235 stars 127 forks source link

[BUG] ORKD-SR sync node not work #985

Closed dongxuanlb closed 8 months ago

dongxuanlb commented 8 months ago

device_information: Found device: 5.1 mxid: 194430102195772700 state: 2

Booting the first available camera (5.1)... Available camera sensors: Socket: CAM_B - OV9782, Socket: CAM_C - OV9782, Product name: OAK-D-SR, board name: DM2080

depthai FOUND, Version:2.25.0

code like:

auto sync = pipeline.create<dai::node::Sync>();
sync->setSyncThreshold(std::chrono::milliseconds(10));
sync->setSyncAttempts(-1);  // Infinite attempts
auto monoLeft = createLRCameraNode(dai::CameraBoardSocket::CAM_B, LEFT_IMG_STREAM_NAME, false);
auto monoRight = createLRCameraNode(dai::CameraBoardSocket::CAM_C, RIGHT_IMG_STREAM_NAME, false);

monoLeft->preview.link(sync->inputs[LEFT_IMG_STREAM_NAME]);
monoRight->preview.link(sync->inputs[RIGHT_IMG_STREAM_NAME]);

auto syncXout = pipeline.create<dai::node::XLinkOut>();
syncXout->setStreamName(SYNC_STREAM_NAME);
sync->out.link(syncXout->input);

devicePtr->getOutputQueue(SYNC_STREAM_NAME, opt.get_fps, false)->addCallback(std::bind(&OAKSR::daiCallback, this, std::placeholders::_1, std::placeholders::_2));

same code with oak-d work, but when use oakd-sr, there is nothing callback.

moratom commented 8 months ago

@dongxuanlb sorry for the issue.

Could you maybe provide a runnable MRE by any chance?

dongxuanlb commented 8 months ago

@dongxuanlb sorry for the issue.

Could you maybe provide a runnable MRE by any chance?

ok.

#include <depthai/depthai.hpp>
#include <iostream>
#include <signal.h>
#include <thread>
#include <functional>

static std::string LEFT_IMG_STREAM_NAME = "leftImg";
static std::string RIGHT_IMG_STREAM_NAME = "rightImg";
static std::string SYNC_STREAM_NAME = "sync";

static bool keepRunning = true;

static int i = 0;

static void sig_handler(int _) {
    (void)_;
    printf("Got Ctrl_C Signal...\n");
    keepRunning = false;
}

void cb(std::string name, std::shared_ptr<dai::ADatatype> data) {
    printf("GOT %s %d\n", name.c_str(), i++);
}

int main() {
    signal(SIGINT, sig_handler);
    // Create pipeline
    dai::Pipeline pipeline;

    auto sync = pipeline.create<dai::node::Sync>();
    sync->setSyncThreshold(std::chrono::milliseconds(10));
    sync->setSyncAttempts(-1);  // Infinite attempts

    // Define sources and outputs
    auto monoLeft = pipeline.create<dai::node::MonoCamera>();
    auto monoRight = pipeline.create<dai::node::MonoCamera>();

    // Properties
    monoLeft->setBoardSocket(dai::CameraBoardSocket::CAM_B);
    monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
    monoLeft->setFps(25);
    monoRight->setBoardSocket(dai::CameraBoardSocket::CAM_C);
    monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
    monoRight->setFps(25);

    // Linking
    monoLeft->out.link(sync->inputs[LEFT_IMG_STREAM_NAME]);
    monoRight->out.link(sync->inputs[RIGHT_IMG_STREAM_NAME]);

    auto syncXout = pipeline.create<dai::node::XLinkOut>();
    syncXout->setStreamName(SYNC_STREAM_NAME);
    sync->out.link(syncXout->input);

    // Connect to device and start pipeline
    dai::Device device(pipeline);

    device.setTimesync(true);
    device.getOutputQueue(SYNC_STREAM_NAME, 25, false)->addCallback(std::bind(cb, std::placeholders::_1, std::placeholders::_2));

     while (keepRunning) {
        std::chrono::milliseconds dura(500);
        std::this_thread::sleep_for(dura);
    }
    return 0;
}

The same code runs correctly using oakd, but there is no output when using oakd-sr. And, The current sample code does not correctly display oakd-sr in rgb. However, in order to compare with oakd, I used mono_preview in the sample code and found that oakd-sr can also be displayed normally (gray image)

Additional explanation, if you do not use synNode, but just use the addCallback style and register the left and right messages separately, there will be no problem.

moratom commented 8 months ago

Thanks for the MRE!

@jakaskerl would you mind checking if we can reproduce this on our side?

moratom commented 8 months ago

I am able to reproduce the issue - it looks like the sensors on the OAK-D-SR are not synced.

@alex-luxonis is this known? With the code below I get Diff is 18 ms for the SR and Diff is 0 ms for the OAK-D. I also checked with a stopwatch and the sensors are indeed not synced on latest v2.25.0 sr_sync

#include <signal.h>

#include <depthai/depthai.hpp>
#include <functional>
#include <iostream>
#include <memory>
#include <thread>

#include "depthai/pipeline/datatype/MessageGroup.hpp"

auto constexpr FPS{30};
static std::string LEFT_IMG_STREAM_NAME = "leftImg";
static std::string RIGHT_IMG_STREAM_NAME = "rightImg";
static std::string SYNC_STREAM_NAME = "sync";

static bool keepRunning = true;

static int i = 0;

static void sig_handler(int _) {
    (void)_;
    printf("Got Ctrl_C Signal...\n");
    keepRunning = false;
}

void cb(std::string name, std::shared_ptr<dai::ADatatype> data) {
    printf("GOT %s %d\n", name.c_str(), i++);
    auto messageGroup = std::dynamic_pointer_cast<dai::MessageGroup>(data);
    auto left = messageGroup->get<dai::ImgFrame>(LEFT_IMG_STREAM_NAME);
    auto right = messageGroup->get<dai::ImgFrame>(RIGHT_IMG_STREAM_NAME);
    auto leftTs = left->getTimestampDevice();
    auto rightTs = right->getTimestampDevice();
    auto diffMs = std::chrono::duration_cast<std::chrono::milliseconds>(rightTs - leftTs).count();
    printf("Diff is %ld ms\n", diffMs);
    cv::imshow("left", left->getCvFrame());
    cv::imshow("right", right->getCvFrame());
    cv::waitKey(1);
}

int main() {
    signal(SIGINT, sig_handler);
    // Create pipeline
    dai::Pipeline pipeline;

    auto sync = pipeline.create<dai::node::Sync>();
    sync->setSyncThreshold(std::chrono::milliseconds(25));
    sync->setSyncAttempts(-1);  // Infinite attempts

    // Define sources and outputs
    auto monoLeft = pipeline.create<dai::node::MonoCamera>();
    auto monoRight = pipeline.create<dai::node::MonoCamera>();

    // Properties
    monoLeft->setBoardSocket(dai::CameraBoardSocket::CAM_B);
    monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
    monoLeft->setFps(FPS);
    monoRight->setBoardSocket(dai::CameraBoardSocket::CAM_C);
    monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
    monoRight->setFps(FPS);

    // Linking
    monoLeft->out.link(sync->inputs[LEFT_IMG_STREAM_NAME]);
    monoRight->out.link(sync->inputs[RIGHT_IMG_STREAM_NAME]);

    auto syncXout = pipeline.create<dai::node::XLinkOut>();
    syncXout->setStreamName(SYNC_STREAM_NAME);
    sync->out.link(syncXout->input);

    // Connect to device and start pipeline
    dai::Device device(pipeline);

    device.setTimesync(true);
    device.getOutputQueue(SYNC_STREAM_NAME, 25, false)->addCallback(std::bind(cb, std::placeholders::_1, std::placeholders::_2));

    while(keepRunning) {
        std::chrono::milliseconds dura(500);
        std::this_thread::sleep_for(dura);
    }
    return 0;
}
moratom commented 8 months ago

The OAK-D-SR-POE works as expected.

alex-luxonis commented 8 months ago

Was supposed to work, but indeed a bug was introduced for OAK-D-SR, when FSYNC for OAK-D-SR-PoE was implemented (with external fsync checks). I pushed a fix to develop: https://github.com/luxonis/depthai-core/commit/12766a70b0e02bf599fa97096163996af06b5418 / https://github.com/luxonis/depthai-python/commit/9cf63e4f4138ba0346d9b1e830cb6bd4d8082d67

moratom commented 8 months ago

Thanks @alex-luxonis !

moratom commented 8 months ago

I can confirm that the latest develop works as expected.