detection overlay update

luxonis · Sep 30, 2024 · 37f4c4b · 37f4c4b
1 parent f803ecd
commit 37f4c4b
Show file tree

Hide file tree

Showing 8 changed files with 19 additions and 11 deletions.
diff --git a/depthai_filters/config/detection.yaml b/depthai_filters/config/detection.yaml
@@ -2,3 +2,5 @@
   camera_i_nn_type: rgb
   rgb_i_enable_preview: true
   nn_i_enable_passthrough: true
+/detection_overlay:
+  desqueeze: true
diff --git a/depthai_filters/launch/example_det2d_overlay.launch b/depthai_filters/launch/example_det2d_overlay.launch
@@ -11,7 +11,8 @@
 
     <node name="overlay" pkg="nodelet" type="nodelet" output="screen" required="true" args="load depthai_filters/Detection2DOverlay $(arg  name)_nodelet_manager">
         <remap from="/rgb/preview/image_raw" to="$(arg name)/nn/passthrough/image_raw"/>
+		<remap from="/rgb/preview/camera_info" to="$(arg name)/nn/passthrough/camera_info"/>
         <remap from="/nn/detections" to="$(arg name)/nn/detections"/>
 
     </node>
-</launch>
+</launch>
diff --git a/depthai_filters/src/detection2d_overlay.cpp b/depthai_filters/src/detection2d_overlay.cpp
@@ -14,7 +14,7 @@ void Detection2DOverlay::onInit() {
     auto pNH = getPrivateNodeHandle();
     previewSub.subscribe(pNH, "/rgb/preview/image_raw", 1);
     detSub.subscribe(pNH, "/nn/detections", 1);
-    infoSub.subscribe(pNH, "rgb/preview/camera_info", 1);
+    infoSub.subscribe(pNH, "/rgb/preview/camera_info", 1);
     sync = std::make_unique<message_filters::Synchronizer<syncPolicy>>(syncPolicy(10), previewSub, infoSub, detSub);
     pNH.getParam("label_map", labelMap);
     pNH.getParam("desqueeze", desqueeze);

diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp
@@ -89,8 +89,9 @@ class Detection : public BaseNode {
 			pubConf.width = width;
 			pubConf.height = height;
             pubConf.daiNodeName = getName();
-            pubConf.topicName = "~/" + getName();
-            pubConf.topicSuffix = "/passthrough";
+            pubConf.topicName = getName() + "/passthrough";
+            pubConf.infoSuffix = "/passthrough";
+			pubConf.infoMgrSuffix = "/passthrough";
             pubConf.socket = static_cast<dai::CameraBoardSocket>(ph->getParam<int>("i_board_socket_id"));
 
             ptPub->setup(device, convConf, pubConf);

diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp
@@ -79,8 +79,9 @@ class SpatialDetection : public BaseNode {
             pubConf.width = width;
             pubConf.height = height;
             pubConf.daiNodeName = getName();
-            pubConf.topicName = "~/" + getName();
-            pubConf.topicSuffix = "/passthrough";
+            pubConf.topicName = getName() + "/passthrough";
+            pubConf.infoSuffix = "/passthrough";
+			pubConf.infoMgrSuffix = "/passthrough";
             pubConf.socket = static_cast<dai::CameraBoardSocket>(ph->getParam<int>("i_board_socket_id"));
 
             ptPub->setup(device, convConf, pubConf);
@@ -100,8 +101,9 @@ class SpatialDetection : public BaseNode {
             pubConf.width = ph->getOtherNodeParam<int>(sensor_helpers::getNodeName(getROSNode(), sensor_helpers::NodeNameEnum::Stereo), "i_width");
             pubConf.height = ph->getOtherNodeParam<int>(sensor_helpers::getNodeName(getROSNode(), sensor_helpers::NodeNameEnum::Stereo), "i_height");
             pubConf.daiNodeName = getName();
-            pubConf.topicName = "~/" + getName();
-            pubConf.topicSuffix = "/passthrough_depth";
+            pubConf.topicName = getName() + "/passthrough_depth";
+            pubConf.infoSuffix = "/passthrough_depth";
+			pubConf.infoMgrSuffix = "/passthrough_depth";
             pubConf.socket = socket;
 
             ptDepthPub->setup(device, convConf, pubConf);

diff --git a/depthai_ros_driver/include/depthai_ros_driver/utils.hpp b/depthai_ros_driver/include/depthai_ros_driver/utils.hpp
@@ -66,6 +66,7 @@ struct ImgPublisherConfig {
     dai::CameraBoardSocket rightSocket = dai::CameraBoardSocket::CAM_C;
     std::string calibrationFile = "";
     std::string topicSuffix = "/image_raw";
+	std::string infoSuffix = "";
     std::string compressedTopicSuffix = "/image_raw/compressed";
     std::string infoMgrSuffix = "";
     bool rectified = false;

diff --git a/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp b/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp
@@ -64,7 +64,7 @@ void ImagePublisher::setup(std::shared_ptr<dai::Device> device, const utils::Img
         } else {
             ffmpegPub = node.advertise<depthai_ros_msgs::FFMPEGPacket>(pubConfig.topicName + pubConfig.compressedTopicSuffix, 10);
         }
-        infoPub = node.advertise<sensor_msgs::CameraInfo>(pubConfig.topicName + "/camera_info", 10);
+        infoPub = node.advertise<sensor_msgs::CameraInfo>(pubConfig.topicName + pubConfig.infoSuffix + "/camera_info", 10);
     } else {
         imgPubIT = it.advertiseCamera(pubConfig.topicName + pubConfig.topicSuffix, 10);
     }
@@ -110,7 +110,7 @@ std::shared_ptr<dai::node::VideoEncoder> ImagePublisher::createEncoder(std::shar
     return enc;
 }
 void ImagePublisher::createInfoManager(std::shared_ptr<dai::Device> device) {
-    infoManager = std::make_shared<camera_info_manager::CameraInfoManager>(ros::NodeHandle(node, pubConfig.daiNodeName),
+    infoManager = std::make_shared<camera_info_manager::CameraInfoManager>(ros::NodeHandle(node, pubConfig.daiNodeName + pubConfig.infoMgrSuffix),
                                                                            "/" + pubConfig.daiNodeName + pubConfig.infoMgrSuffix);
     if(pubConfig.calibrationFile.empty()) {
         auto info = sensor_helpers::getCalibInfo(converter, device, pubConfig.socket, pubConfig.width, pubConfig.height);

diff --git a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp
@@ -99,7 +99,7 @@ void RGB::setupQueues(std::shared_ptr<dai::Device> device) {
 
         utils::ImgPublisherConfig pubConfig;
         pubConfig.daiNodeName = getName();
-        pubConfig.topicName = "~/" + getName();
+        pubConfig.topicName = getName();
         pubConfig.lazyPub = ph->getParam<bool>("i_enable_lazy_publisher");
         pubConfig.socket = static_cast<dai::CameraBoardSocket>(ph->getParam<int>("i_board_socket_id"));
         pubConfig.calibrationFile = ph->getParam<std::string>("i_calibration_file");
@@ -108,6 +108,7 @@ void RGB::setupQueues(std::shared_ptr<dai::Device> device) {
         pubConfig.height = ph->getParam<int>("i_preview_height");
         pubConfig.maxQSize = ph->getParam<int>("i_max_q_size");
         pubConfig.topicSuffix = "/preview/image_raw";
+		pubConfig.infoMgrSuffix = "/preview";
 
         previewPub->setup(device, convConfig, pubConfig);
     };