Finish QoS updates (#1019)

This implements the remainder of #847:

 - Make sure publishers default to system defaults (reliable)
- Add QoS overriding where possible (some of the image_transport /
message_filters stuff doesn't really support that)
 - Use the matching heuristic for subscribers consistently

(cherry picked from commit d272c4e)

# Conflicts:
#	depth_image_proc/src/point_cloud_xyzi_radial.cpp

depth_image_proc/doc/tutorials.rst

@@ -57,3 +57,7 @@ parameter is used:
 Remapping camera_info Topics
 ----------------------------
 See `tutorial in image_pipline <https://docs.ros.org/en/rolling/p/image_pipeline/tutorials.html#remapping-camera-info-topics>`_.
+
+Using QoS Overrides
+-------------------
+See `tutorial in image_pipline <https://docs.ros.org/en/rolling/p/image_pipeline/tutorials.html#using-qos-overrides>`_.

depth_image_proc/package.xml

@@ -32,6 +32,7 @@
   <depend>image_geometry</depend>
   <depend>image_transport</depend>
   <depend>libopencv-dev</depend>
+  <depend>image_proc</depend>
   <depend>message_filters</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

depth_image_proc/src/convert_metric.cpp

@@ -41,6 +41,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <image_transport/image_transport.hpp>
+#include <image_proc/utils.hpp>
 #include <sensor_msgs/image_encodings.hpp>
 
 namespace depth_image_proc
@@ -68,7 +69,7 @@ ConvertMetricNode::ConvertMetricNode(const rclcpp::NodeOptions & options)
   // TransportHints does not actually declare the parameter
   this->declare_parameter<std::string>("image_transport", "raw");
 
-  // Create publisher with connect callback
+  // Setup lazy subscriber using publisher connection callback
   rclcpp::PublisherOptions pub_options;
   pub_options.event_callbacks.matched_callback =
     [this](rclcpp::MatchedInfo &)
@@ -83,18 +84,24 @@ ConvertMetricNode::ConvertMetricNode(const rclcpp::NodeOptions & options)
         std::string topic = node_base->resolve_topic_or_service_name("image_raw", false);
         // Get transport hints
         image_transport::TransportHints hints(this);
+        // Create subscriber with QoS matched to subscribed topic publisher
+        auto qos_profile = image_proc::getTopicQosProfile(this, topic);
         sub_raw_ = image_transport::create_subscription(
           this, topic,
           std::bind(&ConvertMetricNode::depthCb, this, std::placeholders::_1),
-          hints.getTransport());
+          hints.getTransport(),
+          qos_profile);
       }
     };
   // For compressed topics to remap appropriately, we need to pass a
   // fully expanded and remapped topic name to image_transport
   auto node_base = this->get_node_base_interface();
   std::string topic = node_base->resolve_topic_or_service_name("image", false);
-  pub_depth_ =
-    image_transport::create_publisher(this, topic, rmw_qos_profile_default, pub_options);
+
+  // Create publisher - allow overriding QoS settings (history, depth, reliability)
+  pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
+  pub_depth_ = image_transport::create_publisher(this, topic, rmw_qos_profile_default,
+      pub_options);
 }
 
 void ConvertMetricNode::depthCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg)

depth_image_proc/src/crop_foremost.cpp

@@ -38,6 +38,7 @@
 #include "depth_image_proc/visibility.h"
 
 #include <rclcpp/rclcpp.hpp>
+#include <image_proc/utils.hpp>
 #include <image_transport/image_transport.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 
@@ -74,7 +75,8 @@ CropForemostNode::CropForemostNode(const rclcpp::NodeOptions & options)
 
   distance_ = this->declare_parameter("distance", 0.0);
 
-  // Create publisher with connect callback
+
+  // Setup lazy subscriber using publisher connection callback
   rclcpp::PublisherOptions pub_options;
   pub_options.event_callbacks.matched_callback =
     [this](rclcpp::MatchedInfo &)
@@ -89,18 +91,24 @@ CropForemostNode::CropForemostNode(const rclcpp::NodeOptions & options)
         std::string topic = node_base->resolve_topic_or_service_name("image_raw", false);
         // Get transport hints
         image_transport::TransportHints hints(this);
+        // Create publisher with same QoS as subscribed topic publisher
+        auto qos_profile = image_proc::getTopicQosProfile(this, topic);
         sub_raw_ = image_transport::create_subscription(
           this, topic,
           std::bind(&CropForemostNode::depthCb, this, std::placeholders::_1),
-          hints.getTransport());
+          hints.getTransport(),
+          qos_profile);
       }
     };
   // For compressed topics to remap appropriately, we need to pass a
   // fully expanded and remapped topic name to image_transport
   auto node_base = this->get_node_base_interface();
   std::string topic = node_base->resolve_topic_or_service_name("image", false);
