Fixes for low bandwidth - Jazzy (#649)

depthai-ros/CHANGELOG.rst

@@ -2,6 +2,19 @@
 Changelog for package depthai-ros
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.10.5 (2025-01-09)
+-------------------
+* Fix low bandwidth issues
+* New stereo filters
+* Diagnostics update
+* Fix IR calculation
+
+2.10.4 (2024-11-07)
+-------------------
+* Fix rectified topic names
+* Fix pointcloud launch
+* Add sensor parameters for max autoexposure, sharpness, luma and chroma denoise
+
 2.10.3 (2024-10-14)
 -------------------
 * Allow setting USB speed without specifying device information

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.3 LANGUAGES CXX C)
+project(depthai-ros VERSION 2.10.5 LANGUAGES CXX C)
 
 set(CMAKE_CXX_STANDARD 14)
 

depthai-ros/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai-ros</name>
-  <version>2.10.3</version>
+  <version>2.10.5</version>
   <description>The depthai-ros package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->

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.3 LANGUAGES CXX C)
+project(depthai_bridge VERSION 2.10.5 LANGUAGES CXX C)
 
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)

depthai_bridge/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai_bridge</name>
-  <version>2.10.3</version>
+  <version>2.10.5</version>
   <description>The depthai_bridge package</description>
 
   <maintainer email="[email protected]">Adam Serafin</maintainer>
@@ -14,7 +14,6 @@
   <depend>libboost-dev</depend>
   <depend>rclcpp</depend>
 
-  <depend>ament_index_cpp</depend>
   <depend>cv_bridge</depend>
   <depend>camera_info_manager</depend>
   <depend>depthai</depend>

depthai_bridge/src/ImageConverter.cpp

@@ -142,55 +142,6 @@ ImageMsgs::Image ImageConverter::toRosMsgRawPtr(std::shared_ptr<dai::ImgFrame> i
         return outImageMsg;
     }
 
-    if(fromBitstream) {
-        std::string encoding;
-        int decodeFlags;
-        int channels;
-        cv::Mat output;
-        switch(srcType) {
-            case dai::RawImgFrame::Type::BGR888i: {
-                encoding = sensor_msgs::image_encodings::BGR8;
-                decodeFlags = cv::IMREAD_COLOR;
-                channels = CV_8UC3;
-                break;
-            }
-            case dai::RawImgFrame::Type::GRAY8: {
-                encoding = sensor_msgs::image_encodings::MONO8;
-                decodeFlags = cv::IMREAD_GRAYSCALE;
-                channels = CV_8UC1;
-                break;
-            }
-            case dai::RawImgFrame::Type::RAW8: {
-                encoding = sensor_msgs::image_encodings::TYPE_16UC1;
-                decodeFlags = cv::IMREAD_ANYDEPTH;
-                channels = CV_16UC1;
-                break;
-            }
-            default: {
-                std::cout << frameName << static_cast<int>(srcType) << std::endl;
-                throw(std::runtime_error("Converted type not supported!"));
-            }
-        }
-
-        output = cv::imdecode(cv::Mat(inData->getData()), decodeFlags);
-
-        // converting disparity
-        if(dispToDepth) {
-            auto factor = std::abs(baseline * 10) * info.p[0];
-            cv::Mat depthOut = cv::Mat(cv::Size(output.cols, output.rows), CV_16UC1);
-            depthOut.forEach<uint16_t>([&output, &factor](uint16_t& pixel, const int* position) -> void {
-                auto disp = output.at<uint8_t>(position);
-                if(disp == 0)
-                    pixel = 0;
-                else
-                    pixel = factor / disp;
-            });
-            output = depthOut.clone();
-        }
-        cv_bridge::CvImage(header, encoding, output).toImageMsg(outImageMsg);
-        return outImageMsg;
-    }
-
     if(planarEncodingEnumMap.find(inData->getType()) != planarEncodingEnumMap.end()) {
         // cv::Mat inImg = inData->getCvFrame();
         cv::Mat mat, output;
@@ -537,6 +488,7 @@ ImageMsgs::CameraInfo ImageConverter::calibrationToCameraInfo(
                 std::copy(stereoIntrinsics[i].begin(), stereoIntrinsics[i].end(), stereoFlatIntrinsics.begin() + 4 * i);
                 stereoFlatIntrinsics[(4 * i) + 3] = 0;
             }
+
             // Check stereo socket order
             dai::CameraBoardSocket stereoSocketFirst = calibHandler.getStereoLeftCameraId();
             dai::CameraBoardSocket stereoSocketSecond = calibHandler.getStereoRightCameraId();

depthai_bridge/src/ImuConverter.cpp

@@ -143,4 +143,4 @@ void ImuConverter::toRosDaiMsg(std::shared_ptr<dai::IMUData> inData, std::deque<
 }
 
 }  // namespace ros
