Hi,

I have created a ROS 2 driver for the OAK FFC 3P and 2 OAK FFC OV9282 cameras. I have synchronized the left and right raw images with Depthai C++API and after rectifying them, I am giving them the same timestamp in the ROS message header. The problem is that after publishing the rectified left and right images, the images received by SLAM, such as RTABMAP, are not synchronized, and there is also a time difference between consecutive frames when I run Isaac cuVSLAM. What am I doing wrong here? The callback timer and the DepthAI FPS are the same.

One strange thing I have noticed is that the time difference between the left and right images is 0.033 s at 30 fps and 0.016 s at 60 fps, according to RTABMAP. Why are they one frame apart? Also, the time difference between consecutive frames is a multiple of 0.066 seconds, according to Isaac CuVSLAM.

If you could give a solution or hint on what I am doing wrong, then I would really appreciate it.

The code is the following:

#include <cv_bridge/cv_bridge.hpp>
#include <depthai/depthai.hpp>

#include <filesystem>
#include <fstream>
#include <iostream>
#include <json/json.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <opencv2/opencv.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rmw/qos_profiles.h>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>

class ImprovedStereoPublisher : public rclcpp::Node
{
public:
    ImprovedStereoPublisher() : Node("improved_stereo_publisher")
    {
        this->get_logger().set_level(rclcpp::Logger::Level::Debug);
        this->declare_parameter("sync_threshold_ms", 10);
        int sync_threshold_ms = this->get_parameter("sync_threshold_ms").as_int();

        // Create a custom QoS profile for SLAM stereo image publishers

        rectifLeftImagePub =
            this->create_publisher<sensor_msgs::msg::Image>("left/image_rect", qos);
        rectifRightImagePub = this->create_publisher<sensor_msgs::msg::Image>(
            "right/image_rect", qos);
        leftCameraInfoPub = this->create_publisher<sensor_msgs::msg::CameraInfo>(
            "left/camera_info", qos);
        rightCameraInfoPub = this->create_publisher<sensor_msgs::msg::CameraInfo>(
            "right/camera_info", qos);

        load_calibration_data();

        // Create and start the camera pipeline
        run_camera_pipeline(sync_threshold_ms);

        // Set up timer for periodic publishing
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(17), // ~60 FPS
            std::bind(&ImprovedStereoPublisher::timer_callback, this));

        RCLCPP_INFO(this->get_logger(), "ImprovedStereoPublisher initialized");
    }

