allow the rest of the image proc pipeline to support sensor data QOS …

…via a parameter

image_proc/include/image_proc/rectify.hpp

@@ -56,6 +56,7 @@ class RectifyNode
   image_transport::CameraSubscriber sub_camera_;
 
   int queue_size_;
+  bool use_qos_;
   int interpolation;
   std::mutex connect_mutex_;
   image_transport::Publisher pub_rect_;

image_proc/launch/image_proc.launch.py

@@ -46,6 +46,9 @@ def generate_launch_description():
             package='image_proc',
             plugin='image_proc::DebayerNode',
             name='debayer_node',
+            parameters=[{
+                'use_system_default_qos': LaunchConfiguration('use_system_default_qos'),
+            }],
         ),
         ComposableNode(
             package='image_proc',
@@ -57,6 +60,9 @@ def generate_launch_description():
                 ('camera_info', 'camera_info'),
                 ('image_rect', 'image_rect')
             ],
+            parameters=[{
+                'use_system_default_qos': LaunchConfiguration('use_system_default_qos'),
+            }],
         ),
         ComposableNode(
             package='image_proc',
@@ -67,6 +73,9 @@ def generate_launch_description():
                 ('image', 'image_color'),
                 ('image_rect', 'image_rect_color')
             ],
+            parameters=[{
+                'use_system_default_qos': LaunchConfiguration('use_system_default_qos'),
+            }],
         )
     ]
 
@@ -78,6 +87,14 @@ def generate_launch_description():
         )
     )
 
+    arg_use_system_default_qos = DeclareLaunchArgument(
+        name='use_system_default_qos', default_value='False',
+        description=(
+            'if the input image, and output image topics should use system '
+            'default QOS or the sensor data QOS policy'
+        )
+    )
+
     # If an existing container is not provided, start a container and load nodes into it
     image_processing_container = ComposableNodeContainer(
         condition=LaunchConfigurationEquals('container', ''),
@@ -99,6 +116,7 @@ def generate_launch_description():
 
     return LaunchDescription([
         arg_container,
+        arg_use_system_default_qos,
         image_processing_container,
         load_composable_nodes,
     ])

image_proc/src/debayer.cpp

@@ -52,11 +52,13 @@ namespace enc = sensor_msgs::image_encodings;
 DebayerNode::DebayerNode(const rclcpp::NodeOptions & options)
 : Node("DebayerNode", options)
 {
+  bool use_qos_ = this->declare_parameter("use_system_default_qos", false);
   sub_raw_ = image_transport::create_subscription(
     this, "image_raw",
     std::bind(
       &DebayerNode::imageCb, this,
-      std::placeholders::_1), "raw");
+      std::placeholders::_1), "raw",
+    (use_qos_ ? rmw_qos_profile_default : rmw_qos_profile_sensor_data));
 
   pub_mono_ = image_transport::create_publisher(this, "image_mono");
   pub_color_ = image_transport::create_publisher(this, "image_color");

image_proc/src/rectify.cpp

@@ -47,9 +47,12 @@ namespace image_proc
 RectifyNode::RectifyNode(const rclcpp::NodeOptions & options)
 : Node("RectifyNode", options)
 {
+  use_qos_ = this->declare_parameter("use_system_default_qos", false);
   queue_size_ = this->declare_parameter("queue_size", 5);
   interpolation = this->declare_parameter("interpolation", 1);
-  pub_rect_ = image_transport::create_publisher(this, "image_rect");
+  pub_rect_ = image_transport::create_publisher(
+    this, "image_rect",
+    (use_qos_ ? rmw_qos_profile_default : rmw_qos_profile_sensor_data));
   subscribeToCamera();
 }
 
@@ -69,7 +72,8 @@ void RectifyNode::subscribeToCamera()
   sub_camera_ = image_transport::create_camera_subscription(
     this, "image", std::bind(
       &RectifyNode::imageCb,
-      this, std::placeholders::_1, std::placeholders::_2), "raw");
+      this, std::placeholders::_1, std::placeholders::_2), "raw",
+    (use_qos_ ? rmw_qos_profile_default : rmw_qos_profile_sensor_data));
   // }
 }
 

stereo_image_proc/launch/stereo_image_proc.launch.py

@@ -89,10 +89,10 @@ def generate_launch_description():
             remappings=[
                 ('left/camera_info', [LaunchConfiguration('left_namespace'), '/camera_info']),
                 ('right/camera_info', [LaunchConfiguration('right_namespace'), '/camera_info']),
-                (
-                    'left/image_rect_color',
-                    [LaunchConfiguration('left_namespace'), '/image_rect_color']
-                ),
+                ('left/image_rect_color',
+                    [LaunchConfiguration('left_namespace'), '/image_rect_color']),
+                ('right/image_rect_color',
+                    [LaunchConfiguration('right_namespace'), '/image_rect_color']),
             ]
         ),
     ]
@@ -224,7 +224,10 @@ def generate_launch_description():
                     PythonLaunchDescriptionSource([
                         FindPackageShare('image_proc'), '/launch/image_proc.launch.py'
                     ]),
-                    launch_arguments={'container': LaunchConfiguration('container')}.items()
+                    launch_arguments={
+                        'container': LaunchConfiguration('container'),
+                        'use_system_default_qos': LaunchConfiguration('use_system_default_qos')
+                    }.items()
                 ),
             ],
             condition=IfCondition(LaunchConfiguration('launch_image_proc')),
@@ -236,7 +239,10 @@ def generate_launch_description():
                     PythonLaunchDescriptionSource([
                         FindPackageShare('image_proc'), '/launch/image_proc.launch.py'
                     ]),
-                    launch_arguments={'container': LaunchConfiguration('container')}.items()
+                    launch_arguments={
+                        'container': LaunchConfiguration('container'),
+                        'use_system_default_qos': LaunchConfiguration('use_system_default_qos')
+                    }.items()
                 ),
             ],
             condition=IfCondition(LaunchConfiguration('launch_image_proc')),

stereo_image_proc/src/stereo_image_proc/disparity_node.cpp

@@ -265,7 +265,12 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options)
   this->declare_parameters("", double_params);
   this->declare_parameters("", bool_params);
 
-  pub_disparity_ = create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 1);
+  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();
+  }
+  pub_disparity_ = create_publisher<stereo_msgs::msg::DisparityImage>("disparity", image_sub_qos);
 
   // TODO(jacobperron): Replace this with a graph event.
   //                    Only subscribe if there's a subscription listening to our publisher.