feat(autoware_velocity_smoother): use polling subscriber (#7216)

feat(motion_velocity_smoother): use polling subscriber

Signed-off-by: Takayuki Murooka <[email protected]>

planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp

@@ -31,6 +31,7 @@
 #include "tier4_autoware_utils/geometry/geometry.hpp"
 #include "tier4_autoware_utils/math/unit_conversion.hpp"
 #include "tier4_autoware_utils/ros/logger_level_configure.hpp"
+#include "tier4_autoware_utils/ros/polling_subscriber.hpp"
 #include "tier4_autoware_utils/ros/self_pose_listener.hpp"
 #include "tier4_autoware_utils/system/stop_watch.hpp"
 
@@ -86,11 +87,15 @@ class VelocitySmootherNode : public rclcpp::Node
   rclcpp::Publisher<Trajectory>::SharedPtr pub_trajectory_;
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_virtual_wall_;
   rclcpp::Publisher<StopSpeedExceeded>::SharedPtr pub_over_stop_velocity_;
-  rclcpp::Subscription<Odometry>::SharedPtr sub_current_odometry_;
-  rclcpp::Subscription<AccelWithCovarianceStamped>::SharedPtr sub_current_acceleration_;
   rclcpp::Subscription<Trajectory>::SharedPtr sub_current_trajectory_;
-  rclcpp::Subscription<VelocityLimit>::SharedPtr sub_external_velocity_limit_;
-  rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_;
+  tier4_autoware_utils::InterProcessPollingSubscriber<Odometry> sub_current_odometry_{
+    this, "/localization/kinematic_state"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped>
+    sub_current_acceleration_{this, "~/input/acceleration"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<VelocityLimit> sub_external_velocity_limit_{
+    this, "~/input/external_velocity_limit_mps"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<OperationModeState> sub_operation_mode_{
+    this, "~/input/operation_mode_state"};
 
   Odometry::ConstSharedPtr current_odometry_ptr_;  // current odometry
   AccelWithCovarianceStamped::ConstSharedPtr current_acceleration_ptr_;
@@ -180,12 +185,8 @@ class VelocitySmootherNode : public rclcpp::Node
     const std::vector<rclcpp::Parameter> & parameters);
 
   // topic callback
-  void onCurrentOdometry(const Odometry::ConstSharedPtr msg);
-
   void onCurrentTrajectory(const Trajectory::ConstSharedPtr msg);
 
-  void onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg);
-
   void calcExternalVelocityLimit();
 
   // publish methods

planning/autoware_velocity_smoother/src/node.cpp

@@ -58,19 +58,6 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti
   pub_over_stop_velocity_ = create_publisher<StopSpeedExceeded>("~/stop_speed_exceeded", 1);
   sub_current_trajectory_ = create_subscription<Trajectory>(
     "~/input/trajectory", 1, std::bind(&VelocitySmootherNode::onCurrentTrajectory, this, _1));
-  sub_current_odometry_ = create_subscription<Odometry>(
-    "/localization/kinematic_state", 1,
-    std::bind(&VelocitySmootherNode::onCurrentOdometry, this, _1));
-  sub_external_velocity_limit_ = create_subscription<VelocityLimit>(
-    "~/input/external_velocity_limit_mps", 1,
-    std::bind(&VelocitySmootherNode::onExternalVelocityLimit, this, _1));
-  sub_current_acceleration_ = create_subscription<AccelWithCovarianceStamped>(
-    "~/input/acceleration", 1, [this](const AccelWithCovarianceStamped::ConstSharedPtr msg) {
-      current_acceleration_ptr_ = msg;
-    });
-  sub_operation_mode_ = create_subscription<OperationModeState>(
-    "~/input/operation_mode_state", 1,
-    [this](const OperationModeState::ConstSharedPtr msg) { operation_mode_ = *msg; });
 
   // parameter update
   set_param_res_ =
@@ -319,16 +306,6 @@ void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory
     pub_trajectory_, publishing_trajectory.header.stamp);
 }
 
-void VelocitySmootherNode::onCurrentOdometry(const Odometry::ConstSharedPtr msg)
-{
-  current_odometry_ptr_ = msg;
-}
-
-void VelocitySmootherNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg)
-{
-  external_velocity_limit_ptr_ = msg;
-}
-
 void VelocitySmootherNode::calcExternalVelocityLimit()
 {
   if (!external_velocity_limit_ptr_) {
@@ -441,6 +418,15 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr
 
   base_traj_raw_ptr_ = msg;
 
+  // receive data
+  current_odometry_ptr_ = sub_current_odometry_.takeData();
+  current_acceleration_ptr_ = sub_current_acceleration_.takeData();
+  external_velocity_limit_ptr_ = sub_external_velocity_limit_.takeData();
+  const auto operation_mode_ptr = sub_operation_mode_.takeData();
+  if (operation_mode_ptr) {
+    operation_mode_ = *operation_mode_ptr;
+  }
+
   // guard
   if (!checkData()) {
     return;