luxonis · Serafadam · Oct 7, 2024 · Sep 26, 2024 · Sep 26, 2024 · Sep 26, 2024
diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst
@@ -1,8 +1,13 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 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
+* Add desqueeze to NN node
 
 2.10.1 (2024-09-18)
 -------------------

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.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
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai-ros</name>
-  <version>2.10.1</version>
+  <version>2.10.2</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,10 +1,11 @@
 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)
+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_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.10.1</version>
+  <version>2.10.2</version>
   <description>The depthai_bridge package</description>
 
   <maintainer email="[email protected]">Adam Serafin</maintainer>

diff --git 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
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai_descriptions</name>
-  <version>2.10.1</version>
+  <version>2.10.2</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.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
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai_examples</name>
-  <version>2.10.1</version>
+  <version>2.10.2</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,10 +1,11 @@
 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)
 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/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"
 
@@ -27,4 +26,4 @@ class Detection2DOverlay : public rclcpp::Node {
                                          "motorbike",  "person",    "pottedplant", "sheep", "sofa",        "train",  "tvmonitor"};
 };
 
-}  // namespace depthai_filters
+}  // namespace depthai_filters
diff --git a/depthai_filters/launch/example_det2d_overlay.launch.py b/depthai_filters/launch/example_det2d_overlay.launch.py
@@ -27,8 +27,10 @@ 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')]
+                                    ('nn/detections', name+'/nn/detections')],
+                        parameters=[params_file]
                     ),
             ],
         ),
@@ -46,4 +48,4 @@ def generate_launch_description():
 
     return LaunchDescription(
         declared_arguments + [OpaqueFunction(function=launch_setup)]
-    )
+    )
diff --git a/depthai_filters/package.xml b/depthai_filters/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>depthai_filters</name>
-  <version>2.10.1</version>
+  <version>2.10.2</version>
   <description>Depthai filters package</description>
   <maintainer email="[email protected]">Adam Serafin</maintainer>
   <license>MIT</license>

diff --git a/depthai_filters/src/detection2d_overlay.cpp b/depthai_filters/src/detection2d_overlay.cpp
@@ -11,15 +11,15 @@ Detection2DOverlay::Detection2DOverlay(const rclcpp::NodeOptions& options) : rcl
 void Detection2DOverlay::onInit() {
     previewSub.subscribe(this, "rgb/preview/image_raw");
     detSub.subscribe(this, "nn/detections");
-    sync = std::make_unique<message_filters::Synchronizer<syncPolicy>>(syncPolicy(10), previewSub, detSub);
+    sync = std::make_unique<message_filters::Synchronizer<syncPolicy>>(syncPolicy(10), previewSub,  detSub);
     sync->registerCallback(std::bind(&Detection2DOverlay::overlayCB, this, std::placeholders::_1, std::placeholders::_2));
     overlayPub = this->create_publisher<sensor_msgs::msg::Image>("overlay", 10);
     labelMap = this->declare_parameter<std::vector<std::string>>("label_map", labelMap);
 }
 
 void Detection2DOverlay::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview,
                                    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);
 
