feat(tier4_autoware_utils): add the API to check if taken data is sta…

…le (#7404)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

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>

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(