From 4bb8e760aab75e180389670fc8e490fb65f38809 Mon Sep 17 00:00:00 2001
From: mohammad alqudah <alqudah.mohammad@tier4.jp>
Date: Mon, 10 Jun 2024 16:26:46 +0900
Subject: [PATCH] use polling subscriber in simple trajectory follower

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>
---
 .../simple_trajectory_follower.hpp            | 14 ++++++++-----
 .../src/simple_trajectory_follower.cpp        | 20 +++++++++++--------
 2 files changed, 21 insertions(+), 13 deletions(-)

diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp
index e744243969cab..9360f900bca5f 100644
--- a/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp
+++ b/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp
@@ -15,6 +15,8 @@
 #ifndef TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_
 #define TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_
 
+#include "tier4_autoware_utils/ros/polling_subscriber.hpp"
+
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_control_msgs/msg/control.hpp>
@@ -42,20 +44,22 @@ class SimpleTrajectoryFollower : public rclcpp::Node
   ~SimpleTrajectoryFollower() = default;
 
 private:
-  rclcpp::Subscription<Odometry>::SharedPtr sub_kinematics_;
-  rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;
+  tier4_autoware_utils::InterProcessPollingSubscriber<Odometry> sub_kinematics_{
+    this, "~/input/kinematics"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<Trajectory> sub_trajectory_{
+    this, "~/input/trajectory"};
   rclcpp::Publisher<Control>::SharedPtr pub_cmd_;
   rclcpp::TimerBase::SharedPtr timer_;
 
-  Trajectory::SharedPtr trajectory_;
-  Odometry::SharedPtr odometry_;
+  Trajectory::ConstSharedPtr trajectory_;
+  Odometry::ConstSharedPtr odometry_;
   TrajectoryPoint closest_traj_point_;
   bool use_external_target_vel_;
   double external_target_vel_;
   double lateral_deviation_;
 
   void onTimer();
-  bool checkData();
+  bool processData();
   void updateClosest();
   double calcSteerCmd();
   double calcAccCmd();
diff --git a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp
index 7c27eaed41380..bd24516ed033b 100644
--- a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp
+++ b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp
@@ -31,11 +31,6 @@ SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & o
 {
   pub_cmd_ = create_publisher<Control>("output/control_cmd", 1);
 
-  sub_kinematics_ = create_subscription<Odometry>(
-    "input/kinematics", 1, [this](const Odometry::SharedPtr msg) { odometry_ = msg; });
-  sub_trajectory_ = create_subscription<Trajectory>(
-    "input/trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = msg; });
-
   use_external_target_vel_ = declare_parameter<bool>("use_external_target_vel");
   external_target_vel_ = declare_parameter<float>("external_target_vel");
   lateral_deviation_ = declare_parameter<float>("lateral_deviation");
@@ -47,7 +42,7 @@ SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & o
 
 void SimpleTrajectoryFollower::onTimer()
 {
-  if (!checkData()) {
+  if (!processData()) {
     RCLCPP_INFO(get_logger(), "data not ready");
     return;
   }
@@ -110,9 +105,18 @@ double SimpleTrajectoryFollower::calcAccCmd()
   return acc;
 }
 
-bool SimpleTrajectoryFollower::checkData()
+bool SimpleTrajectoryFollower::processData()
 {
-  return (trajectory_ && odometry_);
+  bool is_ready = true;
+  const auto & getData = [](auto & dest, auto & sub) {
+    const auto temp = sub.takeData();
+    if (!temp) return false;
+    dest = temp;
+    return true;
+  };
+  is_ready &= getData(odometry_, sub_kinematics_);
+  is_ready &= getData(trajectory_, sub_trajectory_);
+  return is_ready;
 }
 
 }  // namespace simple_trajectory_follower