QoS improvements for image_proc and stereo_image_proc (ros-perception…

…#922)

First part of ros-perception#847 
* Add QoS overrides for all publishers (in the new, standard way)
* stereo_image_proc: Default subscriber QoS to SensorDataQoS
* Clean up some of the comments around lazy subscribers, make them more
consistent across nodes

image_proc/src/crop_decimate.cpp

@@ -133,14 +133,15 @@ CropDecimateNode::CropDecimateNode(const rclcpp::NodeOptions & options)
   int interpolation = this->declare_parameter("interpolation", 0);
   interpolation_ = static_cast<CropDecimateModes>(interpolation);
 
-  // Create image pub with connection callback
+  // Setup lazy subscriber using publisher connection callback
   rclcpp::PublisherOptions pub_options;
   pub_options.event_callbacks.matched_callback =
     [this](rclcpp::MatchedInfo &)
     {
       if (pub_.getNumSubscribers() == 0) {
         sub_.shutdown();
       } else if (!sub_) {
+        // Create subscriber with QoS matched to subscribed topic publisher
         auto qos_profile = getTopicQosProfile(this, image_topic_);
         image_transport::TransportHints hints(this);
         sub_ = image_transport::create_camera_subscription(
@@ -150,7 +151,10 @@ CropDecimateNode::CropDecimateNode(const rclcpp::NodeOptions & options)
       }
     };
 
-  // Create publisher with same QoS as subscribed topic
+  // Allow overriding QoS settings (history, depth, reliability)
+  pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
+
+  // Create publisher with QoS matched to subscribed topic publisher
   auto qos_profile = getTopicQosProfile(this, image_topic_);
   pub_ = image_transport::create_camera_publisher(this, "out/image_raw", qos_profile, pub_options);
 }

image_proc/src/crop_non_zero.cpp

@@ -61,14 +61,15 @@ CropNonZeroNode::CropNonZeroNode(const rclcpp::NodeOptions & options)
   auto node_base = this->get_node_base_interface();
   image_topic_ = node_base->resolve_topic_or_service_name("image_raw", false);
 
-  // Create image pub with connection callback
+  // Setup lazy subscriber using publisher connection callback
   rclcpp::PublisherOptions pub_options;
   pub_options.event_callbacks.matched_callback =
     [this](rclcpp::MatchedInfo &)
     {
       if (pub_.getNumSubscribers() == 0) {
         sub_raw_.shutdown();
       } else if (!sub_raw_) {
+        // Create subscriber with QoS matched to subscribed topic publisher
         auto qos_profile = getTopicQosProfile(this, image_topic_);
         image_transport::TransportHints hints(this);
         sub_raw_ = image_transport::create_subscription(
@@ -78,8 +79,11 @@ CropNonZeroNode::CropNonZeroNode(const rclcpp::NodeOptions & options)
       }
     };
 
-  // Create publisher with same QoS as subscribed topic
-  auto qos_profile = getTopicQosProfile(this, "image_raw");
+  // Allow overriding QoS settings (history, depth, reliability)
+  pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
+
+  // Create publisher with QoS matched to subscribed topic publisher
+  auto qos_profile = getTopicQosProfile(this, image_topic_);
   pub_ = image_transport::create_publisher(this, "image", qos_profile, pub_options);
 }
 

image_proc/src/debayer.cpp

@@ -61,14 +61,15 @@ DebayerNode::DebayerNode(const rclcpp::NodeOptions & options)
   auto node_base = this->get_node_base_interface();
   image_topic_ = node_base->resolve_topic_or_service_name("image_raw", false);
 
-  // Create publisher options to setup callback
+  // Setup lazy subscriber using publisher connection callback
   rclcpp::PublisherOptions pub_options;
   pub_options.event_callbacks.matched_callback =
     [this](rclcpp::MatchedInfo &)
     {
       if (pub_mono_.getNumSubscribers() == 0 && pub_color_.getNumSubscribers() == 0) {
         sub_raw_.shutdown();
       } else if (!sub_raw_) {
+        // Create subscriber with QoS matched to subscribed topic publisher
         auto qos_profile = getTopicQosProfile(this, image_topic_);
         image_transport::TransportHints hints(this);
         sub_raw_ = image_transport::create_subscription(
@@ -79,7 +80,10 @@ DebayerNode::DebayerNode(const rclcpp::NodeOptions & options)
       }
     };
 
-  // Create publisher with same QoS as subscribed topic
+  // Allow overriding QoS settings (history, depth, reliability)
+  pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
+
+  // Create publisher with QoS matched to subscribed topic publisher
   auto qos_profile = getTopicQosProfile(this, image_topic_);
   pub_mono_ = image_transport::create_publisher(this, "image_mono", qos_profile, pub_options);
   pub_color_ = image_transport::create_publisher(this, "image_color", qos_profile, pub_options);

image_proc/src/rectify.cpp

@@ -63,14 +63,15 @@ RectifyNode::RectifyNode(const rclcpp::NodeOptions & options)
   queue_size_ = this->declare_parameter("queue_size", 5);
   interpolation_ = this->declare_parameter("interpolation", 1);
 
-  // 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 &)
     {
       if (pub_rect_.getNumSubscribers() == 0) {
         sub_camera_.shutdown();
       } else if (!sub_camera_) {
+        // Create subscriber with QoS matched to subscribed topic publisher
         auto qos_profile = getTopicQosProfile(this, image_topic_);
         image_transport::TransportHints hints(this);
         sub_camera_ = image_transport::create_camera_subscription(
@@ -80,7 +81,10 @@ RectifyNode::RectifyNode(const rclcpp::NodeOptions & options)
       }
     };
 
-  // Create publisher with same QoS as subscribed topic
+  // Allow overriding QoS settings (history, depth, reliability)
+  pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
+
+  // Create publisher with QoS matched to subscribed topic publisher
   auto qos_profile = getTopicQosProfile(this, image_topic_);
   pub_rect_ = image_transport::create_publisher(this, "image_rect", qos_profile, pub_options);
 }

image_proc/src/resize.cpp

@@ -68,15 +68,15 @@ ResizeNode::ResizeNode(const rclcpp::NodeOptions & options)
   height_ = this->declare_parameter("height", -1);
   width_ = this->declare_parameter("width", -1);
 
-  // Create image pub with connection callback
+  // Setup lazy subscriber using publisher connection callback
   rclcpp::PublisherOptions pub_options;
   pub_options.event_callbacks.matched_callback =
     [this](rclcpp::MatchedInfo &)
     {
       if (pub_image_.getNumSubscribers() == 0) {
         sub_image_.shutdown();
       } else if (!sub_image_) {
-        // Match the subscriber QoS
+        // Create subscriber with QoS matched to subscribed topic publisher
         auto qos_profile = getTopicQosProfile(this, image_topic_);
         image_transport::TransportHints hints(this);
         sub_image_ = image_transport::create_camera_subscription(
@@ -87,6 +87,11 @@ ResizeNode::ResizeNode(const rclcpp::NodeOptions & options)
             std::placeholders::_2), hints.getTransport(), qos_profile);
       }
     };
+
+  // Allow overriding QoS settings (history, depth, reliability)
+  pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
+
+  // Create publisher with QoS matched to subscribed topic publisher
   auto qos_profile = getTopicQosProfile(this, image_topic_);
   pub_image_ = image_transport::create_camera_publisher(
     this, "resize/image_raw", qos_profile, pub_options);

stereo_image_proc/launch/stereo_image_proc.launch.py

@@ -55,7 +55,6 @@ def generate_launch_description():
             plugin='stereo_image_proc::DisparityNode',
             parameters=[{
                 'approximate_sync': LaunchConfiguration('approximate_sync'),
-                'use_system_default_qos': LaunchConfiguration('use_system_default_qos'),
                 'stereo_algorithm': LaunchConfiguration('stereo_algorithm'),
                 'prefilter_size': LaunchConfiguration('prefilter_size'),
                 'prefilter_cap': LaunchConfiguration('prefilter_cap'),
@@ -85,7 +84,6 @@ def generate_launch_description():
                 'approximate_sync': LaunchConfiguration('approximate_sync'),
                 'avoid_point_cloud_padding': LaunchConfiguration('avoid_point_cloud_padding'),
                 'use_color': LaunchConfiguration('use_color'),
-                'use_system_default_qos': LaunchConfiguration('use_system_default_qos'),
             }],
             remappings=[
                 ('left/camera_info', [LaunchConfiguration('left_namespace'), '/camera_info']),
@@ -94,6 +92,10 @@ def generate_launch_description():
                     'left/image_rect_color',
                     [LaunchConfiguration('left_namespace'), '/image_rect_color']
                 ),
+                (
+                    'right/image_rect_color',
+                    [LaunchConfiguration('right_namespace'), '/image_rect_color']
+                ),
             ]
         ),
     ]
