fix(distortion_corrector_node): replace imu and twist callback with p…

…olling subscriber Changed to read data in bulk using take to reduce subscription callback overhead. Especially effective when the frequency of imu or twist is high, such as 100Hz. Signed-off-by: Takahisa.Ishikawa <[email protected]>
autowarefoundation · Jan 31, 2025 · 8f5480d · 8f5480d
1 parent fcd1c7b
commit 8f5480d
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 28 deletions.
diff --git a/...clude/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp b/...clude/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp
@@ -18,6 +18,7 @@
 #include "autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp"
 
 #include <autoware/universe_utils/ros/debug_publisher.hpp>
+#include <autoware/universe_utils/ros/polling_subscriber.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -40,8 +41,11 @@ class DistortionCorrectorComponent : public rclcpp::Node
   explicit DistortionCorrectorComponent(const rclcpp::NodeOptions & options);
 
 private:
-  rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_;
-  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
+  autoware::universe_utils::InterProcessPollingSubscriber<
+    geometry_msgs::msg::TwistWithCovarianceStamped,
+    autoware::universe_utils::polling_policy::All>::SharedPtr twist_sub_;
+  autoware::universe_utils::InterProcessPollingSubscriber<
+    sensor_msgs::msg::Imu, autoware::universe_utils::polling_policy::All>::SharedPtr imu_sub_;
   rclcpp::Subscription<PointCloud2>::SharedPtr pointcloud_sub_;
 
   rclcpp::Publisher<PointCloud2>::SharedPtr undistorted_pointcloud_pub_;
@@ -59,9 +63,6 @@ class DistortionCorrectorComponent : public rclcpp::Node
   std::unique_ptr<DistortionCorrectorBase> distortion_corrector_;
 
   void pointcloud_callback(PointCloud2::UniquePtr pointcloud_msg);
-  void twist_callback(
-    const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg);
-  void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg);
 };
 
 }  // namespace autoware::pointcloud_preprocessor

diff --git a/...g/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp b/...g/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp
@@ -28,8 +28,8 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt
 {
   // initialize debug tool
 
-  using autoware::universe_utils::DebugPublisher;
-  using autoware::universe_utils::StopWatch;
+  using universe_utils::DebugPublisher;
+  using universe_utils::StopWatch;
   stop_watch_ptr_ = std::make_unique<StopWatch<std::chrono::milliseconds>>();
   debug_publisher_ = std::make_unique<DebugPublisher>(this, "distortion_corrector");
   stop_watch_ptr_->tic("cyclic_time");
@@ -52,13 +52,18 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt
       "~/output/pointcloud", rclcpp::SensorDataQoS(), pub_options);
   }
 
+  // Twist queue size needs to be larger than 'twist frequency' / 'pointcloud frequency'.
+  // To avoid individual tuning, a sufficiently large value is hard-coded.
+  // With 100, it can handle twist updates up to 1000Hz if the pointcloud is 10Hz.
+  const uint16_t TWIST_QUEUE_SIZE = 100;
+
   // Subscriber
-  twist_sub_ = this->create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
-    "~/input/twist", 10,
-    std::bind(&DistortionCorrectorComponent::twist_callback, this, std::placeholders::_1));
-  imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
-    "~/input/imu", 10,
-    std::bind(&DistortionCorrectorComponent::imu_callback, this, std::placeholders::_1));
+  twist_sub_ = universe_utils::InterProcessPollingSubscriber<
+    geometry_msgs::msg::TwistWithCovarianceStamped, universe_utils::polling_policy::All>::
+    create_subscription(this, "~/input/twist", rclcpp::QoS(TWIST_QUEUE_SIZE));
+  imu_sub_ = universe_utils::InterProcessPollingSubscriber<
+    sensor_msgs::msg::Imu, universe_utils::polling_policy::All>::
+    create_subscription(this, "~/input/imu", rclcpp::QoS(TWIST_QUEUE_SIZE));
   pointcloud_sub_ = this->create_subscription<PointCloud2>(
     "~/input/pointcloud", rclcpp::SensorDataQoS(),
     std::bind(&DistortionCorrectorComponent::pointcloud_callback, this, std::placeholders::_1));
@@ -72,21 +77,6 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt
   }
 }
 
-void DistortionCorrectorComponent::twist_callback(
-  const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg)
-{
-  distortion_corrector_->process_twist_message(twist_msg);
-}
-
-void DistortionCorrectorComponent::imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg)
-{
-  if (!use_imu_) {
-    return;
-  }
-
-  distortion_corrector_->process_imu_message(base_frame_, imu_msg);
-}
-
 void DistortionCorrectorComponent::pointcloud_callback(PointCloud2::UniquePtr pointcloud_msg)
 {
   stop_watch_ptr_->toc("processing_time", true);
@@ -97,6 +87,19 @@ void DistortionCorrectorComponent::pointcloud_callback(PointCloud2::UniquePtr po
     return;
   }
 
+  std::vector<geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr> twist_msgs =
+    twist_sub_->takeData();
+  for (const auto & msg : twist_msgs) {
+    distortion_corrector_->process_twist_message(msg);
+  }
+
+  if (use_imu_) {
+    std::vector<sensor_msgs::msg::Imu::ConstSharedPtr> imu_msgs = imu_sub_->takeData();
+    for (const auto & msg : imu_msgs) {
+      distortion_corrector_->process_imu_message(base_frame_, msg);
+    }
+  }
+
   distortion_corrector_->set_pointcloud_transform(base_frame_, pointcloud_msg->header.frame_id);
   distortion_corrector_->initialize();