-  pub_depth_ =
-    image_transport::create_publisher(this, topic, rmw_qos_profile_default, pub_options);
+
+  // Create publisher - allow overriding QoS settings (history, depth, reliability)
+  pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
+  pub_depth_ = image_transport::create_publisher(this, topic, rmw_qos_profile_default,
+    pub_options);
 }
 
 void CropForemostNode::depthCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg)

depth_image_proc/src/disparity.cpp

@@ -119,8 +119,10 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options)
         sub_info_.subscribe(this, "right/camera_info");
       }
     };
+  // Allow overriding QoS settings (history, depth, reliability)
+  pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
   pub_disparity_ = create_publisher<stereo_msgs::msg::DisparityImage>(
-    "left/disparity", rclcpp::SensorDataQoS(), pub_options);
+    "left/disparity", rclcpp::SystemDefaultsQoS(), pub_options);
 }
 
 void DisparityNode::depthCb(

depth_image_proc/src/point_cloud_xyz.cpp

@@ -40,6 +40,7 @@
 
 #include <depth_image_proc/point_cloud_xyz.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <image_proc/utils.hpp>
 #include <image_transport/image_transport.hpp>
 #include <sensor_msgs/image_encodings.hpp>
 #include <depth_image_proc/conversions.hpp>
@@ -75,10 +76,12 @@ PointCloudXyzNode::PointCloudXyzNode(const rclcpp::NodeOptions & options)
         auto node_base = this->get_node_base_interface();
         std::string topic = node_base->resolve_topic_or_service_name("image_rect", false);
 
-        // Get transport and QoS
+        // Get transport hints
         image_transport::TransportHints depth_hints(this, "raw", "depth_image_transport");
-        auto custom_qos = rmw_qos_profile_system_default;
-        custom_qos.depth = queue_size_;
+
+        // Create subscriber with QoS matched to subscribed topic publisher
+        auto qos_profile = image_proc::getTopicQosProfile(this, topic);
+        qos_profile.depth = queue_size_;
 
         sub_depth_ = image_transport::create_camera_subscription(
           this,
@@ -87,10 +90,14 @@ PointCloudXyzNode::PointCloudXyzNode(const rclcpp::NodeOptions & options)
             &PointCloudXyzNode::depthCb, this, std::placeholders::_1,
             std::placeholders::_2),
           depth_hints.getTransport(),
-          custom_qos);
+          qos_profile);
       }
     };
-  pub_point_cloud_ = create_publisher<PointCloud2>("points", rclcpp::SensorDataQoS(), pub_options);
+
+  // Allow overriding QoS settings (history, depth, reliability)
+  pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
+  pub_point_cloud_ =
+    create_publisher<PointCloud2>("points", rclcpp::SystemDefaultsQoS(), pub_options);
 }
 
 void PointCloudXyzNode::depthCb(

depth_image_proc/src/point_cloud_xyz_radial.cpp

@@ -39,6 +39,7 @@
 
 #include <depth_image_proc/point_cloud_xyz_radial.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <image_proc/utils.hpp>
 #include <image_transport/image_transport.hpp>
 #include <sensor_msgs/image_encodings.hpp>
 #include <depth_image_proc/depth_traits.hpp>
@@ -56,7 +57,7 @@ PointCloudXyzRadialNode::PointCloudXyzRadialNode(const rclcpp::NodeOptions & opt
   // Read parameters
   queue_size_ = this->declare_parameter<int>("queue_size", 5);
 
-  // Create publisher with connect callback
+  // Setup lazy subscriber using publisher connection callback
   rclcpp::PublisherOptions pub_options;
   pub_options.event_callbacks.matched_callback =
     [this](rclcpp::MatchedInfo & s)
@@ -69,10 +70,11 @@ PointCloudXyzRadialNode::PointCloudXyzRadialNode(const rclcpp::NodeOptions & opt
         // fully expanded and remapped topic name to image_transport
         auto node_base = this->get_node_base_interface();
         std::string topic = node_base->resolve_topic_or_service_name("depth/image_raw", false);
-        // Get transport and QoS
+        // Get transport hints
         image_transport::TransportHints depth_hints(this, "raw", "depth_image_transport");
-        auto custom_qos = rmw_qos_profile_system_default;
-        custom_qos.depth = queue_size_;
+        // Create subscriber with QoS matched to subscribed topic publisher
+        auto qos_profile = image_proc::getTopicQosProfile(this, topic);
+        qos_profile.depth = queue_size_;
         // Create subscriber
         sub_depth_ = image_transport::create_camera_subscription(
           this,
@@ -81,11 +83,14 @@ PointCloudXyzRadialNode::PointCloudXyzRadialNode(const rclcpp::NodeOptions & opt
             &PointCloudXyzRadialNode::depthCb, this, std::placeholders::_1,
             std::placeholders::_2),
           depth_hints.getTransport(),
-          custom_qos);
+          qos_profile);
       }
     };