private:
    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rectifLeftImagePub,
        rectifRightImagePub;
    rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr leftCameraInfoPub,
        rightCameraInfoPub;
    cv::Mat cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, R1, R2,
        P1, P2, Q;
    cv::Mat map1_x, map1_y, map2_x, map2_y;

    rclcpp::TimerBase::SharedPtr timer_;

    // Pipeline components
    std::unique_ptr<dai::Pipeline> pipeline;
    std::shared_ptr<dai::Device> device;
    std::shared_ptr<dai::node::MonoCamera> monoLeft, monoRight;
    std::shared_ptr<dai::node::Sync> sync;
    std::shared_ptr<dai::node::XLinkOut> xoutLeft, xoutRight;
    std::shared_ptr<dai::DataOutputQueue> leftQueue, rightQueue;
    // add demux node
    // std::shared_ptr<dai::node::XLinkOut> xoutLeftDemux, xoutRightDemux;
    // std::shared_ptr<dai::node::XLinkIn> xinLeft, xinRight;
    std::shared_ptr<dai::node::MessageDemux> demux;

    std::string get_calibration_file_path()
    {
        std::filesystem::path exec_path = std::filesystem::current_path();
        std::filesystem::path json_file_path =
            exec_path / "/home/shivam157/driver/src/depthai_ros_driver/src/"
                        "calibration_data.json";
        return json_file_path.string();
    }

    void load_calibration_data()
    {
        std::string file_path = get_calibration_file_path();
        std::ifstream file(file_path);
        if (!file.is_open())
        {
            RCLCPP_ERROR(this->get_logger(),
                         "Failed to open calibration data file: %s",
                         file_path.c_str());
            return;
        }

        Json::Value root;
        Json::CharReaderBuilder reader;
        std::string errs;

        if (!Json::parseFromStream(reader, file, &root, &errs))
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to parse calibration data: %s",
                         errs.c_str());
            return;
        }

        try
        {
            cameraMatrix1 = cv::Mat(3, 3, CV_64F);
            cameraMatrix2 = cv::Mat(3, 3, CV_64F);
            for (int i = 0; i < 3; ++i)
            {
                for (int j = 0; j < 3; ++j)
                {
                    cameraMatrix1.at<double>(i, j) =
                        root["cameraMatrix1"][i][j].asDouble();
                    cameraMatrix2.at<double>(i, j) =
                        root["cameraMatrix2"][i][j].asDouble();
                }
            }

            distCoeffs1 = cv::Mat(1, 5, CV_64F);
            distCoeffs2 = cv::Mat(1, 5, CV_64F);
            for (int i = 0; i < 5; ++i)
            {
                distCoeffs1.at<double>(0, i) = root["distCoeffs1"][0][i].asDouble();
                distCoeffs2.at<double>(0, i) = root["distCoeffs2"][0][i].asDouble();
            }

            R = cv::Mat(3, 3, CV_64F);
            T = cv::Mat(3, 1, CV_64F);
            R1 = cv::Mat(3, 3, CV_64F);
            R2 = cv::Mat(3, 3, CV_64F);
            for (int i = 0; i < 3; ++i)
            {
                for (int j = 0; j < 3; ++j)
                {
                    R.at<double>(i, j) = root["R"][i][j].asDouble();
                    R1.at<double>(i, j) = root["R1"][i][j].asDouble();
                    R2.at<double>(i, j) = root["R2"][i][j].asDouble();
                }
                T.at<double>(i, 0) = root["T"][i][0].asDouble();
            }

            P1 = cv::Mat(3, 4, CV_64F);
            P2 = cv::Mat(3, 4, CV_64F);
            for (int i = 0; i < 3; ++i)
            {
                for (int j = 0; j < 4; ++j)
                {
                    P1.at<double>(i, j) = root["P1"][i][j].asDouble();
                    P2.at<double>(i, j) = root["P2"][i][j].asDouble();
                }
            }

            cv::Size imageSize(1280, 800);

            cv::initUndistortRectifyMap(cameraMatrix1, distCoeffs1, R1, P1, imageSize,
                                        CV_32FC1, map1_x, map1_y);
            cv::initUndistortRectifyMap(cameraMatrix2, distCoeffs2, R2, P2, imageSize,
                                        CV_32FC1, map2_x, map2_y);
        }
        catch (const std::exception &e)
        {
            RCLCPP_ERROR(this->get_logger(), "Error loading calibration data: %s",
                         e.what());
        }
    }

    void
    updateCameraInfo(std::shared_ptr<sensor_msgs::msg::CameraInfo> &cameraInfo,
                     const cv::Mat &projectionMatrix, const cv::Size &imageSize)
    {
        cameraInfo->height = imageSize.height;
        cameraInfo->width = imageSize.width;
        cameraInfo->distortion_model = "plumb_bob";
        cameraInfo->d = {0, 0, 0, 0, 0};

        cameraInfo->k = {projectionMatrix.at<double>(0, 0),
                         0,
                         projectionMatrix.at<double>(0, 2),
                         0,
                         projectionMatrix.at<double>(1, 1),
                         projectionMatrix.at<double>(1, 2),
                         0,
                         0,
                         1};
        cameraInfo->r = {1, 0, 0, 0, 1, 0, 0, 0, 1};
        cameraInfo->p = {
            projectionMatrix.at<double>(0, 0), projectionMatrix.at<double>(0, 1),
            projectionMatrix.at<double>(0, 2), projectionMatrix.at<double>(0, 3),
            projectionMatrix.at<double>(1, 0), projectionMatrix.at<double>(1, 1),
            projectionMatrix.at<double>(1, 2), projectionMatrix.at<double>(1, 3),
            projectionMatrix.at<double>(2, 0), projectionMatrix.at<double>(2, 1),
            projectionMatrix.at<double>(2, 2), projectionMatrix.at<double>(2, 3)};
    }

    void run_camera_pipeline(int sync_threshold_ms)
    {
        auto pipeline = std::make_unique<dai::Pipeline>();

        // Define sources and outputs
        auto monoLeft = pipeline->create<dai::node::MonoCamera>();
        auto monoRight = pipeline->create<dai::node::MonoCamera>();
        auto sync = pipeline->create<dai::node::Sync>();
        sync->setSyncThreshold(std::chrono::milliseconds(1));
        auto demux = pipeline->create<dai::node::MessageDemux>();

        auto xoutLeft = pipeline->create<dai::node::XLinkOut>();
        auto xoutRight = pipeline->create<dai::node::XLinkOut>();

        xoutLeft->setStreamName("left");
        xoutRight->setStreamName("right");

        // Properties
        monoLeft->setResolution(
            dai::MonoCameraProperties::SensorResolution::THE_800_P);
        monoLeft->setCamera("left");
        monoLeft->setFps(60.0);
        monoRight->setResolution(
            dai::MonoCameraProperties::SensorResolution::THE_800_P);
        monoRight->setCamera("right");
        monoRight->setFps(60.0);

        // Linking
        monoLeft->out.link(sync->inputs["left"]);
        monoRight->out.link(sync->inputs["right"]);

        sync->out.link(demux->input);

        demux->outputs["left"].link(xoutLeft->input);
        demux->outputs["right"].link(xoutRight->input);

        // Connect to device and start pipeline
        auto device =
            std::make_shared<dai::Device>(*pipeline, dai::UsbSpeed::SUPER_PLUS);

        auto leftQueue = device->getOutputQueue("left", 1, false);
        auto rightQueue = device->getOutputQueue("right", 1, false);

        // Store the pointers if needed
        this->pipeline = std::move(pipeline);
        this->device = device;
        this->monoLeft = monoLeft;
        this->monoRight = monoRight;
        this->sync = sync;
        this->xoutLeft = xoutLeft;
        this->xoutRight = xoutRight;
        this->demux = demux;
        this->leftQueue = leftQueue;
        this->rightQueue = rightQueue;

        // // save one frame from each queue to see if initial frames are received
        // auto leftFrame = leftQueue->get<dai::ImgFrame>();
        // auto rightFrame = rightQueue->get<dai::ImgFrame>();
        // // converting the frames to opencv mat
        // cv::Mat leftImg = leftFrame->getCvFrame();
        // cv::Mat rightImg = rightFrame->getCvFrame();
        // // saving the frames
        // cv::imwrite("left.jpg", leftImg);
        // cv::imwrite("right.jpg", rightImg);
    }

    void timer_callback()
    {
        RCLCPP_INFO(this->get_logger(), "Timer callback triggered");
        if (!leftQueue || !rightQueue)
        {
            RCLCPP_WARN(this->get_logger(), "Camera queues not initialized");
            return;
        }

        auto leftFrame = leftQueue->get<dai::ImgFrame>();
        auto rightFrame = rightQueue->get<dai::ImgFrame>();

        if (leftFrame && rightFrame)
        {
            // printing the timestamp of the left and right frames in depthai format
            // in seconds
            // Convert DepthAI timestamps to seconds
            double left_timestamp = std::chrono::duration_cast<std::chrono::duration<double>>(
                                        leftFrame->getTimestamp().time_since_epoch())
                                        .count();
            double right_timestamp = std::chrono::duration_cast<std::chrono::duration<double>>(
                                         rightFrame->getTimestamp().time_since_epoch())
                                         .count();

            // Log the timestamps
            // RCLCPP_INFO(this->get_logger(), "Left frame timestamp: %f", left_timestamp);
            // RCLCPP_INFO(this->get_logger(), "Right frame timestamp: %f", right_timestamp);

            auto now = this->now();

            cv::Mat leftImg = leftFrame->getCvFrame();
            cv::Mat rightImg = rightFrame->getCvFrame();

            cv::Mat rectifiedLeft, rectifiedRight;
            cv::remap(leftImg, rectifiedLeft, map1_x, map1_y, cv::INTER_LINEAR);
            cv::remap(rightImg, rectifiedRight, map2_x, map2_y, cv::INTER_LINEAR);

            auto rectifiedLeftMsg =
                cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", rectifiedLeft)
                    .toImageMsg();
            auto rectifiedRightMsg =
                cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", rectifiedRight)
                    .toImageMsg();

            // Convert double timestamps to builtin_interfaces::msg::Time
            builtin_interfaces::msg::Time left_ros_time;
            left_ros_time.sec = static_cast<int32_t>(left_timestamp);
            left_ros_time.nanosec = static_cast<uint32_t>((left_timestamp - left_ros_time.sec) * 1e9);

            builtin_interfaces::msg::Time right_ros_time;
            right_ros_time.sec = static_cast<int32_t>(right_timestamp);
            right_ros_time.nanosec = static_cast<uint32_t>((right_timestamp - right_ros_time.sec) * 1e9);

            // Assign the converted timestamps to the header.stamp fields
            rectifiedLeftMsg->header.stamp = left_ros_time;
            rectifiedRightMsg->header.stamp = right_ros_time;
            rectifiedLeftMsg->header.frame_id = "left_camera_optical_frame";
            rectifiedRightMsg->header.frame_id = "right_camera_optical_frame";

            auto leftCameraInfo = std::make_shared<sensor_msgs::msg::CameraInfo>();
            auto rightCameraInfo = std::make_shared<sensor_msgs::msg::CameraInfo>();

            updateCameraInfo(leftCameraInfo, P1, rectifiedLeft.size());
            updateCameraInfo(rightCameraInfo, P2, rectifiedRight.size());

            leftCameraInfo->header = rectifiedLeftMsg->header;
            rightCameraInfo->header = rectifiedRightMsg->header;

            rectifLeftImagePub->publish(*rectifiedLeftMsg);
            // print that the left image is published
            RCLCPP_INFO(this->get_logger(), "Left image published");
            rectifRightImagePub->publish(*rectifiedRightMsg);
            // print that the right image is published
            RCLCPP_INFO(this->get_logger(), "Right image published");
            leftCameraInfoPub->publish(*leftCameraInfo);
            rightCameraInfoPub->publish(*rightCameraInfo);
        }
        else
        {
            RCLCPP_WARN(this->get_logger(), "No frames received");
        }
    }
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<ImprovedStereoPublisher>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

