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..71cb5fe32 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,10 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options) this->declare_parameters("", double_params); this->declare_parameters("", bool_params); - pub_disparity_ = create_publisher("disparity", 1); + // Update the publisher options to allow reconfigurable qos settings. + rclcpp::PublisherOptions pub_opts; + pub_opts.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + pub_disparity_ = create_publisher("disparity", 1, pub_opts); // TODO(jacobperron): Replace this with a graph event. // Only subscribe if there's a subscription listening to our publisher. @@ -283,10 +286,14 @@ void DisparityNode::connectCb() image_sub_qos = rclcpp::SystemDefaultsQoS(); } const auto image_sub_rmw_qos = image_sub_qos.get_rmw_qos_profile(); - sub_l_image_.subscribe(this, "left/image_rect", hints.getTransport(), image_sub_rmw_qos); - sub_l_info_.subscribe(this, "left/camera_info", image_sub_rmw_qos); - sub_r_image_.subscribe(this, "right/image_rect", hints.getTransport(), image_sub_rmw_qos); - sub_r_info_.subscribe(this, "right/camera_info", image_sub_rmw_qos); + auto sub_opts = rclcpp::SubscriptionOptions(); + sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + sub_l_image_.subscribe( + this, "left/image_rect", hints.getTransport(), image_sub_rmw_qos, sub_opts); + sub_l_info_.subscribe(this, "left/camera_info", image_sub_rmw_qos, sub_opts); + sub_r_image_.subscribe( + this, "right/image_rect", hints.getTransport(), image_sub_rmw_qos, sub_opts); + sub_r_info_.subscribe(this, "right/camera_info", image_sub_rmw_qos, sub_opts); } void DisparityNode::imageCb( 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 09612c0a9..326438730 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 @@ -130,7 +130,10 @@ PointCloudNode::PointCloudNode(const rclcpp::NodeOptions & options) std::bind(&PointCloudNode::imageCb, this, _1, _2, _3, _4)); } - pub_points2_ = create_publisher("points2", 1); + // Update the publisher options to allow reconfigurable qos settings. + rclcpp::PublisherOptions pub_opts; + pub_opts.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + pub_points2_ = create_publisher("points2", 1, pub_opts); // TODO(jacobperron): Replace this with a graph event. // Only subscribe if there's a subscription listening to our publisher. @@ -148,10 +151,13 @@ void PointCloudNode::connectCb() image_sub_qos = rclcpp::SystemDefaultsQoS(); } const auto image_sub_rmw_qos = image_sub_qos.get_rmw_qos_profile(); - sub_l_image_.subscribe(this, "left/image_rect_color", hints.getTransport(), image_sub_rmw_qos); - sub_l_info_.subscribe(this, "left/camera_info", image_sub_rmw_qos); - sub_r_info_.subscribe(this, "right/camera_info", image_sub_rmw_qos); - sub_disparity_.subscribe(this, "disparity", image_sub_rmw_qos); + auto sub_opts = rclcpp::SubscriptionOptions(); + sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + sub_l_image_.subscribe( + this, "left/image_rect_color", hints.getTransport(), image_sub_rmw_qos, sub_opts); + sub_l_info_.subscribe(this, "left/camera_info", image_sub_rmw_qos, sub_opts); + sub_r_info_.subscribe(this, "right/camera_info", image_sub_rmw_qos, sub_opts); + sub_disparity_.subscribe(this, "disparity", image_sub_rmw_qos, sub_opts); } inline bool isValidPoint(const cv::Vec3f & pt)