+
+  // Allow overriding QoS settings (history, depth, reliability)
+  pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
   pub_point_cloud_ = create_publisher<sensor_msgs::msg::PointCloud2>(
-    "points", rclcpp::SensorDataQoS(), pub_options);
+    "points", rclcpp::SystemDefaultsQoS(), pub_options);
 }
 
 void PointCloudXyzRadialNode::depthCb(

depth_image_proc/src/point_cloud_xyzi.cpp

@@ -111,7 +111,10 @@ PointCloudXyziNode::PointCloudXyziNode(const rclcpp::NodeOptions & options)
         sub_info_.subscribe(this, intensity_info_topic);
       }
     };
-  pub_point_cloud_ = create_publisher<PointCloud>("points", rclcpp::SensorDataQoS(), pub_options);
+  // Allow overriding QoS settings (history, depth, reliability)
+  pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
+  pub_point_cloud_ = create_publisher<PointCloud>("points", rclcpp::SystemDefaultsQoS(),
+      pub_options);
 }
 
 void PointCloudXyziNode::imageCb(

depth_image_proc/src/point_cloud_xyzi_radial.cpp

@@ -37,6 +37,7 @@
 
 #include "depth_image_proc/visibility.h"
 
+#include <image_proc/utils.hpp>
 #include <image_transport/camera_common.hpp>
 #include <image_transport/image_transport.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -73,7 +74,7 @@ PointCloudXyziRadialNode::PointCloudXyziRadialNode(const rclcpp::NodeOptions & o
       std::placeholders::_2,
       std::placeholders::_3));
 
-  // Create publisher with connect callback
+  // Setup lazy subscriber using publisher connection callback
   rclcpp::PublisherOptions pub_options;
   pub_options.event_callbacks.matched_callback =
     [this](rclcpp::MatchedInfo & s)
@@ -98,16 +99,28 @@ PointCloudXyziRadialNode::PointCloudXyziRadialNode(const rclcpp::NodeOptions & o
 
         // depth image can use different transport.(e.g. compressedDepth)
         image_transport::TransportHints depth_hints(this, "raw", "depth_image_transport");
-        sub_depth_.subscribe(this, depth_topic, depth_hints.getTransport());
+        // Create subscriber with QoS matched to subscribed topic publisher
+        auto depth_qos_profile = image_proc::getTopicQosProfile(this, depth_topic);
+        sub_depth_.subscribe(this, depth_topic, depth_hints.getTransport(), depth_qos_profile);
 
         // intensity uses normal ros transport hints.
         image_transport::TransportHints hints(this);
+<<<<<<< HEAD
         sub_intensity_.subscribe(this, intensity_topic, hints.getTransport());
         sub_info_.subscribe(this, intensity_info_topic);
+=======
+        // Create subscriber with QoS matched to subscribed topic publisher
+        auto qos_profile = image_proc::getTopicQosProfile(this, intensity_topic);
+        sub_intensity_.subscribe(this, intensity_topic, hints.getTransport(), qos_profile);
+        sub_info_.subscribe(this, intensity_info_topic, rclcpp::QoS(10));
+>>>>>>> d272c4e (Finish QoS updates (#1019))
       }
     };
+
+  // Allow overriding QoS settings (history, depth, reliability)
+  pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
   pub_point_cloud_ = create_publisher<sensor_msgs::msg::PointCloud2>(
-    "points", rclcpp::SensorDataQoS(), pub_options);
+    "points", rclcpp::SystemDefaultsQoS(), pub_options);
 }
 
 void PointCloudXyziRadialNode::imageCb(

depth_image_proc/src/point_cloud_xyzrgb.cpp

@@ -137,7 +137,10 @@ PointCloudXyzrgbNode::PointCloudXyzrgbNode(const rclcpp::NodeOptions & options)
         sub_info_.subscribe(this, rgb_info_topic);
       }
     };
-  pub_point_cloud_ = create_publisher<PointCloud2>("points", rclcpp::SensorDataQoS(), pub_options);
+  // Allow overriding QoS settings (history, depth, reliability)
+  pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
+  pub_point_cloud_ = create_publisher<PointCloud2>("points", rclcpp::SystemDefaultsQoS(),
+      pub_options);
 }
 
 void PointCloudXyzrgbNode::imageCb(

depth_image_proc/src/point_cloud_xyzrgb_radial.cpp

@@ -121,7 +121,10 @@ PointCloudXyzrgbRadialNode::PointCloudXyzrgbRadialNode(const rclcpp::NodeOptions
         sub_info_.subscribe(this, rgb_info_topic);
       }
     };
