From 9f998ec3286bfac9ed3d0793ba579e5458d68708 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Thu, 26 Sep 2024 11:10:03 +0000 Subject: [PATCH 1/7] 2.10.2 --- depthai-ros/CHANGELOG.rst | 8 ++++++-- depthai-ros/CMakeLists.txt | 2 +- depthai-ros/package.xml | 2 +- depthai_bridge/CMakeLists.txt | 2 +- depthai_bridge/package.xml | 2 +- depthai_descriptions/CMakeLists.txt | 2 +- depthai_descriptions/package.xml | 2 +- depthai_examples/CMakeLists.txt | 2 +- depthai_examples/package.xml | 2 +- depthai_filters/CMakeLists.txt | 2 +- depthai_filters/package.xml | 2 +- depthai_ros_driver/CMakeLists.txt | 2 +- depthai_ros_driver/config/camera.yaml | 8 +++++--- depthai_ros_driver/package.xml | 2 +- depthai_ros_driver/src/camera.cpp | 19 +++++-------------- .../src/dai_nodes/sensors/img_pub.cpp | 1 - .../src/dai_nodes/sensors/sensor_wrapper.cpp | 2 +- depthai_ros_driver/src/dai_nodes/stereo.cpp | 2 +- .../param_handlers/sensor_param_handler.cpp | 2 +- depthai_ros_msgs/CMakeLists.txt | 2 +- depthai_ros_msgs/package.xml | 2 +- 21 files changed, 33 insertions(+), 37 deletions(-) diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst index cf5a0f81..e8ac22bf 100644 --- a/depthai-ros/CHANGELOG.rst +++ b/depthai-ros/CHANGELOG.rst @@ -1,8 +1,12 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package depthai-ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package depthai-ros -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.10.2 (2024-09-26) +------------------- +* Fix Stereo K matrix publishing +* Fix socket ID for NN detections +* Remove catching errors when starting the device since it introduced unexcpected behavior 2.10.1 (2024-09-18) ------------------- diff --git a/depthai-ros/CMakeLists.txt b/depthai-ros/CMakeLists.txt index 9684655d..f2766769 100644 --- 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.10.1 LANGUAGES CXX C) +project(depthai-ros VERSION 2.10.2 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) diff --git a/depthai-ros/package.xml b/depthai-ros/package.xml index 82fd8f15..4cf4aac6 100644 --- a/depthai-ros/package.xml +++ b/depthai-ros/package.xml @@ -1,7 +1,7 @@ depthai-ros - 2.10.1 + 2.10.2 The depthai-ros package diff --git a/depthai_bridge/CMakeLists.txt b/depthai_bridge/CMakeLists.txt index 843880c7..2c6f0c75 100644 --- 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.10.1 LANGUAGES CXX C) +project(depthai_bridge VERSION 2.10.2 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 index c5c05e59..615f70b7 100644 --- a/depthai_bridge/package.xml +++ b/depthai_bridge/package.xml @@ -1,7 +1,7 @@ depthai_bridge - 2.10.1 + 2.10.2 The depthai_bridge package Adam Serafin diff --git a/depthai_descriptions/CMakeLists.txt b/depthai_descriptions/CMakeLists.txt index ad2dc15b..f524c03f 100644 --- a/depthai_descriptions/CMakeLists.txt +++ b/depthai_descriptions/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(depthai_descriptions VERSION 2.10.1) +project(depthai_descriptions VERSION 2.10.2) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/depthai_descriptions/package.xml b/depthai_descriptions/package.xml index a8c064d1..5a190525 100644 --- a/depthai_descriptions/package.xml +++ b/depthai_descriptions/package.xml @@ -1,7 +1,7 @@ depthai_descriptions - 2.10.1 + 2.10.2 The depthai_descriptions package Adam Serafin diff --git a/depthai_examples/CMakeLists.txt b/depthai_examples/CMakeLists.txt index afc582d0..98269251 100644 --- 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.10.1 LANGUAGES CXX C) +project(depthai_examples VERSION 2.10.2 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 index 38327126..35723f24 100644 --- a/depthai_examples/package.xml +++ b/depthai_examples/package.xml @@ -1,7 +1,7 @@ depthai_examples - 2.10.1 + 2.10.2 The depthai_examples package diff --git a/depthai_filters/CMakeLists.txt b/depthai_filters/CMakeLists.txt index 4349a21a..80833c42 100644 --- a/depthai_filters/CMakeLists.txt +++ b/depthai_filters/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(depthai_filters VERSION 2.10.1 LANGUAGES CXX C) +project(depthai_filters VERSION 2.10.2 LANGUAGES CXX C) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/depthai_filters/package.xml b/depthai_filters/package.xml index 1890e840..25698c16 100644 --- a/depthai_filters/package.xml +++ b/depthai_filters/package.xml @@ -2,7 +2,7 @@ depthai_filters - 2.10.1 + 2.10.2 Depthai filters package Adam Serafin MIT diff --git a/depthai_ros_driver/CMakeLists.txt b/depthai_ros_driver/CMakeLists.txt index 23bf624e..dcfc4c6b 100644 --- a/depthai_ros_driver/CMakeLists.txt +++ b/depthai_ros_driver/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.22) -project(depthai_ros_driver VERSION 2.10.1) +project(depthai_ros_driver VERSION 2.10.2) set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(CMAKE_BUILD_SHARED_LIBS ON) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) diff --git a/depthai_ros_driver/config/camera.yaml b/depthai_ros_driver/config/camera.yaml index 51afc242..45e0d655 100644 --- a/depthai_ros_driver/config/camera.yaml +++ b/depthai_ros_driver/config/camera.yaml @@ -3,7 +3,9 @@ camera: i_enable_imu: true i_enable_ir: true - i_nn_type: spatial - i_pipeline_type: RGBD - nn: + i_nn_type: none + i_pipeline_type: Stereo + left: + i_enable_nn: true + left_i_nn: i_nn_config_path: depthai_ros_driver/mobilenet diff --git a/depthai_ros_driver/package.xml b/depthai_ros_driver/package.xml index d17a5ac7..dca5ce86 100644 --- a/depthai_ros_driver/package.xml +++ b/depthai_ros_driver/package.xml @@ -2,7 +2,7 @@ depthai_ros_driver - 2.10.1 + 2.10.2 Depthai ROS Monolithic node. Adam Serafin diff --git a/depthai_ros_driver/src/camera.cpp b/depthai_ros_driver/src/camera.cpp index 574ef7bd..69d77088 100644 --- a/depthai_ros_driver/src/camera.cpp +++ b/depthai_ros_driver/src/camera.cpp @@ -76,20 +76,11 @@ void Camera::diagCB(const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg) } void Camera::start() { - bool success = false; - while(!success) { - try { - RCLCPP_INFO(this->get_logger(), "Starting camera."); - if(!camRunning) { - onConfigure(); - } else { - RCLCPP_INFO(this->get_logger(), "Camera already running!."); - } - success = true; - } catch(const std::exception& e) { - RCLCPP_ERROR(this->get_logger(), "Exception occurred: %s. Retry", e.what()); - camRunning = false; - } + RCLCPP_INFO(this->get_logger(), "Starting camera."); + if(!camRunning) { + onConfigure(); + } else { + RCLCPP_INFO(this->get_logger(), "Camera already running!."); } } diff --git a/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp b/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp index 02e0e9c1..bbc651a9 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp @@ -124,7 +124,6 @@ void ImagePublisher::createInfoManager(std::shared_ptr device) { auto info = sensor_helpers::getCalibInfo(node->get_logger(), converter, device, pubConfig.socket, pubConfig.width, pubConfig.height); if(pubConfig.rectified) { std::fill(info.d.begin(), info.d.end(), 0.0); - std::fill(info.k.begin(), info.k.end(), 0.0); info.r[0] = info.r[4] = info.r[8] = 1.0; } infoManager->setCameraInfo(info); diff --git a/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp b/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp index 6693d579..1bd98b0b 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp @@ -33,9 +33,9 @@ SensorWrapper::SensorWrapper(const std::string& daiNodeName, converter = std::make_unique(true); setNames(); setXinXout(pipeline); - socketID = ph->getParam("i_board_socket_id"); } + socketID = ph->getParam("i_board_socket_id"); if(ph->getParam("i_disable_node") && ph->getParam("i_simulate_from_topic")) { RCLCPP_INFO(getROSNode()->get_logger(), "Disabling node %s, pipeline data taken from topic.", getName().c_str()); } else { diff --git a/depthai_ros_driver/src/dai_nodes/stereo.cpp b/depthai_ros_driver/src/dai_nodes/stereo.cpp index 47b4fe66..a47a9a05 100644 --- a/depthai_ros_driver/src/dai_nodes/stereo.cpp +++ b/depthai_ros_driver/src/dai_nodes/stereo.cpp @@ -47,7 +47,7 @@ Stereo::Stereo(const std::string& daiNodeName, continue; } } - RCLCPP_DEBUG(node->get_logger(), + RCLCPP_INFO(node->get_logger(), "Creating stereo node with left sensor %s and right sensor %s", utils::getSocketName(leftSensInfo.socket).c_str(), utils::getSocketName(rightSensInfo.socket).c_str()); diff --git a/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp b/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp index 23cc3866..0b9fe0dd 100644 --- a/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp @@ -30,7 +30,7 @@ void SensorParamHandler::declareCommonParams(dai::CameraBoardSocket socket) { declareAndLogParam("i_simulated_topic_name", ""); declareAndLogParam("i_disable_node", false); declareAndLogParam("i_get_base_device_timestamp", false); - socketID = static_cast(declareAndLogParam("i_board_socket_id", static_cast(socket), 0)); + socketID = static_cast(declareAndLogParam("i_board_socket_id", static_cast(socket), false)); declareAndLogParam("i_update_ros_base_time_on_ros_msg", false); declareAndLogParam("i_enable_feature_tracker", false); declareAndLogParam("i_enable_nn", false); diff --git a/depthai_ros_msgs/CMakeLists.txt b/depthai_ros_msgs/CMakeLists.txt index 8c3b48bc..45aec171 100644 --- a/depthai_ros_msgs/CMakeLists.txt +++ b/depthai_ros_msgs/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.10.2) # CMake version in Ubuntu 18.04 LTS -project(depthai_ros_msgs VERSION 2.10.1) +project(depthai_ros_msgs VERSION 2.10.2) if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) diff --git a/depthai_ros_msgs/package.xml b/depthai_ros_msgs/package.xml index 2803de5e..615b9272 100644 --- a/depthai_ros_msgs/package.xml +++ b/depthai_ros_msgs/package.xml @@ -1,7 +1,7 @@ depthai_ros_msgs - 2.10.1 + 2.10.2 Package to keep interface independent of the driver Adam Serafin From 74e51e6d8060561ff618b7f9c867af978e44cefe Mon Sep 17 00:00:00 2001 From: Serafadam Date: Thu, 26 Sep 2024 11:13:31 +0000 Subject: [PATCH 2/7] rever camera yaml --- depthai_ros_driver/config/camera.yaml | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/depthai_ros_driver/config/camera.yaml b/depthai_ros_driver/config/camera.yaml index 45e0d655..51afc242 100644 --- a/depthai_ros_driver/config/camera.yaml +++ b/depthai_ros_driver/config/camera.yaml @@ -3,9 +3,7 @@ camera: i_enable_imu: true i_enable_ir: true - i_nn_type: none - i_pipeline_type: Stereo - left: - i_enable_nn: true - left_i_nn: + i_nn_type: spatial + i_pipeline_type: RGBD + nn: i_nn_config_path: depthai_ros_driver/mobilenet From d342ac757d7bd7a0553ee21920c78325c9f9cd6e Mon Sep 17 00:00:00 2001 From: Serafadam Date: Thu, 26 Sep 2024 12:49:31 +0000 Subject: [PATCH 3/7] revert log level --- depthai_ros_driver/src/dai_nodes/stereo.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai_ros_driver/src/dai_nodes/stereo.cpp b/depthai_ros_driver/src/dai_nodes/stereo.cpp index a47a9a05..47b4fe66 100644 --- a/depthai_ros_driver/src/dai_nodes/stereo.cpp +++ b/depthai_ros_driver/src/dai_nodes/stereo.cpp @@ -47,7 +47,7 @@ Stereo::Stereo(const std::string& daiNodeName, continue; } } - RCLCPP_INFO(node->get_logger(), + RCLCPP_DEBUG(node->get_logger(), "Creating stereo node with left sensor %s and right sensor %s", utils::getSocketName(leftSensInfo.socket).c_str(), utils::getSocketName(rightSensInfo.socket).c_str()); From dac67c9433d9cbf13d7d7076c7d81a1c48b1a774 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Fri, 27 Sep 2024 12:23:42 +0000 Subject: [PATCH 4/7] detection2d overlay update --- .../depthai_filters/detection2d_overlay.hpp | 11 ++++-- depthai_filters/src/detection2d_overlay.cpp | 36 ++++++++++++++++--- .../dai_nodes/nn/detection.hpp | 4 ++- .../dai_nodes/nn/spatial_detection.hpp | 2 +- 4 files changed, 44 insertions(+), 9 deletions(-) diff --git a/depthai_filters/include/depthai_filters/detection2d_overlay.hpp b/depthai_filters/include/depthai_filters/detection2d_overlay.hpp index 2b79ef75..ec3a5e95 100644 --- a/depthai_filters/include/depthai_filters/detection2d_overlay.hpp +++ b/depthai_filters/include/depthai_filters/detection2d_overlay.hpp @@ -14,17 +14,22 @@ class Detection2DOverlay : public rclcpp::Node { explicit Detection2DOverlay(const rclcpp::NodeOptions& options); void onInit(); - void overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview, const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections); + void overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info, + const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections); message_filters::Subscriber previewSub; + message_filters::Subscriber infoSub; message_filters::Subscriber detSub; - typedef message_filters::sync_policies::ApproximateTime syncPolicy; + typedef message_filters::sync_policies::ApproximateTime + syncPolicy; std::unique_ptr> sync; rclcpp::Publisher::SharedPtr overlayPub; std::vector 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 \ No newline at end of file +} // namespace depthai_filters diff --git a/depthai_filters/src/detection2d_overlay.cpp b/depthai_filters/src/detection2d_overlay.cpp index c6f667ab..ad1a3bf4 100644 --- a/depthai_filters/src/detection2d_overlay.cpp +++ b/depthai_filters/src/detection2d_overlay.cpp @@ -10,19 +10,43 @@ Detection2DOverlay::Detection2DOverlay(const rclcpp::NodeOptions& options) : rcl } void Detection2DOverlay::onInit() { previewSub.subscribe(this, "rgb/preview/image_raw"); + infoSub.subscribe(this, "stereo/camera_info"); detSub.subscribe(this, "nn/detections"); - sync = std::make_unique>(syncPolicy(10), previewSub, detSub); - sync->registerCallback(std::bind(&Detection2DOverlay::overlayCB, this, std::placeholders::_1, std::placeholders::_2)); + sync = std::make_unique>(syncPolicy(10), previewSub, infoSub, detSub); + sync->registerCallback(std::bind(&Detection2DOverlay::overlayCB, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); overlayPub = this->create_publisher("overlay", 10); + desqueeze = this->declare_parameter("desqueeze", false); labelMap = this->declare_parameter>("label_map", labelMap); } void Detection2DOverlay::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info, const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections) { - cv::Mat previewMat = utils::msgToMat(this->get_logger(), preview, sensor_msgs::image_encodings::BGR8); + cv::Mat previewMat = utils::msgToMat(this->get_logger(), preview, sensor_msgs::image_encodings::BGR8); auto blue = cv::Scalar(255, 0, 0); + double ratioX = 1.0; + double ratioY = 1.0; + int offsetX = 0; + double offsetY = 0; + // if preview size is less than camera info size + if(previewMat.rows < info->height || previewMat.cols < info->width) { + ratioY = double(info->height) / double(previewMat.rows); + if(desqueeze) { + ratioX = double(info->width) / double(previewMat.cols); + } else { + ratioX = ratioY; + offsetX = (info->width - info->height) / 2.0; + } + } else { + ratioY = double(previewMat.rows) / double(info->height); + if(desqueeze) { + ratioX = double(previewMat.cols) / double(info->width); + } else { + ratioX = double(previewMat.cols) / double(info->width); + } + } for(auto& detection : detections->detections) { auto x1 = detection.bbox.center.position.x - detections->detections[0].bbox.size_x / 2.0; auto x2 = detection.bbox.center.position.x + detections->detections[0].bbox.size_x / 2.0; @@ -30,6 +54,10 @@ void Detection2DOverlay::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr auto y2 = detection.bbox.center.position.y + detections->detections[0].bbox.size_y / 2.0; auto labelStr = labelMap[stoi(detection.results[0].hypothesis.class_id)]; auto confidence = detection.results[0].hypothesis.score; + x1 = x1 * ratioX + offsetX; + x2 = x2 * ratioX + offsetX; + y1 = y1 * ratioY + offsetY; + y2 = y2 * ratioY + offsetY; utils::addTextToFrame(previewMat, labelStr, x1 + 10, y1 + 20); std::stringstream confStr; confStr << std::fixed << std::setprecision(2) << confidence * 100; @@ -44,4 +72,4 @@ void Detection2DOverlay::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr } // namespace depthai_filters #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(depthai_filters::Detection2DOverlay); \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(depthai_filters::Detection2DOverlay); 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 index 3986f6c4..0248fce8 100644 --- 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 @@ -84,9 +84,11 @@ class Detection : public BaseNode { convConf.updateROSBaseTimeOnRosMsg = ph->getParam("i_update_ros_base_time_on_ros_msg"); utils::ImgPublisherConfig pubConf; + pubConf.width = width; + pubConf.height = height; pubConf.daiNodeName = getName(); pubConf.topicName = "~/" + getName(); - pubConf.topicSuffix = "passthrough"; + pubConf.topicSuffix = "/passthrough"; pubConf.socket = static_cast(ph->getParam("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 index f22d1986..a72bea2d 100644 --- 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 @@ -77,7 +77,7 @@ class SpatialDetection : public BaseNode { pubConf.height = height; pubConf.daiNodeName = getName(); pubConf.topicName = "~/" + getName(); - pubConf.topicSuffix = "passthrough"; + pubConf.topicSuffix = "/passthrough"; pubConf.socket = static_cast(ph->getParam("i_board_socket_id")); ptPub->setup(device, convConf, pubConf); From 2dfa4fcc89187a33ec1307e929b4b8a296b76845 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Mon, 30 Sep 2024 13:37:30 +0000 Subject: [PATCH 5/7] detection overlay update --- depthai_bridge/CMakeLists.txt | 1 + depthai_filters/CMakeLists.txt | 1 + depthai_filters/config/detection.yaml | 3 +++ depthai_filters/launch/example_det2d_overlay.launch.py | 7 +++++-- depthai_filters/src/detection2d_overlay.cpp | 2 +- .../include/depthai_ros_driver/dai_nodes/nn/detection.hpp | 4 ++-- .../depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp | 8 ++++---- depthai_ros_driver/include/depthai_ros_driver/utils.hpp | 1 + depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp | 6 ++++-- 9 files changed, 22 insertions(+), 11 deletions(-) diff --git a/depthai_bridge/CMakeLists.txt b/depthai_bridge/CMakeLists.txt index 2c6f0c75..13361568 100644 --- a/depthai_bridge/CMakeLists.txt +++ b/depthai_bridge/CMakeLists.txt @@ -5,6 +5,7 @@ project(depthai_bridge VERSION 2.10.2 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 diff --git a/depthai_filters/CMakeLists.txt b/depthai_filters/CMakeLists.txt index 80833c42..98c0d2f3 100644 --- a/depthai_filters/CMakeLists.txt +++ b/depthai_filters/CMakeLists.txt @@ -5,6 +5,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # find dependencies find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() diff --git a/depthai_filters/config/detection.yaml b/depthai_filters/config/detection.yaml index c777b9a3..6b47f74e 100644 --- a/depthai_filters/config/detection.yaml +++ b/depthai_filters/config/detection.yaml @@ -6,3 +6,6 @@ i_enable_preview: true nn: i_enable_passthrough: true +/detection_overlay: + ros__parameters: + desqueeze: true diff --git a/depthai_filters/launch/example_det2d_overlay.launch.py b/depthai_filters/launch/example_det2d_overlay.launch.py index 08d26f88..915f4e33 100644 --- a/depthai_filters/launch/example_det2d_overlay.launch.py +++ b/depthai_filters/launch/example_det2d_overlay.launch.py @@ -27,8 +27,11 @@ def launch_setup(context, *args, **kwargs): ComposableNode( package="depthai_filters", plugin="depthai_filters::Detection2DOverlay", + name="detection_overlay", remappings=[('rgb/preview/image_raw', name+'/nn/passthrough/image_raw'), - ('nn/detections', name+'/nn/detections')] + ('rgb/preview/camera_info', name+'/nn/passthrough/camera_info'), + ('nn/detections', name+'/nn/detections')], + parameters=[params_file] ), ], ), @@ -46,4 +49,4 @@ def generate_launch_description(): return LaunchDescription( declared_arguments + [OpaqueFunction(function=launch_setup)] - ) \ No newline at end of file + ) diff --git a/depthai_filters/src/detection2d_overlay.cpp b/depthai_filters/src/detection2d_overlay.cpp index ad1a3bf4..0cd86dea 100644 --- a/depthai_filters/src/detection2d_overlay.cpp +++ b/depthai_filters/src/detection2d_overlay.cpp @@ -10,7 +10,7 @@ Detection2DOverlay::Detection2DOverlay(const rclcpp::NodeOptions& options) : rcl } void Detection2DOverlay::onInit() { previewSub.subscribe(this, "rgb/preview/image_raw"); - infoSub.subscribe(this, "stereo/camera_info"); + infoSub.subscribe(this, "rgb/preview/camera_info"); detSub.subscribe(this, "nn/detections"); sync = std::make_unique>(syncPolicy(10), previewSub, infoSub, detSub); sync->registerCallback(std::bind(&Detection2DOverlay::overlayCB, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); 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 index 0248fce8..c343a876 100644 --- 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 @@ -87,8 +87,8 @@ 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.socket = static_cast(ph->getParam("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 index a72bea2d..ebff5159 100644 --- 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 @@ -76,8 +76,8 @@ 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.socket = static_cast(ph->getParam("i_board_socket_id")); ptPub->setup(device, convConf, pubConf); @@ -97,8 +97,8 @@ class SpatialDetection : public BaseNode { pubConf.width = ph->getOtherNodeParam(sensor_helpers::getNodeName(getROSNode(), sensor_helpers::NodeNameEnum::Stereo), "i_width"); pubConf.height = ph->getOtherNodeParam(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.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 index c83444d7..9e36e2dd 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/utils.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/utils.hpp @@ -71,6 +71,7 @@ struct ImgPublisherConfig { dai::CameraBoardSocket rightSocket = dai::CameraBoardSocket::CAM_C; std::string calibrationFile = ""; std::string topicSuffix = "/image_raw"; + std::string infoSuffix = "/camera_info"; 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 index bbc651a9..2d861911 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp @@ -69,10 +69,12 @@ void ImagePublisher::setup(std::shared_ptr device, const utils::Img ffmpegPub = node->create_publisher( pubConfig.topicName + pubConfig.compressedTopicSuffix, rclcpp::QoS(10), pubOptions); } - infoPub = node->create_publisher(pubConfig.topicName + "/camera_info", rclcpp::QoS(10), pubOptions); + infoPub = + node->create_publisher(pubConfig.topicName + pubConfig.infoSuffix + "/camera_info", rclcpp::QoS(10), pubOptions); } else if(ipcEnabled) { imgPub = node->create_publisher(pubConfig.topicName + pubConfig.topicSuffix, rclcpp::QoS(10), pubOptions); - infoPub = node->create_publisher(pubConfig.topicName + "/camera_info", rclcpp::QoS(10), pubOptions); + infoPub = + node->create_publisher(pubConfig.topicName + pubConfig.infoSuffix + "/camera_info", rclcpp::QoS(10), pubOptions); } else { imgPubIT = image_transport::create_camera_publisher(node.get(), pubConfig.topicName + pubConfig.topicSuffix); } From 9848ec92cd23dcf78161e5d801d07f46efc42bf7 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 2 Oct 2024 10:00:36 +0000 Subject: [PATCH 6/7] add desqueeze output to nn --- depthai_filters/config/detection.yaml | 3 -- .../depthai_filters/detection2d_overlay.hpp | 10 ++---- .../launch/example_det2d_overlay.launch.py | 1 - depthai_filters/src/detection2d_overlay.cpp | 32 ++----------------- .../dai_nodes/nn/spatial_detection.hpp | 3 ++ .../src/dai_nodes/nn/spatial_nn_wrapper.cpp | 4 +-- 6 files changed, 9 insertions(+), 44 deletions(-) diff --git a/depthai_filters/config/detection.yaml b/depthai_filters/config/detection.yaml index 6b47f74e..c777b9a3 100644 --- a/depthai_filters/config/detection.yaml +++ b/depthai_filters/config/detection.yaml @@ -6,6 +6,3 @@ i_enable_preview: true nn: i_enable_passthrough: true -/detection_overlay: - ros__parameters: - desqueeze: true diff --git a/depthai_filters/include/depthai_filters/detection2d_overlay.hpp b/depthai_filters/include/depthai_filters/detection2d_overlay.hpp index ec3a5e95..3cf32fcf 100644 --- a/depthai_filters/include/depthai_filters/detection2d_overlay.hpp +++ b/depthai_filters/include/depthai_filters/detection2d_overlay.hpp @@ -4,7 +4,6 @@ #include "message_filters/sync_policies/approximate_time.h" #include "message_filters/synchronizer.h" #include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/camera_info.hpp" #include "sensor_msgs/msg/image.hpp" #include "vision_msgs/msg/detection2_d_array.hpp" @@ -14,22 +13,17 @@ class Detection2DOverlay : public rclcpp::Node { explicit Detection2DOverlay(const rclcpp::NodeOptions& options); void onInit(); - void overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview, - const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info, - const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections); + void overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview, const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections); message_filters::Subscriber previewSub; - message_filters::Subscriber infoSub; message_filters::Subscriber detSub; - typedef message_filters::sync_policies::ApproximateTime - syncPolicy; + typedef message_filters::sync_policies::ApproximateTime syncPolicy; std::unique_ptr> sync; rclcpp::Publisher::SharedPtr overlayPub; std::vector 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/launch/example_det2d_overlay.launch.py b/depthai_filters/launch/example_det2d_overlay.launch.py index 915f4e33..5ddd4384 100644 --- a/depthai_filters/launch/example_det2d_overlay.launch.py +++ b/depthai_filters/launch/example_det2d_overlay.launch.py @@ -29,7 +29,6 @@ def launch_setup(context, *args, **kwargs): plugin="depthai_filters::Detection2DOverlay", name="detection_overlay", remappings=[('rgb/preview/image_raw', name+'/nn/passthrough/image_raw'), - ('rgb/preview/camera_info', name+'/nn/passthrough/camera_info'), ('nn/detections', name+'/nn/detections')], parameters=[params_file] ), diff --git a/depthai_filters/src/detection2d_overlay.cpp b/depthai_filters/src/detection2d_overlay.cpp index 0cd86dea..6a620f88 100644 --- a/depthai_filters/src/detection2d_overlay.cpp +++ b/depthai_filters/src/detection2d_overlay.cpp @@ -10,43 +10,19 @@ Detection2DOverlay::Detection2DOverlay(const rclcpp::NodeOptions& options) : rcl } void Detection2DOverlay::onInit() { previewSub.subscribe(this, "rgb/preview/image_raw"); - infoSub.subscribe(this, "rgb/preview/camera_info"); detSub.subscribe(this, "nn/detections"); - sync = std::make_unique>(syncPolicy(10), previewSub, infoSub, detSub); - sync->registerCallback(std::bind(&Detection2DOverlay::overlayCB, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + sync = std::make_unique>(syncPolicy(10), previewSub, detSub); + sync->registerCallback(std::bind(&Detection2DOverlay::overlayCB, this, std::placeholders::_1, std::placeholders::_2)); overlayPub = this->create_publisher("overlay", 10); - desqueeze = this->declare_parameter("desqueeze", false); labelMap = this->declare_parameter>("label_map", labelMap); } void Detection2DOverlay::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview, - const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info, const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections) { cv::Mat previewMat = utils::msgToMat(this->get_logger(), preview, sensor_msgs::image_encodings::BGR8); auto blue = cv::Scalar(255, 0, 0); - double ratioX = 1.0; - double ratioY = 1.0; - int offsetX = 0; - double offsetY = 0; - // if preview size is less than camera info size - if(previewMat.rows < info->height || previewMat.cols < info->width) { - ratioY = double(info->height) / double(previewMat.rows); - if(desqueeze) { - ratioX = double(info->width) / double(previewMat.cols); - } else { - ratioX = ratioY; - offsetX = (info->width - info->height) / 2.0; - } - } else { - ratioY = double(previewMat.rows) / double(info->height); - if(desqueeze) { - ratioX = double(previewMat.cols) / double(info->width); - } else { - ratioX = double(previewMat.cols) / double(info->width); - } - } for(auto& detection : detections->detections) { auto x1 = detection.bbox.center.position.x - detections->detections[0].bbox.size_x / 2.0; auto x2 = detection.bbox.center.position.x + detections->detections[0].bbox.size_x / 2.0; @@ -54,10 +30,6 @@ void Detection2DOverlay::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr auto y2 = detection.bbox.center.position.y + detections->detections[0].bbox.size_y / 2.0; auto labelStr = labelMap[stoi(detection.results[0].hypothesis.class_id)]; auto confidence = detection.results[0].hypothesis.score; - x1 = x1 * ratioX + offsetX; - x2 = x2 * ratioX + offsetX; - y1 = y1 * ratioY + offsetY; - y2 = y2 * ratioY + offsetY; utils::addTextToFrame(previewMat, labelStr, x1 + 10, y1 + 20); std::stringstream confStr; confStr << std::fixed << std::setprecision(2) << confidence * 100; 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 index ebff5159..d56b649c 100644 --- 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 @@ -55,6 +55,9 @@ class SpatialDetection : public BaseNode { if(ph->getParam("i_disable_resize")) { width = ph->getOtherNodeParam(socketName, "i_preview_width"); height = ph->getOtherNodeParam(socketName, "i_preview_height"); + } else if(ph->getParam("i_desqueeze_output")) { + width = ph->getOtherNodeParam(socketName, "i_width"); + height = ph->getOtherNodeParam(socketName, "i_height"); } else { width = imageManip->initialConfig.getResizeConfig().width; height = imageManip->initialConfig.getResizeConfig().height; diff --git a/depthai_ros_driver/src/dai_nodes/nn/spatial_nn_wrapper.cpp b/depthai_ros_driver/src/dai_nodes/nn/spatial_nn_wrapper.cpp index 16443492..adc200a7 100644 --- a/depthai_ros_driver/src/dai_nodes/nn/spatial_nn_wrapper.cpp +++ b/depthai_ros_driver/src/dai_nodes/nn/spatial_nn_wrapper.cpp @@ -19,11 +19,11 @@ SpatialNNWrapper::SpatialNNWrapper(const std::string& daiNodeName, auto family = ph->getNNFamily(); switch(family) { case param_handlers::nn::NNFamily::Yolo: { - nnNode = std::make_unique>(getName(), getROSNode(), pipeline); + nnNode = std::make_unique>(getName(), getROSNode(), pipeline, socket); break; } case param_handlers::nn::NNFamily::Mobilenet: { - nnNode = std::make_unique>(getName(), getROSNode(), pipeline); + nnNode = std::make_unique>(getName(), getROSNode(), pipeline, socket); break; } case param_handlers::nn::NNFamily::Segmentation: { From f4073af698caa5464db8e3c28da8edd09d1c738f Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 2 Oct 2024 12:32:46 +0000 Subject: [PATCH 7/7] update changelog --- depthai-ros/CHANGELOG.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst index e8ac22bf..1836d028 100644 --- a/depthai-ros/CHANGELOG.rst +++ b/depthai-ros/CHANGELOG.rst @@ -7,6 +7,7 @@ Changelog for package depthai-ros * Fix Stereo K matrix publishing * Fix socket ID for NN detections * Remove catching errors when starting the device since it introduced unexcpected behavior +* Add desqueeze to NN node 2.10.1 (2024-09-18) -------------------