feat(pure_pursuit): add polling subscriber (#7330)

RT1-6699 add polling subscriber

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Author: zulfaqar-azmi-t4

control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp

@@ -34,6 +34,7 @@
 #include "pure_pursuit/pure_pursuit_viz.hpp"
 
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/polling_subscriber.hpp>
 #include <tier4_autoware_utils/ros/self_pose_listener.hpp>
 
 #include <autoware_control_msgs/msg/lateral.hpp>
@@ -48,7 +49,6 @@
 #include <tf2_ros/transform_listener.h>
 
 #include <memory>
-#include <vector>
 
 namespace pure_pursuit
 {
@@ -79,8 +79,10 @@ class PurePursuitNode : public rclcpp::Node
 private:
   // Subscriber
   tier4_autoware_utils::SelfPoseListener self_pose_listener_{this};
-  rclcpp::Subscription<autoware_planning_msgs::msg::Trajectory>::SharedPtr sub_trajectory_;
-  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_current_odometry_;
+  tier4_autoware_utils::InterProcessPollingSubscriber<autoware_planning_msgs::msg::Trajectory>
+    sub_trajectory_{this, "input/reference_trajectory"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<nav_msgs::msg::Odometry>
+    sub_current_odometry_{this, "input/current_odometry"};
 
   autoware_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_;
   nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_;

control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp

@@ -71,13 +71,6 @@ PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions & node_options)
   param_.reverse_min_lookahead_distance =
     this->declare_parameter<double>("reverse_min_lookahead_distance");
 
-  // Subscribers
-  using std::placeholders::_1;
-  sub_trajectory_ = this->create_subscription<autoware_planning_msgs::msg::Trajectory>(
-    "input/reference_trajectory", 1, std::bind(&PurePursuitNode::onTrajectory, this, _1));
-  sub_current_odometry_ = this->create_subscription<nav_msgs::msg::Odometry>(
-    "input/current_odometry", 1, std::bind(&PurePursuitNode::onCurrentOdometry, this, _1));
-
   // Publishers
   pub_ctrl_cmd_ =
     this->create_publisher<autoware_control_msgs::msg::Lateral>("output/control_raw", 1);
@@ -118,21 +111,12 @@ bool PurePursuitNode::isDataReady()
   return true;
 }
 
-void PurePursuitNode::onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg)
-{
-  current_odometry_ = msg;
-}
-
-void PurePursuitNode::onTrajectory(
-  const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg)
-{
-  trajectory_ = msg;
-}
-
 void PurePursuitNode::onTimer()
 {
   current_pose_ = self_pose_listener_.getCurrentPose();
 
+  current_odometry_ = sub_current_odometry_.takeData();
+  trajectory_ = sub_trajectory_.takeData();
   if (!isDataReady()) {
     return;
   }