[BUG] Not able to get video when using stereo pipeline

[Shivam7Sharma]

I am creating a ROS 2 node using the DepthAI core library. I was able to get raw videos from the two OV9282 cameras connected to OAK FFC 3P but when I add a stereo pipeline to get rectified stream and depth, I get no video in the queue. I used the stereo_video.cpp file in the examples folder for reference, but it is still not working after I add the stereo pipeline. The sterep_video.cpp file works fine; it gives all 6 outputs.

The raw video does show one frame, and then it freezes. It also looks like the node freezes after a few seconds.

I don't think this is a known issue. Any help is appreciated. I am not using the DepthAI ROS 2 bridge or the ROS 2 driver because they have a lot of bugs.

The following is my code:

#include <cv_bridge/cv_bridge.h>
#include <depthai/depthai.hpp>
#include <depthai_bridge/BridgePublisher.hpp>
#include <depthai_bridge/ImageConverter.hpp>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>

static std::atomic<bool> withDepth{false};

static std::atomic<bool> outputDepth{false};
static std::atomic<bool> outputRectified{true};
static std::atomic<bool> lrcheck{true};
static std::atomic<bool> extended{false};
static std::atomic<bool> subpixel{false};

class DepthAICameraNode : public rclcpp::Node {
public:
  DepthAICameraNode() : Node("depthai_camera_node") {

    // Set logger level to debug
    this->get_logger().set_level(rclcpp::Logger::Level::Debug);
    // Create pipeline
    pipeline = std::make_shared<dai::Pipeline>();

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

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

    // Properties configuration omitted for brevity...

    // XLinkOut
    xoutLeft->setStreamName("left");
    xoutRight->setStreamName("right");
    xoutRectifL->setStreamName("rectified_left");
    xoutRectifR->setStreamName("rectified_right");
    xoutDisp->setStreamName("disparity");
    xoutDepth->setStreamName("depth");
    // Properties
    monoLeft->setResolution(
        dai::MonoCameraProperties::SensorResolution::THE_800_P);
    monoLeft->setCamera("left");
    // monoLeft->setFps(90.0); // Setting the FPS to 90
    monoRight->setResolution(
        dai::MonoCameraProperties::SensorResolution::THE_800_P);
    monoRight->setCamera("right");
    // monoRight->setFps(90.0); // Setting the FPS to 90

    stereo->setDefaultProfilePreset(
        dai::node::StereoDepth::PresetMode::HIGH_DENSITY);
    stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
    stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_5x5);
    stereo->setLeftRightCheck(lrcheck);
    stereo->setExtendedDisparity(extended);
    stereo->setSubpixel(subpixel);

    // Link plugins CAM -> XLINK
    // Linking
    monoLeft->out.link(stereo->left);
    monoRight->out.link(stereo->right);
    // monoLeft->out.link(xoutLeft->input);
    // monoRight->out.link(xoutRight->input);
    stereo->syncedLeft.link(xoutLeft->input);
    stereo->syncedRight.link(xoutRight->input);
    stereo->disparity.link(xoutDisp->input);

    stereo->rectifiedLeft.link(xoutRectifL->input);
    stereo->rectifiedRight.link(xoutRectifR->input);
    stereo->depth.link(xoutDepth->input);

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

    leftQueue = device->getOutputQueue("left", 8, false);
    rightQueue = device->getOutputQueue("right", 8, false);
    rectifLeftQueue = device->getOutputQueue("rectified_left", 8, false);
    rectifRightQueue = device->getOutputQueue("rectified_right", 8, false);

    // Publishers
    leftImagePub =
        this->create_publisher<sensor_msgs::msg::Image>("left/image_raw", 10);
    rightImagePub =
        this->create_publisher<sensor_msgs::msg::Image>("right/image_raw", 10);
    rectifLeftImagePub =
        this->create_publisher<sensor_msgs::msg::Image>("left/image_rect", 10);
    rectifRightImagePub =
        this->create_publisher<sensor_msgs::msg::Image>("right/image_rect", 10);

