From f1e4a9efab65bff28b6c91ab0f08a1f82c93d0d1 Mon Sep 17 00:00:00 2001 From: ralwing <58466562+ralwing@users.noreply.github.com> Date: Wed, 3 Jul 2024 08:46:48 +0200 Subject: [PATCH] feat(pointcloud_preprocessor): runtime configurable output topic qos (#6658) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat(pointcloud_preprocessor): runtime configurable output topic qos Signed-off-by: Grzegorz Głowacki * configurable qos in livox_tag_filter_node Signed-off-by: Grzegorz Głowacki * configurable qos in radar_scan_to_pointcloud2 Signed-off-by: Grzegorz Głowacki --------- Signed-off-by: Grzegorz Głowacki --- .../src/livox_tag_filter_node.cpp | 10 +++++++--- .../concatenate_and_time_sync_nodelet.cpp | 9 +++++++-- .../src/concatenate_data/concatenate_pointclouds.cpp | 4 +++- .../src/crop_box_filter/crop_box_filter_nodelet.cpp | 6 ++++-- .../distortion_corrector/distortion_corrector.cpp | 11 +++++++---- sensing/pointcloud_preprocessor/src/filter.cpp | 4 +++- .../dual_return_outlier_filter_nodelet.cpp | 9 ++++++--- .../outlier_filter/ring_outlier_filter_nodelet.cpp | 8 ++++++-- .../time_synchronizer/time_synchronizer_nodelet.cpp | 4 +++- .../lanelet2_map_filter_nodelet.cpp | 4 +++- .../radar_scan_to_pointcloud2_node.cpp | 12 +++++++++--- 11 files changed, 58 insertions(+), 23 deletions(-) diff --git a/sensing/livox/autoware_livox_tag_filter/src/livox_tag_filter_node.cpp b/sensing/livox/autoware_livox_tag_filter/src/livox_tag_filter_node.cpp index 10da7b931f4ae..c6d5f63e62322 100644 --- a/sensing/livox/autoware_livox_tag_filter/src/livox_tag_filter_node.cpp +++ b/sensing/livox/autoware_livox_tag_filter/src/livox_tag_filter_node.cpp @@ -47,9 +47,13 @@ LivoxTagFilterNode::LivoxTagFilterNode(const rclcpp::NodeOptions & node_options) sub_pointcloud_ = this->create_subscription( "input", rclcpp::SensorDataQoS(), std::bind(&LivoxTagFilterNode::onPointCloud, this, _1)); - // Publisher - pub_pointcloud_ = - this->create_publisher("output", rclcpp::SensorDataQoS()); + { + // Publisher + rclcpp::PublisherOptions pub_options; + pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + pub_pointcloud_ = this->create_publisher( + "output", rclcpp::SensorDataQoS(), pub_options); + } } void LivoxTagFilterNode::onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index 4fd28e02cc74f..8799a70c01736 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -141,8 +141,10 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro // Output Publishers { + rclcpp::PublisherOptions pub_options; + pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); pub_output_ = this->create_publisher( - "output", rclcpp::SensorDataQoS().keep_last(maximum_queue_size_)); + "output", rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), pub_options); } // Subscribers @@ -192,10 +194,13 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro // Transformed Raw PointCloud2 Publisher to publish the transformed pointcloud if (publish_synchronized_pointcloud_) { + rclcpp::PublisherOptions pub_options; + pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + for (auto & topic : input_topics_) { std::string new_topic = replaceSyncTopicNamePostfix(topic, synchronized_pointcloud_postfix_); auto publisher = this->create_publisher( - new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_)); + new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), pub_options); transformed_raw_pc_publisher_map_.insert({topic, publisher}); } } diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp index 29f0bf61a1ab7..89e058d11bb53 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp @@ -97,8 +97,10 @@ PointCloudConcatenationComponent::PointCloudConcatenationComponent( // Output Publishers { + rclcpp::PublisherOptions pub_options; + pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); pub_output_ = this->create_publisher( - "output", rclcpp::SensorDataQoS().keep_last(maximum_queue_size_)); + "output", rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), pub_options); } // Subscribers diff --git a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp index 620e9e42a6864..a56d8f1570234 100644 --- a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp @@ -87,8 +87,10 @@ CropBoxFilterComponent::CropBoxFilterComponent(const rclcpp::NodeOptions & optio // set additional publishers { - crop_box_polygon_pub_ = - this->create_publisher("~/crop_box_polygon", 10); + rclcpp::PublisherOptions pub_options; + pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + crop_box_polygon_pub_ = this->create_publisher( + "~/crop_box_polygon", 10, pub_options); } // set parameter service callback diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index e7aa1f24409e6..8bf511637371a 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -40,10 +40,13 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt time_stamp_field_name_ = declare_parameter("time_stamp_field_name", "time_stamp"); use_imu_ = declare_parameter("use_imu", true); - // Publisher - undistorted_points_pub_ = - this->create_publisher("~/output/pointcloud", rclcpp::SensorDataQoS()); - + { + rclcpp::PublisherOptions pub_options; + pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + // Publisher + undistorted_points_pub_ = this->create_publisher( + "~/output/pointcloud", rclcpp::SensorDataQoS(), pub_options); + } // Subscriber twist_sub_ = this->create_subscription( "~/input/twist", 10, diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp index c9bb6dfc0fc66..035be38f4c342 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -89,8 +89,10 @@ pointcloud_preprocessor::Filter::Filter( // Set publisher { + rclcpp::PublisherOptions pub_options; + pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); pub_output_ = this->create_publisher( - "output", rclcpp::SensorDataQoS().keep_last(max_queue_size_)); + "output", rclcpp::SensorDataQoS().keep_last(max_queue_size_), pub_options); } subscribe(filter_name); diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp index 275c56eaefcc7..ea16b7700e5e3 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp @@ -72,9 +72,12 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent( image_transport::create_publisher(this, "dual_return_outlier_filter/debug/frequency_image"); visibility_pub_ = create_publisher( "dual_return_outlier_filter/debug/visibility", rclcpp::SensorDataQoS()); - noise_cloud_pub_ = create_publisher( - "dual_return_outlier_filter/debug/pointcloud_noise", rclcpp::SensorDataQoS()); - + { + rclcpp::PublisherOptions pub_options; + pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + noise_cloud_pub_ = create_publisher( + "dual_return_outlier_filter/debug/pointcloud_noise", rclcpp::SensorDataQoS(), pub_options); + } using std::placeholders::_1; set_param_res_ = this->add_on_set_parameters_callback( std::bind(&DualReturnOutlierFilterComponent::paramCallback, this, _1)); diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index e05f44240b53a..a7c9e343a00e1 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -33,8 +33,12 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "ring_outlier_filter"); - outlier_pointcloud_publisher_ = - this->create_publisher("debug/ring_outlier_filter", 1); + { + rclcpp::PublisherOptions pub_options; + pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + outlier_pointcloud_publisher_ = + this->create_publisher("debug/ring_outlier_filter", 1, pub_options); + } visibility_pub_ = create_publisher( "ring_outlier_filter/debug/visibility", rclcpp::SensorDataQoS()); stop_watch_ptr_->tic("cyclic_time"); diff --git a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp index 890dcdb974cf3..19920dd1ba5d7 100644 --- a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -160,10 +160,12 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( // Transformed Raw PointCloud2 Publisher { + rclcpp::PublisherOptions pub_options; + pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); for (auto & topic : input_topics_) { std::string new_topic = replaceSyncTopicNamePostfix(topic, synchronized_pointcloud_postfix); auto publisher = this->create_publisher( - new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_)); + new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), pub_options); transformed_raw_pc_publisher_map_.insert({topic, publisher}); } } diff --git a/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp index 8f7cb2abec6a4..9049f2964a850 100644 --- a/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp @@ -45,8 +45,10 @@ Lanelet2MapFilterComponent::Lanelet2MapFilterComponent(const rclcpp::NodeOptions // Set publisher { + rclcpp::PublisherOptions pub_options; + pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); filtered_pointcloud_pub_ = - this->create_publisher("output", rclcpp::SensorDataQoS()); + this->create_publisher("output", rclcpp::SensorDataQoS(), pub_options); } // Set subscriber diff --git a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp index b182147468262..8874b744bced6 100644 --- a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp +++ b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp @@ -111,9 +111,15 @@ RadarScanToPointcloud2Node::RadarScanToPointcloud2Node(const rclcpp::NodeOptions sub_radar_ = create_subscription( "~/input/radar", rclcpp::QoS{1}, std::bind(&RadarScanToPointcloud2Node::onData, this, _1)); - // Publisher - pub_amplitude_pointcloud_ = create_publisher("~/output/amplitude_pointcloud", 1); - pub_doppler_pointcloud_ = create_publisher("~/output/doppler_pointcloud", 1); + { + rclcpp::PublisherOptions pub_options; + pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + // Publisher + pub_amplitude_pointcloud_ = + create_publisher("~/output/amplitude_pointcloud", 1, pub_options); + pub_doppler_pointcloud_ = + create_publisher("~/output/doppler_pointcloud", 1, pub_options); + } } rcl_interfaces::msg::SetParametersResult RadarScanToPointcloud2Node::onSetParam(