H.264 ROS output for RGB and Mono nodes

Dockerfile

@@ -6,6 +6,8 @@ ENV DEBIAN_FRONTEND=noninteractive
 RUN apt-get update \
    && apt-get -y install --no-install-recommends software-properties-common git libusb-1.0-0-dev wget zsh python3-colcon-common-extensions
 
+# install latest
+RUN apt-get -y install ros-${ROS_DISTRO}-depthai
 
 ENV DEBIAN_FRONTEND=dialog
 RUN sh -c "$(wget https://raw.github.com/ohmyzsh/ohmyzsh/master/tools/install.sh -O -)"

depthai_bridge/CMakeLists.txt

@@ -41,6 +41,7 @@ find_package(tf2_ros REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(composition_interfaces REQUIRED)
+find_package(ffmpeg_image_transport_msgs REQUIRED)
 
 set(dependencies
   camera_info_manager
@@ -56,6 +57,7 @@ set(dependencies
   tf2_geometry_msgs
   tf2
   composition_interfaces
+  ffmpeg_image_transport_msgs
 )
 
 include_directories(

depthai_bridge/include/depthai_bridge/ImageConverter.hpp

@@ -11,18 +11,22 @@
 #include "depthai-shared/common/Point2f.hpp"
 #include "depthai/device/CalibrationHandler.hpp"
 #include "depthai/pipeline/datatype/ImgFrame.hpp"
+#include "depthai/pipeline/datatype/EncodedFrame.hpp"
 #include "rclcpp/time.hpp"
 #include "sensor_msgs/msg/camera_info.hpp"
 #include "sensor_msgs/msg/image.hpp"
 #include "std_msgs/msg/header.hpp"
+#include "ffmpeg_image_transport_msgs/msg/ffmpeg_packet.hpp"
 
 namespace dai {
 
 namespace ros {
 
 namespace StdMsgs = std_msgs::msg;
 namespace ImageMsgs = sensor_msgs::msg;
+namespace FFMPEGMsgs = ffmpeg_image_transport_msgs::msg;
 using ImagePtr = ImageMsgs::Image::SharedPtr;
+using FFMPEGPacketPtr = FFMPEGMsgs::FFMPEGPacket::SharedPtr;
 
 using TimePoint = std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration>;
 
@@ -84,6 +88,9 @@ class ImageConverter {
     ImageMsgs::Image toRosMsgRawPtr(std::shared_ptr<dai::ImgFrame> inData, const sensor_msgs::msg::CameraInfo& info = sensor_msgs::msg::CameraInfo());
     ImagePtr toRosMsgPtr(std::shared_ptr<dai::ImgFrame> inData);
 
+    FFMPEGMsgs::FFMPEGPacket toRosVideoMsgRawPtr(std::shared_ptr<dai::EncodedFrame> inData, int fw, int fh);
+    // FFMPEGPacketPtr toRosMsgH264Ptr(std::shared_ptr<dai::EncodedFrame> inData);
+
     void toDaiMsg(const ImageMsgs::Image& inMsg, dai::ImgFrame& outData);
 
     /** TODO(sachin): Add support for ros msg to cv mat since we have some

depthai_bridge/src/ImageConverter.cpp

@@ -218,11 +218,66 @@ void ImageConverter::toRosMsg(std::shared_ptr<dai::ImgFrame> inData, std::deque<
 
 ImagePtr ImageConverter::toRosMsgPtr(std::shared_ptr<dai::ImgFrame> inData) {
     auto msg = toRosMsgRawPtr(inData);
-
     ImagePtr ptr = std::make_shared<ImageMsgs::Image>(msg);
     return ptr;
 }
 
+/**
+ * This should be implemented in dai::EncodedFrame but I'm not gonna
+ * edit another repo for it
+ */
+std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration> getOffsetTimestamp(std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration> ts, CameraExposureOffset offset, std::chrono::microseconds expTime) {
+    switch(offset) {
+        case CameraExposureOffset::START:
+            return ts - expTime;
+        case CameraExposureOffset::MIDDLE:
+            return ts - expTime / 2;
+        case CameraExposureOffset::END:
+        default:
+            return ts;
+    }
+}
+
+FFMPEGMsgs::FFMPEGPacket ImageConverter::toRosVideoMsgRawPtr(std::shared_ptr<dai::EncodedFrame> inData, int fw, int fh) {
+    if(_updateRosBaseTimeOnToRosMsg) {
+        updateRosBaseTime();
+    }
+    std::chrono::_V2::steady_clock::time_point tstamp;
+    if(_getBaseDeviceTimestamp)
+        if(_addExpOffset)
+            tstamp = getOffsetTimestamp(inData->getTimestampDevice(), _expOffset, inData->getExposureTime()); 
+        else
+            tstamp = inData->getTimestampDevice(); 
+    else if(_addExpOffset)
+        tstamp = getOffsetTimestamp(inData->getTimestamp(), _expOffset, inData->getExposureTime()); 
+    else
+        tstamp = tstamp = inData->getTimestamp();
+
+    FFMPEGMsgs::FFMPEGPacket outFrameMsg;
+    StdMsgs::Header header;
+    header.frame_id = _frameName;
+    header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, tstamp);
+    outFrameMsg.header = header;
+
+    auto ft = inData->getFrameType(); // I = keyframe
+
+    outFrameMsg.width = fw;
+    outFrameMsg.height = fh;
+    outFrameMsg.encoding = "h.264";
+    outFrameMsg.pts = header.stamp.sec * 1000000000 + header.stamp.nanosec;
+    outFrameMsg.flags = (int) (ft == RawEncodedFrame::FrameType::I);
+    outFrameMsg.is_bigendian = false;
+    outFrameMsg.data = inData->getData();
+
+    return outFrameMsg;
+}
+
+// FFMPEGPacketPtr ImageConverter::toRosMsgH264Ptr(std::shared_ptr<dai::EncodedFrame> inData) {
+//     auto msg = toRosVideoMsgRawPtr(inData);
+//     FFMPEGPacketPtr ptr = std::make_shared<FFMPEGMsgs::FFMPEGPacket>(msg);
+//     return ptr;
+// }
+
 void ImageConverter::toDaiMsg(const ImageMsgs::Image& inMsg, dai::ImgFrame& outData) {
     std::unordered_map<dai::RawImgFrame::Type, std::string>::iterator revEncodingIter;
     if(_daiInterleaved) {

depthai_ros_driver/CMakeLists.txt

@@ -30,6 +30,7 @@ rclcpp
 sensor_msgs
 diagnostic_updater
 diagnostic_msgs
+ffmpeg_image_transport_msgs
 )
 
 set(SENSOR_DEPS

depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/mono.hpp

@@ -6,6 +6,7 @@
 #include "image_transport/image_transport.hpp"
 #include "sensor_msgs/msg/camera_info.hpp"
 #include "sensor_msgs/msg/image.hpp"
+#include "ffmpeg_image_transport_msgs/msg/ffmpeg_packet.hpp"
 
 namespace dai {
 class Pipeline;
@@ -59,16 +60,17 @@ class Mono : public BaseNode {
     std::unique_ptr<dai::ros::ImageConverter> imageConverter;
     image_transport::CameraPublisher monoPubIT;
     rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr monoPub;
+    rclcpp::Publisher<ffmpeg_image_transport_msgs::msg::FFMPEGPacket>::SharedPtr h264Pub;
     rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub;
     std::shared_ptr<camera_info_manager::CameraInfoManager> infoManager;
     std::shared_ptr<dai::node::MonoCamera> monoCamNode;
-    std::shared_ptr<dai::node::VideoEncoder> videoEnc;
+    std::shared_ptr<dai::node::VideoEncoder> videoEnc, videoEncH264;
     std::unique_ptr<param_handlers::SensorParamHandler> ph;
-    std::shared_ptr<dai::DataOutputQueue> monoQ;
+    std::shared_ptr<dai::DataOutputQueue> monoQ, h264Q;
     std::shared_ptr<dai::DataInputQueue> controlQ;
-    std::shared_ptr<dai::node::XLinkOut> xoutMono;
+    std::shared_ptr<dai::node::XLinkOut> xoutMono, xoutH264;
     std::shared_ptr<dai::node::XLinkIn> xinControl;
-    std::string monoQName, controlQName;
+    std::string monoQName, controlQName, h264QName;
 };
 
 }  // namespace dai_nodes

depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/rgb.hpp

@@ -5,6 +5,7 @@
 #include "image_transport/image_transport.hpp"
 #include "sensor_msgs/msg/camera_info.hpp"
 #include "sensor_msgs/msg/image.hpp"
+#include "ffmpeg_image_transport_msgs/msg/ffmpeg_packet.hpp"
 
 namespace dai {
 class Pipeline;
@@ -63,16 +64,17 @@ class RGB : public BaseNode {
     std::unique_ptr<dai::ros::ImageConverter> imageConverter;
     image_transport::CameraPublisher rgbPubIT, previewPubIT;
     rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgbPub, previewPub;
+    rclcpp::Publisher<ffmpeg_image_transport_msgs::msg::FFMPEGPacket>::SharedPtr h264Pub;
     rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rgbInfoPub, previewInfoPub;
     std::shared_ptr<camera_info_manager::CameraInfoManager> infoManager, previewInfoManager;
     std::shared_ptr<dai::node::ColorCamera> colorCamNode;
-    std::shared_ptr<dai::node::VideoEncoder> videoEnc;
+    std::shared_ptr<dai::node::VideoEncoder> videoEnc, videoEncH264;
     std::unique_ptr<param_handlers::SensorParamHandler> ph;
-    std::shared_ptr<dai::DataOutputQueue> colorQ, previewQ;
+    std::shared_ptr<dai::DataOutputQueue> colorQ, previewQ, h264Q;
     std::shared_ptr<dai::DataInputQueue> controlQ;
-    std::shared_ptr<dai::node::XLinkOut> xoutColor, xoutPreview;
+    std::shared_ptr<dai::node::XLinkOut> xoutColor, xoutPreview, xoutH264;
     std::shared_ptr<dai::node::XLinkIn> xinControl;
-    std::string ispQName, previewQName, controlQName;
+    std::string ispQName, previewQName, controlQName, h264QName;
 };
 
 }  // namespace dai_nodes

depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp

@@ -12,6 +12,7 @@
 #include "depthai/pipeline/datatype/CameraControl.hpp"
 #include "image_transport/camera_publisher.hpp"
 #include "sensor_msgs/msg/camera_info.hpp"
+#include "ffmpeg_image_transport_msgs/msg/ffmpeg_packet.hpp"
 
 namespace dai {
 class Device;
@@ -63,6 +64,14 @@ void cameraPub(const std::string& /*name*/,
                std::shared_ptr<camera_info_manager::CameraInfoManager> infoManager,
                bool lazyPub = true);
 
+void videoPub(const std::string& /*name*/,
+               const std::shared_ptr<dai::ADatatype>& data,
+               dai::ros::ImageConverter& converter,
+               rclcpp::Publisher<ffmpeg_image_transport_msgs::msg::FFMPEGPacket>::SharedPtr pub,
+               int w,
+               int h,
+               bool lazyPub = true);
+
 void splitPub(const std::string& /*name*/,
               const std::shared_ptr<dai::ADatatype>& data,
               dai::ros::ImageConverter& converter,

depthai_ros_driver/package.xml

@@ -18,6 +18,7 @@
   <depend>vision_msgs</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
+  <depend>ffmpeg_image_transport_msgs</depend>
   <depend>cv_bridge</depend>
   <depend>image_transport</depend>
   <depend>image_transport_plugins</depend>

depthai_ros_driver/src/dai_nodes/sensors/mono.cpp

@@ -31,12 +31,13 @@ Mono::Mono(const std::string& daiNodeName,
     ph = std::make_unique<param_handlers::SensorParamHandler>(node, daiNodeName, socket);
     ph->declareParams(monoCamNode, sensor, publish);
     setXinXout(pipeline);
-    RCLCPP_DEBUG(node->get_logger(), "Node %s created", daiNodeName.c_str());
+    RCLCPP_INFO(node->get_logger(), "Node %s created", daiNodeName.c_str());
 }
 Mono::~Mono() = default;
 void Mono::setNames() {
     monoQName = getName() + "_mono";
     controlQName = getName() + "_control";
+    h264QName = getName() + "_h264";
 }
 
 void Mono::setXinXout(std::shared_ptr<dai::Pipeline> pipeline) {
@@ -51,27 +52,42 @@ void Mono::setXinXout(std::shared_ptr<dai::Pipeline> pipeline) {
             monoCamNode->out.link(xoutMono->input);
         }
     }
+    if(ph->getParam<bool>("i_enable_h264")) {
+        RCLCPP_INFO(getROSNode()->get_logger(), "Setting up h264 output %s (q=%d)", h264QName.c_str(), ph->getParam<int>("i_h264_quality"));
+        videoEncH264 = sensor_helpers::createEncoder(pipeline, ph->getParam<int>("i_h264_quality"), dai::VideoEncoderProperties::Profile::H264_HIGH);
+        videoEncH264->setKeyframeFrequency(30); // one kf / second @ 30 hz
+        monoCamNode->out.link(videoEncH264->input);
+
+        xoutH264 = pipeline->create<dai::node::XLinkOut>();
+        xoutH264->setStreamName(h264QName);
+        xoutH264->input.setQueueSize(2);
+        xoutH264->input.setBlocking(false);
+        videoEncH264->out.link(xoutH264->input);
+    }
     xinControl = pipeline->create<dai::node::XLinkIn>();
     xinControl->setStreamName(controlQName);
     xinControl->out.link(monoCamNode->inputControl);
 }
 
 void Mono::setupQueues(std::shared_ptr<dai::Device> device) {
+
+    auto tfPrefix = getTFPrefix(utils::getSocketName(static_cast<dai::CameraBoardSocket>(ph->getParam<int>("i_board_socket_id"))));
+
+    imageConverter = std::make_unique<dai::ros::ImageConverter>(tfPrefix + "_camera_optical_frame", false, ph->getParam<bool>("i_get_base_device_timestamp"));
+    imageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam<bool>("i_update_ros_base_time_on_ros_msg"));
+
+    if(ph->getParam<bool>("i_low_bandwidth")) {
+        imageConverter->convertFromBitstream(dai::RawImgFrame::Type::GRAY8);
+    }
+    if(ph->getParam<bool>("i_add_exposure_offset")) {
+        auto offset = static_cast<dai::CameraExposureOffset>(ph->getParam<int>("i_exposure_offset"));
+        imageConverter->addExposureOffset(offset);
+    }
+    if(ph->getParam<bool>("i_reverse_stereo_socket_order")) {
+        imageConverter->reverseStereoSocketOrder();
+    }
     if(ph->getParam<bool>("i_publish_topic")) {
-        auto tfPrefix = getTFPrefix(utils::getSocketName(static_cast<dai::CameraBoardSocket>(ph->getParam<int>("i_board_socket_id"))));
-        imageConverter =
-            std::make_unique<dai::ros::ImageConverter>(tfPrefix + "_camera_optical_frame", false, ph->getParam<bool>("i_get_base_device_timestamp"));
-        imageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam<bool>("i_update_ros_base_time_on_ros_msg"));
-        if(ph->getParam<bool>("i_low_bandwidth")) {
-            imageConverter->convertFromBitstream(dai::RawImgFrame::Type::GRAY8);
-        }
-        if(ph->getParam<bool>("i_add_exposure_offset")) {
-            auto offset = static_cast<dai::CameraExposureOffset>(ph->getParam<int>("i_exposure_offset"));
-            imageConverter->addExposureOffset(offset);
-        }
-        if(ph->getParam<bool>("i_reverse_stereo_socket_order")) {
-            imageConverter->reverseStereoSocketOrder();
-        }
+        monoQ = device->getOutputQueue(monoQName, ph->getParam<int>("i_max_q_size"), false);
         infoManager = std::make_shared<camera_info_manager::CameraInfoManager>(
             getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName());
         if(ph->getParam<std::string>("i_calibration_file").empty()) {
@@ -84,7 +100,6 @@ void Mono::setupQueues(std::shared_ptr<dai::Device> device) {
         } else {
             infoManager->loadCameraInfo(ph->getParam<std::string>("i_calibration_file"));
         }
-        monoQ = device->getOutputQueue(monoQName, ph->getParam<int>("i_max_q_size"), false);
         if(ipcEnabled()) {
             RCLCPP_DEBUG(getROSNode()->get_logger(), "Enabling intra_process communication!");
             monoPub = getROSNode()->create_publisher<sensor_msgs::msg::Image>("~/" + getName() + "/image_raw", 10);
@@ -109,12 +124,28 @@ void Mono::setupQueues(std::shared_ptr<dai::Device> device) {
                                          ph->getParam<bool>("i_enable_lazy_publisher")));
         }
     }
+    if(ph->getParam<bool>("i_enable_h264")) {
+        RCLCPP_INFO(getROSNode()->get_logger(), "Setting up h264 queue %s", tfPrefix.c_str());
+
+        h264Q = device->getOutputQueue(h264QName, ph->getParam<int>("i_max_q_size"), false);
+        h264Pub = getROSNode()->create_publisher<ffmpeg_image_transport_msgs::msg::FFMPEGPacket>("~/" + getName() + "/h264", 10);
+        h264Q->addCallback(std::bind(sensor_helpers::videoPub,
+                                        std::placeholders::_1,
+                                        std::placeholders::_2,
+                                        *imageConverter,
+                                        h264Pub,
+                                        ph->getParam<int>("i_width"), ph->getParam<int>("i_height"), //it's be nice to get these from EncodedFrame
+                                        ph->getParam<bool>("i_enable_lazy_publisher")));
+    }
     controlQ = device->getInputQueue(controlQName);
 }
 void Mono::closeQueues() {
     if(ph->getParam<bool>("i_publish_topic")) {
         monoQ->close();
     }
+    if(ph->getParam<bool>("i_enable_h264")) {
+        h264Q->close();
+    }
     controlQ->close();
 }