Setting CAM_A to INPUT sync mode on OAK-FFC-4P

WHOI-Stewart-Jamieson

Hi,

We are trying to run 2x IMX477s (Cam A and Cam D) and 2x OV9282 together, synchronized. We are currently testing with just 1x OV9282 on CAM_C.

We have configured the A&D ports to be inputs, and port C to be an output. I can see the clock signal on the OV9282 at the appropriate 24Hz using an oscilloscope. As expected, the IMX477 on CAM_D is synchronized with the OV9282, and will not trigger if the sync cable is disconnected.

Problem 1: Even with the sync cables disconnected, the camera on CAM_A is still triggering as though its frame sync mode were set to OFF. Problem 2: With the cable connected, we have found that it still isn't synchronized.

Both problems seem to be caused by the IMX477 on CAM_A ignoring the initial control msg to input mode. Is this a bug, or are we potentially missing a step? As far as I can tell, our software and electrical configurations for A&D are identical, so we are very confused.

Thanks,
Stewart

erik

Hi WHOI-Stewart-Jamieson ,
Could you share the full script you are using for testing? Also, did you connect together A&D and B&C (docs here)?

WHOI-Stewart-Jamieson

Hi Eric,

I'll reduce our code into a more minimal reproducible example and post it here shortly. We are not linking the A&D and B&C ports - to my understanding that would have no effect since the arducam boards we are using have the fsync lines terminated on the 26-22 pin connector. We have directly linked the XVS ports of the three cameras without connecting anything to the fsync terminals on the cables.

Cheers,

Stewart

WHOI-Stewart-Jamieson

I didn't have a ton of time to make the MRE as minimal as possible but here is something that should compile -- it expects color cameras (IMX477s) on CAM A&D and a mono (OV9282) on CAM_C


#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/Image.h>

#include <cstdio>
#include <functional>
#include <iostream>
#include <tuple>
#include <optional>
#include <bits/stdc++.h>

// Inludes common necessary includes for development using depthai library
#include <depthai/depthai.hpp>
#include <depthai_bridge/BridgePublisher.hpp>
#include <depthai_bridge/ImageConverter.hpp>
#include <depthai_bridge/depthaiUtility.hpp>

using namespace dai::node;

dai::DeviceInfo findDevice(std::string const &mxId = "") {
    std::vector<dai::DeviceInfo> const availableDevices = dai::Device::getAllAvailableDevices();
    dai::DeviceInfo device;
    bool found = false;
    ROS_INFO("Listing available devices...");
    for (auto const &deviceInfo: availableDevices) {
        if (found) continue;
        if (mxId.empty()) {
            if (deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER) {
                device = deviceInfo;
                found = true;
            }
        } else if (deviceInfo.getMxId() == mxId) {
            if (deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER) {
                device = deviceInfo;
                found = true;
            } else {
                throw std::runtime_error(
                        "DepthAI Device with MxId  \"" + mxId + "\" is already booted on different process.");
            }
        }
    }
    if (!found)
        throw std::runtime_error((mxId.empty()) ? ("Failed to find any available DepthAI device.") : (
                "Failed to find DepthAI device with MxId \"" + mxId + "\"."));
    return device;
}

class Modular4PCamera {
    template<class T>
    using SPtr = std::shared_ptr<T>;