    // Timer to publish images
    timer = this->create_wall_timer(
        std::chrono::milliseconds(33),
        std::bind(&DepthAICameraNode::publishImages, this));
  }

private:
  void publishImages() {
    // Fetch and publish images (implementation details omitted for brevity)

    try {
      auto leftFrame = leftQueue->get<dai::ImgFrame>();
      cv::imshow("left", leftFrame->getFrame());
      auto rightFrame = rightQueue->get<dai::ImgFrame>();
      //   cv::imshow("right", rightFrame->getFrame());

      auto rectifL = rectifLeftQueue->get<dai::ImgFrame>();
      auto rectifR = rectifRightQueue->get<dai::ImgFrame>();
      cv::waitKey(1);

      // if (leftFrame) {
      //   cv::Mat leftMat = leftFrame->getCvFrame();
      //   if (leftMat.empty()) {
      //     RCLCPP_ERROR(this->get_logger(), "Left frame is empty!");
      //     return;
      //   }
      //   RCLCPP_DEBUG(this->get_logger(), "Left Frame: %d x %d", leftMat.cols,
      //                leftMat.rows);
      //   auto leftImageMsg =
      //       cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", leftMat)
      //           .toImageMsg();
      //   leftImagePub->publish(*leftImageMsg);
      // } else {
      //   RCLCPP_ERROR(this->get_logger(), "Failed to get left frame");
      // }

      // if (rightFrame) {
      //   cv::Mat rightMat = rightFrame->getCvFrame();
      //   if (rightMat.empty()) {
      //     RCLCPP_ERROR(this->get_logger(), "Right frame is empty!");
      //     return;
      //   }
      //   RCLCPP_DEBUG(this->get_logger(), "Right Frame: %d x %d",
      //   rightMat.cols,
      //                rightMat.rows);
      //   auto rightImageMsg =
      //       cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", rightMat)
      //           .toImageMsg();
      //   rightImagePub->publish(*rightImageMsg);
      // } else {
      //   RCLCPP_ERROR(this->get_logger(), "Failed to get right frame");
      // }
      // if (rectifL) {
      //   cv::Mat rectifLMat = rectifL->getCvFrame();
      //   if (rectifLMat.empty()) {
      //     RCLCPP_ERROR(this->get_logger(), "Rectified left frame is empty!");
      //     return;
      //   }
      //   RCLCPP_DEBUG(this->get_logger(), "Rectified Left Frame: %d x %d",
      //                rectifLMat.cols, rectifLMat.rows);
      //   auto rectifLImageMsg =
      //       cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", rectifLMat)
      //           .toImageMsg();
      //   rectifLeftImagePub->publish(*rectifLImageMsg);
      // } else {
      //   RCLCPP_ERROR(this->get_logger(), "Failed to get rectified left
      //   frame");
      // }

      // if (rectifR) {
      //   cv::Mat rectifRMat = rectifR->getCvFrame();
      //   if (rectifRMat.empty()) {
      //     RCLCPP_ERROR(this->get_logger(), "Rectified right frame is
      //     empty!"); return;
      //   }
      //   RCLCPP_DEBUG(this->get_logger(), "Rectified Right Frame: %d x %d",
      //                rectifRMat.cols, rectifRMat.rows);
      //   auto rectifRImageMsg =
      //       cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", rectifRMat)
      //           .toImageMsg();
      //   rectifRightImagePub->publish(*rectifRImageMsg);
      // } else {
      //   RCLCPP_ERROR(this->get_logger(), "Failed to get rectified right
      //   frame");
      // }

    } catch (const std::exception &e) {
      RCLCPP_ERROR(this->get_logger(), "Exception: %s", e.what());
    }
  }

  std::shared_ptr<dai::Pipeline> pipeline;
  std::shared_ptr<dai::Device> device;
  std::shared_ptr<dai::DataOutputQueue> leftQueue, rightQueue, rectifLeftQueue,
      rectifRightQueue;
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr leftImagePub,
      rightImagePub, rectifLeftImagePub, rectifRightImagePub;
  rclcpp::TimerBase::SharedPtr timer;
};

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

