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

Signed-off-by: vividf <yihsiang.fang@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,6 +73,7 @@ class DistortionCorrector : public DistortionCorrectorBase
   std::deque<geometry_msgs::msg::TwistStamped> twist_queue_;
   std::deque<geometry_msgs::msg::Vector3Stamped> angular_velocity_queue_;
 
+public:
   explicit DistortionCorrector(rclcpp::Node * node)
   : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_)
   {
@@ -81,12 +83,8 @@ class DistortionCorrector : public DistortionCorrectorBase
 
   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);
+  void getIMUTransformation(const std::string & base_frame, const std::string & imu_frame);
+  void enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg);
 
   bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud);
   void getTwistAndIMUIterator(

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);