fix(autoware_radar_threshold_filter): use sensor data qos (#10156)

* fix(autoware_radar_threshold_filter): use sensor data qos

Signed-off-by: Mehmet Emin BAŞOĞLU <mehmeteminbasoglu@hotmail.com>

* change type to fix cpplint

Signed-off-by: Mehmet Emin BAŞOĞLU <mehmeteminbasoglu@hotmail.com>

* update test

Signed-off-by: Mehmet Emin BAŞOĞLU <mehmeteminbasoglu@hotmail.com>

---------

Signed-off-by: Mehmet Emin BAŞOĞLU <mehmeteminbasoglu@hotmail.com>

Author: mebasoglu

sensing/autoware_radar_threshold_filter/config/radar_threshold_filter.param.yaml

@@ -16,3 +16,5 @@
       is_z_filter: false
       z_min: -2.0
       z_max: 5.0
+
+      max_queue_size: 5

sensing/autoware_radar_threshold_filter/src/radar_threshold_filter_node.cpp

@@ -75,13 +75,16 @@ RadarThresholdFilterNode::RadarThresholdFilterNode(const rclcpp::NodeOptions & n
   node_param_.is_z_filter = declare_parameter<bool>("node_params.is_z_filter");
   node_param_.z_min = declare_parameter<double>("node_params.z_min");
   node_param_.z_max = declare_parameter<double>("node_params.z_max");
+  node_param_.max_queue_size = declare_parameter<int64_t>("node_params.max_queue_size");
 
   // Subscriber
   sub_radar_ = create_subscription<RadarScan>(
-    "~/input/radar", rclcpp::QoS{1}, std::bind(&RadarThresholdFilterNode::onData, this, _1));
+    "~/input/radar", rclcpp::SensorDataQoS().keep_last(node_param_.max_queue_size),
+    std::bind(&RadarThresholdFilterNode::onData, this, _1));
 
   // Publisher
-  pub_radar_ = create_publisher<RadarScan>("~/output/radar", 1);
+  pub_radar_ = create_publisher<RadarScan>(
+    "~/output/radar", rclcpp::SensorDataQoS().keep_last(node_param_.max_queue_size));
 }
 
 rcl_interfaces::msg::SetParametersResult RadarThresholdFilterNode::onSetParam(
@@ -103,6 +106,7 @@ rcl_interfaces::msg::SetParametersResult RadarThresholdFilterNode::onSetParam(
       update_param(params, "node_params.is_z_filter", p.is_z_filter);
       update_param(params, "node_params.z_min", p.z_min);
       update_param(params, "node_params.z_max", p.z_max);
+      update_param(params, "node_params.max_queue_size", p.max_queue_size);
     }
   } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
     result.successful = false;

sensing/autoware_radar_threshold_filter/src/radar_threshold_filter_node.hpp

@@ -48,6 +48,7 @@ class RadarThresholdFilterNode : public rclcpp::Node
     bool is_z_filter{};
     double z_min{};
     double z_max{};
+    size_t max_queue_size{};
   };
 
 private:

sensing/autoware_radar_threshold_filter/test/radar_threshold_filter/test_radar_threshold_filter.cpp

@@ -32,6 +32,7 @@ TEST(RadarThresholdFilter, isWithinThreshold)
   const double azimuth_max = 1.2;
   const double z_min = -2.0;
   const double z_max = 5.0;
+  const int64_t max_queue_size = 5;
   // amplitude filter
   {
     rclcpp::NodeOptions node_options;
@@ -48,6 +49,7 @@ TEST(RadarThresholdFilter, isWithinThreshold)
       {"node_params.is_z_filter", false},
       {"node_params.z_min", z_min},
       {"node_params.z_max", z_max},
+      {"node_params.max_queue_size", max_queue_size},
     });
 
     RadarThresholdFilterNode node(node_options);
@@ -78,6 +80,7 @@ TEST(RadarThresholdFilter, isWithinThreshold)
       {"node_params.is_z_filter", false},
       {"node_params.z_min", z_min},
       {"node_params.z_max", z_max},
+      {"node_params.max_queue_size", max_queue_size},
     });
 
     RadarThresholdFilterNode node(node_options);
@@ -107,6 +110,7 @@ TEST(RadarThresholdFilter, isWithinThreshold)
       {"node_params.is_z_filter", false},
       {"node_params.z_min", z_min},
       {"node_params.z_max", z_max},
+      {"node_params.max_queue_size", max_queue_size},
     });
 
     RadarThresholdFilterNode node(node_options);
@@ -135,6 +139,7 @@ TEST(RadarThresholdFilter, isWithinThreshold)
       {"node_params.is_z_filter", true},
       {"node_params.z_min", z_min},
       {"node_params.z_max", z_max},
+      {"node_params.max_queue_size", max_queue_size},
     });
 
     RadarThresholdFilterNode node(node_options);