I have written my own ROS 2 driver using DepthAI C++ API. I create pipeline like the following:
// 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 sync = pipeline.create<dai::node::Sync>();
// auto xoutGrp = pipeline.create<dai::node::XLinkOut>();
auto xoutLeft = pipeline.create<dai::node::XLinkOut>();
auto xoutRight = pipeline.create<dai::node::XLinkOut>();
auto xoutDepth = pipeline.create<dai::node::XLinkOut>();
// XLinkOut
// xoutGrp->setStreamName("xout");
xoutDepth->setStreamName("depth");
xoutLeft->setStreamName("left");
xoutRight->setStreamName("right");
// Properties
monoLeft->setResolution(
dai::MonoCameraProperties::SensorResolution::THE_800_P);
monoLeft->setCamera("left");
monoLeft->setFps(30.0);
monoRight->setResolution(
dai::MonoCameraProperties::SensorResolution::THE_800_P);
monoRight->setCamera("right");
monoRight->setFps(30.0);
stereo->setDefaultProfilePreset(
dai::node::StereoDepth::PresetMode::HIGH_DENSITY);
stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
// stereo->setInputResolution(1280, 720);
stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_5x5);
stereo->setLeftRightCheck(lrcheck);
stereo->setExtendedDisparity(extended);
stereo->setSubpixel(subpixel);
// sync->setSyncThreshold(std::chrono::milliseconds(100));
// Linking
monoLeft->out.link(stereo->left);
monoRight->out.link(stereo->right);
stereo->syncedLeft.link(xoutLeft->input);
stereo->syncedRight.link(xoutRight->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 depthQueue = device.getOutputQueue("depth", 8, false);My question is: how can I synchronize the raw frames from the pipeline or the rectified frames I am generating using OpenCV in my ROS2 driver?
I publish the frames like the following:
while (rclcpp::ok()) {
auto leftFrame = leftQueue->get<dai::ImgFrame>();
auto rightFrame = rightQueue->get<dai::ImgFrame>();
if (leftFrame && rightFrame) {
auto current_time = this->now();
cv::Mat leftMat = leftFrame->getCvFrame();
cv::Mat rightMat = rightFrame->getCvFrame();
if (!leftMat.empty() && !rightMat.empty()) {
cv::Mat rectifiedLeft, rectifiedRight;
cv::remap(leftMat, rectifiedLeft, map1_x, map1_y, cv::INTER_LINEAR);
cv::remap(rightMat, rectifiedRight, map2_x, map2_y, cv::INTER_LINEAR);
auto rectifiedLeftImageMsg =
cv_bridge::CvImage(std_msgs::msg::Header(), "mono8",
rectifiedLeft)
.toImageMsg();
rectifiedLeftImageMsg->header.frame_id = "left_camera_optical_frame";
rectifiedLeftImageMsg->header.stamp = current_time;
auto rectifiedRightImageMsg =
cv_bridge::CvImage(std_msgs::msg::Header(), "mono8",
rectifiedRight)
.toImageMsg();
rectifiedRightImageMsg->header.frame_id =
"right_camera_optical_frame";
rectifiedRightImageMsg->header.stamp = current_time;
// Updating the CameraInfo message
auto leftCameraInfo =
std::make_shared<sensor_msgs::msg::CameraInfo>();
auto rightCameraInfo =
std::make_shared<sensor_msgs::msg::CameraInfo>();
// Fill in new intrinsic parameters derived from P1 and P2 (3x3
// top-left submatrix)
updateCameraInfo(leftCameraInfo, P1, rectifiedLeft.size());
updateCameraInfo(rightCameraInfo, P2, rectifiedRight.size());
leftCameraInfo->header.stamp = current_time;
leftCameraInfo->header.frame_id = "left_camera_optical_frame";
rightCameraInfo->header.stamp = current_time;
rightCameraInfo->header.frame_id = "right_camera_optical_frame";
leftCameraInfoPub->publish(*leftCameraInfo);
rightCameraInfoPub->publish(*rightCameraInfo);
rectifLeftImagePub->publish(*rectifiedLeftImageMsg);
rectifRightImagePub->publish(*rectifiedRightImageMsg);
}
}
}
}
