Shivam7Sharma
commented
2 months ago 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
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: