From 8d255ffbec035661240127e29c018f81f9a52d99 Mon Sep 17 00:00:00 2001 From: Cole Tucker Date: Tue, 4 Jan 2022 16:31:31 -0500 Subject: [PATCH] allow the rest of the image proc pipeline to support sensor data QOS via a parameter --- image_proc/include/image_proc/rectify.hpp | 1 + image_proc/launch/image_proc.launch.py | 18 ++++++++++++++++++ image_proc/src/debayer.cpp | 4 +++- image_proc/src/rectify.cpp | 8 ++++++-- .../launch/stereo_image_proc.launch.py | 18 ++++++++++++------ .../src/stereo_image_proc/disparity_node.cpp | 7 ++++++- 6 files changed, 46 insertions(+), 10 deletions(-) diff --git a/image_proc/include/image_proc/rectify.hpp b/image_proc/include/image_proc/rectify.hpp index 15a2d6b92..9ca3f70c6 100644 --- a/image_proc/include/image_proc/rectify.hpp +++ b/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_; diff --git a/image_proc/launch/image_proc.launch.py b/image_proc/launch/image_proc.launch.py index 91af35f7f..181be23b0 100644 --- a/image_proc/launch/image_proc.launch.py +++ b/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, ]) diff --git a/image_proc/src/debayer.cpp b/image_proc/src/debayer.cpp index 4f008a675..d4b4767ba 100644 --- a/image_proc/src/debayer.cpp +++ b/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"); diff --git a/image_proc/src/rectify.cpp b/image_proc/src/rectify.cpp index 4b396fe03..48943b4b9 100644 --- a/image_proc/src/rectify.cpp +++ b/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)); // } } diff --git a/stereo_image_proc/launch/stereo_image_proc.launch.py b/stereo_image_proc/launch/stereo_image_proc.launch.py index 73fcb0161..e1102e21d 100644 --- a/stereo_image_proc/launch/stereo_image_proc.launch.py +++ b/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')), diff --git a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp index 18277349a..b57a54f3b 100644 --- a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp +++ b/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("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("disparity", image_sub_qos); // TODO(jacobperron): Replace this with a graph event. // Only subscribe if there's a subscription listening to our publisher.