I am only creating my own ROS 2 driver because the official one didn't work, and I asked for help on github and in this forum, but no one helped.

Hi, the timestamps of messages being output by sync node aren't affected by syncing mechanism so they correspond to the latest messages received, to solve that you could set the timestamp of one message (for example left) to both messages which would eliminate the time difference.

    Luxonis-Adam

    Yes, that is what I am doing now. I am using Jetson Orin Nano, and I have noticed that at lower fps there is no synchronization issue. I logged the time stamps of messages received by rtabmap, and I noticed that at higher fps of the depthai pipeline, it was not receiving some of the frames either from left or right, hence causing the synchronization. At lower fps, it seems to be synchronized.

      Luxonis-Adam

      At 30 FPS, I get unsynchronized frames; at 20, it are synchronized. Even if I am using the highest resolution of the camera, it should give at least 30 FPS.

      The following module is not available or enabled in the tegra kernel:

      root@ubuntu:/home/shivam157# modprobe usbmon
modprobe: FATAL: Module usbmon not found in directory /lib/modules/5.15.136-tegra

      So how do I use USBTOP without this?

        @erik @Luxonis-Adam

        After few minutes of running the camera or the camera driver throws the following error:

        [depthai_ros_driver_node-2] terminate called after throwing an instance of 'std::runtime_error'
