Skip to content

Commit 110e4cf

Browse files
interimaddTakahisa.Ishikawavividf
authored
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>
1 parent 40d7a98 commit 110e4cf

File tree

2 files changed

+33
-28
lines changed

2 files changed

+33
-28
lines changed

sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp

+6-5
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
#include "autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp"
1919

2020
#include <autoware/universe_utils/ros/debug_publisher.hpp>
21+
#include <autoware/universe_utils/ros/polling_subscriber.hpp>
2122
#include <autoware/universe_utils/system/stop_watch.hpp>
2223
#include <rclcpp/rclcpp.hpp>
2324

@@ -40,8 +41,11 @@ class DistortionCorrectorComponent : public rclcpp::Node
4041
explicit DistortionCorrectorComponent(const rclcpp::NodeOptions & options);
4142

4243
private:
43-
rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_;
44-
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
44+
autoware::universe_utils::InterProcessPollingSubscriber<
45+
geometry_msgs::msg::TwistWithCovarianceStamped,
46+
autoware::universe_utils::polling_policy::All>::SharedPtr twist_sub_;
47+
autoware::universe_utils::InterProcessPollingSubscriber<
48+
sensor_msgs::msg::Imu, autoware::universe_utils::polling_policy::All>::SharedPtr imu_sub_;
4549
rclcpp::Subscription<PointCloud2>::SharedPtr pointcloud_sub_;
4650

4751
rclcpp::Publisher<PointCloud2>::SharedPtr undistorted_pointcloud_pub_;
@@ -59,9 +63,6 @@ class DistortionCorrectorComponent : public rclcpp::Node
5963
std::unique_ptr<DistortionCorrectorBase> distortion_corrector_;
6064

6165
void pointcloud_callback(PointCloud2::UniquePtr pointcloud_msg);
62-
void twist_callback(
63-
const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg);
64-
void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg);
6566
};
6667

6768
} // namespace autoware::pointcloud_preprocessor

sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp

+27-23
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
#include <memory>
2020
#include <string>
2121
#include <utility>
22+
#include <vector>
2223

2324
namespace autoware::pointcloud_preprocessor
2425
{
@@ -28,8 +29,8 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt
2829
{
2930
// initialize debug tool
3031

31-
using autoware::universe_utils::DebugPublisher;
32-
using autoware::universe_utils::StopWatch;
32+
using universe_utils::DebugPublisher;
33+
using universe_utils::StopWatch;
3334
stop_watch_ptr_ = std::make_unique<StopWatch<std::chrono::milliseconds>>();
3435
debug_publisher_ = std::make_unique<DebugPublisher>(this, "distortion_corrector");
3536
stop_watch_ptr_->tic("cyclic_time");
@@ -52,13 +53,18 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt
5253
"~/output/pointcloud", rclcpp::SensorDataQoS(), pub_options);
5354
}
5455

56+
// Twist queue size needs to be larger than 'twist frequency' / 'pointcloud frequency'.
57+
// To avoid individual tuning, a sufficiently large value is hard-coded.
58+
// With 100, it can handle twist updates up to 1000Hz if the pointcloud is 10Hz.
59+
const uint16_t TWIST_QUEUE_SIZE = 100;
60+
5561
// Subscriber
56-
twist_sub_ = this->create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
57-
"~/input/twist", 10,
58-
std::bind(&DistortionCorrectorComponent::twist_callback, this, std::placeholders::_1));
59-
imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
60-
"~/input/imu", 10,
61-
std::bind(&DistortionCorrectorComponent::imu_callback, this, std::placeholders::_1));
62+
twist_sub_ = universe_utils::InterProcessPollingSubscriber<
63+
geometry_msgs::msg::TwistWithCovarianceStamped, universe_utils::polling_policy::All>::
64+
create_subscription(this, "~/input/twist", rclcpp::QoS(TWIST_QUEUE_SIZE));
65+
imu_sub_ = universe_utils::InterProcessPollingSubscriber<
66+
sensor_msgs::msg::Imu, universe_utils::polling_policy::All>::
67+
create_subscription(this, "~/input/imu", rclcpp::QoS(TWIST_QUEUE_SIZE));
6268
pointcloud_sub_ = this->create_subscription<PointCloud2>(
6369
"~/input/pointcloud", rclcpp::SensorDataQoS(),
6470
std::bind(&DistortionCorrectorComponent::pointcloud_callback, this, std::placeholders::_1));
@@ -72,21 +78,6 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt
7278
}
7379
}
7480

75-
void DistortionCorrectorComponent::twist_callback(
76-
const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg)
77-
{
78-
distortion_corrector_->process_twist_message(twist_msg);
79-
}
80-
81-
void DistortionCorrectorComponent::imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg)
82-
{
83-
if (!use_imu_) {
84-
return;
85-
}
86-
87-
distortion_corrector_->process_imu_message(base_frame_, imu_msg);
88-
}
89-
9081
void DistortionCorrectorComponent::pointcloud_callback(PointCloud2::UniquePtr pointcloud_msg)
9182
{
9283
stop_watch_ptr_->toc("processing_time", true);
@@ -97,6 +88,19 @@ void DistortionCorrectorComponent::pointcloud_callback(PointCloud2::UniquePtr po
9788
return;
9889
}
9990

91+
std::vector<geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr> twist_msgs =
92+
twist_sub_->takeData();
93+
for (const auto & msg : twist_msgs) {
94+
distortion_corrector_->process_twist_message(msg);
95+
}
96+
97+
if (use_imu_) {
98+
std::vector<sensor_msgs::msg::Imu::ConstSharedPtr> imu_msgs = imu_sub_->takeData();
99+
for (const auto & msg : imu_msgs) {
100+
distortion_corrector_->process_imu_message(base_frame_, msg);
101+
}
102+
}
103+
100104
distortion_corrector_->set_pointcloud_transform(base_frame_, pointcloud_msg->header.frame_id);
101105
distortion_corrector_->initialize();
102106

0 commit comments

Comments
 (0)