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
Converter | Description |
---|---|
Disparity converter | Converts stereo disparity messages |
Image converter | Used to convert various types of Image messages to/from DAI/OpenCV formats |
IMG Detections converter | Converts 2D Detections |
IMU converter | Converts IMU data, has the possibility to interpolate IMU readings |
Spatial Detections converter | Converts 3D Spatial detections |
Tracked Features converter | Converts Tracked Features |
Track Detections converter | Converts DAI Tracklets |
TF Publisher | It is used to convert DAI calibration data and publish it as ROS Transform data |
Usage
You can refer todepthai_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 publishingC++
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}