[depthai_ros_driver_node-2]   what():  Communication exception - possible device error/misconfiguration. Original message 'Couldn't read data from stream: 'right' (X_LINK_ERROR)'
[ERROR] [depthai_ros_driver_node-2]: process has died [pid 43078, exit code -6, cmd '/home/shivam157/driver/install/depthai_ros_driver/lib/depthai_ros_driver/depthai_ros_driver_node --ros-args -r __node:=improved_stereo_publisher'].

        Why is this happening, and how can I avoid it?

        Also, I will try to compile my kernel with usbmon enabled so that we can use the USBTOP. Nvidia doesn't have this by default in jetpack 6.

        edit 2:

        receiving the following error when launching this ROS 2 driver:

        2024-08-07 17:41:53.122 [RTPS_TRANSPORT_SHM Error] Failed init_port fastrtps_port14911: open_and_lock_file failed -> Function open_port_internal
2024-08-07 17:41:53.123 [RTPS_TRANSPORT_SHM Error] Failed init_port fastrtps_port14913: open_and_lock_file failed -> Function open_port_internal
        12 days later

        Hi, these errors seem to be related to ROS's DDS mechanism, you could have some leftover processes from previous session. Regarding errors, could you post full logs from that run? Also, is the configuration working better if you lower the resolution or use Video Encoder nodes in the pipeline?