Add spatial bounding boxes in Noetic (#338)

luxonis · Jun 26, 2023 · a5e7b64 · a5e7b64
1 parent d328827
commit a5e7b64
Show file tree

Hide file tree

Showing 36 changed files with 299 additions and 37 deletions.
diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package depthai-ros
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.7.4 (2023-06-26)
+-------------------
+* ROS time update
+* Minor bugfixes
+
 2.7.3 (2023-06-16)
 -------------------
 * Pipeline generation as a plugin
@@ -10,7 +15,6 @@ Changelog for package depthai-ros
 * Camera trigger mechanisms
 * Brightness filter
 
-
 2.7.2 (2023-5-08)
 -------------------
 * IMU improvements

diff --git a/depthai-ros/CMakeLists.txt b/depthai-ros/CMakeLists.txt
@@ -1,6 +1,6 @@
 cmake_minimum_required (VERSION 3.10.2)  # CMake version in Ubuntu 18.04 LTS
 
-project(depthai-ros VERSION 2.7.3 LANGUAGES CXX C)
+project(depthai-ros VERSION 2.7.4 LANGUAGES CXX C)
 
 set(CMAKE_CXX_STANDARD 14)
 

diff --git a/depthai-ros/package.xml b/depthai-ros/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai-ros</name>
-  <version>2.7.3</version>
+  <version>2.7.4</version>
   <description>The depthai-ros package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->

diff --git a/depthai_bridge/CMakeLists.txt b/depthai_bridge/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required (VERSION 3.10.2)  # CMake version in Ubuntu 18.04 LTS
 set (CMAKE_POSITION_INDEPENDENT_CODE ON)
 
-project(depthai_bridge VERSION 2.7.3 LANGUAGES CXX C)
+project(depthai_bridge VERSION 2.7.4 LANGUAGES CXX C)
 
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)

diff --git a/depthai_bridge/package.xml b/depthai_bridge/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai_bridge</name>
-  <version>2.7.3</version>
+  <version>2.7.4</version>
   <description>The depthai_bridge package</description>
 
   <maintainer email="[email protected]">Sachin Guruswamy</maintainer>