@@ -115,10 +117,6 @@ def generate_launch_description():
             name='use_color', default_value='True',
             description='Generate point cloud with rgb data.'
         ),
-        DeclareLaunchArgument(
-            name='use_system_default_qos', default_value='False',
-            description='Use the RMW QoS settings for the image and camera info subscriptions.'
-        ),
         DeclareLaunchArgument(
             name='launch_image_proc', default_value='True',
             description='Whether to launch debayer and rectify nodes from image_proc.'

stereo_image_proc/src/stereo_image_proc/disparity_node.cpp

@@ -301,17 +301,6 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options)
         sub_r_image_.unsubscribe();
         sub_r_info_.unsubscribe();
       } else if (!sub_l_image_.getSubscriber()) {
-        // Optionally switch between system/sensor defaults
-        // TODO(fergs): remove and conform to REP-2003?
-        const bool use_system_default_qos =
-          this->get_parameter("use_system_default_qos").as_bool();
-        rclcpp::QoS image_sub_qos = rclcpp::SensorDataQoS();
-        if (use_system_default_qos) {
-          image_sub_qos = rclcpp::SystemDefaultsQoS();
-        }
-        const auto image_sub_rmw_qos = image_sub_qos.get_rmw_qos_profile();
-        auto sub_opts = rclcpp::SubscriptionOptions();
-
         // 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();
@@ -327,16 +316,22 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options)
           node_base->resolve_topic_or_service_name(
           image_transport::getCameraInfoTopic(right_topic), false);
 
