From 8076d4e930bbe739f3bc745873f7b4309d73c430 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 17 Jan 2024 10:43:05 +0100 Subject: [PATCH] Depth image transport configure susbcribers (#844) (#845) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- depth_image_proc/src/point_cloud_xyzrgb.cpp | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/depth_image_proc/src/point_cloud_xyzrgb.cpp b/depth_image_proc/src/point_cloud_xyzrgb.cpp index c77a1e680..8bc94ae45 100644 --- a/depth_image_proc/src/point_cloud_xyzrgb.cpp +++ b/depth_image_proc/src/point_cloud_xyzrgb.cpp @@ -109,12 +109,27 @@ void PointCloudXyzrgbNode::connectCb() std::string depth_image_transport_param = "depth_image_transport"; image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param); + rclcpp::SubscriptionOptions sub_opts; + // Update the subscription options to allow reconfigurable qos settings. + sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions { + { + // Here all policies that are desired to be reconfigurable are listed. + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Reliability, + }}; + // depth image can use different transport.(e.g. compressedDepth) - sub_depth_.subscribe(this, "depth_registered/image_rect", depth_hints.getTransport()); + sub_depth_.subscribe( + this, "depth_registered/image_rect", + depth_hints.getTransport(), rmw_qos_profile_default, sub_opts); // rgb uses normal ros transport hints. image_transport::TransportHints hints(this, "raw"); - sub_rgb_.subscribe(this, "rgb/image_rect_color", hints.getTransport()); + sub_rgb_.subscribe( + this, "rgb/image_rect_color", + hints.getTransport(), rmw_qos_profile_default, sub_opts); sub_info_.subscribe(this, "rgb/camera_info"); } }