From ceecd10ee67014bc613352ef1ed49e247b74f9c9 Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Wed, 14 Apr 2021 15:37:49 -0700 Subject: [PATCH 1/6] Allow QoS overrides for publishers Signed-off-by: Audrow Nash --- .../src/stereo_image_proc/disparity_node.cpp | 16 +++++++++++++++- .../src/stereo_image_proc/point_cloud_node.cpp | 16 +++++++++++++++- 2 files changed, 30 insertions(+), 2 deletions(-) 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..d2b31f2e2 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,21 @@ 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 {{ + rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, + rclcpp::QosPolicyKind::Deadline, + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Lifespan, + rclcpp::QosPolicyKind::Liveliness, + rclcpp::QosPolicyKind::LivelinessLeaseDuration, + rclcpp::QosPolicyKind::Reliability, + rclcpp::QosPolicyKind::Invalid, + }}; + 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. 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..1c99f4357 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,21 @@ 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 {{ + rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, + rclcpp::QosPolicyKind::Deadline, + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Lifespan, + rclcpp::QosPolicyKind::Liveliness, + rclcpp::QosPolicyKind::LivelinessLeaseDuration, + rclcpp::QosPolicyKind::Reliability, + rclcpp::QosPolicyKind::Invalid, + }}; + 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. From ced5896e65158c2299e429ad53b18b8a070d8f0d Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Thu, 15 Apr 2021 10:32:24 -0700 Subject: [PATCH 2/6] Remove QosPolicyKind::Invalid Signed-off-by: Audrow Nash --- stereo_image_proc/src/stereo_image_proc/disparity_node.cpp | 1 - stereo_image_proc/src/stereo_image_proc/point_cloud_node.cpp | 1 - 2 files changed, 2 deletions(-) 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 d2b31f2e2..a1f19ecab 100644 --- a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp +++ b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp @@ -277,7 +277,6 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options) rclcpp::QosPolicyKind::Liveliness, rclcpp::QosPolicyKind::LivelinessLeaseDuration, rclcpp::QosPolicyKind::Reliability, - rclcpp::QosPolicyKind::Invalid, }}; pub_disparity_ = create_publisher("disparity", 1, pub_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 1c99f4357..693d81504 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 @@ -142,7 +142,6 @@ PointCloudNode::PointCloudNode(const rclcpp::NodeOptions & options) rclcpp::QosPolicyKind::Liveliness, rclcpp::QosPolicyKind::LivelinessLeaseDuration, rclcpp::QosPolicyKind::Reliability, - rclcpp::QosPolicyKind::Invalid, }}; pub_points2_ = create_publisher("points2", 1, pub_opts); From ebedb89a2698791ebf007a70c202df17f90d0cc2 Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Wed, 28 Apr 2021 15:31:16 -0700 Subject: [PATCH 3/6] Add subscriber qos overrides Signed-off-by: Audrow Nash --- .../src/stereo_image_proc/disparity_node.cpp | 20 +++++++++++++++---- .../stereo_image_proc/point_cloud_node.cpp | 20 +++++++++++++++---- 2 files changed, 32 insertions(+), 8 deletions(-) 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 a1f19ecab..76bd7d007 100644 --- a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp +++ b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp @@ -296,10 +296,22 @@ 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::SubscriptionOptionsWithAllocator>(); + sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions {{ + rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, + rclcpp::QosPolicyKind::Deadline, + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Lifespan, + rclcpp::QosPolicyKind::Liveliness, + rclcpp::QosPolicyKind::LivelinessLeaseDuration, + rclcpp::QosPolicyKind::Reliability, + }}; + 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 693d81504..058ce7f48 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 @@ -161,10 +161,22 @@ 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::SubscriptionOptionsWithAllocator>(); + sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions {{ + rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, + rclcpp::QosPolicyKind::Deadline, + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Lifespan, + rclcpp::QosPolicyKind::Liveliness, + rclcpp::QosPolicyKind::LivelinessLeaseDuration, + rclcpp::QosPolicyKind::Reliability, + }}; + 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) From ce39e2566f224aabf57921ee8562a84786d5f747 Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Tue, 18 May 2021 14:57:50 -0700 Subject: [PATCH 4/6] Use SubscriptionOptions Signed-off-by: Audrow Nash --- .../src/stereo_image_proc/disparity_node.cpp | 10 +++++++--- .../src/stereo_image_proc/point_cloud_node.cpp | 6 ++++-- 2 files changed, 11 insertions(+), 5 deletions(-) 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 76bd7d007..9d61f2be3 100644 --- a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp +++ b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp @@ -296,7 +296,7 @@ void DisparityNode::connectCb() image_sub_qos = rclcpp::SystemDefaultsQoS(); } const auto image_sub_rmw_qos = image_sub_qos.get_rmw_qos_profile(); - auto sub_opts = rclcpp::SubscriptionOptionsWithAllocator>(); + auto sub_opts = rclcpp::SubscriptionOptions(); sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions {{ rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, rclcpp::QosPolicyKind::Deadline, @@ -308,9 +308,13 @@ void DisparityNode::connectCb() rclcpp::QosPolicyKind::LivelinessLeaseDuration, rclcpp::QosPolicyKind::Reliability, }}; - sub_l_image_.subscribe(this, "left/image_rect", hints.getTransport(), image_sub_rmw_qos, sub_opts); + 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_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); } 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 058ce7f48..5b1c5eb86 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 @@ -161,7 +161,7 @@ void PointCloudNode::connectCb() image_sub_qos = rclcpp::SystemDefaultsQoS(); } const auto image_sub_rmw_qos = image_sub_qos.get_rmw_qos_profile(); - auto sub_opts = rclcpp::SubscriptionOptionsWithAllocator>(); + auto sub_opts = rclcpp::SubscriptionOptions(); sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions {{ rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, rclcpp::QosPolicyKind::Deadline, @@ -173,7 +173,9 @@ void PointCloudNode::connectCb() rclcpp::QosPolicyKind::LivelinessLeaseDuration, rclcpp::QosPolicyKind::Reliability, }}; - sub_l_image_.subscribe(this, "left/image_rect_color", hints.getTransport(), image_sub_rmw_qos, sub_opts); + 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); From 2aebe8568def2fcd9e376e43c62a10bd02c9ce5c Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Wed, 19 May 2021 11:09:46 -0700 Subject: [PATCH 5/6] Improve formatting Signed-off-by: Audrow Nash --- stereo_image_proc/src/stereo_image_proc/disparity_node.cpp | 6 ++---- .../src/stereo_image_proc/point_cloud_node.cpp | 3 +-- 2 files changed, 3 insertions(+), 6 deletions(-) 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 9d61f2be3..c701c0a56 100644 --- a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp +++ b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp @@ -309,12 +309,10 @@ void DisparityNode::connectCb() rclcpp::QosPolicyKind::Reliability, }}; sub_l_image_.subscribe( - this, "left/image_rect", hints.getTransport(), image_sub_rmw_qos, - sub_opts); + 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); + 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); } 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 5b1c5eb86..a1d11b290 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 @@ -174,8 +174,7 @@ void PointCloudNode::connectCb() rclcpp::QosPolicyKind::Reliability, }}; sub_l_image_.subscribe( - this, "left/image_rect_color", - hints.getTransport(), image_sub_rmw_qos, sub_opts); + 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); From 67848feca6a7b0eeabe8338f56c9c96859aa5d44 Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Wed, 19 May 2021 15:31:08 -0700 Subject: [PATCH 6/6] Use with_default_policies Signed-off-by: Audrow Nash --- .../src/stereo_image_proc/disparity_node.cpp | 24 ++----------------- .../stereo_image_proc/point_cloud_node.cpp | 24 ++----------------- 2 files changed, 4 insertions(+), 44 deletions(-) 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 c701c0a56..71cb5fe32 100644 --- a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp +++ b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp @@ -267,17 +267,7 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options) // Update the publisher options to allow reconfigurable qos settings. rclcpp::PublisherOptions pub_opts; - pub_opts.qos_overriding_options = rclcpp::QosOverridingOptions {{ - rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, - rclcpp::QosPolicyKind::Deadline, - rclcpp::QosPolicyKind::Depth, - rclcpp::QosPolicyKind::Durability, - rclcpp::QosPolicyKind::History, - rclcpp::QosPolicyKind::Lifespan, - rclcpp::QosPolicyKind::Liveliness, - rclcpp::QosPolicyKind::LivelinessLeaseDuration, - rclcpp::QosPolicyKind::Reliability, - }}; + 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. @@ -297,17 +287,7 @@ void DisparityNode::connectCb() } const auto image_sub_rmw_qos = image_sub_qos.get_rmw_qos_profile(); auto sub_opts = rclcpp::SubscriptionOptions(); - sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions {{ - rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, - rclcpp::QosPolicyKind::Deadline, - rclcpp::QosPolicyKind::Depth, - rclcpp::QosPolicyKind::Durability, - rclcpp::QosPolicyKind::History, - rclcpp::QosPolicyKind::Lifespan, - rclcpp::QosPolicyKind::Liveliness, - rclcpp::QosPolicyKind::LivelinessLeaseDuration, - rclcpp::QosPolicyKind::Reliability, - }}; + 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); 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 a1d11b290..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 @@ -132,17 +132,7 @@ PointCloudNode::PointCloudNode(const rclcpp::NodeOptions & options) // Update the publisher options to allow reconfigurable qos settings. rclcpp::PublisherOptions pub_opts; - pub_opts.qos_overriding_options = rclcpp::QosOverridingOptions {{ - rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, - rclcpp::QosPolicyKind::Deadline, - rclcpp::QosPolicyKind::Depth, - rclcpp::QosPolicyKind::Durability, - rclcpp::QosPolicyKind::History, - rclcpp::QosPolicyKind::Lifespan, - rclcpp::QosPolicyKind::Liveliness, - rclcpp::QosPolicyKind::LivelinessLeaseDuration, - rclcpp::QosPolicyKind::Reliability, - }}; + 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. @@ -162,17 +152,7 @@ void PointCloudNode::connectCb() } const auto image_sub_rmw_qos = image_sub_qos.get_rmw_qos_profile(); auto sub_opts = rclcpp::SubscriptionOptions(); - sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions {{ - rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, - rclcpp::QosPolicyKind::Deadline, - rclcpp::QosPolicyKind::Depth, - rclcpp::QosPolicyKind::Durability, - rclcpp::QosPolicyKind::History, - rclcpp::QosPolicyKind::Lifespan, - rclcpp::QosPolicyKind::Liveliness, - rclcpp::QosPolicyKind::LivelinessLeaseDuration, - rclcpp::QosPolicyKind::Reliability, - }}; + 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);