@@ -36,35 +36,76 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt
3636
3737 // Parameter
3838 time_stamp_field_name_ = declare_parameter (" time_stamp_field_name" " time_stamp"
39+ use_imu_ = declare_parameter (" use_imu" true );
3940
4041 // Publisher
4142 undistorted_points_pub_ =
4243 this ->
create_publisher <PointCloud2>(
" ~/output/pointcloud" ,
rclcpp::SensorDataQoS ());
4344
4445 // Subscriber
45- velocity_report_sub_ = this ->create_subscription <VelocityReport>(
46- " ~/input/velocity_report" 10 ,
47- std::bind (&DistortionCorrectorComponent::onVelocityReport, this , std::placeholders::_1));
46+ twist_sub_ = this ->create_subscription <geometry_msgs::msg::TwistWithCovarianceStamped>(
47+ " ~/input/twist" 10 ,
48+ std::bind (
49+ &DistortionCorrectorComponent::onTwistWithCovarianceStamped, this , std::placeholders::_1));
50+ imu_sub_ = this ->create_subscription <sensor_msgs::msg::Imu>(
51+ " ~/input/imu" 10 ,
52+ std::bind (&DistortionCorrectorComponent::onImu, this , std::placeholders::_1));
4853 input_points_sub_ =
this ->
create_subscription <PointCloud2>(
4954 " ~/input/pointcloud" rclcpp::SensorDataQoS (),
5055 std::bind (&DistortionCorrectorComponent::onPointCloud, this , std::placeholders::_1));
5156}
5257
53- void DistortionCorrectorComponent::onVelocityReport (
54- const VelocityReport:: ConstSharedPtr velocity_report_msg )
58+ void DistortionCorrectorComponent::onTwistWithCovarianceStamped (
59+ const geometry_msgs::msg::TwistWithCovarianceStamped:: ConstSharedPtr twist_msg )
5560{
56- velocity_report_queue_.push_back (*velocity_report_msg);
61+ geometry_msgs::msg::TwistStamped msg;
62+ msg.header = twist_msg->header ;
63+ msg.twist = twist_msg->twist .twist ;
64+ twist_queue_.push_back (msg);
5765
58- while (!velocity_report_queue_.empty ()) {
66+ while (!twist_queue_.empty ()) {
67+ // for replay rosbag
68+ if (rclcpp::Time (twist_queue_.front ().header .stamp ) > rclcpp::Time (twist_msg->header .stamp )) {
69+ twist_queue_.pop_front ();
70+ } else if ( // NOLINT
71+ rclcpp::Time (twist_queue_.front ().header .stamp ) <
72+ rclcpp::Time (twist_msg->header .stamp ) - rclcpp::Duration::from_seconds (1.0 )) {
73+ twist_queue_.pop_front ();
74+ }
75+ break ;
76+ }
77+ }
78+
79+ void DistortionCorrectorComponent::onImu (const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg)
80+ {
81+ if (!use_imu_) {
82+ return ;
83+ }
84+
85+ tf2::Transform tf2_imu_link_to_base_link{};
86+ getTransform (base_link_frame_, imu_msg->header .frame_id , &tf2_imu_link_to_base_link);
87+ geometry_msgs::msg::TransformStamped::SharedPtr tf_base2imu_ptr =
88+ std::make_shared<geometry_msgs::msg::TransformStamped>();
89+ tf_base2imu_ptr->transform .rotation = tf2::toMsg (tf2_imu_link_to_base_link.getRotation ());
90+
91+ geometry_msgs::msg::Vector3Stamped angular_velocity;
92+ angular_velocity.vector = imu_msg->angular_velocity ;
93+
94+ geometry_msgs::msg::Vector3Stamped transformed_angular_velocity;
95+ tf2::doTransform (angular_velocity, transformed_angular_velocity, *tf_base2imu_ptr);
96+ transformed_angular_velocity.header = imu_msg->header ;
97+ angular_velocity_queue_.push_back (transformed_angular_velocity);
98+
99+ while (!angular_velocity_queue_.empty ()) {
59100 // for replay rosbag
60101 if (
61- rclcpp::Time (velocity_report_queue_ .front ().header .stamp ) >
62- rclcpp::Time (velocity_report_msg ->header .stamp )) {
63- velocity_report_queue_ .pop_front ();
102+ rclcpp::Time (angular_velocity_queue_ .front ().header .stamp ) >
103+ rclcpp::Time (imu_msg ->header .stamp )) {
104+ angular_velocity_queue_ .pop_front ();
64105 } else if ( // NOLINT
65- rclcpp::Time (velocity_report_queue_ .front ().header .stamp ) <
66- rclcpp::Time (velocity_report_msg ->header .stamp ) - rclcpp::Duration::from_seconds (1.0 )) {
67- velocity_report_queue_ .pop_front ();
106+ rclcpp::Time (angular_velocity_queue_ .front ().header .stamp ) <
107+ rclcpp::Time (imu_msg ->header .stamp ) - rclcpp::Duration::from_seconds (1.0 )) {
108+ angular_velocity_queue_ .pop_front ();
68109 }
69110 break ;
70111 }
@@ -83,11 +124,10 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr points_ms
83124 tf2::Transform tf2_base_link_to_sensor{};
84125 getTransform (points_msg->header .frame_id , base_link_frame_, &tf2_base_link_to_sensor);
85126
86- undistortPointCloud (velocity_report_queue_, tf2_base_link_to_sensor, *points_msg);
127+ undistortPointCloud (tf2_base_link_to_sensor, *points_msg);
128+
129+ undistorted_points_pub_->publish (std::move (points_msg));
87130
88- if (points_sub_count > 0 ) {
89- undistorted_points_pub_->publish (std::move (points_msg));
90- }
91131 // add processing time for debug
92132 if (debug_publisher_) {
93133 const double cyclic_time_ms = stop_watch_ptr_->toc (" cyclic_time" true );
@@ -126,13 +166,12 @@ bool DistortionCorrectorComponent::getTransform(
126166}
127167
128168bool DistortionCorrectorComponent::undistortPointCloud (
129- const std::deque<VelocityReport> & velocity_report_queue_,
130169 const tf2::Transform & tf2_base_link_to_sensor, PointCloud2 & points)
131170{
132- if (points.data .empty () || velocity_report_queue_ .empty ()) {
171+ if (points.data .empty () || twist_queue_ .empty ()) {
133172 RCLCPP_WARN_STREAM_THROTTLE (
134173 get_logger (), *get_clock (), 10000 /* ms */
135- " input_pointcloud->points or velocity_report_queue_ is empty."
174+ " input_pointcloud->points or twist_queue_ is empty."
136175 return false ;
137176 }
138177
@@ -159,34 +198,58 @@ bool DistortionCorrectorComponent::undistortPointCloud(
159198 double prev_time_stamp_sec{*it_time_stamp};
160199 const double first_point_time_stamp_sec{*it_time_stamp};
161200
162- auto velocity_report_it = std::lower_bound (
163- std::begin (velocity_report_queue_ ), std::end (velocity_report_queue_) ,
164- first_point_time_stamp_sec, [](const VelocityReport & x, const double t) {
201+ auto twist_it = std::lower_bound (
202+ std::begin (twist_queue_ ), std::end (twist_queue_), first_point_time_stamp_sec ,
203+ [](const geometry_msgs::msg::TwistStamped & x, const double t) {
165204 return rclcpp::Time (x.header .stamp ).seconds () < t;
166205 });
167- velocity_report_it = velocity_report_it == std::end (velocity_report_queue_)
168- ? std::end (velocity_report_queue_) - 1
169- : velocity_report_it;
206+ twist_it = twist_it == std::end (twist_queue_) ? std::end (twist_queue_) - 1 : twist_it;
207+
208+ decltype (angular_velocity_queue_)::iterator imu_it;
209+ if (use_imu_ && !angular_velocity_queue_.empty ()) {
210+ imu_it = std::lower_bound (
211+ std::begin (angular_velocity_queue_), std::end (angular_velocity_queue_),
212+ first_point_time_stamp_sec, [](const geometry_msgs::msg::Vector3Stamped & x, const double t) {
213+ return rclcpp::Time (x.header .stamp ).seconds () < t;
214+ });
215+ imu_it =
216+ imu_it == std::end (angular_velocity_queue_) ? std::end (angular_velocity_queue_) - 1 : imu_it;
217+ }
170218
171219 const tf2::Transform tf2_base_link_to_sensor_inv{tf2_base_link_to_sensor.inverse ()};
172220 for (; it_x != it_x.end (); ++it_x, ++it_y, ++it_z, ++it_time_stamp) {
173221 for (;
174- (velocity_report_it != std::end (velocity_report_queue_ ) - 1 &&
175- *it_time_stamp > rclcpp::Time (velocity_report_it ->header .stamp ).seconds ());
176- ++velocity_report_it ) {
222+ (twist_it != std::end (twist_queue_ ) - 1 &&
223+ *it_time_stamp > rclcpp::Time (twist_it ->header .stamp ).seconds ());
224+ ++twist_it ) {
177225 }
178226
179- float v{static_cast <float >(velocity_report_it-> longitudinal_velocity )};
180- float w{static_cast <float >(velocity_report_it-> heading_rate )};
227+ float v{static_cast <float >(twist_it-> twist . linear . x )};
228+ float w{static_cast <float >(twist_it-> twist . angular . z )};
181229
182- if (std::abs (*it_time_stamp - rclcpp::Time (velocity_report_it ->header .stamp ).seconds ()) > 0.1 ) {
230+ if (std::abs (*it_time_stamp - rclcpp::Time (twist_it ->header .stamp ).seconds ()) > 0.1 ) {
183231 RCLCPP_WARN_STREAM_THROTTLE (
184232 get_logger (), *get_clock (), 10000 /* ms */
185- " velocity_report time_stamp is too late. Cloud not interpolate."
233+ " twist time_stamp is too late. Could not interpolate."
186234 v = 0 .0f ;
187235 w = 0 .0f ;
188236 }
189237
238+ if (use_imu_ && !angular_velocity_queue_.empty ()) {
239+ for (;
240+ (imu_it != std::end (angular_velocity_queue_) - 1 &&
241+ *it_time_stamp > rclcpp::Time (imu_it->header .stamp ).seconds ());
242+ ++imu_it) {
243+ }
244+ if (std::abs (*it_time_stamp - rclcpp::Time (imu_it->header .stamp ).seconds ()) > 0.1 ) {
245+ RCLCPP_WARN_STREAM_THROTTLE (
246+ get_logger (), *get_clock (), 10000 /* ms */
247+ " imu time_stamp is too late. Could not interpolate."
248+ } else {
249+ w = static_cast <float >(imu_it->vector .z );
250+ }
251+ }
252+
190253 const float time_offset = static_cast <float >(*it_time_stamp - prev_time_stamp_sec);
191254
192255 const tf2::Vector3 sensorTF_point{*it_x, *it_y, *it_z};
![pre-commit-ci[bot]](https://avatars.githubusercontent.com/in/68672?v=4&size=40){kind=avatar}
0 commit comments