[Shivam7Sharma]

I changed the code to the following, and now stereo works. I would still like to know what was wrong with my first code.

#include <cv_bridge/cv_bridge.h>
#include <depthai/depthai.hpp>
#include <depthai_bridge/BridgePublisher.hpp>
#include <depthai_bridge/ImageConverter.hpp>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>

static std::atomic<bool> outputDepth{false};
static std::atomic<bool> outputRectified{true};
static std::atomic<bool> lrcheck{true};
static std::atomic<bool> extended{false};
static std::atomic<bool> subpixel{false};

class EventDrivenPublisher : public rclcpp::Node {
public:
  EventDrivenPublisher() : Node("event_driven_publisher") {
    // Publishers
    this->get_logger().set_level(rclcpp::Logger::Level::Debug);

    leftImagePub =
        this->create_publisher<sensor_msgs::msg::Image>("left/image_raw", 10);
    rightImagePub =
        this->create_publisher<sensor_msgs::msg::Image>("right/image_raw", 10);
    rectifLeftImagePub =
        this->create_publisher<sensor_msgs::msg::Image>("left/image_rect", 10);
    rectifRightImagePub =
        this->create_publisher<sensor_msgs::msg::Image>("right/image_rect", 10);
    process_and_publish();
  }

private:
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr leftImagePub,
      rightImagePub, rectifLeftImagePub, rectifRightImagePub;

  void process_and_publish() {

    // Create pipeline
    dai::Pipeline pipeline;

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

    auto xoutLeft = pipeline.create<dai::node::XLinkOut>();
    auto xoutRight = pipeline.create<dai::node::XLinkOut>();
    auto xoutDisp = pipeline.create<dai::node::XLinkOut>();
    auto xoutDepth = pipeline.create<dai::node::XLinkOut>();
    auto xoutRectifL = pipeline.create<dai::node::XLinkOut>();
    auto xoutRectifR = pipeline.create<dai::node::XLinkOut>();

    // Properties configuration omitted for brevity...

    // XLinkOut
    xoutLeft->setStreamName("left");
    xoutRight->setStreamName("right");
    xoutRectifL->setStreamName("rectified_left");
    xoutRectifR->setStreamName("rectified_right");
    xoutDisp->setStreamName("disparity");
    xoutDepth->setStreamName("depth");
    // Properties
    monoLeft->setResolution(
        dai::MonoCameraProperties::SensorResolution::THE_400_P);
    monoLeft->setCamera("left");
    monoLeft->setFps(60.0); // Setting the FPS to 90
    monoRight->setResolution(
        dai::MonoCameraProperties::SensorResolution::THE_400_P);
    monoRight->setCamera("right");
    monoRight->setFps(60.0); // Setting the FPS to 90

    stereo->setDefaultProfilePreset(
        dai::node::StereoDepth::PresetMode::HIGH_DENSITY);
    stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
    stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_5x5);
    stereo->setLeftRightCheck(lrcheck);
    stereo->setExtendedDisparity(extended);
    stereo->setSubpixel(subpixel);

    // Link plugins CAM -> XLINK
    // Linking
    monoLeft->out.link(stereo->left);
    monoRight->out.link(stereo->right);
    // monoLeft->out.link(xoutLeft->input);
    // monoRight->out.link(xoutRight->input);
    stereo->syncedLeft.link(xoutLeft->input);
    stereo->syncedRight.link(xoutRight->input);
    stereo->disparity.link(xoutDisp->input);

    stereo->rectifiedLeft.link(xoutRectifL->input);
    stereo->rectifiedRight.link(xoutRectifR->input);
    stereo->depth.link(xoutDepth->input);

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

