Reformatted code for PR

depthai_bridge/include/depthai_bridge/SpatialDetection2DConverter.hpp

@@ -4,9 +4,9 @@
 #include <memory>
 #include <string>
 
+#include "depthai/pipeline/datatype/SpatialImgDetections.hpp"
 #include "rclcpp/time.hpp"
 #include "vision_msgs/msg/detection2_d_array.hpp"
-#include "depthai/pipeline/datatype/SpatialImgDetections.hpp"
 
 namespace dai {
 

depthai_bridge/include/depthai_bridge/TrackDetectionConverter.hpp

@@ -3,11 +3,11 @@
 #include <deque>
 #include <memory>
 #include <string>
-#include "vision_msgs/msg/detection2_d_array.hpp"
-#include "depthai_ros_msgs/msg/track_detection2_d_array.hpp"
 
 #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 {
 

depthai_bridge/include/depthai_bridge/TrackSpatialDetectionConverter.hpp

@@ -3,11 +3,11 @@
 #include <deque>
 #include <memory>
 #include <string>
-#include "vision_msgs/msg/detection2_d_array.hpp"
-#include "depthai_ros_msgs/msg/track_detection2_d_array.hpp"
 
 #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 {
 

depthai_bridge/src/SpatialDetection2DConverter.cpp

@@ -1,78 +1,74 @@
 #include "depthai_bridge/SpatialDetection2DConverter.hpp"
 
-#include "depthai_bridge/depthaiUtility.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();
+  :	_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) {
-
-    std::chrono::_V2::steady_clock::time_point tstamp;
-    if(_getBaseDeviceTimestamp)
-        tstamp = inNetData->getTimestampDevice();
-    else
-        tstamp = inNetData->getTimestamp();
+  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());
+  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;
-        }
+  // 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);
+    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;
+    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.;
-    }
+    // 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);
+  opDetectionMsgs.push_back(opDetectionMsg);
 }
 
 }  // namespace ros

depthai_bridge/src/TrackDetectionConverter.cpp

@@ -1,95 +1,91 @@
 #include "depthai_bridge/TrackDetectionConverter.hpp"
 
-#include "depthai_bridge/depthaiUtility.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();
+  :	_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();
+  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
-        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);
-}
+      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.;
 
-TrackDetection2DArrayPtr TrackDetectionConverter::toRosMsgPtr(
-    std::shared_ptr<dai::Tracklets> trackData) {
+    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);
+}
 
-    std::deque<DepthaiMsgs::TrackDetection2DArray> msgQueue;
-    toRosMsg(trackData, msgQueue);
-    auto msg = msgQueue.front();
+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);
+  TrackDetection2DArrayPtr ptr = std::make_shared<DepthaiMsgs::TrackDetection2DArray>(msg);
 
-    return ptr;
+  return ptr;
 }
 
 }  // namespace ros