    dai::Pipeline pipeline;
    int rgbWidth = 0, rgbHeight = 0, rawWidth = 0, rawHeight = 0;
    SPtr<ColorCamera> leftCam, rightCam;
    SPtr<MonoCamera> monoCam;
    SPtr<VideoEncoder> leftEncoder, rightEncoder;
    SPtr<ImageManip> leftResizer, rightResizer;
    SPtr<StereoDepth> stereoDepth;
    SPtr<XLinkIn> leftControlIn, rightControlIn, stereoControlIn, monoControlIn;
    SPtr<XLinkOut> rawOut, leftOut, rightOut, leftStereoOut, rightStereoOut, compressedLeftOut, compressedRightOut;
    SPtr<XLinkOut> depthOut, stereoControlOut;

public:
    Modular4PCamera(dai::CameraBoardSocket const left_camera,
                    dai::CameraBoardSocket const right_camera,
                    bool const enableRight,
                    bool const enableUncompressed,
                    bool const enableCompressed,
                    int const compressionQuality,
                    int const compressionKBitrate,
                    float const fps,
                    int const rgbScaleNumerator,
                    int const rgbScaleDenominator) {
        pipeline.setXLinkChunkSize(0);

        // RGB image
        leftCam = pipeline.create<dai::node::ColorCamera>();
        leftCam->setBoardSocket(left_camera);
        leftCam->setFps(fps);

        dai::node::ColorCamera::Properties::SensorResolution rawResolution;
        rawResolution = dai::node::ColorCamera::Properties::SensorResolution::THE_1080_P;
        rawWidth = 1920;
        rawHeight = 1080;

        leftCam->setResolution(rawResolution);
        leftCam->setIspScale(rgbScaleNumerator, rgbScaleDenominator);
        std::tie(rgbWidth, rgbHeight) = leftCam->getIspSize();
        leftCam->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR);
        leftCam->setInterleaved(false);

        leftControlIn = pipeline.create<dai::node::XLinkIn>();
        leftControlIn->setStreamName("leftControl");
        leftControlIn->out.link(leftCam->inputControl);

        monoCam = pipeline.create<dai::node::MonoCamera>();
        monoCam->setBoardSocket(dai::CameraBoardSocket::CAM_C);
        monoCam->setFps(24);

        monoControlIn = pipeline.create<dai::node::XLinkIn>();
        monoControlIn->setStreamName("monoControl");
        monoControlIn->out.link(monoCam->inputControl);

        if (enableUncompressed) {
            ROS_INFO("Configuring uncompressed video stream at %dx%d", rgbWidth, rgbHeight);
            leftOut = pipeline.create<dai::node::XLinkOut>();
            leftOut->setStreamName("left");
            leftCam->isp.link(leftOut->input);
        }

        if (enableCompressed) {
            ROS_INFO("Configuring compressed video stream at %dx%d", rgbWidth, rgbHeight);

            leftEncoder = pipeline.create<dai::node::VideoEncoder>();
            leftEncoder->setDefaultProfilePreset(fps, dai::VideoEncoderProperties::Profile::H264_MAIN);
            leftEncoder->setQuality(compressionQuality);
            leftEncoder->setBitrateKbps(compressionKBitrate);
            leftEncoder->setLossless(false);
            compressedLeftOut = pipeline.create<dai::node::XLinkOut>();
            compressedLeftOut->setStreamName("left_stream");

            leftCam->video.link(leftEncoder->input);
            leftEncoder->bitstream.link(compressedLeftOut->input);
        }

        if (enableRight) {
            // RGB image
            rightCam = pipeline.create<dai::node::ColorCamera>();
            rightCam->setBoardSocket(right_camera);
            rightCam->setFps(fps);

            rightCam->setResolution(rawResolution);
            rightCam->setIspScale(rgbScaleNumerator, rgbScaleDenominator);
            int rightRgbWidth, rightRgbHeight;
            std::tie(rightRgbWidth, rightRgbHeight) = rightCam->getIspSize();
            rightCam->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR);
            rightCam->setInterleaved(false);

            rightControlIn = pipeline.create<dai::node::XLinkIn>();
            rightControlIn->setStreamName("rightControl");
            rightControlIn->out.link(rightCam->inputControl);

            if (enableUncompressed) {
                ROS_INFO("Configuring uncompressed right video stream at %dx%d", rgbWidth, rgbHeight);
                rightOut = pipeline.create<dai::node::XLinkOut>();
                rightOut->setStreamName("right");
                rightCam->isp.link(rightOut->input);
            }

