Noetic low bandwidth fixes (#648)

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>
 
   <maintainer email="[email protected]">Adam Serafin</maintainer>

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)
@@ -12,6 +12,9 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 if(POLICY CMP0057)
     cmake_policy(SET CMP0057 NEW)
 endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic -Wno-deprecated-declarations -Wno-maybe-uninitialized -Wno-reorder -Wno-unused-parameter)
+endif()
 
 set(opencv_version 4)
 find_package(OpenCV ${opencv_version} QUIET COMPONENTS imgproc highgui calib3d)

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>

depthai_bridge/src/TFPublisher.cpp

@@ -246,7 +246,7 @@ std::string TFPublisher::getURDF() {
     ROS_DEBUG("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.10.2)
-project(depthai_descriptions VERSION 2.10.3 LANGUAGES CXX C)
+project(depthai_descriptions VERSION 2.10.5 LANGUAGES CXX C)
 
 
 find_package(catkin REQUIRED

depthai_descriptions/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <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,15 +1,17 @@
 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)
 set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
-add_compile_options(-g)
 
 ## is used, also find other catkin packages
 if(POLICY CMP0057)
     cmake_policy(SET CMP0057 NEW)
 endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Wno-deprecated-declarations -Wno-maybe-uninitialized)
+endif()
 
 set(_opencv_version 4)
 find_package(OpenCV 4 QUIET COMPONENTS imgproc highgui)

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>
 
   <maintainer email="[email protected]">Adam Serafin</maintainer>

depthai_examples/src/rgb_stereo_node.cpp

@@ -118,7 +118,6 @@ int main(int argc, char** argv) {
     auto stereoQueue = device.getOutputQueue("depth", 30, false);
     auto previewQueue = device.getOutputQueue("video", 30, false);
 
-    bool latched_cam_info = true;
     std::string stereo_uri = camera_param_uri + "/" + "right.yaml";
     std::string color_uri = camera_param_uri + "/" + "color.yaml";
 

depthai_filters/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.10.2)
-project(depthai_filters VERSION 2.10.3 LANGUAGES CXX C)
+project(depthai_filters VERSION 2.10.5 LANGUAGES CXX C)
 
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)

depthai_filters/package.xml

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

depthai_ros_driver/CMakeLists.txt

@@ -1,18 +1,17 @@
 cmake_minimum_required(VERSION 3.16.3)
-project(depthai_ros_driver VERSION 2.10.3)
+project(depthai_ros_driver VERSION 2.10.5)
 if(NOT CMAKE_C_STANDARD)
   set(CMAKE_C_STANDARD 99)
 endif()
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 11)
+  set(CMAKE_CXX_STANDARD 14)
 endif()
 
 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 -Wno-reorder -Wno-unused-parameter)
 endif()
 
 set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
-add_compile_options(-g)
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 set(BUILD_SHARED_LIBS ON)

depthai_ros_driver/include/depthai_ros_driver/camera.hpp

