From cb557db2c5a5128bbe46d9c5077f387bc1a49d91 Mon Sep 17 00:00:00 2001
From: Mamoru Sobue <hilo.soblin@gmail.com>
Date: Mon, 10 Jun 2024 20:31:27 +0900
Subject: [PATCH] feat(tier4_autoware_utils): add the API to check if taken
 data is stale (#7404)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
---
 .../ros/polling_subscriber.hpp                | 25 +++++++++++++++++++
 .../lane_departure_checker_node.cpp           |  2 +-
 2 files changed, 26 insertions(+), 1 deletion(-)

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 b274f4ab55d91..d0b6dfac2dcff 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
@@ -64,6 +64,12 @@ class InterProcessPollingSubscriber<T, N, typename std::enable_if<N == 1>::type>
         "serialization while updateLatestData()");
     }
   };
+  /*
+   * @brief take and return the latest data from DDS queue if such data existed, otherwise return
+   * previous taken data("stale" data)
+   * @note if you want to ignore "stale" data, you should use takeNewData()
+   * instead
+   */
   typename T::ConstSharedPtr takeData()
   {
     auto new_data = std::make_shared<T>();
@@ -75,6 +81,25 @@ class InterProcessPollingSubscriber<T, N, typename std::enable_if<N == 1>::type>
 
     return data_;
   };
+
+  /*
+   * @brief take and return the latest data from DDS queue if such data existed, otherwise return
+   * nullptr instead
+   * @note this API allows you to avoid redundant computation on the taken data which is unchanged
+   * since the previous cycle
+   */
+  typename T::ConstSharedPtr takeNewData()
+  {
+    auto new_data = std::make_shared<T>();
+    rclcpp::MessageInfo message_info;
+    const bool success = subscriber_->take(*new_data, message_info);
+    if (success) {
+      data_ = new_data;
+      return data_;
+    } else {
+      return nullptr;
+    }
+  }
 };
 
 template <typename T, int N>
diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp
index b25b3fd843f4b..f95ae0f4ac2ed 100644
--- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp
+++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp
@@ -259,7 +259,7 @@ void LaneDepartureCheckerNode::onTimer()
   reference_trajectory_ = sub_reference_trajectory_.takeData();
   predicted_trajectory_ = sub_predicted_trajectory_.takeData();
 
-  const auto lanelet_map_bin_msg = sub_lanelet_map_bin_.takeData();
+  const auto lanelet_map_bin_msg = sub_lanelet_map_bin_.takeNewData();
   if (lanelet_map_bin_msg) {
     lanelet_map_ = std::make_shared<lanelet::LaneletMap>();
     lanelet::utils::conversion::fromBinMsg(