diff --git a/depth_image_proc/src/disparity.cpp b/depth_image_proc/src/disparity.cpp index ce8a06f4a..9e1c18329 100644 --- a/depth_image_proc/src/disparity.cpp +++ b/depth_image_proc/src/disparity.cpp @@ -116,7 +116,7 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options) std::string topic = node_base->resolve_topic_or_service_name("left/image_rect", false); image_transport::TransportHints hints(this); sub_depth_image_.subscribe(this, topic, hints.getTransport()); - sub_info_.subscribe(this, "right/camera_info"); + sub_info_.subscribe(this, "right/camera_info", rclcpp::QoS(10)); } }; pub_disparity_ = create_publisher( diff --git a/depth_image_proc/src/point_cloud_xyzi.cpp b/depth_image_proc/src/point_cloud_xyzi.cpp index 0abc0ff8c..8a340f524 100644 --- a/depth_image_proc/src/point_cloud_xyzi.cpp +++ b/depth_image_proc/src/point_cloud_xyzi.cpp @@ -108,7 +108,7 @@ PointCloudXyziNode::PointCloudXyziNode(const rclcpp::NodeOptions & options) // intensity uses normal ros transport hints. image_transport::TransportHints hints(this, "raw"); sub_intensity_.subscribe(this, intensity_topic, hints.getTransport()); - sub_info_.subscribe(this, intensity_info_topic); + sub_info_.subscribe(this, intensity_info_topic, rclcpp::QoS(10)); } }; pub_point_cloud_ = create_publisher("points", rclcpp::SensorDataQoS(), pub_options); diff --git a/depth_image_proc/src/point_cloud_xyzi_radial.cpp b/depth_image_proc/src/point_cloud_xyzi_radial.cpp index c592851e0..d14afbe89 100644 --- a/depth_image_proc/src/point_cloud_xyzi_radial.cpp +++ b/depth_image_proc/src/point_cloud_xyzi_radial.cpp @@ -103,7 +103,7 @@ PointCloudXyziRadialNode::PointCloudXyziRadialNode(const rclcpp::NodeOptions & o // intensity uses normal ros transport hints. image_transport::TransportHints hints(this); sub_intensity_.subscribe(this, intensity_topic, hints.getTransport()); - sub_info_.subscribe(this, intensity_info_topic); + sub_info_.subscribe(this, intensity_info_topic, rclcpp::QoS(10)); } }; pub_point_cloud_ = create_publisher( diff --git a/depth_image_proc/src/point_cloud_xyzrgb.cpp b/depth_image_proc/src/point_cloud_xyzrgb.cpp index 5ef736d96..d48f6190a 100644 --- a/depth_image_proc/src/point_cloud_xyzrgb.cpp +++ b/depth_image_proc/src/point_cloud_xyzrgb.cpp @@ -133,8 +133,9 @@ PointCloudXyzrgbNode::PointCloudXyzrgbNode(const rclcpp::NodeOptions & options) image_transport::TransportHints hints(this); sub_rgb_.subscribe( this, rgb_topic, - hints.getTransport(), rmw_qos_profile_default, sub_opts); - sub_info_.subscribe(this, rgb_info_topic); + hints.getTransport(), + rmw_qos_profile_default, sub_opts); + sub_info_.subscribe(this, rgb_info_topic, rclcpp::QoS(10)); } }; pub_point_cloud_ = create_publisher("points", rclcpp::SensorDataQoS(), pub_options); diff --git a/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp b/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp index 7d2c92477..d5d7f5f0b 100644 --- a/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp +++ b/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp @@ -118,7 +118,7 @@ PointCloudXyzrgbRadialNode::PointCloudXyzrgbRadialNode(const rclcpp::NodeOptions // rgb uses normal ros transport hints. image_transport::TransportHints hints(this, "raw"); sub_rgb_.subscribe(this, rgb_topic, hints.getTransport()); - sub_info_.subscribe(this, rgb_info_topic); + sub_info_.subscribe(this, rgb_info_topic, rclcpp::QoS(10)); } }; pub_point_cloud_ = create_publisher("points", rclcpp::SensorDataQoS(), pub_options); diff --git a/depth_image_proc/src/register.cpp b/depth_image_proc/src/register.cpp index 853671967..b64e4f8b1 100644 --- a/depth_image_proc/src/register.cpp +++ b/depth_image_proc/src/register.cpp @@ -139,8 +139,8 @@ RegisterNode::RegisterNode(const rclcpp::NodeOptions & options) 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"); - sub_rgb_info_.subscribe(this, "rgb/camera_info"); + sub_depth_info_.subscribe(this, "depth/camera_info", rclcpp::QoS(10)); + sub_rgb_info_.subscribe(this, "rgb/camera_info", rclcpp::QoS(10)); } }; // For compressed topics to remap appropriately, we need to pass a diff --git a/image_view/src/stereo_view_node.cpp b/image_view/src/stereo_view_node.cpp index 3cd75faca..ec5fd39ab 100644 --- a/image_view/src/stereo_view_node.cpp +++ b/image_view/src/stereo_view_node.cpp @@ -130,7 +130,7 @@ StereoViewNode::StereoViewNode(const rclcpp::NodeOptions & options) // Subscribe to three input topics. left_sub_.subscribe(this, left_topic, hints.getTransport()); right_sub_.subscribe(this, right_topic, hints.getTransport()); - disparity_sub_.subscribe(this, disparity_topic); + disparity_sub_.subscribe(this, disparity_topic, rclcpp::QoS(10)); RCLCPP_INFO( this->get_logger(), 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 188e3ec22..0a9f41945 100644 --- a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp +++ b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp @@ -315,7 +315,7 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options) image_transport::getCameraInfoTopic(right_topic), false); // REP-2003 specifies that subscriber should be SensorDataQoS - const auto sensor_data_qos = rclcpp::SensorDataQoS().get_rmw_qos_profile(); + const auto sensor_data_qos = rclcpp::SensorDataQoS(); // Support image transport for compression image_transport::TransportHints hints(this); @@ -325,11 +325,13 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options) sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); sub_l_image_.subscribe( - this, left_topic, hints.getTransport(), sensor_data_qos, sub_opts); - sub_l_info_.subscribe(this, left_info_topic, sensor_data_qos, sub_opts); + this, left_topic, hints.getTransport(), sensor_data_qos.get_rmw_qos_profile(), sub_opts); + sub_l_info_.subscribe(this, left_info_topic, + sensor_data_qos, sub_opts); sub_r_image_.subscribe( - this, right_topic, hints.getTransport(), sensor_data_qos, sub_opts); - sub_r_info_.subscribe(this, right_info_topic, sensor_data_qos, sub_opts); + this, right_topic, hints.getTransport(), sensor_data_qos.get_rmw_qos_profile(), 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 14b1a047e..36c3757a4 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 @@ -180,7 +180,7 @@ PointCloudNode::PointCloudNode(const rclcpp::NodeOptions & options) image_transport::getCameraInfoTopic(left_topic), false); // REP-2003 specifies that subscriber should be SensorDataQoS - const auto sensor_data_qos = rclcpp::SensorDataQoS().get_rmw_qos_profile(); + const auto sensor_data_qos = rclcpp::SensorDataQoS(); // Support image transport for compression image_transport::TransportHints hints(this); @@ -190,10 +190,13 @@ PointCloudNode::PointCloudNode(const rclcpp::NodeOptions & options) sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); sub_l_image_.subscribe( - 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); + this, left_topic, hints.getTransport(), sensor_data_qos.get_rmw_qos_profile(), 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);