Merge pull request #1847 from tier4/sync-awf-latest

chore: sync awf-latest
tier4 · Feb 19, 2025 · 505f97e · 505f97e
2 parents f0d8327 + 5d090f2
commit 505f97e
Show file tree

Hide file tree

Showing 4 changed files with 19 additions and 6 deletions.
diff --git a/sensing/autoware_radar_scan_to_pointcloud2/config/radar_scan_to_pointcloud2.param.yaml b/sensing/autoware_radar_scan_to_pointcloud2/config/radar_scan_to_pointcloud2.param.yaml
@@ -2,3 +2,4 @@
   ros__parameters:
     publish_amplitude_pointcloud: true
     publish_doppler_pointcloud: false
+    max_queue_size: 5
diff --git a/sensing/autoware_radar_scan_to_pointcloud2/schema/radar_scan_to_pointcloud2.schema.json b/sensing/autoware_radar_scan_to_pointcloud2/schema/radar_scan_to_pointcloud2.schema.json
@@ -15,9 +15,15 @@
           "type": "boolean",
           "description": "Whether publish radar pointcloud whose intensity is doppler velocity.",
           "default": "false"
+        },
+        "max_queue_size": {
+          "type": "integer",
+          "default": "5",
+          "minimum": 1,
+          "description": "Max queue size of input/output topics."
         }
       },
-      "required": ["publish_amplitude_pointcloud", "publish_doppler_pointcloud"]
+      "required": ["publish_amplitude_pointcloud", "publish_doppler_pointcloud", "max_queue_size"]
     }
   },
   "properties": {

diff --git a/sensing/autoware_radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.cpp b/sensing/autoware_radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.cpp
@@ -106,19 +106,23 @@ RadarScanToPointcloud2Node::RadarScanToPointcloud2Node(const rclcpp::NodeOptions
   node_param_.publish_amplitude_pointcloud =
     declare_parameter<bool>("publish_amplitude_pointcloud");
   node_param_.publish_doppler_pointcloud = declare_parameter<bool>("publish_doppler_pointcloud");
+  node_param_.max_queue_size = declare_parameter<int64_t>("max_queue_size");
 
   // Subscriber
   sub_radar_ = create_subscription<RadarScan>(
-    "~/input/radar", rclcpp::QoS{1}, std::bind(&RadarScanToPointcloud2Node::onData, this, _1));
+    "~/input/radar", rclcpp::SensorDataQoS().keep_last(node_param_.max_queue_size),
+    std::bind(&RadarScanToPointcloud2Node::onData, this, _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);
+    pub_amplitude_pointcloud_ = create_publisher<PointCloud2>(
+      "~/output/amplitude_pointcloud",
+      rclcpp::SensorDataQoS().keep_last(node_param_.max_queue_size), pub_options);
+    pub_doppler_pointcloud_ = create_publisher<PointCloud2>(
+      "~/output/doppler_pointcloud", rclcpp::SensorDataQoS().keep_last(node_param_.max_queue_size),
+      pub_options);
   }
 }
 
@@ -131,6 +135,7 @@ rcl_interfaces::msg::SetParametersResult RadarScanToPointcloud2Node::onSetParam(
     auto & p = node_param_;
     update_param(params, "publish_amplitude_pointcloud", p.publish_amplitude_pointcloud);
     update_param(params, "publish_doppler_pointcloud", p.publish_doppler_pointcloud);
+    update_param(params, "max_queue_size", p.max_queue_size);
   } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
     result.successful = false;
     result.reason = e.what();

diff --git a/sensing/autoware_radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.hpp b/sensing/autoware_radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.hpp
@@ -39,6 +39,7 @@ class RadarScanToPointcloud2Node : public rclcpp::Node
   {
     bool publish_amplitude_pointcloud{};
     bool publish_doppler_pointcloud{};
+    size_t max_queue_size{};
   };
 
 private: