From 49403f329653181ffbd8959d3feb66736e980048 Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Sun, 2 Jun 2024 00:46:00 +0900
Subject: [PATCH] feat(surround_obstacle_checker): use polling subscriber

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
---
 .../surround_obstacle_checker/node.hpp        | 10 ++++--
 .../surround_obstacle_checker/src/node.cpp    | 31 +++----------------
 2 files changed, 11 insertions(+), 30 deletions(-)

diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp
index e009c77d3ea1..d5b2c49df51c 100644
--- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp
+++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp
@@ -17,6 +17,7 @@
 
 #include "surround_obstacle_checker/debug_marker.hpp"
 #include "tier4_autoware_utils/ros/logger_level_configure.hpp"
+#include "tier4_autoware_utils/ros/polling_subscriber.hpp"
 
 #include <motion_utils/vehicle/vehicle_state_checker.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -110,9 +111,12 @@ class SurroundObstacleCheckerNode : public rclcpp::Node
   rclcpp::TimerBase::SharedPtr timer_;
 
   // publisher and subscriber
-  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_pointcloud_;
-  rclcpp::Subscription<PredictedObjects>::SharedPtr sub_dynamic_objects_;
-  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odometry_;
+  tier4_autoware_utils::InterProcessPollingSubscriber<nav_msgs::msg::Odometry> sub_odometry_{
+    this, "~/input/odometry"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<sensor_msgs::msg::PointCloud2>
+    sub_pointcloud_{this, "~/input/pointcloud"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<PredictedObjects> sub_dynamic_objects_{
+    this, "~/input/objects"};
   rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticStatus>::SharedPtr pub_stop_reason_;
   rclcpp::Publisher<VelocityLimitClearCommand>::SharedPtr pub_clear_velocity_limit_;
   rclcpp::Publisher<VelocityLimit>::SharedPtr pub_velocity_limit_;
diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp
index c19a04bbfeac..af6dba8fcc0c 100644
--- a/planning/surround_obstacle_checker/src/node.cpp
+++ b/planning/surround_obstacle_checker/src/node.cpp
@@ -206,17 +206,6 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio
   pub_velocity_limit_ = this->create_publisher<VelocityLimit>(
     "~/output/max_velocity", rclcpp::QoS{1}.transient_local());
 
-  // Subscribers
-  sub_pointcloud_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
-    "~/input/pointcloud", rclcpp::SensorDataQoS(),
-    std::bind(&SurroundObstacleCheckerNode::onPointCloud, this, std::placeholders::_1));
-  sub_dynamic_objects_ = this->create_subscription<PredictedObjects>(
-    "~/input/objects", 1,
-    std::bind(&SurroundObstacleCheckerNode::onDynamicObjects, this, std::placeholders::_1));
-  sub_odometry_ = this->create_subscription<nav_msgs::msg::Odometry>(
-    "~/input/odometry", 1,
-    std::bind(&SurroundObstacleCheckerNode::onOdometry, this, std::placeholders::_1));
-
   // Parameter callback
   set_param_res_ = this->add_on_set_parameters_callback(
     std::bind(&SurroundObstacleCheckerNode::onParam, this, std::placeholders::_1));
@@ -282,6 +271,10 @@ rcl_interfaces::msg::SetParametersResult SurroundObstacleCheckerNode::onParam(
 
 void SurroundObstacleCheckerNode::onTimer()
 {
+  odometry_ptr_ = sub_odometry_.takeData();
+  pointcloud_ptr_ = sub_pointcloud_.takeData();
+  object_ptr_ = sub_dynamic_objects_.takeData();
+
   if (!odometry_ptr_) {
     RCLCPP_INFO_THROTTLE(
       this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for current velocity...");
@@ -377,22 +370,6 @@ void SurroundObstacleCheckerNode::onTimer()
   debug_ptr_->publish();
 }
 
-void SurroundObstacleCheckerNode::onPointCloud(
-  const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
-{
-  pointcloud_ptr_ = msg;
-}
-
-void SurroundObstacleCheckerNode::onDynamicObjects(const PredictedObjects::ConstSharedPtr msg)
-{
-  object_ptr_ = msg;
-}
-
-void SurroundObstacleCheckerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg)
-{
-  odometry_ptr_ = msg;
-}
-
 std::optional<Obstacle> SurroundObstacleCheckerNode::getNearestObstacle() const
 {
   const auto nearest_pointcloud = getNearestObstacleByPointCloud();