@@ -44,4 +44,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);
+RCLCPP_COMPONENTS_REGISTER_NODE(depthai_filters::Detection2DOverlay);
diff --git 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/include/depthai_ros_driver/dai_nodes/nn/detection.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp
@@ -87,9 +87,11 @@ class Detection : public BaseNode {
             convConf.updateROSBaseTimeOnRosMsg = ph->getParam<bool>("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.topicName = "~/" + getName() + "/passthrough";
+            pubConf.infoSuffix = "/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
@@ -55,6 +55,9 @@ class SpatialDetection : public BaseNode {
         if(ph->getParam<bool>("i_disable_resize")) {
             width = ph->getOtherNodeParam<int>(socketName, "i_preview_width");
             height = ph->getOtherNodeParam<int>(socketName, "i_preview_height");
+		} else if(ph->getParam<bool>("i_desqueeze_output")) {
+			width = ph->getOtherNodeParam<int>(socketName, "i_width");
+			height = ph->getOtherNodeParam<int>(socketName, "i_height");
         } else {
             width = imageManip->initialConfig.getResizeConfig().width;
             height = imageManip->initialConfig.getResizeConfig().height;
@@ -76,8 +79,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<dai::CameraBoardSocket>(ph->getParam<int>("i_board_socket_id"));
 
             ptPub->setup(device, convConf, pubConf);
@@ -97,8 +100,8 @@ 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.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
@@ -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/package.xml b/depthai_ros_driver/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>depthai_ros_driver</name>
-  <version>2.10.1</version>
+  <version>2.10.2</version>
   <description>Depthai ROS Monolithic node.</description>
   <maintainer email="[email protected]">Adam Serafin</maintainer>
 

diff --git 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/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<dai_nodes::nn::SpatialDetection<dai::node::YoloSpatialDetectionNetwork>>(getName(), getROSNode(), pipeline);
+            nnNode = std::make_unique<dai_nodes::nn::SpatialDetection<dai::node::YoloSpatialDetectionNetwork>>(getName(), getROSNode(), pipeline, socket);
             break;
         }
         case param_handlers::nn::NNFamily::Mobilenet: {
-            nnNode = std::make_unique<dai_nodes::nn::SpatialDetection<dai::node::MobileNetSpatialDetectionNetwork>>(getName(), getROSNode(), pipeline);
+            nnNode = std::make_unique<dai_nodes::nn::SpatialDetection<dai::node::MobileNetSpatialDetectionNetwork>>(getName(), getROSNode(), pipeline, socket);
             break;
         }
         case param_handlers::nn::NNFamily::Segmentation: {

diff --git 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<dai::Device> device, const utils::Img
             ffmpegPub = node->create_publisher<ffmpeg_image_transport_msgs::msg::FFMPEGPacket>(
                 pubConfig.topicName + pubConfig.compressedTopicSuffix, rclcpp::QoS(10), pubOptions);
         }
-        infoPub = node->create_publisher<sensor_msgs::msg::CameraInfo>(pubConfig.topicName + "/camera_info", rclcpp::QoS(10), pubOptions);
+        infoPub =
+            node->create_publisher<sensor_msgs::msg::CameraInfo>(pubConfig.topicName + pubConfig.infoSuffix + "/camera_info", rclcpp::QoS(10), pubOptions);
     } else if(ipcEnabled) {
         imgPub = node->create_publisher<sensor_msgs::msg::Image>(pubConfig.topicName + pubConfig.topicSuffix, rclcpp::QoS(10), pubOptions);
-        infoPub = node->create_publisher<sensor_msgs::msg::CameraInfo>(pubConfig.topicName + "/camera_info", rclcpp::QoS(10), pubOptions);
+        infoPub =
+            node->create_publisher<sensor_msgs::msg::CameraInfo>(pubConfig.topicName + pubConfig.infoSuffix + "/camera_info", rclcpp::QoS(10), pubOptions);
     } else {
         imgPubIT = image_transport::create_camera_publisher(node.get(), pubConfig.topicName + pubConfig.topicSuffix);
     }
@@ -124,7 +126,6 @@ void ImagePublisher::createInfoManager(std::shared_ptr<dai::Device> 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
@@ -33,9 +33,9 @@ SensorWrapper::SensorWrapper(const std::string& daiNodeName,
         converter = std::make_unique<dai::ros::ImageConverter>(true);
         setNames();
         setXinXout(pipeline);
-        socketID = ph->getParam<int>("i_board_socket_id");
     }
 
+    socketID = ph->getParam<int>("i_board_socket_id");
     if(ph->getParam<bool>("i_disable_node") && ph->getParam<bool>("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/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<std::string>("i_simulated_topic_name", "");
     declareAndLogParam<bool>("i_disable_node", false);
     declareAndLogParam<bool>("i_get_base_device_timestamp", false);
-    socketID = static_cast<dai::CameraBoardSocket>(declareAndLogParam<int>("i_board_socket_id", static_cast<int>(socket), 0));
+    socketID = static_cast<dai::CameraBoardSocket>(declareAndLogParam<int>("i_board_socket_id", static_cast<int>(socket), false));
     declareAndLogParam<bool>("i_update_ros_base_time_on_ros_msg", false);
     declareAndLogParam<bool>("i_enable_feature_tracker", false);
     declareAndLogParam<bool>("i_enable_nn", false);

diff --git 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
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai_ros_msgs</name>
-  <version>2.10.1</version>
+  <version>2.10.2</version>
   <description>Package to keep interface independent of the driver</description>
 
   <maintainer email="[email protected]">Adam Serafin</maintainer>