    auto leftQueue = device.getOutputQueue("left", 8, false);
    auto rightQueue = device.getOutputQueue("right", 8, false);
    auto dispQueue = device.getOutputQueue("disparity", 8, false);
    auto depthQueue = device.getOutputQueue("depth", 8, false);
    auto rectifLeftQueue = device.getOutputQueue("rectified_left", 8, false);
    auto rectifRightQueue = device.getOutputQueue("rectified_right", 8, false);

    while (rclcpp::ok()) {
      // Fetch and publish images (implementation details omitted for brevity)

      try {
        auto leftFrame = leftQueue->get<dai::ImgFrame>();
        // cv::imshow("left", leftFrame->getFrame());
        auto rightFrame = rightQueue->get<dai::ImgFrame>();
        //   cv::imshow("right", rightFrame->getFrame());

        auto rectifL = rectifLeftQueue->get<dai::ImgFrame>();
        auto rectifR = rectifRightQueue->get<dai::ImgFrame>();
        // cv::waitKey(1);

        if (leftFrame) {
          cv::Mat leftMat = leftFrame->getCvFrame();
          if (leftMat.empty()) {
            RCLCPP_ERROR(this->get_logger(), "Left frame is empty!");
            return;
          }
          RCLCPP_DEBUG(this->get_logger(), "Left Frame: %d x %d", leftMat.cols,
                       leftMat.rows);
          auto leftImageMsg =
              cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", leftMat)
                  .toImageMsg();
          leftImagePub->publish(*leftImageMsg);
        } else {
          RCLCPP_ERROR(this->get_logger(), "Failed to get left frame");
        }

        if (rightFrame) {
          cv::Mat rightMat = rightFrame->getCvFrame();
          if (rightMat.empty()) {
            RCLCPP_ERROR(this->get_logger(), "Right frame is empty!");
            return;
          }
          RCLCPP_DEBUG(this->get_logger(), "Right Frame: %d x %d",
                       rightMat.cols, rightMat.rows);
          auto rightImageMsg =
              cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", rightMat)
                  .toImageMsg();
          rightImagePub->publish(*rightImageMsg);
        } else {
          RCLCPP_ERROR(this->get_logger(), "Failed to get right frame");
        }
        if (rectifL) {
          cv::Mat rectifLMat = rectifL->getCvFrame();
          if (rectifLMat.empty()) {
            RCLCPP_ERROR(this->get_logger(), "Rectified left frame is empty");
            return;
          }
          RCLCPP_DEBUG(this->get_logger(), "Rectified Left Frame: %d x %d",
                       rectifLMat.cols, rectifLMat.rows);
          auto rectifLImageMsg =
              cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", rectifLMat)
                  .toImageMsg();
          rectifLeftImagePub->publish(*rectifLImageMsg);
        } else {
          RCLCPP_ERROR(this->get_logger(),
                       "Failed to get rectified left frame");
        }

        if (rectifR) {
          cv::Mat rectifRMat = rectifR->getCvFrame();
          if (rectifRMat.empty()) {
            RCLCPP_ERROR(this->get_logger(), "Rectified right frame is empty!");
            return;
          }
          RCLCPP_DEBUG(this->get_logger(), "Rectified Right Frame: %d x %d",
                       rectifRMat.cols, rectifRMat.rows);
          auto rectifRImageMsg =
              cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", rectifRMat)
                  .toImageMsg();
          rectifRightImagePub->publish(*rectifRImageMsg);
        } else {
          RCLCPP_ERROR(this->get_logger(),
                       "Failed to get rectified right frame");
        }

      } catch (const std::exception &e) {
        RCLCPP_ERROR(this->get_logger(), "Exception: %s", e.what());
      }
    }
  }
};

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

[Serafadam]

Hi, I think the major difference is that in the former example depth /disparity frames are not consumed which might lead to blocking. Also, using queue->getdai::ImgFrame() is a blocking call, you can try changing that to tryGet().