-}  // namespace dai
+}  // namespace dai

depthai_bridge/src/TFPublisher.cpp

@@ -242,7 +242,7 @@ std::string TFPublisher::getURDF() {
     RCLCPP_DEBUG(logger, "Xacro command: %s", cmd.c_str());
     std::array<char, 128> buffer;
     std::string result;
-    std::unique_ptr<FILE, decltype(&pclose)> pipe(popen(cmd.c_str(), "r"), pclose);
+    std::unique_ptr<FILE, int (*)(FILE*)> pipe(popen(cmd.c_str(), "r"), pclose);
     if(!pipe) {
         throw std::runtime_error("popen() failed!");
     }

depthai_descriptions/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(depthai_descriptions VERSION 2.10.3)
+project(depthai_descriptions VERSION 2.10.5)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)

depthai_descriptions/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai_descriptions</name>
-  <version>2.10.3</version>
+  <version>2.10.5</version>
   <description>The depthai_descriptions package</description>
 
   <maintainer email="[email protected]">Adam Serafin</maintainer>

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.3 LANGUAGES CXX C)
+project(depthai_examples VERSION 2.10.5 LANGUAGES CXX C)
 
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)

depthai_examples/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai_examples</name>
-  <version>2.10.3</version>
+  <version>2.10.5</version>
   <description>The depthai_examples package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->

depthai_filters/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(depthai_filters VERSION 2.10.3 LANGUAGES CXX C)
+project(depthai_filters VERSION 2.10.5 LANGUAGES CXX C)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)

depthai_filters/config/usb_cam_overlay.yaml

@@ -9,4 +9,4 @@
       i_simulated_topic_name: /image_raw
       i_low_bandwidth: true
     nn:
-      i_enable_passthrough: true
+      i_enable_passthrough: true

depthai_filters/include/depthai_filters/feature_tracker_overlay.hpp

@@ -42,4 +42,5 @@ class FeatureTrackerOverlay : public rclcpp::Node {
     std::unordered_set<featureIdType> trackedIDs;
     std::unordered_map<featureIdType, std::deque<geometry_msgs::msg::Point>> trackedFeaturesPath;
 };
-}  // namespace depthai_filters
+
+}  // namespace depthai_filters

depthai_filters/launch/example_det2d_overlay.launch.py

@@ -26,11 +26,11 @@ def launch_setup(context, *args, **kwargs):
             composable_node_descriptions=[
                     ComposableNode(
                         package="depthai_filters",
+                        name="detection_overlay",
                         plugin="depthai_filters::Detection2DOverlay",
-                        name=name+"detection_overlay",
                         remappings=[('rgb/preview/image_raw', name+'/nn/passthrough/image_raw'),
                                     ('nn/detections', name+'/nn/detections')],
-                        parameters=[params_file]
+                        parameters=[params_file],
                     ),
             ],
         ),

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.3</version>
+  <version>2.10.5</version>
   <description>Depthai filters package</description>
   <maintainer email="[email protected]">Adam Serafin</maintainer>
   <license>MIT</license>

depthai_filters/src/detection2d_overlay.cpp

