Qos overrides (#339)

luxonis · Jun 23, 2023 · 131bff4 · 131bff4
1 parent 657068b
commit 131bff4
Show file tree

Hide file tree

Showing 5 changed files with 23 additions and 11 deletions.
diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst
@@ -2,15 +2,16 @@
 Changelog for package depthai-ros
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-2.7.3 (20230-06-16)
-
+2.7.3 (2023-06-16)
+-------------------
 * Pipeline generation as a plugin
 * Fixed bounding box generation issue
 * Stereo rectified streams publishing
 * Camera trigger mechanisms
 * Brightness filter
 
-2.7.2 (20230-05-08)
+2.7.2 (2023-05-08)
+-------------------
 * IMU improvements
 
 2.7.1 (2023-03-29)

diff --git a/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp b/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp
@@ -8,6 +8,7 @@
 #include "depthai/device/DataQueue.hpp"
 #include "image_transport/image_transport.hpp"
 #include "rclcpp/node.hpp"
+#include "rclcpp/qos.hpp"
 #include "sensor_msgs/msg/camera_info.hpp"
 #include "sensor_msgs/msg/image.hpp"
 #include "std_msgs/msg/header.hpp"
@@ -147,7 +148,9 @@ BridgePublisher<RosMsg, SimMsg>::BridgePublisher(std::shared_ptr<dai::DataOutput
 
 template <class RosMsg, class SimMsg>
 typename rclcpp::Publisher<RosMsg>::SharedPtr BridgePublisher<RosMsg, SimMsg>::advertise(int queueSize, std::false_type) {
-    return _node->create_publisher<RosMsg>(_rosTopic, queueSize);
+    rclcpp::PublisherOptions options;
+    options.qos_overriding_options = rclcpp::QosOverridingOptions();
+    return _node->create_publisher<RosMsg>(_rosTopic, queueSize, options);
 }
 
 template <class RosMsg, class SimMsg>
@@ -158,7 +161,9 @@ std::shared_ptr<image_transport::Publisher> BridgePublisher<RosMsg, SimMsg>::adv
         if(_cameraParamUri.empty()) {
             _camInfoManager->setCameraInfo(_cameraInfoData);
         }
-        _cameraInfoPublisher = _node->create_publisher<ImageMsgs::CameraInfo>(_cameraName + "/camera_info", queueSize);
+        rclcpp::PublisherOptions options;
+        options.qos_overriding_options = rclcpp::QosOverridingOptions();
+        _cameraInfoPublisher = _node->create_publisher<ImageMsgs::CameraInfo>(_cameraName + "/camera_info", queueSize, options);
     }
     return std::make_shared<image_transport::Publisher>(_it.advertise(_rosTopic, queueSize));
 }

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
@@ -58,7 +58,9 @@ class Detection : public BaseNode {
                                                                          height,
                                                                          false,
                                                                          ph->getParam<bool>("i_get_base_device_timestamp"));
-        detPub = getROSNode()->template create_publisher<vision_msgs::msg::Detection2DArray>("~/" + getName() + "/detections", 10);
+        rclcpp::PublisherOptions options;
+        options.qos_overriding_options = rclcpp::QosOverridingOptions();
+        detPub = getROSNode()->template create_publisher<vision_msgs::msg::Detection2DArray>("~/" + getName() + "/detections", 10, options);
         nnQ->addCallback(std::bind(&Detection::detectionCB, this, std::placeholders::_1, std::placeholders::_2));
 
         if(ph->getParam<bool>("i_enable_passthrough")) {

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
@@ -61,7 +61,9 @@ class SpatialDetection : public BaseNode {
                                                                              false,
                                                                              ph->getParam<bool>("i_get_base_device_timestamp"));
         nnQ->addCallback(std::bind(&SpatialDetection::spatialCB, this, std::placeholders::_1, std::placeholders::_2));
-        detPub = getROSNode()->template create_publisher<vision_msgs::msg::Detection3DArray>("~/" + getName() + "/spatial_detections", 10);
+        rclcpp::PublisherOptions options;
+        options.qos_overriding_options = rclcpp::QosOverridingOptions();
+        detPub = getROSNode()->template create_publisher<vision_msgs::msg::Detection3DArray>("~/" + getName() + "/spatial_detections", 10, options);
 
         if(ph->getParam<bool>("i_enable_passthrough")) {
             ptQ = device->getOutputQueue(ptQName, ph->getParam<int>("i_max_q_size"), false);

diff --git a/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp b/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp
@@ -38,6 +38,8 @@ void Imu::setupQueues(std::shared_ptr<dai::Device> device) {
     imuQ = device->getOutputQueue(imuQName, ph->getParam<int>("i_max_q_size"), false);
     auto tfPrefix = std::string(getROSNode()->get_name()) + "_" + getName();
     auto imuMode = ph->getSyncMethod();
+    rclcpp::PublisherOptions options;
+    options.qos_overriding_options = rclcpp::QosOverridingOptions();
     imuConverter = std::make_unique<dai::ros::ImuConverter>(tfPrefix + "_frame",
                                                             imuMode,
                                                             ph->getParam<float>("i_acc_cov"),
@@ -49,18 +51,18 @@ void Imu::setupQueues(std::shared_ptr<dai::Device> device) {
     param_handlers::imu::ImuMsgType msgType = ph->getMsgType();
     switch(msgType) {
         case param_handlers::imu::ImuMsgType::IMU: {
-            rosImuPub = getROSNode()->create_publisher<sensor_msgs::msg::Imu>("~/" + getName() + "/data", 10);
+            rosImuPub = getROSNode()->create_publisher<sensor_msgs::msg::Imu>("~/" + getName() + "/data", 10, options);
             imuQ->addCallback(std::bind(&Imu::imuRosQCB, this, std::placeholders::_1, std::placeholders::_2));
             break;
         }
         case param_handlers::imu::ImuMsgType::IMU_WITH_MAG: {
-            daiImuPub = getROSNode()->create_publisher<depthai_ros_msgs::msg::ImuWithMagneticField>("~/" + getName() + "/data", 10);
+            daiImuPub = getROSNode()->create_publisher<depthai_ros_msgs::msg::ImuWithMagneticField>("~/" + getName() + "/data", 10, options);
             imuQ->addCallback(std::bind(&Imu::imuDaiRosQCB, this, std::placeholders::_1, std::placeholders::_2));
             break;
         }
         case param_handlers::imu::ImuMsgType::IMU_WITH_MAG_SPLIT: {
-            rosImuPub = getROSNode()->create_publisher<sensor_msgs::msg::Imu>("~/" + getName() + "/data", 10);
-            magPub = getROSNode()->create_publisher<sensor_msgs::msg::MagneticField>("~/" + getName() + "/mag", 10);
+            rosImuPub = getROSNode()->create_publisher<sensor_msgs::msg::Imu>("~/" + getName() + "/data", 10, options);
+            magPub = getROSNode()->create_publisher<sensor_msgs::msg::MagneticField>("~/" + getName() + "/mag", 10, options);
             imuQ->addCallback(std::bind(&Imu::imuMagQCB, this, std::placeholders::_1, std::placeholders::_2));
             break;
         }