-  pub_point_cloud_ = create_publisher<PointCloud2>("points", rclcpp::SensorDataQoS(), pub_options);
+  // Allow overriding QoS settings (history, depth, reliability)
+  pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
+  pub_point_cloud_ = create_publisher<PointCloud2>("points", rclcpp::SystemDefaultsQoS(),
+      pub_options);
 }
 
 void PointCloudXyzrgbRadialNode::imageCb(

depth_image_proc/src/register.cpp

@@ -149,6 +149,8 @@ RegisterNode::RegisterNode(const rclcpp::NodeOptions & options)
   std::string topic =
     node_base->resolve_topic_or_service_name("depth_registered/image_rect", false);
 
+  // Allow overriding QoS settings (history, depth, reliability)
+  pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
   pub_registered_ =
     image_transport::create_camera_publisher(
     this, topic,

image_pipeline/doc/tutorials.rst

@@ -24,3 +24,83 @@ camera info topic. An example:
  * The fully resolved name for the camera info is now ``my_camera/camera_info``.
  * If your camera driver actually publishes ``another_ns/camera_info``, then
    you would have to remap ``my_camera/camera_info`` to ``another_ns/camera_info``.
+
+.. _`Using QoS Overrides`:
+
+Using QoS Overrides
+-------------------
+Most components in image_pipeline follow the Quality of Service (QoS) recommendations
+of `REP-2003 <https://ros.org/reps/rep-2003.html>`_ by default. This means that
+subscribers are configured with the "Sensor Data" QoS (which uses "best effort"),
+and publishers are configured with "System Default" QoS (which uses "reliable" transport).
+
+These QoS settings work well for many applications, but can be overridden using the
+standard features of recent ROS 2 releases. This involves adding additional parameters
+to your YAML or launch file. For example, we could update the
+`image_publisher_file.launch.py` launch file to change the QoS settings:
+
+.. code-block:: python
+
+    import os
+
+    from ament_index_python.packages import get_package_share_directory
+    from launch import LaunchDescription
+    import launch_ros.actions
+
+    def generate_launch_description():
+        filename = os.path.join(get_package_share_directory('image_publisher'), 'launch',
+                                'splash.png')
+        return LaunchDescription([
+
+            launch_ros.actions.Node(
+                package='image_publisher', executable='image_publisher_node', output='screen',
+                arguments=[filename],
+                parameters=[{
+                    'qos_overrides': {
+                        '/camera/image_raw': {
+                            'publisher': {
+                                'reliability': 'best_effort',
+                                'history': 'keep_last',
+                                'depth': 100,
+                            }
+                        }
+                    },
+                }],
+                remappings=[('image_raw', '/camera/image_raw'),
+                            ('camera_info', '/camera/camera_info')]),
+        ])
+
+If we then run the launch file, we can see our settings are applied:
+
+.. code-block:: bash
+
+    $ ros2 topic info /camera/image_raw -v
+    Type: sensor_msgs/msg/Image
+
+    Publisher count: 1
+
+    Node name: ImagePublisher
+    Node namespace: /
+    Topic type: sensor_msgs/msg/Image
+    Topic type hash: RIHS01_d31d41a9a4c4bc8eae9be757b0beed306564f7526c88ea6a4588fb9582527d47
+    Endpoint type: PUBLISHER
+    GID: 01.10.bf.bd.b7.85.a8.33.58.34.5c.ae.00.00.17.03
+    QoS profile:
+      Reliability: BEST_EFFORT
+      History (Depth): KEEP_LAST (100)
+      Durability: VOLATILE
+      Lifespan: Infinite
+      Deadline: Infinite
+      Liveliness: AUTOMATIC
+      Liveliness lease duration: Infinite
+
+    Subscription count: 0
+
+A few things to note:
+
+ * The topic name (``/camera/image_raw``) must be the fully resolved topic name,
+   and therefore we use the remapped topic name rather than the name in the code
+   for the component.
+ * Only ``reliability``, ``history``, and ``depth`` can be overwritten.
+
+For more information on QoS overrides, see the `design doc <https://design.ros2.org/articles/qos_configurability.html>`_.

image_proc/doc/tutorials.rst

@@ -75,6 +75,10 @@ Remapping camera_info Topics
 ----------------------------
 See `tutorial in image_pipline <https://docs.ros.org/en/rolling/p/image_pipeline/tutorials.html#remapping-camera-info-topics>`_.
 
+Using QoS Overrides
+-------------------
+See `tutorial in image_pipline <https://docs.ros.org/en/rolling/p/image_pipeline/tutorials.html#using-qos-overrides>`_.
+
 .. _Using image_proc Launch File:
 
 Using image_proc Launch File