diff --git a/image_proc/src/crop_decimate.cpp b/image_proc/src/crop_decimate.cpp index 084b98d4f..d439bb058 100644 --- a/image_proc/src/crop_decimate.cpp +++ b/image_proc/src/crop_decimate.cpp @@ -133,7 +133,7 @@ CropDecimateNode::CropDecimateNode(const rclcpp::NodeOptions & options) int interpolation = this->declare_parameter("interpolation", 0); interpolation_ = static_cast(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 &) @@ -141,6 +141,7 @@ CropDecimateNode::CropDecimateNode(const rclcpp::NodeOptions & options) 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); } diff --git a/image_proc/src/crop_non_zero.cpp b/image_proc/src/crop_non_zero.cpp index 3add8add0..488300e6d 100644 --- a/image_proc/src/crop_non_zero.cpp +++ b/image_proc/src/crop_non_zero.cpp @@ -61,7 +61,7 @@ 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 &) @@ -69,6 +69,7 @@ CropNonZeroNode::CropNonZeroNode(const rclcpp::NodeOptions & options) 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); } diff --git a/image_proc/src/debayer.cpp b/image_proc/src/debayer.cpp index 1c66d1da0..7908a8c2c 100644 --- a/image_proc/src/debayer.cpp +++ b/image_proc/src/debayer.cpp @@ -61,7 +61,7 @@ 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 &) @@ -69,6 +69,7 @@ DebayerNode::DebayerNode(const rclcpp::NodeOptions & options) 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); diff --git a/image_proc/src/rectify.cpp b/image_proc/src/rectify.cpp index 405d6b2fd..24d37768c 100644 --- a/image_proc/src/rectify.cpp +++ b/image_proc/src/rectify.cpp @@ -63,7 +63,7 @@ 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 &) @@ -71,6 +71,7 @@ RectifyNode::RectifyNode(const rclcpp::NodeOptions & options) 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); } diff --git a/image_proc/src/resize.cpp b/image_proc/src/resize.cpp index d0122d26b..a9b1d37ff 100644 --- a/image_proc/src/resize.cpp +++ b/image_proc/src/resize.cpp @@ -68,7 +68,7 @@ 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 &) @@ -76,7 +76,7 @@ ResizeNode::ResizeNode(const rclcpp::NodeOptions & options) 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); diff --git a/stereo_image_proc/launch/stereo_image_proc.launch.py b/stereo_image_proc/launch/stereo_image_proc.launch.py index 4825b3568..afe9c0cb2 100644 --- a/stereo_image_proc/launch/stereo_image_proc.launch.py +++ b/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.' 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 ed7384017..97a32f7ad 100644 --- a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp +++ b/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); } }; diff --git a/stereo_image_proc/src/stereo_image_proc/point_cloud_node.cpp b/stereo_image_proc/src/stereo_image_proc/point_cloud_node.cpp index 1d7d0f232..c654f231c 100644 --- a/stereo_image_proc/src/stereo_image_proc/point_cloud_node.cpp +++ b/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("points2", 1, pub_opts); diff --git a/stereo_image_proc/test/test_disparity_node.py b/stereo_image_proc/test/test_disparity_node.py index 35cd951a4..5fc46f434 100644 --- a/stereo_image_proc/test/test_disparity_node.py +++ b/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(), diff --git a/stereo_image_proc/test/test_point_cloud_node.py b/stereo_image_proc/test/test_point_cloud_node.py index 91810b73d..e09caa20b 100644 --- a/stereo_image_proc/test/test_point_cloud_node.py +++ b/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'),