fix(pointcloud_preprocessor): fix the bug where the geometry message was not saved correctly. (autowarefoundation#7886)

* fix: fix bug that geometry message didn't save correctly

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

* chore: change some functions from public to protected

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

---------

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
Co-authored-by: Kenzo Lobos Tsunekawa <kenzo.lobos@tier4.jp>

Author: vividf

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp

@@ -61,7 +61,8 @@ class DistortionCorrectorBase
 template <class T>
 class DistortionCorrector : public DistortionCorrectorBase
 {
-public:
+protected:
+  geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr_;
   bool pointcloud_transform_needed_{false};
   bool pointcloud_transform_exists_{false};
   bool imu_transform_exists_{false};
@@ -72,28 +73,12 @@ class DistortionCorrector : public DistortionCorrectorBase
   std::deque<geometry_msgs::msg::TwistStamped> twist_queue_;
   std::deque<geometry_msgs::msg::Vector3Stamped> angular_velocity_queue_;
 
-  explicit DistortionCorrector(rclcpp::Node * node)
-  : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_)
-  {
-  }
-  void processTwistMessage(
-    const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override;
-
-  void processIMUMessage(
-    const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override;
-  void getIMUTransformation(
-    const std::string & base_frame, const std::string & imu_frame,
-    geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr);
-  void enqueueIMU(
-    const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg,
-    geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr);
-
-  bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud);
+  void getIMUTransformation(const std::string & base_frame, const std::string & imu_frame);
+  void enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg);
   void getTwistAndIMUIterator(
     bool use_imu, double first_point_time_stamp_sec,
     std::deque<geometry_msgs::msg::TwistStamped>::iterator & it_twist,
     std::deque<geometry_msgs::msg::Vector3Stamped>::iterator & it_imu);
-  void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override;
   void warnIfTimestampIsTooLate(bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late);
   void undistortPoint(
     sensor_msgs::PointCloud2Iterator<float> & it_x, sensor_msgs::PointCloud2Iterator<float> & it_y,
@@ -105,6 +90,19 @@ class DistortionCorrector : public DistortionCorrectorBase
     static_cast<T *>(this)->undistortPointImplementation(
       it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid);
   };
+
+public:
+  explicit DistortionCorrector(rclcpp::Node * node)
+  : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_)
+  {
+  }
+  void processTwistMessage(
+    const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override;
+
+  void processIMUMessage(
+    const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override;
+  void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override;
+  bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud);
 };
 
 class DistortionCorrector2D : public DistortionCorrector<DistortionCorrector2D>

sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp

@@ -47,21 +47,20 @@ template <class T>
 void DistortionCorrector<T>::processIMUMessage(
   const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg)
 {
-  geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr =
-    std::make_shared<geometry_msgs::msg::TransformStamped>();
-  getIMUTransformation(base_frame, imu_msg->header.frame_id, geometry_imu_to_base_link_ptr);
-  enqueueIMU(imu_msg, geometry_imu_to_base_link_ptr);
+  getIMUTransformation(base_frame, imu_msg->header.frame_id);
+  enqueueIMU(imu_msg);
 }
 
 template <class T>
 void DistortionCorrector<T>::getIMUTransformation(
-  const std::string & base_frame, const std::string & imu_frame,
-  geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr)
+  const std::string & base_frame, const std::string & imu_frame)
 {
   if (imu_transform_exists_) {
     return;
   }
 
+  geometry_imu_to_base_link_ptr_ = std::make_shared<geometry_msgs::msg::TransformStamped>();
+
   tf2::Transform tf2_imu_to_base_link;
   if (base_frame == imu_frame) {
     tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0));
@@ -83,20 +82,18 @@ void DistortionCorrector<T>::getIMUTransformation(
     }
   }
 
-  geometry_imu_to_base_link_ptr->transform.rotation =
+  geometry_imu_to_base_link_ptr_->transform.rotation =
     tf2::toMsg(tf2_imu_to_base_link.getRotation());
 }
 
 template <class T>
-void DistortionCorrector<T>::enqueueIMU(
-  const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg,
-  geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr)
+void DistortionCorrector<T>::enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg)
 {
   geometry_msgs::msg::Vector3Stamped angular_velocity;
   angular_velocity.vector = imu_msg->angular_velocity;
 
   geometry_msgs::msg::Vector3Stamped transformed_angular_velocity;
-  tf2::doTransform(angular_velocity, transformed_angular_velocity, *geometry_imu_to_base_link_ptr);
+  tf2::doTransform(angular_velocity, transformed_angular_velocity, *geometry_imu_to_base_link_ptr_);
   transformed_angular_velocity.header = imu_msg->header;
   angular_velocity_queue_.push_back(transformed_angular_velocity);