@@ -19,6 +19,10 @@ namespace depthai_ros_driver {
 using Trigger = std_srvs::Trigger;
 class Camera : public nodelet::Nodelet {
    public:
+    /**
+     * @brief      Destructor of the class Camera. Stops the device and destroys the pipeline.
+     */
+    ~Camera();
     void onInit() override;
     /**
      * @brief Creates the pipeline and starts the device. Also sets up parameter callback and services.
@@ -92,4 +96,4 @@ class Camera : public nodelet::Nodelet {
     double laserDotBrightness;
     std::unique_ptr<dai::ros::TFPublisher> tfPub;
 };
-}  // namespace depthai_ros_driver
+}  // namespace depthai_ros_driver

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

@@ -53,6 +53,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(ros::NodeHandle node);
+std::string tfPrefix(ros::NodeHandle node);
 std::string getSocketName(ros::NodeHandle node, dai::CameraBoardSocket socket);
 std::string getNodeName(ros::NodeHandle node, NodeNameEnum name);
 void basicCameraPub(const std::string& /*name*/,

depthai_ros_driver/include/depthai_ros_driver/utils.hpp

@@ -47,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;

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>
   <author>Adam Serafin</author>

depthai_ros_driver/src/camera.cpp

@@ -1,5 +1,7 @@
 #include "depthai_ros_driver/camera.hpp"
 
+#include <XLink/XLinkPublicDefines.h>
+
 #include <fstream>
 #include <memory>
 
@@ -52,6 +54,10 @@ void Camera::onInit() {
     diagSub = pNH.subscribe("/diagnostics", 1, &Camera::diagCB, this);
 }
 
+Camera::~Camera() {
+    stop();
+}
+
 void Camera::diagCB(const diagnostic_msgs::DiagnosticArray::ConstPtr& msg) {
     for(const auto& status : msg->status) {
         std::string nodeletName = pNH.getNamespace() + "_nodelet_manager";
@@ -241,7 +247,7 @@ void Camera::startDevice() {
                         }
                     } else if(!ip.empty() && info.name == ip) {
                         ROS_INFO("Connecting to the camera using ip: %s", ip.c_str());
-                        if(info.state == X_LINK_UNBOOTED || info.state == X_LINK_BOOTLOADER) {
+                        if(info.state == X_LINK_UNBOOTED || info.state == X_LINK_BOOTLOADER || info.state == X_LINK_ANY_STATE) {
                             device = std::make_shared<dai::Device>(info);
                             camRunning = true;
                         } else if(info.state == X_LINK_BOOTED) {

depthai_ros_driver/src/dai_nodes/base_node.cpp

@@ -32,7 +32,7 @@ std::string BaseNode::getSocketName(dai::CameraBoardSocket socket) {
     return sensor_helpers::getSocketName(getROSNode(), socket);
 }
 std::string BaseNode::getTFPrefix(const std::string& frameName) {
-    auto prefix = std::string(getROSNode().getNamespace()) + "_" + frameName;
+    auto prefix = sensor_helpers::tfPrefix(getROSNode()) + "_" + frameName;
     prefix.erase(0, 1);
     return prefix;
 }
@@ -48,22 +48,22 @@ dai::Node::Input BaseNode::getInput(int /*linkType*/) {
 }
 dai::Node::Input BaseNode::getInputByName(const std::string& /*name*/) {
     throw(std::runtime_error("getInputByName() not implemented"));
-};
+}
 
 void BaseNode::closeQueues() {
     throw(std::runtime_error("closeQueues() not implemented"));
 }
 std::shared_ptr<dai::node::XLinkOut> BaseNode::setupXout(std::shared_ptr<dai::Pipeline> pipeline, const std::string& name) {
     return utils::setupXout(pipeline, name);
-};
+}
 
 std::shared_ptr<sensor_helpers::ImagePublisher> BaseNode::setupOutput(std::shared_ptr<dai::Pipeline> pipeline,
                                                                       const std::string& qName,
                                                                       std::function<void(dai::Node::Input input)> nodeLink,
                                                                       bool isSynced,
                                                                       const utils::VideoEncoderConfig& encoderConfig) {
     return std::make_shared<sensor_helpers::ImagePublisher>(getROSNode(), pipeline, qName, nodeLink, isSynced, encoderConfig);
-};
+}
 
 void BaseNode::setNames() {
     throw(std::runtime_error("setNames() not implemented"));
@@ -82,7 +82,7 @@ void BaseNode::link(dai::Node::Input /*in*/, int /*linkType = 0*/) {
 }
 std::vector<std::shared_ptr<sensor_helpers::ImagePublisher>> BaseNode::getPublishers() {
     return std::vector<std::shared_ptr<sensor_helpers::ImagePublisher>>();
-};
+}
 
 void BaseNode::updateParams(parametersConfig& /*config*/) {
     return;

depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp

@@ -21,7 +21,7 @@ ImagePublisher::ImagePublisher(ros::NodeHandle node,
                                std::function<void(dai::Node::Input in)> linkFunc,
                                bool synced,
                                const utils::VideoEncoderConfig& encoderConfig)
-    : node(node), encConfig(encoderConfig), qName(qName), synced(synced), it(node) {
+    : node(node), it(node), encConfig(encoderConfig), qName(qName), synced(synced) {
     if(!synced) {
         xout = utils::setupXout(pipeline, qName);
     }
@@ -89,7 +89,7 @@ void ImagePublisher::createImageConverter(std::shared_ptr<dai::Device> device) {
     if(convConfig.alphaScalingEnabled) {
         converter->setAlphaScaling(convConfig.alphaScaling);
     }
-    if(convConfig.outputDisparity) {
+    if(convConfig.isStereo && !convConfig.outputDisparity) {
         auto calHandler = device->readCalibration();
         double baseline = calHandler.getBaselineDistance(pubConfig.leftSocket, pubConfig.rightSocket, false);
         if(convConfig.reverseSocketOrder) {
@@ -105,8 +105,10 @@ std::shared_ptr<dai::node::VideoEncoder> ImagePublisher::createEncoder(std::shar
     auto enc = pipeline->create<dai::node::VideoEncoder>();
     enc->setQuality(encoderConfig.quality);
     enc->setProfile(encoderConfig.profile);
-    enc->setBitrate(encoderConfig.bitrate);
-    enc->setKeyframeFrequency(encoderConfig.frameFreq);
+    if(encoderConfig.profile != dai::VideoEncoderProperties::Profile::MJPEG) {
+        enc->setBitrate(encoderConfig.bitrate);
+        enc->setKeyframeFrequency(encoderConfig.frameFreq);
+    }
     return enc;
 }
 void ImagePublisher::createInfoManager(std::shared_ptr<dai::Device> device) {
@@ -122,10 +124,10 @@ void ImagePublisher::createInfoManager(std::shared_ptr<dai::Device> device) {
     } else {
         infoManager->loadCameraInfo(pubConfig.calibrationFile);
     }
-};
+}
 ImagePublisher::~ImagePublisher() {
     closeQueue();
-};
+}
 
 void ImagePublisher::closeQueue() {
     if(dataQ) dataQ->close();

depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp

@@ -95,6 +95,15 @@ bool rsCompabilityMode(ros::NodeHandle node) {
     node.getParam("camera_i_rs_compat", compat);
     return compat;
 }
+std::string tfPrefix(ros::NodeHandle node) {
+    bool pubTF = false;
+    std::string tfPrefix = node.getNamespace();
+    node.getParam("camera_i_publish_tf_from_calibration", pubTF);
+    if(pubTF) {
+        node.getParam("camera_i_tf_base_frame", tfPrefix);
+    }
+    return tfPrefix;
+}
 std::string getNodeName(ros::NodeHandle node, NodeNameEnum name) {
     if(rsCompabilityMode(node)) {
         return rsNodeNameMap.at(name);

depthai_ros_driver/src/dai_nodes/sensors/stereo.cpp

@@ -75,8 +75,9 @@ void Stereo::setNames() {
 
 void Stereo::setXinXout(std::shared_ptr<dai::Pipeline> pipeline) {
     bool outputDisparity = ph->getParam<bool>("i_output_disparity");
+    bool lowBandwidth = ph->getParam<bool>("i_low_bandwidth");
     std::function<void(dai::Node::Input)> stereoLinkChoice;
-    if(outputDisparity) {
+    if(outputDisparity || lowBandwidth) {
         stereoLinkChoice = [&](auto input) { stereoCamNode->disparity.link(input); };
     } else {
         stereoLinkChoice = [&](auto input) { stereoCamNode->depth.link(input); };
@@ -87,7 +88,7 @@ void Stereo::setXinXout(std::shared_ptr<dai::Pipeline> pipeline) {
         encConf.bitrate = ph->getParam<int>("i_low_bandwidth_bitrate");
         encConf.frameFreq = ph->getParam<int>("i_low_bandwidth_frame_freq");
         encConf.quality = ph->getParam<int>("i_low_bandwidth_quality");
-        encConf.enabled = ph->getParam<bool>("i_low_bandwidth");
+        encConf.enabled = lowBandwidth;
 
         stereoPub = setupOutput(pipeline, stereoQName, stereoLinkChoice, ph->getParam<bool>("i_synced"), encConf);
     }
@@ -188,6 +189,7 @@ void Stereo::setupStereoQueue(std::shared_ptr<dai::Device> device) {
         convConfig.alphaScaling = ph->getParam<double>("i_alpha_scaling");
     }
     convConfig.outputDisparity = ph->getParam<bool>("i_output_disparity");
+    convConfig.isStereo = true;
 
     utils::ImgPublisherConfig pubConf;
     pubConf.daiNodeName = getName();

depthai_ros_driver/src/dai_nodes/sys_logger.cpp

@@ -72,7 +72,22 @@ void SysLogger::produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper&
         auto logData = loggerQ->get<dai::SystemInformation>(std::chrono::seconds(5), timeout);
         if(!timeout) {
             stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "System Information");
-            stat.add("System Information", sysInfoToString(*logData));
+            auto sysInfo = *logData;
+            stat.add("Leon CSS CPU Usage", sysInfo.leonCssCpuUsage.average * 100);
+            stat.add("Leon MSS CPU Usage", sysInfo.leonMssCpuUsage.average * 100);
+            stat.add("Ddr Memory Usage", sysInfo.ddrMemoryUsage.used / (1024.0f * 1024.0f));
+            stat.add("Ddr Memory Total", sysInfo.ddrMemoryUsage.total / (1024.0f * 1024.0f));
+            stat.add("Cmx Memory Usage", sysInfo.cmxMemoryUsage.used / (1024.0f * 1024.0f));
+            stat.add("Cmx Memory Total", sysInfo.cmxMemoryUsage.total);
+            stat.add("Leon CSS Memory Usage", sysInfo.leonCssMemoryUsage.used / (1024.0f * 1024.0f));
+            stat.add("Leon CSS Memory Total", sysInfo.leonCssMemoryUsage.total / (1024.0f * 1024.0f));
+            stat.add("Leon MSS Memory Usage", sysInfo.leonMssMemoryUsage.used / (1024.0f * 1024.0f));
+            stat.add("Leon MSS Memory Total", sysInfo.leonMssMemoryUsage.total / (1024.0f * 1024.0f));
+            stat.add("Average Chip Temperature", sysInfo.chipTemperature.average);
+            stat.add("Leon CSS Chip Temperature", sysInfo.chipTemperature.css);
+            stat.add("Leon MSS Chip Temperature", sysInfo.chipTemperature.mss);
+            stat.add("UPA Chip Temperature", sysInfo.chipTemperature.upa);
+            stat.add("DSS Chip Temperature", sysInfo.chipTemperature.dss);
         } else {
             stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No Data");
         }