Tracking converter for ROS2 Foxy (#494)

Co-authored-by: Daniel Silva <[email protected]>

depthai_bridge/CMakeLists.txt

@@ -61,7 +61,10 @@ file(GLOB LIB_SRC
 "src/ImageConverter.cpp"
 "src/ImgDetectionConverter.cpp"
 "src/SpatialDetectionConverter.cpp"
+"src/SpatialDetection2DConverter.cpp"
 "src/ImuConverter.cpp"
+"src/TrackDetectionConverter.cpp"
+"src/TrackSpatialDetectionConverter.cpp"
 )
 
 add_library(${PROJECT_NAME} SHARED ${LIB_SRC})

depthai_bridge/include/depthai_bridge/SpatialDetection2DConverter.hpp

@@ -0,0 +1,38 @@
+#pragma once
+
+#include <deque>
+#include <memory>
+#include <string>
+
+#include "depthai/pipeline/datatype/SpatialImgDetections.hpp"
+#include "rclcpp/time.hpp"
+#include "vision_msgs/msg/detection2_d_array.hpp"
+
+namespace dai {
+
+namespace ros {
+
+namespace VisionMsgs = vision_msgs::msg;
+using SpatialDetection2DArrayPtr = VisionMsgs::Detection2DArray;
+
+class SpatialDetection2DConverter {
+   public:
+    SpatialDetection2DConverter(std::string frameName, int width, int height, bool normalized = false, bool getBaseDeviceTimestamp = false);
+    ~SpatialDetection2DConverter();
+    void toRosVisionMsg(std::shared_ptr<dai::SpatialImgDetections> inNetData, std::deque<VisionMsgs::Detection2DArray>& opDetectionMsg);
+
+   private:
+    int _width, _height;
+    const std::string _frameName;
+    bool _normalized;
+    std::chrono::time_point<std::chrono::steady_clock> _steadyBaseTime;
+
+    rclcpp::Time _rosBaseTime;
+    bool _getBaseDeviceTimestamp;
+};
+
+}  // namespace ros
+
+namespace rosBridge = ros;
+
+}  // namespace dai

depthai_bridge/include/depthai_bridge/TrackDetectionConverter.hpp

@@ -0,0 +1,43 @@
+#pragma once
+
+#include <deque>
+#include <memory>
+#include <string>
+
+#include "depthai/pipeline/datatype/Tracklets.hpp"
+#include "depthai_ros_msgs/msg/track_detection2_d_array.hpp"
+#include "rclcpp/time.hpp"
+#include "vision_msgs/msg/detection2_d_array.hpp"
+
+namespace dai {
+
+namespace ros {
+
+namespace VisionMsgs = vision_msgs::msg;
+namespace DepthaiMsgs = depthai_ros_msgs::msg;
+using Detection2DArrayPtr = VisionMsgs::Detection2DArray::SharedPtr;
+using TrackDetection2DArrayPtr = DepthaiMsgs::TrackDetection2DArray::SharedPtr;
+
+class TrackDetectionConverter {
+   public:
+    TrackDetectionConverter(std::string frameName, int width, int height, bool normalized = false, float thresh = 0.0, bool getBaseDeviceTimestamp = false);
+    ~TrackDetectionConverter();
+    void toRosMsg(std::shared_ptr<dai::Tracklets> trackData, std::deque<DepthaiMsgs::TrackDetection2DArray>& opDetectionMsgs);
+
+    TrackDetection2DArrayPtr 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;
+    rclcpp::Time _rosBaseTime;
+    bool _getBaseDeviceTimestamp;
+};
+
+}  // namespace ros
+
+namespace rosBridge = ros;
+
+}  // namespace dai

depthai_bridge/include/depthai_bridge/TrackSpatialDetectionConverter.hpp

@@ -0,0 +1,44 @@
+#pragma once
+
+#include <deque>
+#include <memory>
+#include <string>
+
+#include "depthai/pipeline/datatype/Tracklets.hpp"
+#include "depthai_ros_msgs/msg/track_detection2_d_array.hpp"
+#include "rclcpp/time.hpp"
+#include "vision_msgs/msg/detection2_d_array.hpp"
+
+namespace dai {
+
+namespace ros {
+
+namespace VisionMsgs = vision_msgs::msg;
+namespace DepthaiMsgs = depthai_ros_msgs::msg;
+using Detection2DArrayPtr = VisionMsgs::Detection2DArray::SharedPtr;
+using TrackDetection2DArrayPtr = DepthaiMsgs::TrackDetection2DArray::SharedPtr;
+
+class TrackSpatialDetectionConverter {
+   public:
+    TrackSpatialDetectionConverter(
+        std::string frameName, int width, int height, bool normalized = false, float thresh = 0.0, bool getBaseDeviceTimestamp = false);
+    ~TrackSpatialDetectionConverter();
+    void toRosMsg(std::shared_ptr<dai::Tracklets> trackData, std::deque<DepthaiMsgs::TrackDetection2DArray>& opDetectionMsgs);
+
+    TrackDetection2DArrayPtr 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;
+    rclcpp::Time _rosBaseTime;
+    bool _getBaseDeviceTimestamp;
+};
+
+}  // namespace ros
+
+namespace rosBridge = ros;
+
+}  // namespace dai

depthai_bridge/src/ImgDetectionConverter.cpp

@@ -32,10 +32,9 @@ void ImgDetectionConverter::toRosMsg(std::shared_ptr<dai::ImgDetections> inNetDa
     opDetectionMsg.header.frame_id = _frameName;
     opDetectionMsg.detections.resize(inNetData->detections.size());
 
-    // TODO(Sachin): check if this works fine for normalized detection
     // publishing
     for(int i = 0; i < inNetData->detections.size(); ++i) {
-        int xMin, yMin, xMax, yMax;
+        float xMin, yMin, xMax, yMax;
         if(_normalized) {
             xMin = inNetData->detections[i].xmin;
             yMin = inNetData->detections[i].ymin;

depthai_bridge/src/SpatialDetection2DConverter.cpp

@@ -0,0 +1,75 @@
+#include "depthai_bridge/SpatialDetection2DConverter.hpp"
+
+#include "depthai/depthai.hpp"
+#include "depthai_bridge/depthaiUtility.hpp"
+
+namespace dai {
+
+namespace ros {
+
+SpatialDetection2DConverter::SpatialDetection2DConverter(std::string frameName, int width, int height, bool normalized, bool getBaseDeviceTimestamp)
+    : _frameName(frameName),
+      _width(width),
+      _height(height),
+      _normalized(normalized),
+      _steadyBaseTime(std::chrono::steady_clock::now()),
+      _getBaseDeviceTimestamp(getBaseDeviceTimestamp) {
+    _rosBaseTime = rclcpp::Clock().now();
+}
+
+SpatialDetection2DConverter::~SpatialDetection2DConverter() = default;
+
+void SpatialDetection2DConverter::toRosVisionMsg(std::shared_ptr<dai::SpatialImgDetections> inNetData,
+                                                 std::deque<VisionMsgs::Detection2DArray>& opDetectionMsgs) {
+    // setting the header
+    std::chrono::_V2::steady_clock::time_point tstamp;
+    if(_getBaseDeviceTimestamp)
+        tstamp = inNetData->getTimestampDevice();
+    else
+        tstamp = inNetData->getTimestamp();
+
+    VisionMsgs::Detection2DArray opDetectionMsg;
+    opDetectionMsg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, tstamp);
+    opDetectionMsg.header.frame_id = _frameName;
+    opDetectionMsg.detections.resize(inNetData->detections.size());
+
+    // publishing
+    for(int i = 0; i < inNetData->detections.size(); ++i) {
+        float xMin, yMin, xMax, yMax;
+        if(_normalized) {
+            xMin = inNetData->detections[i].xmin;
+            yMin = inNetData->detections[i].ymin;
+            xMax = inNetData->detections[i].xmax;
+            yMax = inNetData->detections[i].ymax;
+        } else {
+            xMin = inNetData->detections[i].xmin * _width;
+            yMin = inNetData->detections[i].ymin * _height;
+            xMax = inNetData->detections[i].xmax * _width;
+            yMax = inNetData->detections[i].ymax * _height;
+        }
+
+        float xSize = xMax - xMin;
+        float ySize = yMax - yMin;
+        float xCenter = xMin + xSize / 2;
+        float yCenter = yMin + ySize / 2;
+        opDetectionMsg.detections[i].results.resize(1);
+
+        opDetectionMsg.detections[i].results[0].id = std::to_string(inNetData->detections[i].label);
+        opDetectionMsg.detections[i].results[0].score = inNetData->detections[i].confidence;
+        opDetectionMsg.detections[i].bbox.center.x = xCenter;
+        opDetectionMsg.detections[i].bbox.center.y = yCenter;
+        opDetectionMsg.detections[i].bbox.size_x = xSize;
+        opDetectionMsg.detections[i].bbox.size_y = ySize;
+
+        // converting mm to meters since per ros rep-103 lenght should always be in meters
+        opDetectionMsg.detections[i].results[0].pose.pose.position.x = inNetData->detections[i].spatialCoordinates.x / 1000.;
+        opDetectionMsg.detections[i].results[0].pose.pose.position.y = inNetData->detections[i].spatialCoordinates.y / 1000.;
+        opDetectionMsg.detections[i].results[0].pose.pose.position.z = inNetData->detections[i].spatialCoordinates.z / 1000.;
+    }
+
+    opDetectionMsgs.push_back(opDetectionMsg);
+}
+
+}  // namespace ros
+
+}  // namespace dai

depthai_bridge/src/SpatialDetectionConverter.cpp

@@ -30,10 +30,9 @@ void SpatialDetectionConverter::toRosMsg(std::shared_ptr<dai::SpatialImgDetectio
     opDetectionMsg.header.frame_id = _frameName;
     opDetectionMsg.detections.resize(inNetData->detections.size());
 
-    // TODO(Sachin): check if this works fine for normalized detection
     // publishing
     for(int i = 0; i < inNetData->detections.size(); ++i) {
-        int xMin, yMin, xMax, yMax;
+        float xMin, yMin, xMax, yMax;
         if(_normalized) {
             xMin = inNetData->detections[i].xmin;
             yMin = inNetData->detections[i].ymin;

depthai_bridge/src/TrackDetectionConverter.cpp

@@ -0,0 +1,91 @@
+#include "depthai_bridge/TrackDetectionConverter.hpp"
+
+#include "depthai/depthai.hpp"
+#include "depthai_bridge/depthaiUtility.hpp"
+
+namespace dai {
+
+namespace ros {
+
+TrackDetectionConverter::TrackDetectionConverter(std::string frameName, int width, int height, bool normalized, float thresh, bool getBaseDeviceTimestamp)
+    : _frameName(frameName),
+      _width(width),
+      _height(height),
+      _normalized(normalized),
+      _thresh(thresh),
+      _steadyBaseTime(std::chrono::steady_clock::now()),
+      _getBaseDeviceTimestamp(getBaseDeviceTimestamp) {
+    _rosBaseTime = rclcpp::Clock().now();
+}
+
+TrackDetectionConverter::~TrackDetectionConverter() = default;
+
+void TrackDetectionConverter::toRosMsg(std::shared_ptr<dai::Tracklets> trackData, std::deque<DepthaiMsgs::TrackDetection2DArray>& opDetectionMsgs) {
+    // setting the header
+    std::chrono::_V2::steady_clock::time_point tstamp;
+    if(_getBaseDeviceTimestamp)
+        tstamp = trackData->getTimestampDevice();
+    else
+        tstamp = trackData->getTimestamp();
+
+    DepthaiMsgs::TrackDetection2DArray opDetectionMsg;
+    opDetectionMsg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, tstamp);
+    opDetectionMsg.header.frame_id = _frameName;
+    opDetectionMsg.detections.resize(trackData->tracklets.size());
+
+    // publishing
+    for(int i = 0; i < trackData->tracklets.size(); ++i) {
+        dai::Tracklet t = trackData->tracklets[i];
+        dai::Rect roi;
+        float xMin, yMin, xMax, yMax;
+
+        if(_normalized)
+            roi = t.roi;
+        else
+            roi = t.roi.denormalize(_width, _height);
+
+        xMin = roi.topLeft().x;
+        yMin = roi.topLeft().y;
+        xMax = roi.bottomRight().x;
+        yMax = roi.bottomRight().y;
+
+        float xSize = xMax - xMin;
+        float ySize = yMax - yMin;
+        float xCenter = xMin + xSize / 2.;
+        float yCenter = yMin + ySize / 2.;
+
+        opDetectionMsg.detections[i].results.resize(1);
+
+        opDetectionMsg.detections[i].results[0].id = std::to_string(t.label);
+
+        opDetectionMsg.detections[i].results[0].score = _thresh;
+
+        opDetectionMsg.detections[i].bbox.center.x = xCenter;
+        opDetectionMsg.detections[i].bbox.center.y = yCenter;
+        opDetectionMsg.detections[i].bbox.size_x = xSize;
+        opDetectionMsg.detections[i].bbox.size_y = ySize;
+
+        opDetectionMsg.detections[i].is_tracking = true;
+        std::stringstream track_id_str;
+        track_id_str << "" << t.id;
+        opDetectionMsg.detections[i].tracking_id = track_id_str.str();
+        opDetectionMsg.detections[i].tracking_age = t.age;
+        opDetectionMsg.detections[i].tracking_status = static_cast<int32_t>(t.status);
+    }
+
+    opDetectionMsgs.push_back(opDetectionMsg);
+}
+
+TrackDetection2DArrayPtr TrackDetectionConverter::toRosMsgPtr(std::shared_ptr<dai::Tracklets> trackData) {
+    std::deque<DepthaiMsgs::TrackDetection2DArray> msgQueue;
+    toRosMsg(trackData, msgQueue);
+    auto msg = msgQueue.front();
+
+    TrackDetection2DArrayPtr ptr = std::make_shared<DepthaiMsgs::TrackDetection2DArray>(msg);
+
+    return ptr;
+}
+
+}  // namespace ros
+
+}  // namespace dai