            if (enableCompressed) {
                ROS_INFO("Configuring compressed right video stream at %dx%d", rgbWidth, rgbHeight);

                rightEncoder = pipeline.create<dai::node::VideoEncoder>();
                rightEncoder->setDefaultProfilePreset(fps, dai::VideoEncoderProperties::Profile::H264_MAIN);
                rightEncoder->setQuality(compressionQuality);
                rightEncoder->setBitrateKbps(compressionKBitrate);
                rightEncoder->setLossless(false);
                compressedRightOut = pipeline.create<dai::node::XLinkOut>();
                compressedRightOut->setStreamName("right_stream");

                rightCam->video.link(rightEncoder->input);
                rightEncoder->bitstream.link(compressedRightOut->input);
            }
        }
    }

    dai::Pipeline *getPipeline() {
        return &pipeline;
    }

    std::pair<int, int> getRgbSize() const {
        return {rgbWidth, rgbHeight};
    }

    std::pair<int, int> getRawRgbSize() const {
        return {rawWidth, rawHeight};
    }
};

sensor_msgs::CameraInfo
tryLoadCameraInfo(dai::rosBridge::ImageConverter &converter, dai::CalibrationHandler &calibHandler,
                  dai::CameraBoardSocket const &cameraId, int const &width, int const &height) {
    try {
        return converter.calibrationToCameraInfo(calibHandler, cameraId, width, height);
    } catch (std::runtime_error const &e) {
        sensor_msgs::CameraInfo info;
        info.width = width;
        info.height = height;
        return info;
    }
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "oak_ffc_4p_node");
    ros::NodeHandle pnh("~");

    std::string tfPrefix, mxId, robotName;
    std::string rgbResolution = "1080p";
    uint64_t frame;
    int badParams = 0;
    float fps, hfov;
    int rgbScaleNumerator, rgbScaleDenominator, compressionQuality, compressionKBitrate;
    bool swapLeftRight;
    bool enableRight, enableUncompressed, enableCompressed;

    badParams += !pnh.param<std::string>("mxid", mxId, "");
    badParams += !pnh.param<std::string>("robot_name", robotName, "");
    badParams += !pnh.param<bool>("swap_left_right", swapLeftRight, false);

    badParams += !pnh.param<std::string>("tf_prefix", tfPrefix, "");
    badParams += !pnh.param("enable_right", enableRight, true);
    badParams += !pnh.param("enable_uncompressed", enableUncompressed, true);
    badParams += !pnh.param("enable_compressed", enableCompressed, true);
    badParams += !pnh.param("compression_quality", compressionQuality, 100);
    badParams += !pnh.param("compression_kbitrate", compressionKBitrate, 0);

    badParams += !pnh.param<float>("fps", fps, 15);
    badParams += !pnh.param<float>("hfov", hfov, 120);

    badParams += !pnh.param("rgbScaleNumerator", rgbScaleNumerator, 1);
    badParams += !pnh.param("rgbScaleDenominator", rgbScaleDenominator, 1);

    if (badParams) ROS_WARN("Couldn't find %d parameters", badParams);

    ROS_DEBUG("Building DepthAI pipeline");

    int rgbWidth, rgbHeight, rawWidth, rawHeight;
    auto const left_camera = (swapLeftRight) ? dai::CameraBoardSocket::CAM_D : dai::CameraBoardSocket::CAM_A;
    auto const right_camera = (swapLeftRight) ? dai::CameraBoardSocket::CAM_A : dai::CameraBoardSocket::CAM_D;

    Modular4PCamera oakFfc4p(left_camera,
                             right_camera,
                             enableRight,
                             enableUncompressed,
                             enableCompressed,
                             compressionQuality,
                             compressionKBitrate,
                             fps,
                             rgbScaleNumerator,
                             rgbScaleDenominator);

    std::shared_ptr<dai::Device> device = std::make_shared<dai::Device>(*oakFfc4p.getPipeline(), findDevice(mxId));

    std::tie(rgbWidth, rgbHeight) = oakFfc4p.getRgbSize();
    std::tie(rawWidth, rawHeight) = oakFfc4p.getRawRgbSize();

    std::shared_ptr<dai::DataOutputQueue> leftQueue, rightQueue, leftStreamQueue, rightStreamQueue;
    std::shared_ptr<dai::DataInputQueue> leftControlQueue, rightControlQueue, monoControlQueue;
    std::shared_ptr<dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame>> leftPublish, rightPublish;
    std::shared_ptr<dai::rosBridge::BridgePublisher<sensor_msgs::CompressedImage, dai::ImgFrame>> leftStreamPublish, rightStreamPublish;
    std::shared_ptr<dai::rosBridge::ImageConverter> leftConverter, rightConverter;
    std::optional<ros::Publisher> leftInfoPublisher, rightInfoPublisher;
    sensor_msgs::CameraInfo leftInfo, rightInfo;

    if (!device)
        throw std::runtime_error(
                "ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId  \"" + mxId +
                "\" not found.  \"");

    ROS_INFO("Reading calibrations");

    auto calib = device->readCalibration();

    auto const time_ref = std::make_pair(ros::Time::now(), std::chrono::steady_clock::now());

    ROS_INFO("Loading converters");
    leftConverter = std::make_shared<dai::rosBridge::ImageConverter>(tfPrefix + "_left_camera_optical_frame", true);
    leftInfo = tryLoadCameraInfo(*leftConverter, calib, left_camera, rgbWidth, rgbHeight);
    leftInfo.header.frame_id = tfPrefix + "_left_camera_optical_frame";

    ROS_INFO("Loading queues");
    leftControlQueue = device->getInputQueue("leftControl");
    rightControlQueue = device->getInputQueue("rightControl");
    monoControlQueue = device->getInputQueue("monoControl");

    auto initialControlMsg = dai::CameraControl();
    initialControlMsg.setFrameSyncMode(dai::CameraControl::FrameSyncMode::INPUT);
    leftControlQueue->send(initialControlMsg);
    rightControlQueue->send(initialControlMsg);

    auto monoControlMsg = dai::CameraControl();
    monoControlMsg.setFrameSyncMode(dai::CameraControl::FrameSyncMode::OUTPUT);
    monoControlQueue->send(monoControlMsg);

    ROS_INFO("Configuring callbacks");
    if (enableUncompressed) {
        leftQueue = device->getOutputQueue("left", 1, false);
        leftPublish = std::make_shared<dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame>>(
                leftQueue,
                pnh,
                "left/image_color",
                [&leftConverter](std::shared_ptr<dai::ImgFrame> const &inData,
                                                            auto &outMsg) {
                    leftConverter->toRosMsg(inData, outMsg);
                },
                3,
                leftInfo,
                "left");
        leftPublish->addPublisherCallback();
    } else {
        leftInfoPublisher = pnh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1, true);
    }

    if (enableCompressed) {
        ROS_INFO("Advertising compressed video stream topic");
        leftStreamQueue = device->getOutputQueue("left_stream", 1, false);
        auto const profile = dai::VideoEncoderProperties::Profile::H264_MAIN;
        std::string const topic = "left/image_stream/h264", format = "h264";
        leftStreamPublish = std::make_shared<dai::rosBridge::BridgePublisher<sensor_msgs::CompressedImage, dai::ImgFrame>>(
                leftStreamQueue,
                pnh,
                topic,
                [&enableUncompressed, &leftInfoPublisher, &leftInfo, time_ref, format](
                        std::shared_ptr<dai::ImgFrame> const &inData,
                        auto &outMsg) {
                    using namespace std::chrono;
                    auto const currentTime = std::chrono::steady_clock::now();
                    auto const dataTime = inData->getTimestamp();
                    sensor_msgs::CompressedImage msg;
                    msg.data = inData->getData();
                    msg.format = format;
                    msg.header = leftInfo.header;
                    // msg.header.stamp = ros::Time(duration<double, std::ratio<1>>(dataTime.time_since_epoch()).count());
                    msg.header.stamp = dai::ros::getFrameTime(time_ref.first, time_ref.second, dataTime);
                    if (!enableUncompressed) {
                        leftInfo.header = msg.header;
                        leftInfoPublisher->publish(leftInfo);
                    }
                    outMsg.push_back(std::move(msg));
                    ROS_INFO_THROTTLE(2, "Compressed RGB Latency: %lu ms",
                                      std::chrono::duration_cast<std::chrono::milliseconds>(
                                              currentTime - dataTime).count());
                },
                3,
                leftInfo,
                "left");
        leftStreamPublish->addPublisherCallback();
    }

    if (enableRight) {
        rightConverter = std::make_shared<dai::rosBridge::ImageConverter>(tfPrefix + "_right_camera_optical_frame",
                                                                          true);
        rightInfo = tryLoadCameraInfo(*rightConverter, calib, right_camera,
                                      rgbWidth,
                                      rgbHeight);
        rightInfo.header.frame_id = tfPrefix + "_right_camera_optical_frame";

        if (enableUncompressed) {
            rightQueue = device->getOutputQueue("right", 1, false);
            rightPublish = std::make_shared<dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame>>(
                    rightQueue,
                    pnh,
                    "right/image_color",
                    [&rightConverter](auto const &inData, auto &outMsg) {
                        rightConverter->toRosMsg(inData, outMsg);
                    },
                    3,
                    rightInfo,
                    "right");
            rightPublish->addPublisherCallback();
        } else {
            rightInfoPublisher = pnh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1, true);
        }

        if (enableCompressed) {
            ROS_INFO("Advertising compressed right camera video stream topic");
            rightStreamQueue = device->getOutputQueue("right_stream", 1, false);
            std::string const topic = "right/image_stream/h264", format = "h264";
            rightStreamPublish = std::make_shared<dai::rosBridge::BridgePublisher<sensor_msgs::CompressedImage, dai::ImgFrame>>(
                    rightStreamQueue,
                    pnh,
                    topic,
                    [&enableUncompressed, &rightInfoPublisher, &rightInfo, time_ref, format](
                            std::shared_ptr<dai::ImgFrame> const &inData,
                            auto &outMsg) {
                        using namespace std::chrono;
                        auto const currentTime = std::chrono::steady_clock::now();
                        auto const dataTime = inData->getTimestamp();
                        sensor_msgs::CompressedImage msg;
                        msg.data = inData->getData();
                        msg.format = format;
                        msg.header = rightInfo.header;
                        msg.header.stamp = dai::ros::getFrameTime(time_ref.first, time_ref.second, dataTime);
                        if (!enableUncompressed) {
                            rightInfo.header = msg.header;
                            rightInfoPublisher->publish(rightInfo);
                        }
                        outMsg.push_back(std::move(msg));
                        ROS_INFO_THROTTLE(2, "Compressed RGB Latency: %lu ms",
                                          std::chrono::duration_cast<std::chrono::milliseconds>(
                                                  currentTime - dataTime).count());
                    },
                    3,
                    rightInfo,
                    "right");
            rightStreamPublish->addPublisherCallback();
        }
    }

    ROS_INFO("Ready! Spinning...");
    ros::spin();
    return 0;
}

