feat(autoware_lane_departure_checker): use polling subscriber (#7358)

* use polling subscriber

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

---------

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

Author: kyoichi-sugahara

control/lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker_node.hpp

@@ -16,6 +16,7 @@
 #define AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_
 
 #include "autoware_lane_departure_checker/lane_departure_checker.hpp"
+#include "tier4_autoware_utils/ros/polling_subscriber.hpp"
 
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -66,11 +67,16 @@ class LaneDepartureCheckerNode : public rclcpp::Node
 
 private:
   // Subscriber
-  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
-  rclcpp::Subscription<LaneletMapBin>::SharedPtr sub_lanelet_map_bin_;
-  rclcpp::Subscription<LaneletRoute>::SharedPtr sub_route_;
-  rclcpp::Subscription<Trajectory>::SharedPtr sub_reference_trajectory_;
-  rclcpp::Subscription<Trajectory>::SharedPtr sub_predicted_trajectory_;
+  tier4_autoware_utils::InterProcessPollingSubscriber<nav_msgs::msg::Odometry> sub_odom_{
+    this, "~/input/odometry"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<LaneletMapBin> sub_lanelet_map_bin_{
+    this, "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local()};
+  tier4_autoware_utils::InterProcessPollingSubscriber<LaneletRoute> sub_route_{
+    this, "~/input/route"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<Trajectory> sub_reference_trajectory_{
+    this, "~/input/reference_trajectory"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<Trajectory> sub_predicted_trajectory_{
+    this, "~/input/predicted_trajectory"};
 
   // Data Buffer
   nav_msgs::msg::Odometry::ConstSharedPtr current_odom_;

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

@@ -169,22 +169,6 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
   lane_departure_checker_ = std::make_unique<LaneDepartureChecker>();
   lane_departure_checker_->setParam(param_, vehicle_info);
 
-  // Subscriber
-  sub_odom_ = this->create_subscription<nav_msgs::msg::Odometry>(
-    "~/input/odometry", 1, std::bind(&LaneDepartureCheckerNode::onOdometry, this, _1));
-  sub_lanelet_map_bin_ = this->create_subscription<LaneletMapBin>(
-    "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local(),
-    std::bind(&LaneDepartureCheckerNode::onLaneletMapBin, this, _1));
-  sub_route_ = this->create_subscription<LaneletRoute>(
-    "~/input/route", rclcpp::QoS{1}.transient_local(),
-    std::bind(&LaneDepartureCheckerNode::onRoute, this, _1));
-  sub_reference_trajectory_ = this->create_subscription<Trajectory>(
-    "~/input/reference_trajectory", 1,
-    std::bind(&LaneDepartureCheckerNode::onReferenceTrajectory, this, _1));
-  sub_predicted_trajectory_ = this->create_subscription<Trajectory>(
-    "~/input/predicted_trajectory", 1,
-    std::bind(&LaneDepartureCheckerNode::onPredictedTrajectory, this, _1));
-
   // Publisher
   // Nothing
 
@@ -201,36 +185,6 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
     this, get_clock(), period_ns, std::bind(&LaneDepartureCheckerNode::onTimer, this));
 }
 
-void LaneDepartureCheckerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg)
-{
-  current_odom_ = msg;
-}
-
-void LaneDepartureCheckerNode::onLaneletMapBin(const LaneletMapBin::ConstSharedPtr msg)
-{
-  lanelet_map_ = std::make_shared<lanelet::LaneletMap>();
-  lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_, &traffic_rules_, &routing_graph_);
-
-  // get all shoulder lanes
-  lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_);
-  shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets);
-}
-
-void LaneDepartureCheckerNode::onRoute(const LaneletRoute::ConstSharedPtr msg)
-{
-  route_ = msg;
-}
-
-void LaneDepartureCheckerNode::onReferenceTrajectory(const Trajectory::ConstSharedPtr msg)
-{
-  reference_trajectory_ = msg;
-}
-
-void LaneDepartureCheckerNode::onPredictedTrajectory(const Trajectory::ConstSharedPtr msg)
-{
-  predicted_trajectory_ = msg;
-}
-
 bool LaneDepartureCheckerNode::isDataReady()
 {
   if (!current_odom_) {
@@ -300,6 +254,22 @@ void LaneDepartureCheckerNode::onTimer()
   tier4_autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch;
   stop_watch.tic("Total");
 
+  current_odom_ = sub_odom_.takeData();
+  route_ = sub_route_.takeData();
+  reference_trajectory_ = sub_reference_trajectory_.takeData();
+  predicted_trajectory_ = sub_predicted_trajectory_.takeData();
+
+  const auto lanelet_map_bin_msg = sub_lanelet_map_bin_.takeData();
+  if (lanelet_map_bin_msg) {
+    lanelet_map_ = std::make_shared<lanelet::LaneletMap>();
+    lanelet::utils::conversion::fromBinMsg(
+      *lanelet_map_bin_msg, lanelet_map_, &traffic_rules_, &routing_graph_);
+
+    // get all shoulder lanes
+    lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_);
+    shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets);
+  }
+
   if (!isDataReady()) {
     return;
   }