feat(path_smoother): use polling subscriber (autowarefoundation#7214)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Author: takayuki5168

planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp

@@ -22,6 +22,7 @@
 #include "path_smoother/type_alias.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "tier4_autoware_utils/ros/logger_level_configure.hpp"
+#include "tier4_autoware_utils/ros/polling_subscriber.hpp"
 
 #include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
@@ -72,9 +73,6 @@ class ElasticBandSmoother : public rclcpp::Node
   CommonParam common_param_{};
   EgoNearestParam ego_nearest_param_{};
 
-  // variables for subscribers
-  Odometry::ConstSharedPtr ego_state_ptr_;
-
   // variables for previous information
   std::shared_ptr<std::vector<TrajectoryPoint>> prev_optimized_traj_points_ptr_;
 
@@ -84,7 +82,7 @@ class ElasticBandSmoother : public rclcpp::Node
 
   // interface subscriber
   rclcpp::Subscription<Path>::SharedPtr path_sub_;
-  rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
+  tier4_autoware_utils::InterProcessPollingSubscriber<Odometry> odom_sub_{this, "~/input/odometry"};
 
   // debug publisher
   rclcpp::Publisher<Trajectory>::SharedPtr debug_extended_traj_pub_;
@@ -104,7 +102,8 @@ class ElasticBandSmoother : public rclcpp::Node
   void resetPreviousData();
 
   // main functions
-  bool isDataReady(const Path & path, rclcpp::Clock clock) const;
+  bool isDataReady(
+    const Path & path, const Odometry::ConstSharedPtr ego_state_ptr, rclcpp::Clock clock) const;
   void applyInputVelocity(
     std::vector<TrajectoryPoint> & output_traj_points,
     const std::vector<TrajectoryPoint> & input_traj_points,

planning/path_smoother/src/elastic_band_smoother.cpp

@@ -79,8 +79,6 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option
   // interface subscriber
   path_sub_ = create_subscription<Path>(
     "~/input/path", 1, std::bind(&ElasticBandSmoother::onPath, this, std::placeholders::_1));
-  odom_sub_ = create_subscription<Odometry>(
-    "~/input/odometry", 1, [this](const Odometry::ConstSharedPtr msg) { ego_state_ptr_ = msg; });
 
   // debug publisher
   debug_extended_traj_pub_ = create_publisher<Trajectory>("~/debug/extended_traj", 1);
@@ -156,7 +154,8 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr)
   time_keeper_ptr_->tic(__func__);
 
   // check if data is ready and valid
-  if (!isDataReady(*path_ptr, *get_clock())) {
+  const auto ego_state_ptr = odom_sub_.takeData();
+  if (!isDataReady(*path_ptr, ego_state_ptr, *get_clock())) {
     return;
   }
 
@@ -181,7 +180,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr)
   // 1. calculate trajectory with Elastic Band
   // 1.a check if replan (= optimization) is required
   PlannerData planner_data(
-    input_traj_points, ego_state_ptr_->pose.pose, ego_state_ptr_->twist.twist.linear.x);
+    input_traj_points, ego_state_ptr->pose.pose, ego_state_ptr->twist.twist.linear.x);
   const bool is_replan_required = [&]() {
     if (replan_checker_ptr_->isResetRequired(planner_data)) {
       // NOTE: always replan when resetting previous optimization
@@ -195,15 +194,15 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr)
   replan_checker_ptr_->updateData(planner_data, is_replan_required, now());
   time_keeper_ptr_->tic(__func__);
   auto smoothed_traj_points = is_replan_required ? eb_path_smoother_ptr_->smoothTrajectory(
-                                                     input_traj_points, ego_state_ptr_->pose.pose)
+                                                     input_traj_points, ego_state_ptr->pose.pose)
                                                  : *prev_optimized_traj_points_ptr_;
   time_keeper_ptr_->toc(__func__, "    ");
 
   prev_optimized_traj_points_ptr_ =
     std::make_shared<std::vector<TrajectoryPoint>>(smoothed_traj_points);
 
   // 2. update velocity
-  applyInputVelocity(smoothed_traj_points, input_traj_points, ego_state_ptr_->pose.pose);
+  applyInputVelocity(smoothed_traj_points, input_traj_points, ego_state_ptr->pose.pose);
 
   // 3. extend trajectory to connect the optimized trajectory and the following path smoothly
   auto full_traj_points = extendTrajectory(input_traj_points, smoothed_traj_points);
@@ -230,9 +229,10 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr)
   published_time_publisher_->publish_if_subscribed(path_pub_, path_ptr->header.stamp);
 }
 
-bool ElasticBandSmoother::isDataReady(const Path & path, rclcpp::Clock clock) const
+bool ElasticBandSmoother::isDataReady(
+  const Path & path, const Odometry::ConstSharedPtr ego_state_ptr, rclcpp::Clock clock) const
 {
-  if (!ego_state_ptr_) {
+  if (!ego_state_ptr) {
     RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Waiting for ego pose and twist.");
     return false;
   }