-        // Setup hints and QoS overrides
+        // REP-2003 specifies that subscriber should be SensorDataQoS
+        const auto sensor_data_qos = rclcpp::SensorDataQoS().get_rmw_qos_profile();
+
+        // Support image transport for compression
         image_transport::TransportHints hints(this);
+
+        // Allow overriding QoS settings (history, depth, reliability)
+        auto sub_opts = rclcpp::SubscriptionOptions();
         sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
 
         sub_l_image_.subscribe(
-          this, left_topic, hints.getTransport(), image_sub_rmw_qos, sub_opts);
-        sub_l_info_.subscribe(this, left_info_topic, image_sub_rmw_qos, sub_opts);
+          this, left_topic, hints.getTransport(), sensor_data_qos, sub_opts);
+        sub_l_info_.subscribe(this, left_info_topic, sensor_data_qos, sub_opts);
         sub_r_image_.subscribe(
-          this, right_topic, hints.getTransport(), image_sub_rmw_qos, sub_opts);
-        sub_r_info_.subscribe(this, right_info_topic, image_sub_rmw_qos, sub_opts);
+          this, right_topic, hints.getTransport(), sensor_data_qos, sub_opts);
+        sub_r_info_.subscribe(this, right_info_topic, sensor_data_qos, sub_opts);
       }
     };
 

stereo_image_proc/src/stereo_image_proc/point_cloud_node.cpp

@@ -113,7 +113,6 @@ PointCloudNode::PointCloudNode(const rclcpp::NodeOptions & options)
   int queue_size = this->declare_parameter("queue_size", 5);
   bool approx = this->declare_parameter("approximate_sync", false);
   double approx_sync_epsilon = this->declare_parameter("approximate_sync_tolerance_seconds", 0.0);