@@ -19,7 +19,8 @@ void Detection2DOverlay::onInit() {
 
 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);
 
     for(auto& detection : detections->detections) {

depthai_filters/src/feature_tracker_overlay.cpp

@@ -73,3 +73,4 @@ void FeatureTrackerOverlay::drawFeatures(cv::Mat& img) {
 }
 }  // namespace depthai_filters
 #include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(depthai_filters::FeatureTrackerOverlay);

depthai_filters/src/features_3d.cpp

@@ -66,3 +66,4 @@ void Features3D::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& depth,
 
 }  // namespace depthai_filters
 #include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(depthai_filters::Features3D);

depthai_filters/src/spatial_bb.cpp

@@ -145,3 +145,4 @@ void SpatialBB::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview
 
 }  // namespace depthai_filters
 #include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(depthai_filters::SpatialBB);

depthai_ros_driver/CMakeLists.txt

@@ -1,11 +1,11 @@
 cmake_minimum_required(VERSION 3.22)
-project(depthai_ros_driver VERSION 2.10.3)
+project(depthai_ros_driver VERSION 2.10.5)
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 set(CMAKE_BUILD_SHARED_LIBS ON)
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
+    add_compile_options(-Wall -Wextra -Wpedantic -Wno-deprecated-declarations -Wno-maybe-uninitialized)
 endif()
 # Default to C99
 if(NOT CMAKE_C_STANDARD)

depthai_ros_driver/config/pcl.yaml

@@ -6,4 +6,5 @@
       i_align_depth: true
       i_board_socket_id: 2
       i_subpixel: true
-      i_publish_right_rect: true
+      i_right_rect_publish_topic: true
+      i_right_rect_synced: false

depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp

@@ -55,9 +55,6 @@ 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;

depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp

@@ -56,6 +56,7 @@ extern const std::unordered_map<std::string, dai::ColorCameraProperties::SensorR
 extern const std::unordered_map<std::string, dai::CameraControl::FrameSyncMode> fSyncModeMap;
 extern const std::unordered_map<std::string, dai::CameraImageOrientation> cameraImageOrientationMap;
 bool rsCompabilityMode(std::shared_ptr<rclcpp::Node> node);
