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

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

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;
}

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

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

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