-  this->declare_parameter("use_system_default_qos", false);
   rcl_interfaces::msg::ParameterDescriptor descriptor;
   // TODO(ivanpauno): Confirm if using point cloud padding in `sensor_msgs::msg::PointCloud2`
   // can improve performance in some cases or not.
@@ -166,16 +165,6 @@ PointCloudNode::PointCloudNode(const rclcpp::NodeOptions & options)
         sub_r_info_.unsubscribe();
         sub_disparity_.unsubscribe();
       } else if (!sub_l_image_.getSubscriber()) {
-        // Optionally switch between system/sensor defaults
-        // TODO(fergs): remove and conform to REP-2003?
-        const bool use_system_default_qos =
-          this->get_parameter("use_system_default_qos").as_bool();
-        rclcpp::QoS image_sub_qos = rclcpp::SensorDataQoS();
-        if (use_system_default_qos) {
-          image_sub_qos = rclcpp::SystemDefaultsQoS();
-        }
-        const auto image_sub_rmw_qos = image_sub_qos.get_rmw_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();
@@ -190,16 +179,21 @@ PointCloudNode::PointCloudNode(const rclcpp::NodeOptions & options)
           node_base->resolve_topic_or_service_name(
           image_transport::getCameraInfoTopic(left_topic), false);
 
-        // Setup hints and QoS overrides
+        // REP-2003 specifies that subscriber should be SensorDataQoS
+        const auto sensor_data_qos = rclcpp::SensorDataQoS().get_rmw_qos_profile();
+
+        // Support image transport for compression
         image_transport::TransportHints hints(this);
+
+        // Allow overriding QoS settings (history, depth, reliability)
         auto sub_opts = rclcpp::SubscriptionOptions();
         sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
 
         sub_l_image_.subscribe(
-          this, left_topic, hints.getTransport(), image_sub_rmw_qos, sub_opts);
-        sub_l_info_.subscribe(this, left_info_topic, image_sub_rmw_qos, sub_opts);
-        sub_r_info_.subscribe(this, right_topic, image_sub_rmw_qos, sub_opts);
-        sub_disparity_.subscribe(this, disparity_topic, image_sub_rmw_qos, sub_opts);
+          this, left_topic, hints.getTransport(), sensor_data_qos, sub_opts);
+        sub_l_info_.subscribe(this, left_info_topic, sensor_data_qos, sub_opts);
+        sub_r_info_.subscribe(this, right_topic, sensor_data_qos, sub_opts);
+        sub_disparity_.subscribe(this, disparity_topic, sensor_data_qos, sub_opts);
       }
     };
   pub_points2_ = create_publisher<sensor_msgs::msg::PointCloud2>("points2", 1, pub_opts);

stereo_image_proc/test/test_disparity_node.py

@@ -73,9 +73,20 @@ def generate_test_description():
             package='stereo_image_proc',
             executable='disparity_node',
             name='disparity_node',
-            parameters=[
-                {'use_system_default_qos': True}
-            ],
+            parameters=[{
+                'qos_overrides': {
+                    '/left/image_rect_color': {
+                        'subscription': {
+                            'reliability': 'reliable'
+                        }
+                    },
+                    '/right/image_rect_color': {
+                        'subscription': {
+                            'reliability': 'reliable'
+                        }
+                    }
+                },
+            }],
             output='screen'
         ),
         launch_testing.actions.ReadyToTest(),

stereo_image_proc/test/test_point_cloud_node.py

@@ -76,7 +76,13 @@ def generate_test_description():
             output='screen',
             parameters=[{
                 'use_color': True,
-                'use_system_default_qos': True
+                'qos_overrides': {
+                    '/left/image_rect_color': {
+                        'subscription': {
+                            'reliability': 'reliable'
+                        }
+                    }
+                },
             }],
         ),
         # PointCloudNode (color disabled)
@@ -87,7 +93,13 @@ def generate_test_description():
             output='screen',
             parameters=[{
                 'use_color': False,
-                'use_system_default_qos': True
+                'qos_overrides': {
+                    '/left/image_rect_color': {
+                        'subscription': {
+                            'reliability': 'reliable'
+                        }
+                    }
+                },
             }],
             remappings=[
                 ('/points2', '/xyz/points2'),