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..4c0227de7c701 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 @@ -19,6 +19,7 @@ #include #include #include +#include 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>(); debug_publisher_ = std::make_unique(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( - "~/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 +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 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();