From 8f5480dbb136ba2590f6d1e15fe746120ad84ce4 Mon Sep 17 00:00:00 2001 From: "Takahisa.Ishikawa" Date: Fri, 31 Jan 2025 13:47:24 +0900 Subject: [PATCH] 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 --- .../distortion_corrector_node.hpp | 11 +++-- .../distortion_corrector_node.cpp | 49 ++++++++++--------- 2 files changed, 32 insertions(+), 28 deletions(-) diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp index b96774c37f621..0188c1af97ab7 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp +++ b/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 +#include #include #include @@ -40,8 +41,11 @@ class DistortionCorrectorComponent : public rclcpp::Node explicit DistortionCorrectorComponent(const rclcpp::NodeOptions & options); private: - rclcpp::Subscription::SharedPtr twist_sub_; - rclcpp::Subscription::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::SharedPtr pointcloud_sub_; rclcpp::Publisher::SharedPtr undistorted_pointcloud_pub_; @@ -59,9 +63,6 @@ class DistortionCorrectorComponent : public rclcpp::Node std::unique_ptr 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/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp index cc05a6cc2765c..186009c0359b6 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp +++ b/sensing/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>(); debug_publisher_ = std::make_unique(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( - "~/input/twist", 10, - std::bind(&DistortionCorrectorComponent::twist_callback, this, std::placeholders::_1)); - imu_sub_ = this->create_subscription( - "~/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( "~/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 twist_msgs = + twist_sub_->takeData(); + for (const auto & msg : twist_msgs) { + distortion_corrector_->process_twist_message(msg); + } + + if (use_imu_) { + std::vector 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();