feat(pointcloud_preprocessor): runtime configurable output topic qos (#…

…6658) * feat(pointcloud_preprocessor): runtime configurable output topic qos Signed-off-by: Grzegorz Głowacki <[email protected]> * configurable qos in livox_tag_filter_node Signed-off-by: Grzegorz Głowacki <[email protected]> * configurable qos in radar_scan_to_pointcloud2 Signed-off-by: Grzegorz Głowacki <[email protected]> --------- Signed-off-by: Grzegorz Głowacki <[email protected]>
autowarefoundation · Jul 3, 2024 · f1e4a9e · f1e4a9e
1 parent ced8e37
commit f1e4a9e
Show file tree

Hide file tree

Showing 11 changed files with 58 additions and 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
@@ -47,9 +47,13 @@ LivoxTagFilterNode::LivoxTagFilterNode(const rclcpp::NodeOptions & node_options)
   sub_pointcloud_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
     "input", rclcpp::SensorDataQoS(), std::bind(&LivoxTagFilterNode::onPointCloud, this, _1));
 
-  // Publisher
-  pub_pointcloud_ =
-    this->create_publisher<sensor_msgs::msg::PointCloud2>("output", rclcpp::SensorDataQoS());
+  {
+    // Publisher
+    rclcpp::PublisherOptions pub_options;
+    pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
+    pub_pointcloud_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(
+      "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
@@ -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<PointCloud2>(
-      "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<sensor_msgs::msg::PointCloud2>(
-        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
@@ -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<PointCloud2>(
-      "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
@@ -87,8 +87,10 @@ CropBoxFilterComponent::CropBoxFilterComponent(const rclcpp::NodeOptions & optio
 
   // set additional publishers
   {
-    crop_box_polygon_pub_ =
-      this->create_publisher<geometry_msgs::msg::PolygonStamped>("~/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<geometry_msgs::msg::PolygonStamped>(
+      "~/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
@@ -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<PointCloud2>("~/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<PointCloud2>(
+      "~/output/pointcloud", rclcpp::SensorDataQoS(), pub_options);
+  }
   // Subscriber
   twist_sub_ = this->create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
     "~/input/twist", 10,

diff --git 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<PointCloud2>(
-      "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
@@ -72,9 +72,12 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent(
     image_transport::create_publisher(this, "dual_return_outlier_filter/debug/frequency_image");
   visibility_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
     "dual_return_outlier_filter/debug/visibility", rclcpp::SensorDataQoS());
-  noise_cloud_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>(
-    "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<sensor_msgs::msg::PointCloud2>(
+      "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
@@ -33,8 +33,12 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
     using autoware::universe_utils::StopWatch;
     stop_watch_ptr_ = std::make_unique<StopWatch<std::chrono::milliseconds>>();
     debug_publisher_ = std::make_unique<DebugPublisher>(this, "ring_outlier_filter");
-    outlier_pointcloud_publisher_ =
-      this->create_publisher<PointCloud2>("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<PointCloud2>("debug/ring_outlier_filter", 1, pub_options);
+    }
     visibility_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
       "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
@@ -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<sensor_msgs::msg::PointCloud2>(
-        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
@@ -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<PointCloud2>("output", rclcpp::SensorDataQoS());
+      this->create_publisher<PointCloud2>("output", rclcpp::SensorDataQoS(), pub_options);
   }
 
   // Set subscriber

diff --git a/...scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp b/...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<RadarScan>(
     "~/input/radar", rclcpp::QoS{1}, std::bind(&RadarScanToPointcloud2Node::onData, this, _1));
 
-  // Publisher
-  pub_amplitude_pointcloud_ = create_publisher<PointCloud2>("~/output/amplitude_pointcloud", 1);
-  pub_doppler_pointcloud_ = create_publisher<PointCloud2>("~/output/doppler_pointcloud", 1);
+  {
+    rclcpp::PublisherOptions pub_options;
+    pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
+    // Publisher
+    pub_amplitude_pointcloud_ =
+      create_publisher<PointCloud2>("~/output/amplitude_pointcloud", 1, pub_options);
+    pub_doppler_pointcloud_ =
+      create_publisher<PointCloud2>("~/output/doppler_pointcloud", 1, pub_options);
+  }
 }
 
 rcl_interfaces::msg::SetParametersResult RadarScanToPointcloud2Node::onSetParam(