WHOI-Stewart-Jamieson

As an update on this, we can only seem to successfully put the camera on the CAM_D port into INPUT mode. If we put the same camera on any other port (A-C) it ignores the XVS/sync pin and seems to follow the software trigger, regardless of whether we set the frame sync mode to INPUT/OUTPUT/OFF.

We are using R4M1E4 OAK-FFC-4P boards, in case that is relevant.

Cheers,
Stewart

WHOI-Stewart-Jamieson

Oh, and we are using the Ubuntu 20.04 apt package ros-noetic-depthai-ros

WHOI-Stewart-Jamieson

Hi @erik, have you folks had the chance to look into this?

erik

Hi WHOI-Stewart-Jamieson , Sorry about the delay, we still have it on the backlog - such issues require quite a bit of engineering effort.

WHOI-Stewart-Jamieson

We have also attempted this after building the v2.7.3 library from source but it hasn't solved any of our issues.

erik

WHOI-Stewart-Jamieson why did you try to run 2.7.3? Latest depthai version is 2.22.0

WHOI-Stewart-Jamieson

Hi @erik,

Should have specified that is depthai-ros noetic v2.7.3 (latest). We also installed the depthai core library (v2.22.0) from source using the install script in the Readme for depthai-ros (v2.7.3).

Best,

Stewart

erik

Hi WHOI-Stewart-Jamieson ,
Could you first try without ROS driver, with just the latest depthai library (2.22, python lib) and check whether that works? As there might be issue either with ROS driver, or with the library itself (that ROS driver uses).