diff --git a/depthai_bridge/src/ImageConverter.cpp b/depthai_bridge/src/ImageConverter.cpp
@@ -42,6 +42,9 @@ void ImageConverter::toRosMsgFromBitStream(std::shared_ptr<dai::ImgFrame> inData
                                            std::deque<ImageMsgs::Image>& outImageMsgs,
                                            dai::RawImgFrame::Type type,
                                            const sensor_msgs::CameraInfo& info) {
+    if(_updateRosBaseTimeOnToRosMsg) {
+        updateRosBaseTime();
+    }
     std::chrono::_V2::steady_clock::time_point tstamp;
     if(_getBaseDeviceTimestamp)
         tstamp = inData->getTimestampDevice();

diff --git a/depthai_bridge/src/ImuConverter.cpp b/depthai_bridge/src/ImuConverter.cpp
@@ -113,6 +113,9 @@ void ImuConverter::toRosMsg(std::shared_ptr<dai::IMUData> inData, std::deque<Imu
 }
 
 void ImuConverter::toRosDaiMsg(std::shared_ptr<dai::IMUData> inData, std::deque<depthai_ros_msgs::ImuWithMagneticField>& outImuMsgs) {
+    if(_updateRosBaseTimeOnToRosMsg) {
+        updateRosBaseTime();
+    }
     if(_syncMode != ImuSyncMethod::COPY) {
         FillImuData_LinearInterpolation(inData->packets, outImuMsgs);
     } else {

diff --git a/depthai_bridge/src/SpatialDetectionConverter.cpp b/depthai_bridge/src/SpatialDetectionConverter.cpp
@@ -77,7 +77,9 @@ void SpatialDetectionConverter::toRosMsg(std::shared_ptr<dai::SpatialImgDetectio
 
 void SpatialDetectionConverter::toRosVisionMsg(std::shared_ptr<dai::SpatialImgDetections> inNetData,
                                                std::deque<vision_msgs::Detection3DArray>& opDetectionMsgs) {
-    // setting the header
+    if(_updateRosBaseTimeOnToRosMsg) {
+        updateRosBaseTime();
+    }
     std::chrono::_V2::steady_clock::time_point tstamp;
     if(_getBaseDeviceTimestamp)
         tstamp = inNetData->getTimestampDevice();

diff --git a/depthai_descriptions/CMakeLists.txt b/depthai_descriptions/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.10.2)
-project(depthai_descriptions VERSION 2.7.3 LANGUAGES CXX C)
+project(depthai_descriptions VERSION 2.7.4 LANGUAGES CXX C)
 
 
 find_package(catkin REQUIRED

diff --git a/depthai_descriptions/package.xml b/depthai_descriptions/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>depthai_descriptions</name>
-  <version>2.7.3</version>
+  <version>2.7.4</version>
   <description>The depthai_descriptions package</description>
 
   <maintainer email="[email protected]">Adam Serafin</maintainer>

diff --git a/depthai_examples/CMakeLists.txt b/depthai_examples/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required (VERSION 3.10.2)  # CMake version in Ubuntu 18.04 LTS
-project(depthai_examples VERSION 2.7.3 LANGUAGES CXX C)
+project(depthai_examples VERSION 2.7.4 LANGUAGES CXX C)
 
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)

diff --git a/depthai_examples/package.xml b/depthai_examples/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai_examples</name>
-  <version>2.7.3</version>
+  <version>2.7.4</version>
   <description>The depthai_examples package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->

diff --git a/depthai_filters/CMakeLists.txt b/depthai_filters/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.10.2)
-project(depthai_filters VERSION 2.7.3 LANGUAGES CXX C)
+project(depthai_filters VERSION 2.7.4 LANGUAGES CXX C)
 
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
@@ -22,7 +22,7 @@ endif()
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure message_filters sensor_msgs vision_msgs cv_bridge image_transport nodelet)
+find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure message_filters sensor_msgs vision_msgs visualization_msgs cv_bridge image_transport nodelet)
 
 
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
@@ -34,7 +34,7 @@ generate_dynamic_reconfigure_options(
 catkin_package(
  INCLUDE_DIRS include
  LIBRARIES ${PROJECT_NAME}
- CATKIN_DEPENDS roscpp dynamic_reconfigure message_filters sensor_msgs vision_msgs cv_bridge image_transport nodelet
+ CATKIN_DEPENDS roscpp dynamic_reconfigure message_filters sensor_msgs vision_msgs visualization_msgs cv_bridge image_transport nodelet
  DEPENDS OpenCV
 )
 
@@ -55,6 +55,7 @@ add_library(${PROJECT_NAME} SHARED
     src/detection2d_overlay.cpp
     src/segmentation_overlay.cpp
     src/wls_filter.cpp
+    src/spatial_bb.cpp
     src/utils.cpp
 )
 

diff --git a/depthai_filters/config/spatial_bb.yaml b/depthai_filters/config/spatial_bb.yaml
@@ -0,0 +1,9 @@
+/oak:
+  camera_i_nn_type: spatial
+  rgb_i_enable_preview: true
+  rgb_i_keep_preview_aspect_ratio: false
+  nn_i_enable_passthrough: true
+  nn_i_disable_resize: true
+  stereo_i_subpixel: true
+/spatial_bb_node:
+  desqueeze: true
diff --git a/depthai_filters/include/depthai_filters/detection2d_overlay.hpp b/depthai_filters/include/depthai_filters/detection2d_overlay.hpp
@@ -22,8 +22,8 @@ class Detection2DOverlay : public nodelet::Nodelet {
     typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, vision_msgs::Detection2DArray> syncPolicy;
     std::unique_ptr<message_filters::Synchronizer<syncPolicy>> sync;
     ros::Publisher overlayPub;
-    const std::vector<std::string> labelMap = {"background", "aeroplane", "bicycle",     "bird",  "boat",        "bottle", "bus",
-                                               "car",        "cat",       "chair",       "cow",   "diningtable", "dog",    "horse",
-                                               "motorbike",  "person",    "pottedplant", "sheep", "sofa",        "train",  "tvmonitor"};
+    std::vector<std::string> labelMap = {"background", "aeroplane", "bicycle",     "bird",  "boat",        "bottle", "bus",
+                                         "car",        "cat",       "chair",       "cow",   "diningtable", "dog",    "horse",
+                                         "motorbike",  "person",    "pottedplant", "sheep", "sofa",        "train",  "tvmonitor"};
 };
 }  // namespace depthai_filters
diff --git a/depthai_filters/include/depthai_filters/segmentation_overlay.hpp b/depthai_filters/include/depthai_filters/segmentation_overlay.hpp
@@ -21,8 +21,8 @@ class SegmentationOverlay : public nodelet::Nodelet {
     typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> syncPolicy;
     std::unique_ptr<message_filters::Synchronizer<syncPolicy>> sync;
     ros::Publisher overlayPub;
-    const std::vector<std::string> labelMap = {"background", "aeroplane", "bicycle",     "bird",  "boat",        "bottle", "bus",
-                                               "car",        "cat",       "chair",       "cow",   "diningtable", "dog",    "horse",
-                                               "motorbike",  "person",    "pottedplant", "sheep", "sofa",        "train",  "tvmonitor"};
+    std::vector<std::string> labelMap = {"background", "aeroplane", "bicycle",     "bird",  "boat",        "bottle", "bus",
+                                         "car",        "cat",       "chair",       "cow",   "diningtable", "dog",    "horse",
+                                         "motorbike",  "person",    "pottedplant", "sheep", "sofa",        "train",  "tvmonitor"};
 };
 }  // namespace depthai_filters
