feat(distortion_corrector_node): replace imu and twist callback with polling subscriber (autowarefoundation#10057)

* fix(distortion_corrector_node): replace imu and twist callback with polling 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 <takahisa.ishikawa@tier4.jp>

* fix(distortion_corrector_node): include vector header for cpplint check

Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp>

---------

Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp>
Co-authored-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp>
Co-authored-by: Yi-Hsiang Fang (Vivid) <146902905+vividf@users.noreply.github.com>

Author: 3 people

sensing/autoware_pointcloud_preprocessor/include/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

sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp

@@ -19,6 +19,7 @@
 #include <memory>
 #include <string>
 #include <utility>
+#include <vector>
 
 namespace autoware::pointcloud_preprocessor
 {
@@ -28,8 +29,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 +53,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 +78,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 +88,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();