+std::string tfPrefix(std::shared_ptr<rclcpp::Node> node);
 std::string getSocketName(std::shared_ptr<rclcpp::Node> node, dai::CameraBoardSocket socket);
 std::string getNodeName(std::shared_ptr<rclcpp::Node> node, NodeNameEnum name);
 void basicCameraPub(const std::string& /*name*/,

depthai_ros_driver/include/depthai_ros_driver/param_handlers/imu_param_handler.hpp

@@ -6,7 +6,6 @@
 
 #include "depthai-shared/properties/IMUProperties.hpp"
 #include "depthai/pipeline/datatype/CameraControl.hpp"
-#include "depthai-shared/properties/IMUProperties.hpp"
 #include "depthai_bridge/ImuConverter.hpp"
 #include "depthai_ros_driver/param_handlers/base_param_handler.hpp"
 

depthai_ros_driver/include/depthai_ros_driver/param_handlers/nn_param_handler.hpp

@@ -40,7 +40,7 @@ class NNParamHandler : public BaseParamHandler {
     template <typename T>
     void declareParams(std::shared_ptr<T> nn, std::shared_ptr<dai::node::ImageManip> imageManip) {
         declareAndLogParam<bool>("i_disable_resize", false);
-		declareAndLogParam<bool>("i_desqueeze_output", false);
+        declareAndLogParam<bool>("i_desqueeze_output", false);
         declareAndLogParam<bool>("i_enable_passthrough", false);
         declareAndLogParam<bool>("i_enable_passthrough_depth", false);
         declareAndLogParam<bool>("i_get_base_device_timestamp", false);

depthai_ros_driver/include/depthai_ros_driver/utils.hpp

@@ -4,11 +4,6 @@
 #include <stdexcept>
 #include <string>
 #include <unordered_map>
-#include <vector>
-
-namespace dai {
-enum class CameraBoardSocket;
-}  // namespace dai
 
 #include "depthai-shared/common/CameraBoardSocket.hpp"
 #include "depthai-shared/datatype/RawImgFrame.hpp"
@@ -52,6 +47,7 @@ struct ImgConverterConfig {
     bool getBaseDeviceTimestamp = false;
     bool updateROSBaseTimeOnRosMsg = false;
     bool lowBandwidth = false;
+    bool isStereo = false;
     dai::RawImgFrame::Type encoding = dai::RawImgFrame::Type::BGR888i;
     bool addExposureOffset = false;
     dai::CameraExposureOffset expOffset = dai::CameraExposureOffset::START;
@@ -71,7 +67,7 @@ struct ImgPublisherConfig {
     dai::CameraBoardSocket rightSocket = dai::CameraBoardSocket::CAM_C;
     std::string calibrationFile = "";
     std::string topicSuffix = "/image_raw";
-	std::string infoSuffix = "";
+    std::string infoSuffix = "";
     std::string compressedTopicSuffix = "/image_raw/compressed";
     std::string infoMgrSuffix = "";
     bool rectified = false;

depthai_ros_driver/launch/camera.launch.py

@@ -143,8 +143,8 @@ def launch_setup(context, *args, **kwargs):
             },
         }
         parameter_overrides["depth"] = {
-            "i_publish_left_rect": True,
-            "i_publish_right_rect": True,
+            "i_left_rect_publish_topic": True,
+            "i_right_rect_publish_topic": True,
         }
 
     tf_params = {}

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.3</version>
+  <version>2.10.5</version>
   <description>Depthai ROS Monolithic node.</description>
   <maintainer email="[email protected]">Adam Serafin</maintainer>
 

depthai_ros_driver/plugins.xml

@@ -29,4 +29,4 @@
     <class type="depthai_ros_driver::pipeline_gen::RGBToF" base_class_type="depthai_ros_driver::pipeline_gen::BasePipeline">
         <description>RGB + ToF Pipeline.</description>
     </class>
-</library>
+</library>

depthai_ros_driver/src/camera.cpp

@@ -20,7 +20,9 @@ Camera::Camera(const rclcpp::NodeOptions& options) : rclcpp::Node("camera", opti
     });
     rclcpp::on_shutdown([this]() { stop(); });
 }
-Camera::~Camera() = default;
+Camera::~Camera() {
+    stop();
+}
 void Camera::onConfigure() {
     getDeviceType();
     createPipeline();
@@ -116,7 +118,6 @@ void Camera::saveCalib() {
     savePath << "/tmp/" << device->getMxId().c_str() << "_calibration.json";
     RCLCPP_INFO(get_logger(), "Saving calibration to: %s", savePath.str().c_str());
     calibHandler.eepromToJsonFile(savePath.str());
-    auto json = calibHandler.eepromToJson();
 }
 
 void Camera::loadCalib(const std::string& path) {
@@ -287,13 +288,13 @@ rcl_interfaces::msg::SetParametersResult Camera::parameterCB(const std::vector<r
             if(p.get_name() == ph->getFullParamName("i_laser_dot_brightness")) {
                 float laserdotBrightness = float(p.get_value<int>());
                 if(laserdotBrightness > 1.0) {
-                    laserdotBrightness = 1200.0 / laserdotBrightness;
+                    laserdotBrightness = laserdotBrightness / 1200.0;
                 }
                 device->setIrLaserDotProjectorIntensity(laserdotBrightness);
             } else if(p.get_name() == ph->getFullParamName("i_floodlight_brightness")) {
                 float floodlightBrightness = float(p.get_value<int>());
                 if(floodlightBrightness > 1.0) {
-                    floodlightBrightness = 1500.0 / floodlightBrightness;
+                    floodlightBrightness = floodlightBrightness / 1500.0;
                 }
                 device->setIrFloodLightIntensity(floodlightBrightness);
             }