diff --git a/depthai_filters/include/depthai_filters/spatial_bb.hpp b/depthai_filters/include/depthai_filters/spatial_bb.hpp
@@ -0,0 +1,36 @@
+#pragma once
+
+#include "image_transport/image_transport.h"
+#include "message_filters/subscriber.h"
+#include "message_filters/sync_policies/approximate_time.h"
+#include "message_filters/synchronizer.h"
+#include "nodelet/nodelet.h"
+#include "ros/ros.h"
+#include "sensor_msgs/CameraInfo.h"
+#include "sensor_msgs/Image.h"
+#include "vision_msgs/Detection3DArray.h"
+#include "visualization_msgs/MarkerArray.h"
+
+namespace depthai_filters {
+class SpatialBB : public nodelet::Nodelet {
+   public:
+    void onInit() override;
+
+    void overlayCB(const sensor_msgs::ImageConstPtr& preview,
+                   const sensor_msgs::CameraInfoConstPtr& info,
+                   const vision_msgs::Detection3DArrayConstPtr& detections);
+
+    message_filters::Subscriber<sensor_msgs::Image> previewSub;
+    message_filters::Subscriber<sensor_msgs::CameraInfo> infoSub;
+    message_filters::Subscriber<vision_msgs::Detection3DArray> detSub;
+
+    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo, vision_msgs::Detection3DArray> syncPolicy;
+    std::unique_ptr<message_filters::Synchronizer<syncPolicy>> sync;
+    ros::Publisher overlayPub;
+    ros::Publisher markerPub;
+    std::vector<std::string> labelMap = {"background", "aeroplane", "bicycle",     "bird",  "boat",        "bottle", "bus",
+                                         "car",        "cat",       "chair",       "cow",   "diningtable", "dog",    "horse",
+                                         "motorbike",  "person",    "pottedplant", "sheep", "sofa",        "train",  "tvmonitor"};
+    bool desqueeze = false;
+};
+}  // namespace depthai_filters
diff --git a/depthai_filters/include/depthai_filters/utils.hpp b/depthai_filters/include/depthai_filters/utils.hpp
@@ -6,5 +6,6 @@
 namespace depthai_filters {
 namespace utils {
 cv::Mat msgToMat(const sensor_msgs::ImageConstPtr& img, const std::string& encoding);
+void addTextToFrame(cv::Mat& frame, const std::string& text, int x, int y);
 }  // namespace utils
 }  // namespace depthai_filters
