From 3aafdd4bb3ff1b1c1dcf9bf8ed8e5b5f1dac8a32 Mon Sep 17 00:00:00 2001 From: Stuart Alldritt Date: Tue, 19 Nov 2024 20:26:44 -0800 Subject: [PATCH 1/2] Support QoS override parameters in depth_image_proc/register --- depth_image_proc/src/register.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/depth_image_proc/src/register.cpp b/depth_image_proc/src/register.cpp index 7db28843..437257ce 100644 --- a/depth_image_proc/src/register.cpp +++ b/depth_image_proc/src/register.cpp @@ -133,14 +133,18 @@ RegisterNode::RegisterNode(const rclcpp::NodeOptions & options) sub_depth_info_.unsubscribe(); sub_rgb_info_.unsubscribe(); } else if (!sub_depth_image_.getSubscriber()) { + // Allow overriding QoS settings (history, depth, reliability) + rclcpp::SubscriptionOptions sub_options; + sub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + // 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("depth/image_rect", false); image_transport::TransportHints hints(this, "raw", "depth_image_transport"); - sub_depth_image_.subscribe(this, topic, hints.getTransport()); - sub_depth_info_.subscribe(this, "depth/camera_info", rclcpp::QoS(10)); - sub_rgb_info_.subscribe(this, "rgb/camera_info", rclcpp::QoS(10)); + sub_depth_image_.subscribe(this, topic, hints.getTransport(), rmw_qos_profile_default, sub_options); + sub_depth_info_.subscribe(this, "depth/camera_info", rclcpp::QoS(10), sub_options); + sub_rgb_info_.subscribe(this, "rgb/camera_info", rclcpp::QoS(10), sub_options); } }; // For compressed topics to remap appropriately, we need to pass a From 00ba6e91a02f1c8b4c04497328d49f8d909ffc36 Mon Sep 17 00:00:00 2001 From: Stuart Alldritt Date: Fri, 22 Nov 2024 16:51:34 -0800 Subject: [PATCH 2/2] Fix lint issue in register.cpp --- depth_image_proc/src/register.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/depth_image_proc/src/register.cpp b/depth_image_proc/src/register.cpp index 437257ce..690d28f0 100644 --- a/depth_image_proc/src/register.cpp +++ b/depth_image_proc/src/register.cpp @@ -142,7 +142,8 @@ RegisterNode::RegisterNode(const rclcpp::NodeOptions & options) auto node_base = this->get_node_base_interface(); std::string topic = node_base->resolve_topic_or_service_name("depth/image_rect", false); image_transport::TransportHints hints(this, "raw", "depth_image_transport"); - sub_depth_image_.subscribe(this, topic, hints.getTransport(), rmw_qos_profile_default, sub_options); + sub_depth_image_.subscribe(this, topic, hints.getTransport(), rmw_qos_profile_default, + sub_options); sub_depth_info_.subscribe(this, "depth/camera_info", rclcpp::QoS(10), sub_options); sub_rgb_info_.subscribe(this, "rgb/camera_info", rclcpp::QoS(10), sub_options); }