2.10.0 init port

depthai_bridge/CMakeLists.txt

@@ -5,6 +5,7 @@ project(depthai_bridge VERSION 2.9.0 LANGUAGES CXX C)
 
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 
 # Update the policy setting to avoid an error when loading the ament_cmake package
 # at the current cmake version level
@@ -55,6 +56,8 @@ FILE(GLOB LIB_SRC
 "src/ImuConverter.cpp"
 "src/TFPublisher.cpp"
 "src/TrackedFeaturesConverter.cpp"
+"src/TrackedDetectionConverter.cpp"
+"src/TrackedSpatialDetectionConverter.cpp"
 )
 
 catkin_package(

depthai_bridge/include/depthai_bridge/ImageConverter.hpp

@@ -10,19 +10,26 @@
 #include "depthai-shared/common/Point2f.hpp"
 #include "depthai/device/CalibrationHandler.hpp"
 #include "depthai/pipeline/datatype/ImgFrame.hpp"
+#include "depthai/pipeline/datatype/EncodedFrame.hpp"
 #include "ros/time.h"
 #include "sensor_msgs/CameraInfo.h"
 #include "sensor_msgs/Image.h"
+#include "sensor_msgs/CompressedImage.h"
 #include "std_msgs/Header.h"
+#include "depthai_ros_msgs/FFMPEGPacket.h"
 
 namespace dai {
 
 namespace ros {
 
 namespace StdMsgs = std_msgs;
 namespace ImageMsgs = sensor_msgs;
+namespace DepthAiRosMsgs = depthai_ros_msgs;
 using ImagePtr = ImageMsgs::ImagePtr;
 using TimePoint = std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration>;
+using FFMPegImagePtr = DepthAiRosMsgs::FFMPEGPacketPtr;
+using CompImagePtr = ImageMsgs::CompressedImagePtr;
+
 
 class ImageConverter {
    public:
@@ -44,7 +51,7 @@ class ImageConverter {
      * @param update: bool whether to automatically update the ROS base time on message conversion
      */
     void setUpdateRosBaseTimeOnToRosMsg(bool update = true) {
-        _updateRosBaseTimeOnToRosMsg = update;
+        updateRosBaseTimeOnToRosMsg = update;
     }
 
     /**
@@ -77,10 +84,21 @@ class ImageConverter {
      */
     void setAlphaScaling(double alphaScalingFactor = 0.0);
 
+	/**
+     * @brief Sets the encoding of the image when converting to FFMPEG message. Default is libx264.
+     * @param encoding: The encoding to be used.
+     */
+    void setFFMPEGEncoding(const std::string& encoding);
+
     ImageMsgs::Image toRosMsgRawPtr(std::shared_ptr<dai::ImgFrame> inData, const sensor_msgs::CameraInfo& info = sensor_msgs::CameraInfo());
     void toRosMsg(std::shared_ptr<dai::ImgFrame> inData, std::deque<ImageMsgs::Image>& outImageMsgs);
     ImagePtr toRosMsgPtr(std::shared_ptr<dai::ImgFrame> inData);
 
+	DepthAiRosMsgs::FFMPEGPacket toRosFFMPEGPacket(std::shared_ptr<dai::EncodedFrame> inData);
+
+    ImageMsgs::CompressedImage toRosCompressedMsg(std::shared_ptr<dai::ImgFrame> inData);
+
+
     void toDaiMsg(const ImageMsgs::Image& inMsg, dai::ImgFrame& outData);
 
     /** TODO(sachin): Add support for ros msg to cv mat since we have some
@@ -99,29 +117,31 @@ class ImageConverter {
     static std::unordered_map<dai::RawImgFrame::Type, std::string> encodingEnumMap;
     static std::unordered_map<dai::RawImgFrame::Type, std::string> planarEncodingEnumMap;
 
-    // dai::RawImgFrame::Type _srcType;
-    bool _daiInterleaved;
+    bool daiInterleaved;
     // bool c
-    const std::string _frameName = "";
+    const std::string frameName = "";
     void planarToInterleaved(const std::vector<uint8_t>& srcData, std::vector<uint8_t>& destData, int w, int h, int numPlanes, int bpp);
     void interleavedToPlanar(const std::vector<uint8_t>& srcData, std::vector<uint8_t>& destData, int w, int h, int numPlanes, int bpp);
-    std::chrono::time_point<std::chrono::steady_clock> _steadyBaseTime;
+    std::chrono::time_point<std::chrono::steady_clock> steadyBaseTime;
 
-    ::ros::Time _rosBaseTime;
-    bool _getBaseDeviceTimestamp;
+    ::ros::Time rosBaseTime;
+    bool getBaseDeviceTimestamp;
     // For handling ROS time shifts and debugging
-    int64_t _totalNsChange{0};
+    int64_t totalNsChange{0};
     // Whether to update the ROS base time on each message conversion
-    bool _updateRosBaseTimeOnToRosMsg{false};
-    dai::RawImgFrame::Type _srcType;
-    bool _fromBitstream = false;
-    bool _convertDispToDepth = false;
-    bool _addExpOffset = false;
-    dai::CameraExposureOffset _expOffset;
-    bool _reverseStereoSocketOrder = false;
-    double _baseline;
-    bool _alphaScalingEnabled = false;
-    double _alphaScalingFactor = 0.0;
+    bool updateRosBaseTimeOnToRosMsg{false};
+    dai::RawImgFrame::Type srcType;
+    bool fromBitstream = false;
+    bool dispToDepth = false;
+    bool addExpOffset = false;
+    dai::CameraExposureOffset expOffset;
+    bool reversedStereoSocketOrder = false;
+    double baseline;
+    bool alphaScalingEnabled = false;
+    double alphaScalingFactor = 0.0;
+	int camHeight = -1;
+    int camWidth = -1;
+    std::string ffmpegEncoding = "libx264";
 };
 
 }  // namespace ros

depthai_bridge/include/depthai_bridge/TFPublisher.hpp

@@ -65,23 +65,23 @@ class TFPublisher {
      */
     bool modelNameAvailable();
     std::string getCamSocketName(int socketNum);
-    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> _tfPub;
-    std::shared_ptr<robot_state_publisher::RobotStatePublisher> _rsp;
-    std::string _camName;
-    std::string _camModel;
-    std::string _baseFrame;
-    std::string _parentFrame;
-    std::string _camPosX;
-    std::string _camPosY;
-    std::string _camPosZ;
-    std::string _camRoll;
-    std::string _camPitch;
-    std::string _camYaw;
-    std::string _imuFromDescr;
-    std::string _customURDFLocation;
-    std::string _customXacroArgs;
-    std::vector<dai::CameraFeatures> _camFeatures;
-    const std::unordered_map<dai::CameraBoardSocket, std::string> _socketNameMap = {
+    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tfPub;
+    std::shared_ptr<robot_state_publisher::RobotStatePublisher> rsp;
+    std::string camName;
+    std::string camModel;
+    std::string baseFrame;
+    std::string parentFrame;
+    std::string camPosX;
+    std::string camPosY;
+    std::string camPosZ;
+    std::string camRoll;
+    std::string camPitch;
+    std::string camYaw;
+    std::string imuFromDescr;
+    std::string customURDFLocation;
+    std::string customXacroArgs;
+    std::vector<dai::CameraFeatures> camFeatures;
+    const std::unordered_map<dai::CameraBoardSocket, std::string> socketNameMap = {
         {dai::CameraBoardSocket::AUTO, "rgb"},
         {dai::CameraBoardSocket::CAM_A, "rgb"},
         {dai::CameraBoardSocket::CAM_B, "left"},
@@ -91,4 +91,4 @@ class TFPublisher {
     };
 };
 }  // namespace ros
-}  // namespace dai
+}  // namespace dai

depthai_bridge/include/depthai_bridge/TrackDetectionConverter.hpp

@@ -0,0 +1,56 @@
+#pragma once
+
+#include <deque>
+#include <memory>
+#include <tuple>
+
+#include "depthai/pipeline/datatype/Tracklets.hpp"
+#include "depthai_ros_msgs/TrackDetection2DArray.h"
+#include "ros/time.h"
+#include "vision_msgs/Detection2DArray.h"
+
+namespace dai {
+
+namespace ros {
+
+class TrackDetectionConverter {
+   public:
+    TrackDetectionConverter(std::string frameName, int width, int height, bool normalized, float thresh, bool getBaseDeviceTimestamp = false);
+    ~TrackDetectionConverter() = default;
+
+    /**
+     * @brief Handles cases in which the ROS time shifts forward or backward
+     *  Should be called at regular intervals or on-change of ROS time, depending
+     *  on monitoring.
+     *
+     */
+    void updateRosBaseTime();
+
+    /**
+     * @brief Commands the converter to automatically update the ROS base time on message conversion based on variable
+     *
+     * @param update: bool whether to automatically update the ROS base time on message conversion
+     */
+    void setUpdateRosBaseTimeOnToRosMsg(bool update = true) {
+        _updateRosBaseTimeOnToRosMsg = update;
+    }
+
+    void toRosMsg(std::shared_ptr<dai::Tracklets> trackData, std::deque<depthai_ros_msgs::TrackDetection2DArray>& opDetectionMsgs);
+
+    depthai_ros_msgs::TrackDetection2DArray::Ptr toRosMsgPtr(std::shared_ptr<dai::Tracklets> trackData);
+
+   private:
+    int _width, _height;
+    const std::string _frameName;
+    bool _normalized;
+    float _thresh;
+    std::chrono::time_point<std::chrono::steady_clock> _steadyBaseTime;
+    ::ros::Time _rosBaseTime;
+    bool _getBaseDeviceTimestamp;
+    // For handling ROS time shifts and debugging
+    int64_t _totalNsChange{0};
+    // Whether to update the ROS base time on each message conversion
+    bool _updateRosBaseTimeOnToRosMsg{false};
+};
+}  // namespace ros
+}  // namespace dai

depthai_bridge/include/depthai_bridge/TrackSpatialDetectionConverter.hpp

@@ -0,0 +1,60 @@
+#pragma once
+
+#include <deque>
+#include <memory>
+#include <string>
+
+#include "depthai/pipeline/datatype/Tracklets.hpp"
+#include "depthai_ros_msgs/TrackDetection2DArray.h"
+#include "ros/time.h"
+#include "vision_msgs/Detection2DArray.h"
+
+namespace dai {
+
+namespace ros {
+
+class TrackSpatialDetectionConverter {
+   public:
+    TrackSpatialDetectionConverter(std::string frameName, int width, int height, bool normalized, bool getBaseDeviceTimestamp = false);
+    ~TrackSpatialDetectionConverter() = default;
+
+    /**
+     * @brief Handles cases in which the ROS time shifts forward or backward
+     *  Should be called at regular intervals or on-change of ROS time, depending
+     *  on monitoring.
+     *
+     */
+    void updateRosBaseTime();
+
+    /**
+     * @brief Commands the converter to automatically update the ROS base time on message conversion based on variable
+     *
+     * @param update: bool whether to automatically update the ROS base time on message conversion
+     */
+    void setUpdateRosBaseTimeOnToRosMsg(bool update = true) {
+        _updateRosBaseTimeOnToRosMsg = update;
+    }
+
+    void toRosMsg(std::shared_ptr<dai::Tracklets> trackData, std::deque<depthai_ros_msgs::TrackDetection2DArray>& opDetectionMsgs);
+
+    depthai_ros_msgs::TrackDetection2DArray::Ptr toRosMsgPtr(std::shared_ptr<dai::Tracklets> trackData);
+
+   private:
+    int _width, _height;
+    const std::string _frameName;
+    bool _normalized;
+    float _thresh;
+    std::chrono::time_point<std::chrono::steady_clock> _steadyBaseTime;
+    ::ros::Time _rosBaseTime;
+    bool _getBaseDeviceTimestamp;
+    // For handling ROS time shifts and debugging
+    int64_t _totalNsChange{0};
+    // Whether to update the ROS base time on each message conversion
+    bool _updateRosBaseTimeOnToRosMsg{false};
+};
+
+}  // namespace ros
+
+namespace rosBridge = ros;
+
+}  // namespace dai