From 775db2ffc1df28b06a150c445d1d4ae0f2dea765 Mon Sep 17 00:00:00 2001
From: Mamoru Sobue <mamoru.sobue@tier4.jp>
Date: Fri, 10 May 2024 18:37:47 +0900
Subject: [PATCH] feat(tier4_autoware_utils): default QoS setting of polling
 subscriber

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
---
 .../ros/polling_subscriber.hpp                 | 18 +++++++++++++++---
 1 file changed, 15 insertions(+), 3 deletions(-)

diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp
index f8d5baaf02777..5dd17ddc69744 100644
--- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp
+++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp
@@ -30,7 +30,8 @@ class InterProcessPollingSubscriber
   std::optional<T> data_;
 
 public:
-  explicit InterProcessPollingSubscriber(rclcpp::Node * node, const std::string & topic_name)
+  explicit InterProcessPollingSubscriber(
+    rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1})
   {
     auto noexec_callback_group =
       node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
@@ -38,16 +39,27 @@ class InterProcessPollingSubscriber
     noexec_subscription_options.callback_group = noexec_callback_group;
 
     subscriber_ = node->create_subscription<T>(
-      topic_name, rclcpp::QoS{1},
+      topic_name, qos,
       [node]([[maybe_unused]] const typename T::ConstSharedPtr msg) { assert(false); },
       noexec_subscription_options);
+    if (qos.get_rmw_qos_profile().depth > 1) {
+      RCLCPP_WARN(
+        node->get_logger(),
+        "InterProcessPollingSubscriber will be used with depth > 1, which may cause inefficient "
+        "serialization while updateLatestData()");
+    }
   };
   bool updateLatestData()
   {
     rclcpp::MessageInfo message_info;
     T tmp;
     // The queue size (QoS) must be 1 to get the last message data.
-    if (subscriber_->take(tmp, message_info)) {
+    bool is_latest_message_consumed = false;
+    // pop the queue until latest data
+    while (subscriber_->take(tmp, message_info)) {
+      is_latest_message_consumed = true;
+    }
+    if (is_latest_message_consumed) {
       data_ = tmp;
     }
     return data_.has_value();