diff --git a/depthai_filters/launch/spatial_bb.launch b/depthai_filters/launch/spatial_bb.launch
@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="params_file" default="$(find depthai_filters)/config/spatial_bb.yaml"/>
+
+    <arg name="name" default="oak" />
+
+    <include file="$(find depthai_ros_driver)/launch/rgbd_pcl.launch">
+        <arg name="name" value="$(arg name)"/>
+        <arg name="rectify_rgb" value="true"/>
+        <arg name="params_file" value="$(arg params_file)"/>
+    </include>
+
+    <node name="spatial_bb_node" pkg="nodelet" type="nodelet" output="screen" required="true" args="load depthai_filters/SpatialBB $(arg  name)_nodelet_manager">
+        <remap from="/rgb/preview/image_raw" to="$(arg name)/nn/passthrough/image_raw"/>
+        <remap from="/nn/spatial_detections" to="$(arg name)/nn/spatial_detections"/>
+        <remap from="/stereo/camera_info" to="$(arg name)/stereo/camera_info"/>
+
+    </node>
+</launch>
diff --git a/depthai_filters/nodelet_plugins.xml b/depthai_filters/nodelet_plugins.xml
@@ -1,17 +1,22 @@
 <library path="lib/libdepthai_filters">
   <class name="depthai_filters/Detection2DOverlay" type="depthai_filters::Detection2DOverlay" base_class_type="nodelet::Nodelet">
-  <description>
+    <description>
   Detection 2D Overlay.
-  </description>
+    </description>
   </class>
-    <class name="depthai_filters/SegmentationOverlay" type="depthai_filters::SegmentationOverlay" base_class_type="nodelet::Nodelet">
-  <description>
+  <class name="depthai_filters/SegmentationOverlay" type="depthai_filters::SegmentationOverlay" base_class_type="nodelet::Nodelet">
+    <description>
   Segmentation Overlay.
-  </description>
+    </description>
   </class>
-    <class name="depthai_filters/WLSFilter" type="depthai_filters::WLSFilter" base_class_type="nodelet::Nodelet">
-  <description>
+  <class name="depthai_filters/WLSFilter" type="depthai_filters::WLSFilter" base_class_type="nodelet::Nodelet">
+    <description>
   WLS Filter.
-  </description>
+    </description>
+  </class>
+  <class name="depthai_filters/SpatialBB" type="depthai_filters::SpatialBB" base_class_type="nodelet::Nodelet">
+    <description>
+  Spatial Bounding Box publisher.
+    </description>
   </class>
 </library>
diff --git a/depthai_filters/package.xml b/depthai_filters/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>depthai_filters</name>
-  <version>2.7.3</version>
+  <version>2.7.4</version>
   <description>The depthai_filters package</description>
 
   <maintainer email="[email protected]">Adam Serafin</maintainer>
@@ -20,6 +20,7 @@
   <depend>image_pipeline</depend>
   <depend>nodelet</depend>
   <depend>libopencv-dev</depend>
+  <depend>visualization_msgs</depend>
 
 
   <export>

diff --git a/depthai_filters/src/detection2d_overlay.cpp b/depthai_filters/src/detection2d_overlay.cpp
@@ -14,6 +14,7 @@ void Detection2DOverlay::onInit() {
     previewSub.subscribe(pNH, "/rgb/preview/image_raw", 1);
     detSub.subscribe(pNH, "/nn/detections", 1);
     sync = std::make_unique<message_filters::Synchronizer<syncPolicy>>(syncPolicy(10), previewSub, detSub);
+    pNH.getParam("label_map", labelMap);
     sync->registerCallback(std::bind(&Detection2DOverlay::overlayCB, this, std::placeholders::_1, std::placeholders::_2));
     overlayPub = pNH.advertise<sensor_msgs::Image>("overlay", 10);
 }

diff --git a/depthai_filters/src/segmentation_overlay.cpp b/depthai_filters/src/segmentation_overlay.cpp
@@ -13,6 +13,7 @@ void SegmentationOverlay::onInit() {
     previewSub.subscribe(pNH, "/rgb/preview/image_raw", 1);
     segSub.subscribe(pNH, "/nn/image_raw", 1);
     sync = std::make_unique<message_filters::Synchronizer<syncPolicy>>(syncPolicy(10), previewSub, segSub);
+    pNH.getParam("label_map", labelMap);
     sync->registerCallback(std::bind(&SegmentationOverlay::overlayCB, this, std::placeholders::_1, std::placeholders::_2));
     overlayPub = pNH.advertise<sensor_msgs::Image>("overlay", 10);
 }