ON THIS PAGE

  • DepthAI Bridge
  • Introduction
  • Available Converters
  • Usage

DepthAI Bridge

Introduction

DepthAI ROS Bridge (depthai_bridge package) is a ROS package that allows creating pipelines in a more controlled way using C++ DepthAI API and ROS message converters.

Available Converters

ConverterDescription
Disparity converterConverts stereo disparity messages
Image converterUsed to convert various types of Image messages to/from DAI/OpenCV formats
IMG Detections converterConverts 2D Detections
IMU converterConverts IMU data, has the possibility to interpolate IMU readings
Spatial Detections converterConverts 3D Spatial detections
Tracked Features converterConverts Tracked Features
Track Detections converterConverts DAI Tracklets
TF PublisherIt is used to convert DAI calibration data and publish it as ROS Transform data

Usage

You can refer to depthai_examples package on how to set up pipeline and use BridgePublisher class and ImageConverters to publish data in ROS format. Below is a simple example for RGB publishing
C++
1#include <cstdio>
2
3#include "depthai/device/Device.hpp"
4#include "depthai/pipeline/Pipeline.hpp"
5#include "depthai/pipeline/node/Camera.hpp"
6#include "depthai_bridge/BridgePublisher.hpp"
7#include "depthai_bridge/ImageConverter.hpp"
8#include "depthai_bridge/TFPublisher.hpp"
9#include "depthai_bridge/depthaiUtility.hpp"
10#include "rclcpp/node.hpp"
11
12int main(int argc, char** argv) {
13    int width = 1280;
14    int height = 720;
15    std::string tfPrefix = "oak";
16    rclcpp::init(argc, argv);
17    auto node = rclcpp::Node::make_shared("rgb_publisher");
18
19    auto device = std::make_shared<dai::Device>();
20    dai::Pipeline pipeline(device);
21
22    // Define sources and outputs
23    auto rgbCamera = pipeline.create<dai::node::Camera>()->build(dai::CameraBoardSocket::CAM_A);
24
25    // Create output queue
26    auto rgbOutputQueue = rgbCamera->requestOutput({width, height})->createOutputQueue(8, false);
27
28    pipeline.start();
29
30    // Create a bridge publisher for RGB images
31    auto rgbConverter = std::make_shared<depthai_bridge::ImageConverter>(
32        depthai_bridge::getOpticalFrameName(tfPrefix, depthai_bridge::getSocketName(dai::CameraBoardSocket::CAM_A, device->getDeviceName())), false);
33
34    auto calibrationHandler = device->readCalibration();
35    auto tfPub =
36        std::make_unique<depthai_bridge::TFPublisher>(node, calibrationHandler, device->getConnectedCameraFeatures(), tfPrefix, device->getDeviceName());
37    auto rgbCameraInfo = rgbConverter->calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, width, height);
38
39    auto rgbPub = std::make_unique<depthai_bridge::BridgePublisher<sensor_msgs::msg::Image, dai::ImgFrame>>(
40        rgbOutputQueue,
41        node,
42        "rgb/image",
43        [rgbConverter](std::shared_ptr<dai::ImgFrame> msg, std::deque<sensor_msgs::msg::Image>& rosMsgs) { rgbConverter->toRosMsg(msg, rosMsgs); },
44        30,
45        rgbCameraInfo,
46        "rgb");
47
48    rgbPub->addPublisherCallback();
49
50    while(rclcpp::ok() && pipeline.isRunning()) {
51        rclcpp::spin(node);
52    }
53
54    return 0;
55}