From b7ea57dd773071e679f84e291b9cbc7b96dbffed Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Tue, 6 Aug 2024 15:20:19 +0900
Subject: [PATCH 01/72] feat: add option for updating distance and azimuth
 value

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector_node.param.yaml      |   1 +
 .../distortion_corrector.hpp                  |  18 +-
 .../distortion_corrector_node.hpp             |   1 +
 .../distortion_corrector_node.launch.xml      |   2 +-
 .../distortion_corrector_node.schema.json     |  12 +-
 .../distortion_corrector.cpp                  |  27 +-
 .../distortion_corrector_node.cpp             |   4 +-
 .../test/test_distortion_corrector_node.cpp   | 282 ++++++++++++++++--
 8 files changed, 315 insertions(+), 32 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml
index 3afa4816271cb..f077d8423ecf1 100644
--- a/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml
+++ b/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml
@@ -3,3 +3,4 @@
     base_frame: base_link
     use_imu: true
     use_3d_distortion_correction: false
+    sensor_azimuth_coordinate: hi
diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
index 8226cb6eb774c..3b505992f048c 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -34,6 +34,8 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
 
+#include <opencv2/opencv.hpp>
+
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/buffer_interface.h>
 #include <tf2_ros/transform_listener.h>
@@ -48,8 +50,8 @@ namespace autoware::pointcloud_preprocessor
 class DistortionCorrectorBase
 {
 public:
-  virtual bool pointcloud_transform_exists() = 0;
-  virtual bool pointcloud_transform_needed() = 0;
+  virtual bool pointcloudTransformExists() = 0;
+  virtual bool pointcloudTransformNeeded() = 0;
   virtual std::deque<geometry_msgs::msg::TwistStamped> get_twist_queue() = 0;
   virtual std::deque<geometry_msgs::msg::Vector3Stamped> get_angular_velocity_queue() = 0;
 
@@ -60,7 +62,9 @@ class DistortionCorrectorBase
   virtual void setPointCloudTransform(
     const std::string & base_frame, const std::string & lidar_frame) = 0;
   virtual void initialize() = 0;
-  virtual void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
+  virtual void undistortPointCloud(
+    bool use_imu, std::string sensor_azimuth_coordinate,
+    sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
 };
 
 template <class T>
@@ -101,8 +105,8 @@ class DistortionCorrector : public DistortionCorrectorBase
   : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_)
   {
   }
-  bool pointcloud_transform_exists();
-  bool pointcloud_transform_needed();
+  bool pointcloudTransformExists();
+  bool pointcloudTransformNeeded();
   std::deque<geometry_msgs::msg::TwistStamped> get_twist_queue();
   std::deque<geometry_msgs::msg::Vector3Stamped> get_angular_velocity_queue();
   void processTwistMessage(
@@ -110,7 +114,9 @@ class DistortionCorrector : public DistortionCorrectorBase
 
   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;
+  void undistortPointCloud(
+    bool use_imu, std::string sensor_azimuth_coordinate,
+    sensor_msgs::msg::PointCloud2 & pointcloud) override;
   bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud);
 };
 
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 0d8c9436bda4b..43ef2d0dc5d96 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
@@ -52,6 +52,7 @@ class DistortionCorrectorComponent : public rclcpp::Node
   std::string base_frame_;
   bool use_imu_;
   bool use_3d_distortion_correction_;
+  std::string sensor_azimuth_coordinate_;
 
   std::unique_ptr<DistortionCorrectorBase> distortion_corrector_;
 
diff --git a/sensing/autoware_pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml
index bde9ca9e59ddd..62edeb6373232 100644
--- a/sensing/autoware_pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml
+++ b/sensing/autoware_pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml
@@ -1,5 +1,5 @@
 <launch>
-  <arg name="input/pointcloud" default="/sensing/lidar/top/mirror_cropped/pointcloud_ex"/>
+  <arg name="input/pointcloud" default="/sensing/lidar/top/pointcloud_raw_ex"/>
   <arg name="input/twist" default="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
   <arg name="input/imu" default="/sensing/imu/imu_data"/>
   <arg name="output/pointcloud" default="/sensing/lidar/top/rectified/pointcloud_ex"/>
diff --git a/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json
index acf67fd2746c4..6964478625981 100644
--- a/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json
+++ b/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json
@@ -20,9 +20,19 @@
           "type": "boolean",
           "description": "Use 3d distortion correction algorithm, otherwise, use 2d distortion correction algorithm.",
           "default": "false"
+        },
+        "sensor_azimuth_coordinate": {
+          "type": "string",
+          "description": "The azimuth coordinate of sensor. Currently support Hesai and Velodyne. ",
+          "default": ""
         }
       },
-      "required": ["base_frame", "use_imu", "use_3d_distortion_correction"]
+      "required": [
+        "base_frame",
+        "use_imu",
+        "use_3d_distortion_correction",
+        "sensor_azimuth_coordinate"
+      ]
     }
   },
   "properties": {
diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index 652edd9126358..f17d2b1f3bf7f 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -23,13 +23,13 @@ namespace autoware::pointcloud_preprocessor
 {
 
 template <class T>
-bool DistortionCorrector<T>::pointcloud_transform_exists()
+bool DistortionCorrector<T>::pointcloudTransformExists()
 {
   return pointcloud_transform_exists_;
 }
 
 template <class T>
-bool DistortionCorrector<T>::pointcloud_transform_needed()
+bool DistortionCorrector<T>::pointcloudTransformNeeded()
 {
   return pointcloud_transform_needed_;
 }
@@ -203,13 +203,15 @@ bool DistortionCorrector<T>::isInputValid(sensor_msgs::msg::PointCloud2 & pointc
 
 template <class T>
 void DistortionCorrector<T>::undistortPointCloud(
-  bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud)
+  bool use_imu, std::string sensor_azimuth_coordinate, sensor_msgs::msg::PointCloud2 & pointcloud)
 {
   if (!isInputValid(pointcloud)) return;
 
   sensor_msgs::PointCloud2Iterator<float> it_x(pointcloud, "x");
   sensor_msgs::PointCloud2Iterator<float> it_y(pointcloud, "y");
   sensor_msgs::PointCloud2Iterator<float> it_z(pointcloud, "z");
+  sensor_msgs::PointCloud2Iterator<float> it_azimuth(pointcloud, "azimuth");
+  sensor_msgs::PointCloud2Iterator<float> it_distance(pointcloud, "distance");
   sensor_msgs::PointCloud2ConstIterator<std::uint32_t> it_time_stamp(pointcloud, "time_stamp");
 
   double prev_time_stamp_sec{
@@ -272,6 +274,25 @@ void DistortionCorrector<T>::undistortPointCloud(
     // Undistort a single point based on the strategy
     undistortPoint(it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid);
 
+    if (!sensor_azimuth_coordinate.empty() && pointcloudTransformNeeded()) {
+      // Input frame should be in the sensor frame.
+      *it_distance = sqrt(*it_x * *it_x + *it_y * *it_y + *it_z * *it_z);
+      float cartesian_coordinate_azimuth = cv::fastAtan2(*it_y, *it_x);
+      if (sensor_azimuth_coordinate == "velodyne") {
+        *it_azimuth = (360 - cartesian_coordinate_azimuth) * M_PI / 180;
+      } else if (sensor_azimuth_coordinate == "hesai") {
+        *it_azimuth = (90 - cartesian_coordinate_azimuth) < 0
+                        ? (90 - cartesian_coordinate_azimuth + 360) * M_PI / 180
+                        : (90 - cartesian_coordinate_azimuth) * M_PI / 180;
+      } else {
+        throw std::runtime_error(
+          sensor_azimuth_coordinate + " azimuth coordinate is not supported");
+      }
+
+      ++it_azimuth;
+      ++it_distance;
+    }
+
     prev_time_stamp_sec = global_point_stamp;
   }
 
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 822bb2ba5add5..58c3ca330a73b 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
@@ -35,6 +35,7 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt
   base_frame_ = declare_parameter<std::string>("base_frame");
   use_imu_ = declare_parameter<bool>("use_imu");
   use_3d_distortion_correction_ = declare_parameter<bool>("use_3d_distortion_correction");
+  sensor_azimuth_coordinate_ = declare_parameter<std::string>("sensor_azimuth_coordinate");
 
   // Publisher
   {
@@ -91,9 +92,8 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointclou
   }
 
   distortion_corrector_->setPointCloudTransform(base_frame_, pointcloud_msg->header.frame_id);
-
   distortion_corrector_->initialize();
-  distortion_corrector_->undistortPointCloud(use_imu_, *pointcloud_msg);
+  distortion_corrector_->undistortPointCloud(use_imu_, sensor_azimuth_coordinate_, *pointcloud_msg);
 
   if (debug_publisher_) {
     auto pipeline_latency_ms =
diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index f90314a001105..19d436f3f856e 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -211,16 +211,22 @@ class DistortionCorrectorTest : public ::testing::Test
       sensor_msgs::PointCloud2Iterator<float> iter_x(pointcloud_msg, "x");
       sensor_msgs::PointCloud2Iterator<float> iter_y(pointcloud_msg, "y");
       sensor_msgs::PointCloud2Iterator<float> iter_z(pointcloud_msg, "z");
+      sensor_msgs::PointCloud2Iterator<float> iter_azimuth(pointcloud_msg, "azimuth");
+      sensor_msgs::PointCloud2Iterator<float> iter_distance(pointcloud_msg, "distance");
       sensor_msgs::PointCloud2Iterator<std::uint32_t> iter_t(pointcloud_msg, "time_stamp");
 
       for (size_t i = 0; i < number_of_points_; ++i) {
         *iter_x = points[i].x();
         *iter_y = points[i].y();
         *iter_z = points[i].z();
+        *iter_azimuth = std::atan2(points[i].y(), points[i].x());
+        *iter_distance = points[i].norm();
         *iter_t = timestamps[i];
         ++iter_x;
         ++iter_y;
         ++iter_z;
+        ++iter_azimuth;
+        ++iter_distance;
         ++iter_t;
       }
     } else {
@@ -277,7 +283,7 @@ class DistortionCorrectorTest : public ::testing::Test
   static constexpr int imu_msgs_interval_ms_{27};
 
   // for debugging or regenerating the ground truth point cloud
-  bool debug_{false};
+  bool debug_{true};
 };
 
 TEST_F(DistortionCorrectorTest, TestProcessTwistMessage)
@@ -329,22 +335,22 @@ TEST_F(DistortionCorrectorTest, TestIsInputValid)
 TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithBaseLink)
 {
   distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
-  EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_exists());
-  EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_needed());
+  EXPECT_TRUE(distortion_corrector_2d_->pointcloudTransformExists());
+  EXPECT_FALSE(distortion_corrector_2d_->pointcloudTransformNeeded());
 }
 
 TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithLidarFrame)
 {
   distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
-  EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_exists());
-  EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_needed());
+  EXPECT_TRUE(distortion_corrector_2d_->pointcloudTransformExists());
+  EXPECT_TRUE(distortion_corrector_2d_->pointcloudTransformNeeded());
 }
 
 TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithMissingFrame)
 {
   distortion_corrector_2d_->setPointCloudTransform("base_link", "missing_lidar_frame");
-  EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_exists());
-  EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_needed());
+  EXPECT_FALSE(distortion_corrector_2d_->pointcloudTransformExists());
+  EXPECT_FALSE(distortion_corrector_2d_->pointcloudTransformNeeded());
 }
 
 TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyTwist)
@@ -355,7 +361,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyTwist)
 
   // Process empty twist queue
   distortion_corrector_2d_->initialize();
-  distortion_corrector_2d_->undistortPointCloud(false, pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(false, "", pointcloud);
 
   // Verify the point cloud is not changed
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
@@ -398,7 +404,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyPointCloud)
 
   // Process empty point cloud
   distortion_corrector_2d_->initialize();
-  distortion_corrector_2d_->undistortPointCloud(true, empty_pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(true, "", empty_pointcloud);
 
   // Verify the point cloud is still empty
   EXPECT_EQ(empty_pointcloud.width, 0);
@@ -420,7 +426,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink)
   // Test using only twist
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_2d_->undistortPointCloud(false, pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(false, "", pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
@@ -471,7 +477,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink)
 
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_2d_->undistortPointCloud(true, pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(true, "", pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
@@ -522,7 +528,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame)
 
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
-  distortion_corrector_2d_->undistortPointCloud(true, pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(true, "", pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
@@ -573,7 +579,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink)
   // Test using only twist
   distortion_corrector_3d_->initialize();
   distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_3d_->undistortPointCloud(false, pointcloud);
+  distortion_corrector_3d_->undistortPointCloud(false, "", pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
@@ -624,7 +630,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink)
 
   distortion_corrector_3d_->initialize();
   distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_3d_->undistortPointCloud(true, pointcloud);
+  distortion_corrector_3d_->undistortPointCloud(true, "", pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
@@ -679,7 +685,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame)
 
   distortion_corrector_3d_->initialize();
   distortion_corrector_3d_->setPointCloudTransform("base_link", "lidar_top");
-  distortion_corrector_3d_->undistortPointCloud(true, pointcloud);
+  distortion_corrector_3d_->undistortPointCloud(true, "", pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
@@ -726,12 +732,12 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion)
   distortion_corrector_2d_->processTwistMessage(twist_msg);
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_2d_->undistortPointCloud(false, test2d_pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(false, "", test2d_pointcloud);
 
   distortion_corrector_3d_->processTwistMessage(twist_msg);
   distortion_corrector_3d_->initialize();
   distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_3d_->undistortPointCloud(false, test3d_pointcloud);
+  distortion_corrector_3d_->undistortPointCloud(false, "", test3d_pointcloud);
 
   // Generate expected point cloud for testing
   sensor_msgs::msg::PointCloud2 expected_pointcloud_msg =
@@ -813,12 +819,12 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion)
   distortion_corrector_2d_->processTwistMessage(twist_msg);
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_2d_->undistortPointCloud(false, test2d_pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(false, "", test2d_pointcloud);
 
   distortion_corrector_3d_->processTwistMessage(twist_msg);
   distortion_corrector_3d_->initialize();
   distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_3d_->undistortPointCloud(false, test3d_pointcloud);
+  distortion_corrector_3d_->undistortPointCloud(false, "", test3d_pointcloud);
 
   // Generate expected point cloud for testing
   sensor_msgs::msg::PointCloud2 expected_pointcloud_msg =
@@ -901,6 +907,244 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion)
   }
 }
 
+TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdateAzimuthAndDistance)
+{
+  // Test the case when the cloud will not update the azimuth and distance values
+  // 1. when pointcloud is in the base_link
+  // 2. when user didn't specify the sensor azimuth coordinate
+
+  // Generate the point cloud message in base_link
+  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  sensor_msgs::msg::PointCloud2 pointcloud_base_link =
+    generatePointCloudMsg(true, false, timestamp);
+
+  // Generate and process multiple twist messages
+  auto twist_msgs = generateTwistMsgs(timestamp);
+  for (const auto & twist_msg : twist_msgs) {
+    distortion_corrector_2d_->processTwistMessage(twist_msg);
+  }
+
+  // Generate and process multiple IMU messages
+  auto imu_msgs = generateImuMsgs(timestamp);
+  for (const auto & imu_msg : imu_msgs) {
+    distortion_corrector_2d_->processIMUMessage("base_link", imu_msg);
+  }
+
+  distortion_corrector_2d_->initialize();
+  distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
+  // set the azimuth coordinate system
+  distortion_corrector_2d_->undistortPointCloud(true, "velodyne", pointcloud_base_link);
+
+  sensor_msgs::msg::PointCloud2 original_pointcloud_base_link =
+    generatePointCloudMsg(true, false, timestamp);
+
+  sensor_msgs::PointCloud2ConstIterator<float> test_iter_azimuth_base_link(
+    pointcloud_base_link, "azimuth");
+  sensor_msgs::PointCloud2ConstIterator<float> test_iter_distance_base_link(
+    pointcloud_base_link, "distance");
+
+  sensor_msgs::PointCloud2ConstIterator<float> original_iter_azimuth_base_link(
+    original_pointcloud_base_link, "azimuth");
+  sensor_msgs::PointCloud2ConstIterator<float> original_iter_distance_base_link(
+    original_pointcloud_base_link, "distance");
+
+  size_t i = 0;
+  std::ostringstream oss;
+
+  oss << "Expected pointcloud:\n";
+  for (; test_iter_azimuth_base_link != test_iter_azimuth_base_link.end();
+       ++test_iter_azimuth_base_link, ++test_iter_distance_base_link,
+       ++original_iter_azimuth_base_link, ++original_iter_distance_base_link, ++i) {
+    oss << "Point " << i << " - Output azimuth and distance: (" << *test_iter_azimuth_base_link
+        << ", " << *test_iter_distance_base_link << ")"
+        << " vs Original azimuth and distance: (" << *original_iter_azimuth_base_link << ", "
+        << *original_iter_distance_base_link << ")\n";
+
+    EXPECT_FLOAT_EQ(*test_iter_azimuth_base_link, *original_iter_azimuth_base_link);
+    EXPECT_FLOAT_EQ(*test_iter_distance_base_link, *original_iter_distance_base_link);
+  }
+
+  if (debug_) {
+    RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str());
+  }
+
+  // Generate the point cloud message in sensor frame
+  sensor_msgs::msg::PointCloud2 pointcloud_lidar_top = generatePointCloudMsg(true, true, timestamp);
+
+  // Generate and process multiple twist messages
+  twist_msgs = generateTwistMsgs(timestamp);
+  for (const auto & twist_msg : twist_msgs) {
+    distortion_corrector_2d_->processTwistMessage(twist_msg);
+  }
+
+  // Generate and process multiple IMU messages
+  imu_msgs = generateImuMsgs(timestamp);
+  for (const auto & imu_msg : imu_msgs) {
+    distortion_corrector_2d_->processIMUMessage("base_link", imu_msg);
+  }
+
+  distortion_corrector_2d_->initialize();
+  distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
+  // set the empty coordinate system
+  distortion_corrector_2d_->undistortPointCloud(true, "", pointcloud_lidar_top);
+
+  sensor_msgs::msg::PointCloud2 original_pointcloud_lidar_top =
+    generatePointCloudMsg(true, true, timestamp);
+
+  sensor_msgs::PointCloud2ConstIterator<float> test_iter_azimuth_lidar_top(
+    pointcloud_lidar_top, "azimuth");
+  sensor_msgs::PointCloud2ConstIterator<float> test_iter_distance_lidar_top(
+    pointcloud_lidar_top, "distance");
+
+  sensor_msgs::PointCloud2ConstIterator<float> original_iter_azimuth_lidar_top(
+    original_pointcloud_lidar_top, "azimuth");
+  sensor_msgs::PointCloud2ConstIterator<float> original_iter_distance_lidar_top(
+    original_pointcloud_lidar_top, "distance");
+
+  i = 0;
+  oss.str("");
+  oss.clear();
+
+  oss << "Expected pointcloud:\n";
+  for (; test_iter_azimuth_lidar_top != test_iter_azimuth_lidar_top.end();
+       ++test_iter_azimuth_lidar_top, ++test_iter_distance_lidar_top,
+       ++original_iter_azimuth_lidar_top, ++original_iter_distance_lidar_top, ++i) {
+    oss << "Point " << i << " - Output azimuth and distance: (" << *test_iter_azimuth_lidar_top
+        << ", " << *test_iter_distance_lidar_top << ")"
+        << " vs Original azimuth and distance: (" << *original_iter_azimuth_lidar_top << ", "
+        << *original_iter_distance_lidar_top << ")\n";
+
+    EXPECT_FLOAT_EQ(*test_iter_azimuth_lidar_top, *original_iter_azimuth_lidar_top);
+    EXPECT_FLOAT_EQ(*test_iter_distance_lidar_top, *original_iter_distance_lidar_top);
+  }
+
+  if (debug_) {
+    RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str());
+  }
+}
+
+TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceInVelodyne)
+{
+  // Generate the point cloud message in sensor frame
+  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, timestamp);
+
+  // Generate and process multiple twist messages
+  auto twist_msgs = generateTwistMsgs(timestamp);
+  for (const auto & twist_msg : twist_msgs) {
+    distortion_corrector_2d_->processTwistMessage(twist_msg);
+  }
+
+  // Generate and process multiple IMU messages
+  auto imu_msgs = generateImuMsgs(timestamp);
+  for (const auto & imu_msg : imu_msgs) {
+    distortion_corrector_2d_->processIMUMessage("base_link", imu_msg);
+  }
+
+  distortion_corrector_2d_->initialize();
+  distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
+  // set the azimuth coordinate system
+  distortion_corrector_2d_->undistortPointCloud(true, "velodyne", pointcloud);
+
+  sensor_msgs::msg::PointCloud2 original_pointcloud = generatePointCloudMsg(true, false, timestamp);
+
+  sensor_msgs::PointCloud2ConstIterator<float> iter_azimuth(pointcloud, "azimuth");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_distance(pointcloud, "distance");
+
+  // Expected undistorted azimuth and distance values
+  std::array<std::array<float, 2>, 10> expected_distance_azimuth = {{
+    {0.0f, 10.0f},         // points: (10.0f, -1.77636e-15f, -4.44089e-16f)
+    {4.71736f, 10.0614f},  // points: (0.049989f, 10.0608f, 0.0924992f)
+    {5.39603f, 10.2f},     // points: (0.106107f, 0.130237f, 10.1986f)
+    {6.27278f, 20.1745f},  // points: (20.1709f, 0.210011f, 0.32034f)
+    {4.72327f, 20.2789f},  // points: (0.220674f, 20.2734f, 0.417974f)
+    {5.38091f, 20.5389f},  // points: (0.274146f, 0.347043f, 20.5341f)
+    {6.26812f, 30.3789f},  // points: (30.3673f, 0.457564f, 0.700818f)
+    {4.72608f, 30.5395f},  // points: (0.418014f, 30.5259f, 0.807963f)
+    {5.3706f, 30.9385f},   // points: (0.464088f, 0.600081f, 30.9292f)
+    {5.49086f, 18.6938f}   // points: (10.5657f, 10.7121f, 11.094f)
+  }};
+
+  size_t i = 0;
+  std::ostringstream oss;
+
+  oss << "Expected pointcloud:\n";
+  for (; iter_azimuth != iter_azimuth.end(); ++iter_azimuth, ++iter_distance, ++i) {
+    oss << "Point " << i << " - Output azimuth and distance: (" << *iter_azimuth << ", "
+        << *iter_distance << ")"
+        << " vs Original azimuth and distance: (" << expected_distance_azimuth[i][0] << ", "
+        << expected_distance_azimuth[i][1] << ")\n";
+
+    EXPECT_NEAR(*iter_azimuth, expected_distance_azimuth[i][0], standard_tolerance_);
+    EXPECT_NEAR(*iter_distance, expected_distance_azimuth[i][1], standard_tolerance_);
+  }
+
+  if (debug_) {
+    RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str());
+  }
+}
+
+TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceInHesai)
+{
+  // Generate the point cloud message in sensor frame
+  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, timestamp);
+
+  // Generate and process multiple twist messages
+  auto twist_msgs = generateTwistMsgs(timestamp);
+  for (const auto & twist_msg : twist_msgs) {
+    distortion_corrector_2d_->processTwistMessage(twist_msg);
+  }
+
+  // Generate and process multiple IMU messages
+  auto imu_msgs = generateImuMsgs(timestamp);
+  for (const auto & imu_msg : imu_msgs) {
+    distortion_corrector_2d_->processIMUMessage("base_link", imu_msg);
+  }
+
+  distortion_corrector_2d_->initialize();
+  distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
+  // set the azimuth coordinate system
+  distortion_corrector_2d_->undistortPointCloud(true, "hesai", pointcloud);
+
+  sensor_msgs::msg::PointCloud2 original_pointcloud = generatePointCloudMsg(true, false, timestamp);
+
+  sensor_msgs::PointCloud2ConstIterator<float> iter_azimuth(pointcloud, "azimuth");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_distance(pointcloud, "distance");
+
+  // Expected undistorted azimuth and distance values
+  std::array<std::array<float, 2>, 10> expected_distance_azimuth = {{
+    {1.5708f, 10.0f},         // points: (10.0f, -1.77636e-15f, -4.44089e-16f)
+    {0.00496759f, 10.0614f},  // points: (0.049989f, 10.0608f, 0.0924992f)
+    {0.683643f, 10.2f},       // points: (0.106107f, 0.130237f, 10.1986f)
+    {1.56039f, 20.1745f},     // points: (20.1709f, 0.210011f, 0.32034f)
+    {0.0108822f, 20.2789f},   // points: (0.220674f, 20.2734f, 0.417974f)
+    {0.668525f, 20.5389f},    // points: (0.274146f, 0.347043f, 20.5341f)
+    {1.55573f, 30.3789f},     // points: (30.3673f, 0.457564f, 0.700818f)
+    {0.01369f, 30.5395f},     // points: (0.418014f, 30.5259f, 0.807963f)
+    {0.658213f, 30.9385f},    // points: (0.464088f, 0.600081f, 30.9292f)
+    {0.778473f, 18.6938f}     // points: (10.5657f, 10.7121f, 11.094f)
+  }};
+
+  size_t i = 0;
+  std::ostringstream oss;
+
+  oss << "Expected pointcloud:\n";
+  for (; iter_azimuth != iter_azimuth.end(); ++iter_azimuth, ++iter_distance, ++i) {
+    oss << "Point " << i << " - Output azimuth and distance: (" << *iter_azimuth << ", "
+        << *iter_distance << ")"
+        << " vs Original azimuth and distance: (" << expected_distance_azimuth[i][0] << ", "
+        << expected_distance_azimuth[i][1] << ")\n";
+
+    EXPECT_NEAR(*iter_azimuth, expected_distance_azimuth[i][0], standard_tolerance_);
+    EXPECT_NEAR(*iter_distance, expected_distance_azimuth[i][1], standard_tolerance_);
+  }
+
+  if (debug_) {
+    RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str());
+  }
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

From 1bd37c4b9e304c53ba4ec944848bf9a5b3209abb Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Tue, 6 Aug 2024 15:49:17 +0900
Subject: [PATCH 02/72] chore: clean code

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../config/distortion_corrector_node.param.yaml                 | 2 +-
 .../launch/distortion_corrector_node.launch.xml                 | 2 +-
 .../test/test_distortion_corrector_node.cpp                     | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml
index f077d8423ecf1..8c160ecdae23d 100644
--- a/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml
+++ b/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml
@@ -3,4 +3,4 @@
     base_frame: base_link
     use_imu: true
     use_3d_distortion_correction: false
-    sensor_azimuth_coordinate: hi
+    sensor_azimuth_coordinate: ""
diff --git a/sensing/autoware_pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml
index 62edeb6373232..bde9ca9e59ddd 100644
--- a/sensing/autoware_pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml
+++ b/sensing/autoware_pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml
@@ -1,5 +1,5 @@
 <launch>
-  <arg name="input/pointcloud" default="/sensing/lidar/top/pointcloud_raw_ex"/>
+  <arg name="input/pointcloud" default="/sensing/lidar/top/mirror_cropped/pointcloud_ex"/>
   <arg name="input/twist" default="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
   <arg name="input/imu" default="/sensing/imu/imu_data"/>
   <arg name="output/pointcloud" default="/sensing/lidar/top/rectified/pointcloud_ex"/>
diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index 19d436f3f856e..0c496b0273153 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -283,7 +283,7 @@ class DistortionCorrectorTest : public ::testing::Test
   static constexpr int imu_msgs_interval_ms_{27};
 
   // for debugging or regenerating the ground truth point cloud
-  bool debug_{true};
+  bool debug_{false};
 };
 
 TEST_F(DistortionCorrectorTest, TestProcessTwistMessage)

From 336f709820d89716bd155593a98196a63eea6dc7 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Tue, 6 Aug 2024 15:50:59 +0900
Subject: [PATCH 03/72] chore: remove space

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../schema/distortion_corrector_node.schema.json                | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json
index 6964478625981..2e17ef6b66193 100644
--- a/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json
+++ b/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json
@@ -23,7 +23,7 @@
         },
         "sensor_azimuth_coordinate": {
           "type": "string",
-          "description": "The azimuth coordinate of sensor. Currently support Hesai and Velodyne. ",
+          "description": "The azimuth coordinate of sensor. Currently support Hesai and Velodyne.",
           "default": ""
         }
       },

From c8378ac55ab543c6bd0b4acb449fc6d4e3983b42 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Tue, 6 Aug 2024 16:19:58 +0900
Subject: [PATCH 04/72] chore: add documentation

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../docs/distortion-corrector.md               |  17 +++++++++++++++++
 .../docs/image/hesai.drawio.png                | Bin 0 -> 12136 bytes
 .../docs/image/velodyne.drawio.png             | Bin 0 -> 12192 bytes
 3 files changed, 17 insertions(+)
 create mode 100644 sensing/autoware_pointcloud_preprocessor/docs/image/hesai.drawio.png
 create mode 100644 sensing/autoware_pointcloud_preprocessor/docs/image/velodyne.drawio.png

diff --git a/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md
index ab5a07b5279bc..eaccf23a2a444 100644
--- a/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md
+++ b/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md
@@ -48,3 +48,20 @@ ros2 launch autoware_pointcloud_preprocessor distortion_corrector.launch.xml
 
 - The node requires time synchronization between the topics from lidars, twist, and IMU.
 - If you want to use a 3D distortion corrector without IMU, please check that the linear and angular velocity fields of your twist message are not empty.
+- The node updates the per-point azimuth and distance values based on the undistorted XYZ coordinates when the input point cloud is in the sensor frame and the `sensor_azimuth_coordinate` parameter is set to either `velodyne` or `hesai`. The azimuth values are calculated using the `cv::fastAtan2` function. Please note that updating the azimuth and distance fields increases the execution time by approximately 13%. Additionally, due to the `cv::fastAtan2` algorithm's accuracy of about 0.3 degrees, there is a **possibility of changing beam order for high azimuth resolution LiDAR**.
+- Currently, `sensor_azimuth_coordinate` only support `velodyne` and `hesai`. The `velodyne` and `hesai` coordinate are illustrated below. If a LiDAR uses the same coordinates as velodyne or hesai, users can use them as well.
+
+<table>
+  <tr>
+    <td><img src="./image/velodyne.drawio.png" alt="velodyne azimuth coordinate"></td>
+    <td><img src="./image/hesai.drawio.png" alt="hesai azimuth coordinate"></td>
+   </tr>
+   <tr>
+    <td><p style="text-align: center;">Velodyne azimuth coordinate.</p></td>
+    <td><p style="text-align: center;">Hesai azimuth coordinate</p></td>
+  </tr>
+</table>
+
+## References/External links
+
+<https://docs.opencv.org/3.4/db/de0/group__core__utils.html#ga7b356498dd314380a0c386b059852270>
diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/hesai.drawio.png b/sensing/autoware_pointcloud_preprocessor/docs/image/hesai.drawio.png
new file mode 100644
index 0000000000000000000000000000000000000000..d406eb3896850425576f44e1b449fabbd671bbe4
GIT binary patch
literal 12136
zcmeHt2|U#8+JB@7$r1|5p2#vY_ASOTWGq>;H_Tu%%ot<Mz9ke{N4AnI6<Wx?ltD#G
zA$zuxHESsG{w6&2JnwU!_x#WKob!Ip=lzdmeskUTecji6U)%S(xNN8oJ4nkxyJydy
zgF4z8M!@?=;HQyhKkz$+&Q%3?qwp|-LH885b4~8q!}5Zld5+-f<A}zf_6S1Mx1R*T
zlFnEUf*?dg5DZ4(aN<Zb!U>OX^$>SM5dab}j&MS`?M%?a`k^rxgdkW;T3iws5<Uk4
z2|}R26CCa4=>xp`$jR8rNSy?p)NnWq${b~{iw5{;Nke4CAu{A8y4ogsa6zy-@Qgvb
zpnx|R%E1LoUZU=d$GQR(Ef82nT$20+5RDOz2t4{17G#;cP<RhC)@@sCu#~v0IKZ1c
z;NgJ4pti}NZHhe>k3`|ONdP~vAVgD8QVp0&{)VXUoEP}uh5+{daSw*Tk;QhFqmkP*
zzq6)rj2=i1p=Rfe^|RN;fE*qF$ZeOWs}Bs1z&Yz-kthtn80oVk5J-MUN~G^LMOIRJ
zTSzDT?mj!)k_pCWzulw!5F9M&<cUV2Ja)KkuOwix7y=sid!hr@%?;(SqqH542s|F^
z{rhQ-Sj^7pcIM!Kga3{@nPB|iu9QqQM<WT&+jHflcf=#pwNYp%=iQw_k~<@=h~3FM
z1P^Bf66?La{s-w-t!&@p?rq|+Sb*OzO8a$qcap1*ItoL+?A?3Z*$1$j|M#`O-3I<Q
z%l~KC`KrljyLmgS>v$L;WgWci)xAx<#dZ|$g}`|3T=}*>JqW(L1`HesP%JPC^>#)R
zP{uff19_@9U|Rsqnc#{6K9T(o;ep%Mwj<gH1!xos_&7!#i@|Q2J;YI({6(f?-3Y(X
zAwOOK-GhL~x}bg;k@@igMz)2*A%5P(&gB3eNI>lxHhI=xYv`w*U?{-H33y*XS3Aog
zayyr?;}sC{WU@1R|FkjLohi=0nAa{1v18Rvzi}on{(CeJ%-J!Vzm}dHE&pI&f2!0S
z_x?+zx*^rbo(QPU0fX@HKsx}p{EIH}SWh=3S&14zbo!$rk<utRq|~1&Ob%=hk&*dr
zWY;?Zwft=rqL9Dy_P@S@KULr_3fheUzba^#hDTu#1hm)hL(89O?e7ucryzz{G?18n
zGb1VRAIwNvUVMkLyO#&n6Yqf9S@-uD$@X@Cb5qFwyeR>JcR~^VflVPmjs)!UH;4J_
z+u51@!%Y9DnSY-(%Wiw&zs#B)<s48BWb^-T;m2N1S_&|p-I4#B)-1Js{kuU?W+y2A
z>ugyH{QE7zyZ*bo<$tUt%l(NZ%U}Rh@gpMwA;jUgWGUnBNroFh)P%^(OG-*11;IZ_
zl0UQaE|Hv4#XNo>H8Q9K$>D&k2CUv42cDeB`~Yygy(8e$<TbnWZOMM}1p`@?JpY%J
zfWV-g$WYe-KqF)%3nk}XG=R#~b_QJ0NF*6FK=CL*5`_H^06}&i963V%$d1xbL1_(u
zg773@x264o9DeV4Zdf-mmtWDtF6sBcK}$;#cmY$lrT^E$11U+_-8jDE3t-SrT>mBi
zKz7{Y*Zd>(2bcNZ2MF^2m?6p{C1rtV{L`lX2}1;c*MG1f{yPi~EWcwnyP<d2j{f7!
z4YYHRUs5<gkwl`L@F<|p{!uXhtT+Cd$n$3v%I~4<zeRJqCBd%d<aRFL7tQVF(O(0_
zZt&PmbpQK-0_dy!P|q)#BU|OaLUsSLr19_2_#Z((n2gl_P&@7#!M|JMKM?uf=w9q%
z`=5QHUorcS(cR9}_Q1{pa`$e#)b^B&E8eq*6RxA7X5wQzYeP*ifu+<!DFtFO9w2ih
zqm;sVt*EXk%cz}Cq{h7AkUTGW{)mLn0q%w#>Tjo1kYRhZs37X+!1U_PJWq+SQM!!k
zV9F56>T;zbrW;z{ySfIuip(c)axj}M)2@Keo2yf%-$m+A7-T7h99C0hl%%CN&6{`j
zYkWHmB`pPS-jPtA^E{ML_}*kb8)m9-qW#{`;1C)b&_R=Ef(|32ee^*Z%KdyiBq_s_
zZv@p;qfW~3&>V>hW_+u9TRW8sV9*o%cQdf4)Gc=#eA*DQ_kr({W6km#U7|{Wf`a$l
zJs$asUrzb0Ohu?)Vz_BiXhc->7N6?6o62jwzWAZ>fkO);2gh}Ujj;#~NUtM2^~>1C
zo!Xw)Zp<8;tEwhkJqn(OFXb0E44tLxaQsdc{&+33@z%xFL2stx$K$Yd>-%AG+?q(F
zz*R2!P;*<~eOEZ7p{*%mpbMSC_iSr9)FbJiRC-LrD_{J={^FfxTT;?tWNXUHkq2_d
zXQL+R{TA;*&+C@I!nw~<)OaqsP_aIbS!<PDaERDaJ(v1!zAr;9#ZbhiN-9O%Cc3Z8
z<?h5#;5Sz(Opl@O%ZI4e(J!<8G&D4Z^<DQYSRjsVsu~(3W7m6j_1t0m4~Ev)pRV$p
z&YOxKUnV9B={+57iW!-hxa9cqdf|Kg__L6uiC4E=-W6B4k45@XDEZ9qDY7bWKEAaE
z8-Jci?a&<iq_fc2eSHySU5VfK{p(6&najI)QFHOO40)E7>38M<_fNcMrdd?ZGp)!C
z^rKO#S`p0=JqXoansR>Vd_+boAi5$TpV4zhbo|*;g5YcIIzio(T;@jx*(%XTG8H_H
zc*Se;iwKi%_UWa|XuhutFy2bZ$cSYYJac|?WBo;S`*msEYs*g~=+@RJvKKmF#}V&}
zZK4g&o;~aQCin7UjX%ZwZAsLN!>n56uKi{aF9xf<VNXK$Jqe5_3J?G-nAr8qZGQjO
zQEa0~YHqe4tn&2wU`44Gg=Q5m#2%us(*9N*r!ew*+_{zao}Sy*=6CncL&~f#xK9UK
z`xeO#`RqNW=tb&z;P3$ZzOGYzd2W!?^sZUceMF<tme(lhKxdH^yZ79{*&tnQZB;`<
zMsb^}@VL{PM?SQsn%|Ucy)Nz2$>lZOT_kK<Blih?GVV4sg7%24TLz!wO=yJ4CwL{h
zgsnzXG^^=xSA<xt;)naMzKmOyaUDPv@d>TW^4(vbMaz8~V@UdZ>~hq)IyfRpZ2ghb
zD{hm!W(R>)>LHZ-4$DuLX)9SxzP=g8=VlqFgMAjCbhEICikf;1P~Z{C^LvC-hY})&
z%A7lL<Z#2mv`+GNWr9(brOqK)$`|vPJyz!ih0TjI?&yZ*=?S>&!(Y|lMxfP;MWMW;
z(r99*0eeVNF7n>J?-ZN9a%>SUDVo*FZ$~CR`{#_Wyt6Z3B7J<Behc|Lp~815Z!uV=
zq#2j!VQ6(T6_wZh5%l?eecV8$hiII9;D(P$vGq~F`2yVNBk0(hAG`J&78(hzSo!!=
zEWVbB(}7-!h$xg7LiLR2vyw>3%Ijt?wE#7N3bVy2skf$KB=}{m@i%9CUc1v?*mPo-
zyjNzavN5lojLMXA=aa$o?1{X)>?Oed{5gr91=P{e5iOT*kgcz`|3MLnq!At-e!e3|
z)4A(TGA#)3)gq-&Pr|4do}b7rc%8iH_$sSNNsvV;!??T97#2|txP$4E!RF>EQ1AXk
zeSWRjJrfn~?(RNY-&ais#jGCpBx6~rdGkz3xtks|0}5Ugq8Av{ixzCo2EY$9Y)mSW
z;5JvUQrD|)o)J97s})DmXgj^RaHwWuv6TpM(!bE0xKz!p>{ozG(8<|H$0nShWMx)l
zdAo+s!0|S_0}%)m`ek3_eHW3^>;fTn0V}4-0``j56G)mi`IC>z+G^@nx~*PVd0a&=
z5i!$%cMe+<ZHY^5BaN5tN~Ses`Rnp%FSdwS8*N!oWeMaD=U4H^XT(P_00KRp+0@qe
ziTxxJb`4^8|8acxv1j=<`IvzU(5Kq|QuNr>ylfYBr99aA)l4l16`RO_<6_O}id(Tc
zD<ZiJGcWQQ&%NC+C{Qkh<v}gI`AM&rfe5<xr5~FNL!N8ZU`aH{5{AMJZiu^bTxtM<
zLzjO7lAfNP6n=ok#Cf`qkF<Pv@l7$$JS8~`aR0+uV06~T?}+zawTKv>Z<0!VLA-ge
zk=5Gf#H}EfwX3|JZ;r6r@eK>Q<g>4FbEF-=#t2#Jd7e1QgY)V&%2;2ZPP#G`d%wJj
zMk`H2H7ZSKc>KWx(~a^0l^YVwW1snXglS4%UcWN)q&}04r-=i|TP{92v+O)LnXj-6
ziz<P#NS~K&=`q_((Tv*DVy;1X?_!D4)D%yigeGeOM`NQ%oSNi6pKhh{&E?!g`Ps*B
zttiuX2VU{VFVFKnJ~R`PyS6qFxcus9RI|rNULI|5sTl?qh2AGtm@ZD~lnNVbOZYsS
z!}7(;JZaYX`}yK~^yQ2YQ5U<$o=LGSnfuu<NPVJhZD93Q`TWBzKpItuOzeCZrrKAD
zx>~aypKo$<K#401+I44aXno1Yf3=*Yqshe*!poZ{U6bp@L4`@{!SwDm%u_!M<+mCh
zc=2KQ;Q<yERG&BL_7zUXYWz_n=0wJY6|Obv{j`w`JUk@A9)ZFH70RvZfV+Iw+KGAd
zORMI|6<5*xLXd~23=$IEeOrarCfY8i+PR#<DFg%gn<73pdW<F)25-quxh_N2Ih`dF
zqy1Efaa18S?AoFPK=OmD{rJE~>b$TQ3NOM%OD+)oFsj=`ZsWTLX+UL9Pd+6%Og#Xg
z05_Lrv=6*UhKJNin@__MSJ<*2;DI}Qauih77tClZn!yKG%H!q)c6o9(H>&0TGyOks
zr)|teKsXKW={ZNnDCQ-$O)t}B-6z_Q4b%r6KXBlH_L(!#2<la)O-XL2rH?IFUNb*S
zl{ikzDiRK0A$|b?s@uA$TJ0I~kEm?h>5a&E%wo6~NVB6r-e3EWYKH^RPtqw1Bc^-W
z*Z87wzDpl9i>)gU_fk?);ui8*r|cfcl<<3Nung2*G-q0CzQP&(nqAyl)!AA2X=CHr
z+h<sl#H?iKZa4=-LE+QH^$nJHC3Y=ygVhg$wgNTf5(@m+7o#--0|O<vwjz>SSwXs`
z)4>U|iYn%-&l6AZ^a8m0@>ok;m~DmEY+r$S33oJ`*wj*wf`Te<-YsoZh~EkQjE3$a
zEBB9WM>SJ8q%R16cD#+$Xmdh!&>dIuF|4I}`t<4RTpbpZ0>dX~5>+hBqb3tCl*tJD
z-3PE7pSU;!8La})4ze8k#q$0ccsiHg8=Gn`M9bA<!qyd%eWmEc+l|&h;NbjH*tf8-
zaIO+B$Z%XGmQBp^hFaKu{Wyt7BzXUPd#2(8MB{!kHks`&hv9Jd-+o_|p@71%Z;5%E
zz<JqqQ^hrlEX$I-Ut}p)vprXcdVW<vNJvB?aJYejFgI}2bE>oB&1ozXkSN&n&@&U>
z?*sCgC1?A}jF%KLHQ7O~0i*t_`PqR+2v-2*6TG^dEuipQUAM|T08rQ%x{t2&x_!QW
zCRjPn|7;0?CDGM=^XRc-Mcw)rSLd$SH$_kGYgR>*3z5-iad%()FEhPSv9TH&8sVbX
z4Z${Qa4V;B?E9+#5WQ(vA9!KCIW>myZED%Kk_SyB((rUiy;g~BtqHU#xha{1tWPWW
z`Y|0VdA?zfNr~-Xvid>+hMb7J1I7eCWXOBWw<~_GKn1++A_>@AV>4y3++V-UPCpty
zY+sed$?Xy{GCh$UR17`sE!I~ZMwG#WrH;#Bhz5~mP<UKNbJ;;i2gJ2cN<&k#QpsOW
zk=dK+EnBy^)nj^o*wy9A@OZ|zWgGLv<&(yF1(L6lFJ~?9pU}5|7$6jWq&V&<q}kzI
zbq<0zPt<VbKD^i%ACg74G$%eMB-GM!g?WHyM0Yv^3iq}uj_Dt}ngt~4v9LobxaSHe
zt-)0)P-XjiuT%C-RBMXLBX#Ylv}?D0J3bG5`?}C_{4R=VP|=So;t2~0`MyFus#Q4K
zfAO-xOcATS!UcC*!_WQL`Us%l81i4zi(Bb7CXEN17g<J(k0S}6hXx(S-gVt)G-glW
zPHC(y8PJe)8<2D5d;@_97%(R;O<iU=apxuVP#`#z5d!IU8iv>fuHUx2+qp2A0%=?l
z0QF)6!OaJ0!l(~<IUeB-GK-pYXO(+T!7;k}J}9o^7EDp1P3J(CkWls3WPWxIfZ^Qw
zO-2NelPcmQ!oE9hDe^Aer;=wDO3TOZ>+uNH3MH!TrDh;J5?Yo%LF&A88z!(Ye6P?r
z{|*MJMUASw6(+=(*m|N(KWJ^_lI_KYRK~Z*x~HDXtTBOJ5dy}p$XfSAkL3W2E`Cd`
z-`E!t5;En0!=3`Hz0>l#O58C;vmz@?<(KWIjWQ8LrGU8U>p<~^?=rLz0o>2X$3`?m
zS70De)}BQ3P6nw~b&`Zk1@LO|<4NDKH<uaThIXWxB_mF~o9lb*>H>cU=lclM67l>n
zhB&v1XZP&GiD{bc!4qry4(Lo4&U#O(oo5E6r3uQ{F0``n=21|!!r6Vw8s<kDO|`ni
zG51{Qg_sh1EgT)4+K3~b>_-oPAdR$pvqthL9l5Cwav)W)n}hojqn_IataO=+ILsEa
za1l<bM-|fP$hP*@_(FWnP#B2F7814KSyHdY#M7l+9GT%qzi39K@oG4l4W|m!9BU09
zR6#R=uMh|Jk@SuXeQDB35&Sro3wy>iT+3b%xX{Et)x-F<-CX(#Q7JyQC64}>LT8)o
zy-Zt_yuAuMwq@=S=e3N4sE@3QzR3^YczJECEqv;Il<$7YFii^{`7MS`jB;TyF98*`
zKD<y89skX~oM&AJyk9mmszz;Jt&X|DN^&|a(S6*tU5NE<)N_|lX_X%x_hLod_&*vr
zIgSk7vifw^oql8Ep>jlrcS}B>Fwc==FKY9TRj<KT?=Q7&4Nt@bsT<*Q61J+D56=5_
zW%c#r^Hm)kDhJ{3#B8d*x^aLiDZ={M(lS@_MrG`|>&uS!ONhbJ!L?`kR_i}!MqR`m
zZ8Zprh~&lQ;BWf%-LKyIB<_`A8>7y?b#*)`*QBy{;~~QEt<Ez}6ICm#EJjkvvhSW`
z9Ty_$iIhEFERi~Kfk6F3-m!7mx#d=kq!-3a*95C`?Mo-76u2&bQ}i?2pb4R9w=?vU
z7?JVHunZ&WDqdR@_#kg%5_Fql)9__=tsu1^i4f}ug;V4D{1;#u)gFgyMXsnbY^ko0
zR$KCORL_G-MP|7yh@Cet4IHl)w1FzTRxZq;2#F{9gmYcJWT#;&H3K;lrZJ(REj+y^
zFf~XytZU*U-)Qnx8VN&A_d_M;*9#>&a!h2Co?A6h9Qblq>CBmix}KLJPnLuyZyG-w
zA8F%PwSRhu=A~6Pjs2O$7781cCoKNn8?T;aDxV&rpN^|Fe>ajEG^hURjKj!h_Tk|0
z*t-G(Dh*j4>Gx&5AlxSel~#i#1OnxRjZ<~O-SHP)p5ZTt`FyT^OhH|OHSO&+b*j_S
zo|DuF_rlGiTkc-FAuuuIk)FMHqb^|s<QWz+#(_7`>pefr)9!uzJIm<|VkCb3?%49|
zky1L&VEZSxx%~V5%7R!3EJx-Zb_#dUxZd?!z1fiG_WV&X?>iM<tHo2pt<o=D7TDtz
zVH5<q;|HWqy1VOej^dR);``1I*Xj!8x!HR^;1G*R)th1Cc~pe%KGjEG79(9gpSxrl
z#Q#Z4;I;rSxaQUprLw@2&R2CxRF?)FKRlA4<B<2AcEVJb(%U?R@fk3@@+l&X!=BB3
zu1)XJ`)=45dbFbEj=!)g#b|Dl#wkT<mUTRvm*4Cg=pz-2F48ScIHl!PHg{FddX>7{
z-C}Wr16QUO*qK(Q7{?Et8f{24YT~A>8Jt=eYtcH^j7oUyGjwQ``QljbL%05fmU+)_
zA6P{Qu8fP+H3{EL6A7P^13(LFXA*;x#zghrXt7Plps7{TDeJk`FwsH}olnE7`JNn>
ze#Uwr&zUdgOJk{S1l%ypS_B!&mU~EYl0${1llR?x+KqjaVO46qNii;ZhL^0ZUC5)f
z6voAVtECF#cf1W;RQrNxDa149*J9nrnvZ%j#8;a%#pM8ir9j_%zL~S`&FJ`JPq_EV
z6Qbpf68an8zt=8$i5~MCi;MwsMv3BpeC-1|kmtE@UhwGHSQqO=&9m-Y&V~VsL*?%m
zax86qk8{mz^q!sjv=r0FXzypa_j>>PYHx||61x)pNXRUe@=J9+)|L-USx0&vqI7{$
zxyAHlg=sP4+ag?bK4=L1a@}mAJ#+BwjZ0P*RfNLp=1X6k?M_1Bb{*r8vEkGcU3bmR
zSJT(<Been`5Xjb7z9{@*9VX4Ixmmp+PcJ>Ppg<UaJ1<W#$@#BIrifWZUgL*xcwdTn
z6dcn+U{edro=`K%4%(Fex-vcC$zJYAB`Ry$mZi+u7<mvf*Ze5<>)f-?P&mwRaa4hR
z>0?`3nD>xxi_<e5qxN*!Ljm8un2uCQu<_<eTv$m1UCS8oZ$KT!9&$Nahqnk%r}0lt
zGRbCy&@m>e1J(Cy2{8DL3kwv!jOi=YiTIdj({r~!Hk??+Bz=yX1xm+uN_AnhsiV;N
zM2W@3xB`kmkSeyVjUKAK_{}=!gmNO2JqKLC-1EwG#(-7pBOLraFX@=_#*vJRb^+*2
zF#jMe2m*Zu3z}fy6>ocY$CS>suN34qrYNg@&!(D<nt{uZQ_vp;i@Wu48EU^+^dkJp
zP4!ELcT7&UWh%0-OriWMF?IgyxS1YMcx0qP-=V?@3aX0oloQF2n{RS7>7<-rH|(wT
zsnZFpDg!WNa>0f7b+raF0cEr^beDSz&hnRS;W`V>o~!kfj-)>t>M_xdTJ!4KX{3Hx
zE}J)_7G8m91xuXw%de=AGn@cGjrX^W#HD9Hf$aUo(V@*rv*u_0C5Tj_tlNNL!$4gC
zCj~{l%{B8!j(TlG&{*X?ZdtuU98%f=c#`j)X)a|y6JL1Nth{5}5zkF@8W`32l_l{I
zP*YrRy@G{3Ru7X4+Eii_w~kWsT`X|Q)6b;M^78udf;IWF!y=z~u{BZNBM;2Yc5yM2
zLh!Ci>&g?Lxf;4u1@v%lX<gwEkMk>d58xSlpznC0t{;Ht*%nyO=BqQziQz}(a6CXa
zIYFczK+Vp5rG=I~Wvqc)W;q%M+OkjE`I={)R!^|4z^O4QC@ERj?h`dHzE`*B#sI6R
zS%IfC_oWA@R<?P;2F6ODToJJrKX;$!>+<BSsOJ&_&O9@8{kJL8N~5k^X#(V#dEvc3
z>E7n4Lu-At7t5b2IJIZcI*1$0@0*%o7hR5Ber;U`wD076y7t;pb_M}mv_}fdg=6Kq
zZC5yDL!DCQ`pYHCz{XubU(~e!1||aA3$&X@fns3FU^!e(O|613nZP%A==4Ug=X7`B
zd!K47(AYK21nL1%l%$g5NT|D!QNkblp+Fb5w)1{U^f;nUpk8@>dB`XO7A#=Dtj^p~
zoqWoInU|MW*eEyZjqy;8ujyfR%6>l4scYf4>VRc2>9W{6sS^Chr(&-i_I#1*X~tk~
zmwT+5gsd?r(H!kPbBW>DYe2+&C8250**?>Vn~$F!GF{iQeu%tWwN9ihWfh4&GY3GA
zudvXE@GK>Zmv*kCan5R>wiFSCk(;C-wA!mSnc3$OX7`9oW+$1mputFoTxh7f-nH;C
zx-_cs)|7B0uiD<ddsz`_*c+Sj0lYT>B^GI)`p`tvBPgUc(SL30?cRq#J*LK#k7$f6
zG|~b4sie2$Cg3O9i<CYBz0uCOpkj4ptBQuf3U`N^)uD@1CnE2_uk}Y&0_|VviD9ej
zShMtvQoK)6r1E->k6J@IlMliy#Y061MT{D0r;B_{?tUJ$ki#Wh#y-`IVY><FM}!0z
zbm^kwhELg<WRVNDI-gtQTY(zbbTKkp03_H>VtQX!zxDldvTR@q5fi49&&014>|yQ5
z@IGiOpsY4F>ur`l)+~D2hS7%?Bws(>aqAN833>KmFY$!&-lyvEkqpOZ^=IA(tlF?T
zCd68QD@Ex61?UZ>lW`WgEhz0)M&c5?pGL7ohvJehO{9uU-1Eo!nF<j=e_m9g?L#8D
zfhm771~`u;`8@NV+OtcilQKjTYT8I=n5Z5qP;&DNNdzE#UKk{38y4&86D8GVm*e?p
zke*aZTJYi^25zMzLEoqCx|*qk!Z6b*7B(lrfocs7q{E-~QK&Vj0L{Dy3~CLkK;LP<
zh-Q@l@JFbo{Ztftfu<Bk!QsgSpv!fblaY!Z=nJB680G4Q0v#m`@CPl%kgdJ(2`$l8
Uw`Sdef1Tf>qp7b^3_TzEUmlR0;s5{u

literal 0
HcmV?d00001

diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/velodyne.drawio.png b/sensing/autoware_pointcloud_preprocessor/docs/image/velodyne.drawio.png
new file mode 100644
index 0000000000000000000000000000000000000000..b3b8eda6140a45e0b923bee0b5f5852b67d0b92b
GIT binary patch
literal 12192
zcmeHt2|SeT-aj%bB(jq&3CT8$vF|aGvc%XKW-wWXnXzV#l2Rl)X))4R%aVO5B(i6z
ztXYyJ#DlE;?+MT2dCv1c=e*~<=e(cudEXi1zVF|4UB7Gj{+93jJ`<sTLHh_b2Q?WP
z*%2L>rXg^h0zPGjsDS^iWo%5qg&b$7txlHLb{uG7Y{8#3#k=}Apj}X80#X_~cLHDu
zCkzfRAf+h)1|!_v#gJ%(BNpL`6LUl10TIxSa74N7cF@E4p<P@M0$?qOm;}%yWGX2s
zAf*o6okzQQ`T$oSSs5D{X<^__&E4Gv1xMNGq5(Zx5GkmblniNzF6@%tc>%BnaPNY4
zMgbRXl)W>CG(^J*i*W@cT9RNHF$vNe5E>yI5Lom+6{IzJp|ChK#%*WWU}-U^7@(Wf
zfU`%qpmxNPI}$q#7Ky^{hyZ<H0jaYB5^6wK(w~&Z?tXy>Hv};E&vVcRwk*0k9F5$O
z`573WchQrSMX1?$WBlx3E|Lxof7G_8)73{Ci*R?+!yr*EfMTT2jtU6$=lp=ccSj&a
zIuX*U9kF}U?9NIO7@__4cCtIp(G!hC;daLEcEn>aE_k&2?}hdlH#e01E@rzO5m+q7
z`}f@(FfO~B+wJ2H?E81xNdlw4I8l-mjz;30cKXUfc9%zz!%%2Pr@fgaC3ag}5qq6?
z1vn=J663uy{-<a^C_BfvcbHfV2GF+;v;EC`om_o1P%fmi-aE(Lc>uHduaEupG_b!K
z{y(G6S4|e?=Ix}RgEK@z?Y-<Yyp6p@Av?f(AzVCnk9<df!~5>pFR&p1SfEwi+X;<F
z8M!0uNnO1G(*k5pcvlzTiR69=ocj*i4rm_~fT%j);w~B(7tD^;OF2MDZ;~A2hTkWb
z`uPInI6M~PjM~>C^YaC??5+}p#iMqO_)iY!r|7RE^czmvD8R+>SYH6EU5!$*yN9vo
z6{OB2ukilO#-O`hoc5X5o(!>T)sDYvCJp|3kO%tg8qQx|Jt<iJ!M^?!)Ln1>tDw3e
z)kvNQKxglQz~RvLz$x#;5{vP4Ly|z$1cK8afkZ-3vPkJa156fdCnY2EYs;Q@0>N$<
zLKN~ZT>Cfc{zn1a2hd&&*bmU242yC>;L%>c4=sO&+FwUu$A*5hBz+7T$V<PPku>-Z
zW(1KF+m-Cig~NDa?NPhq{yro58<R@??Md+ntRo8l4@@crq)5O%e|4C@emc9of12sv
z>iPFsGxYBWQU8oJJILCj>`CVT7vaZF79tH8&tA*_PHUFlIsUz%D6<<B|8=%34gUR<
z;64A{oAN)_l4bwIl4V=~R`D|<0wKiy*JLT<;Yp$!K-83ylar8;Mhbv`6G{A*o%e*K
zlq!n*$<#=kQc~6($ZEjoy>{Tvk)#jc#yc|tK1~|4C*N7wZ+gK%q9yg;ml6;zXh#y$
zwFlS;iO8x0<QI<ySee>xgDV<|B#{PnEDBf&!tN)5Ai0k_DMJ3tju3SLh$bLGc;YcT
ztNqCwe(!m17&nrZ{p?{+^n2o<r6mEpfUdje`Y)vi(h|_UIKJx(V9DLMzAyhs?YhVQ
z{3HDbm-(M32y*|JAwrQ7P#_xrW>f!!Ap*qfKiCle9R>%M+qIj$(7R_x|8eFfxx0~l
zgcOiSAW@E36!0~h#DM;rBF~?FL-{?G{dXWI4HOxEPXfVv{A?d~dzo~9sMrf1d#Ub!
zK2!jOm7f6Z!wyWUAN(uO`<La7e+TGbncqV$wO3`?2f6I7BmBD||4GXKKLw0EdjDIc
zXg_WL*}7Mo+G*GwK&s#E)HL4FdILp1&Z|0_YL|ShzurIWbGxs+@yA`ZF5V&`sNCx!
z=8W!+$A>QW;x5mry;ale>oBCFQ_6rBLy7{vlpUAkay5t`ismI|T{f}vp*$Oza5Mc0
zebT^Wjh}ZiZ^6pY@Q~SXTSZ!CyYHPGOn~!N-+I7&#Zd8zdEn~YV8ub5$H79jITV2u
z#ODG0%#5H|3JM}Er9rJbh|;i50hT35eTV?2(j>2AriUGw=Ov~I@kTeZ(SvB=s=ScO
zP=ep%VAa$1s!S4=<RRS)bTQKrfPzT!|Ehvo5$aUyVj9V>%BrD1xNn?3VBuJ~R69QL
z9&(xdV@^tN2TS{v3^A`O5vrF<xC=c3pq>w|7do$u7TXuno1SKin9LuUY5o!MEbn%Q
z5cb(^i_agjXb5RN@N?~i+QDon!}6?cm~?)~DVlIZKxS0*vkh5!uI=NXo64umQR%=A
zH0rXcWe0{PfAq3%2%e2d$$nI=n#C7`d~OnQJ1FhyJsV7eR8=yc-H^U6jf(H$NxzlZ
zh^>v)bpN%*L@7tQ>7ML_F@>AVBG>D}kMc`POE*>ywed##S$tF6uKuN4I_K#U-F5&k
zw}7*;dc}RCPm>SY!NwdiE;r&uE$q6V8hoFtn3uqR(NC54q+VN^3>sI+xhk!s#I?3)
z*2isP&Rgu_15=Zyp54gLXV^e6C5v)hOT5GLxc7>cT3_Ey{MsVwQr4xzHQVbV6ciMf
z?iOF}c_sM3|LTn!H_qgnmbh_I9p$DC3JSu{4_Qsf;lD%|5VGO<uM;Khqf=8Ofo0px
z4U`EPJ)w<>iRs9^$m7!gLa!>EC+uD3Mf;amK|<z5PyHMg$D3q>wiU%JOM?Tq)<1P+
z=n7euN<6vv;LiH;Otw4{9_Yn!h+q;)C#<0xk{#Y|>zj*ss#MK+IdUqjYCiDV`=+-Y
z9>b18+b(9gVD7kx1={1X^aqb}x4h{sc7{v4eq;*RSZV!UQdA@zD{8^A^tsi@#QSqg
z67_p2$2OtTy1KgOo--zwa?P3FJ<&7L7qh88Ug*&J>halI&(QBqErhB<%YBzVH)jNn
zMhJFPcw<1s2M>5I_vR3%U#-r4C_MP}W3h(6zha(sRkG_E1+ia4mD*i9<O+Jq)XbZ*
zE5@(`G3@vzVVDQ4({v+Ch7xoOWH!2zXH&y%-*~41a22VnkJ)f>#nk~En~aO;>l!X4
zKPJza-iBi5zV4b%bt)>V(Sb4?b#22fR(oKsX-tXRp!&Db2II6on;@$Y9oA%YZ7?}Z
zPtQ0yUdTYbtnbX~?1#dqh8fgu1EqIAt@jd*4jN@%Fy^^B+ni|}WO__@y6w$9$71Fi
zTS6QaKUP52gDaA0IdHkLG8#@gThAqLgUL|nfY+50%y3=~J0G4v$0h#|SdrNo;U(ar
zSC(~$iHRu;SU#dDMu1Hvu<~l#U0_3ZN=s#b`O>MerEQvYN-vr4xq=SnJ^MZ+GtH0s
z%Ici)GY2|#fm5eW-)i-CfCo->Wq=I7G6=A{5^Li`;QD!%atlFd_N3GByd2)k9S_yC
zQVty#eOwpQWzHgOsA<+UU-N^zv$NB*3_Edv13x#wew!2e`k`8A_wbjtCj<ggwH!Bj
zA?(^UqMWc5N_#DdW_|Ikc%i6@LT5wFN386*j6$PeoF)~Xtv=%BO~bkQc9kEg46sog
zrO3&Xlr*ejn%O3~v7GXLzc`^L*g$zExyi}lU8k|iYYnV6Pixy_g^lPL84pn17qhOs
z!D{WJW`+(I@KkN~mav7`)gN-dkQyBwTr%WGQ|Q=EC3@Lwf`$OjTMDB{JzD+kHD~h#
zlG1B?bB#K2<6V|<_QFa(CKS$Bwe~rsnOPvw(!x{I1~XipduBG0T`E@KNxv>M$-vgU
z8SsCRY%X?o;=ou-G`C9NaUeFpdy{m*KsbQoOrOj)v%;3@>EpEg%tKmc9;R9xLu%ob
zlIjPS3FU?_E($NSLWhM|fqE@VSlmn+<Eiti8!KOxK3)jvPG3CtshU962M?!Zzly2Z
zGI70X%iHu^Y`V#dnvix0-pm3zqp!+atg0!j&a^-Pg0EmtT)|m#8YX&}D80c=cfAmW
z!wC$g1+mUYCm&@}b}4p$I0b=-3Lmxzr&AAYci<h7l#Bah5WpXuER9{#x4NJu-Vl+`
znr@gWcIZm3Gxz1PB|T=?{l4>Q?ndP1ouUWUPY8Ttp`bX>>GauU{OIXf2V(U`sp@n^
zfV0cn3t9oU8Fdqo-4}DF(kssKLcD}F92&qD2HG@eJ$?tR6TCuipqyVFvMCLrpxDOy
zMzxZxRX>Q&#DcUk+VbV#rrb@U(Vy?$k$IFv^q#YQTU!m&JMaHcej7yd$Yb1+lJXp3
zHPD07kWmZ6O{{A|8OT0I8obCovVntMns#b^gseVERGrqXLc3cyoeg4pfAK87P&aY`
z-1Sy2i!cnNFqE{<*^m}oC&kMK{UU!~OF8;m$9kMcUQfM)Rd4q{vbEFWnWYQq7SWK#
z)YLO{wkD{MF)QHJoeEiGi|yD;nd)o}8dkVG+yZDv;hOwP0?Z7BCqCIc)gNLJHJSk#
zvQu-zM}kUROct(5qp53tsJw`oA<)6tKcLDJ;;%2I$kf)@s%>hgzvj+Qu(IZj{+N;J
zpObbpw7W6c$qYwn_?8fRHPt|sw+F7v+tYUrYyLD?HB0=7YF2*Sc#%GN$R(Tez(=GB
z{RjZMIuZDo#Jp1nQgwhN$S@%HWac#>U9a0w6V}ajF3<q`U2b54Ef>e6vYh6Pp1vg<
zJ?$(YCwd8(fYs>#CiuT2`X{Ui!d!Y~Rcv%@%;VE5kY%M013b_2C6Ei<XL?0X3JCB)
zAk1-?VfF9#HB)KD?!0}3GRQHNj1@AVKsCpz!C;3@UwRsR6-0Bny0fJ4Frj*|#@czX
z>7Jm+_XWgpIV|O5`vYwtgD|;doQpGD03AG-?*8c&b!BicS*lf;<rciWRYd4mCuLv0
z4P3&e+TZMDz~>hBu<O@PC@LQB$uiaehE?{myt6(To!rYWlhrO>vw4+E!OONiO^I5W
zV!1`CUFhNF<|g-t7%7o>Vc6RjGr`!e3d+%|LseIJzc=3D(y+8V?sR{qn;7c1JpHLj
z@Iletb8~QGUQe4kMfj`EbnWN9OAe{B9_k@Rg${bC<y0LxHNPOUqBbCM8}WoadnO@`
z?xF-Trq}1V7p)@EA79K=+IX11fH?Q^^6`-$;gr$bT3^N$C4T7XGJ|nVbi8sKuI92R
zaXSkn>&_>7oDaOOfeh!PJo~F8B_$a|Omb>N4jk-we3qv2mo8nml9$FXsT!aD7p^xV
zBGi_rdpfHFu26zNWme*hH1sgNS?18`BIiD_GgjqH04gRXB0<}m@!f6IFtfX{u}r8z
zfc@VRvnn3}vStw>@D%fj=;UtB>pc_M`PGImQ@p-@B>Fy#YdQ_%@*LgtnP`q{Uj8~%
zRS{?0x?DinWjRl=blQEYOQ7X}icPMW*WAE|3E#<da|Z~)pLH_Zz`)?L>kD|kwUAYL
zk?&}^K7U9zsMCLqLCm)$O^LHL8KR-78P=FQeHg|SH&}q8k#TuXB9%k|z6!yR?l~nV
z#Fo9iy`*#RnO|l<7=i2+lN1jN&$ROP)od?|)3QmBK`wVMAdM8QU#L!dM11{5LBYY!
z9-$nt?t|Lud>3u#nK#*XUx5`Ml*N#z6htMqMJLgz(ZKbo%x0xuda1}Us?%g~a~sbP
zbuKgO)C6v>?a!=jyqy4d=pY&iMpcw1^(?Ru==z}Y@*MoycZyIFI=qkv{87CNbw^b$
z<vkP()jaAP4CR~HU_bv@SH+=)Eoyoy;uXm7;?v9bz-4sJtz-j~BVWf|VIP#;nnxUH
zD{|AR2}9iXuUAGXMD*rbgnJM9HuBFrlmmWJ!8;*8U4(wuQGn95_<H^r49B6T{M~!5
zVQXXfM_#;n&X5j41n#EVT<+m`EWu$Y=tn^R>@qXVQ#<bH#0J2fKF!ZZ!EFK{edci%
z(FHF7dZcb{&S4$2wQT8JBPGrPyKnIJ;38IB40<M#6WUhLx|sJSCM6ont?V(XS={-O
zPEh#}Aum#bQ$@=DHEl7=<%agwfxCQAK2+736x7X&4UD{@(7MoENI`@nWyS=K$KGNQ
zef6a?9ap7Zc0?HT{Y!dON3I1l-*v>@7$4GYJmK<vpe0Gl_@Piqu26;)xuLlcRSXr~
zaVI>6nxN$!<rz2j2ysvFqKQFkD&672x!ey4ND6{w3?g<TKWHnvGd$%mVTkQ6Rb{Bk
zjn7FXLF?}x&_2D*9v`djztjp9y7V-99Z^995Gi&%lh^nDxqVOh)56SkGt1kNqvxLy
za%eB|jh9ay<nbRNJ}I}#Q=NVh_L;R7#p*_WT+%K~G7bVVqy~FVUn(~4=$>E;=}tt<
zl-=y;nfPc6xRcWAF+p9K!(Bbu@O0G>in8*uF@wd!gbQ?H3J#OvR)?J2T*>_t9Zsn=
z9}-uCEVF_sh+IV6<F?8)#P@|yrwg3bFZCIC-s45ZCNK<LR9>rpqx+cBP>`yUs}OtO
z0DofW!J{%+>!;MtA(Ve?;+WS~dYhQq@YhwRBh6)6Dm=!t7JdPxEw~wij~J$PwDYUl
z1g@d(Aq_nlV7F(EIg8ZWR4%Z98NJw1Zb|59>?#A)Kk@rpwh-xqr(%reAFBojh!<|n
z7PfbH3O%{6;><x%jJ&O#Q@OR$-y0|(Dx}BTs#UKb9P!=*tJKwH)KPiPk%rKA|HSJk
zo+xf5KQ?c&1VHP#XYQZW^~K-CsQTGN4P2-ujtp?s97eBpNf4$qDGw?m%xlnn;hb!2
zW#gj#rv%>dR(a0E-wejthoYNgVequX2u|<((c7o(M<x@*Z&}+k=xN!i(#tDC-e9>H
z=Z9)vF#ZUA(6$}iUBqO(y@axPCDOk9Y}0D!OtgWOn{VN?jCA(28wX)v_GRw9$i?LN
z$WZCIX>J#}5?a^OE9xIk)C`Z^b>C2o-`bSp2&=rAB`|IFWGuI$@A>j1<bBYJHX$R^
zAvUF5oQDd!MfNfz%3>vNK<Gy1(S-%kd%ZAXD6qxc*zkgH-PG@5Jfcr!4mP~vXEdmG
zVwM)^gJ|U6XYCuceHHb^U;-n=G_s9)4vh(H=pD;uxZ=jyHJ_o<Ar9GIIJRv%IAjug
z`xtCFnh~~)Z+=t!ZMIL=^DDidvH~4wlZF^%!vtlIVm*D^gn8gVI60qro6@%wZtBc?
z#&NS^r|FNS+=xC9v2e?W?ucI$_BqoGH=}o-j%fR^V#xf3R<(K_`4gMp<YA>a^0#-|
zRc|d08c3EV-^)O?20|f+E5@K%BOQa?3T|-%M=SEk_^$ELb67SVf(f{fBW;H2l>C`j
zC2x+4-+|Yy+xImT%P!6je)%?Q`h5c=Yxc3OP%vZQb(<0AGm$|47^8dKZ^miV_+^^A
z1=w`uJ71|I55ERUdfVK5S#2YeEi|9SSV(Vpl9u5Sm!;5DXB553)$*C{*Jt8;;1t?V
z+#H@&@(-TTmys&v9a*mU@p?4fXyMW~j_+%IrbRTI!Ch(-Rzf=ScZU&!WaaheA#Y_g
z`zL2Tg0Wt?(w-I%t@C}v6I^KU2F`g&*5?#MPk^|!R~POMT%NVlS&u?_g_c>%o4PtY
zn+Hb{IcJxz)GNrLLaT1cEA*asR$2p3$GXkg3Fj%TaOTY1O-{S%@QKSxq&_(F{1!2p
zybDRZx_PO!fm_aNS<pX7jKiu<`>|8Vz;g#-wD;ZN;cCuVy{*oYTN<zUqne_stzsb%
zB~WK$X1eLi7`Ob}<))wqzAg1*M(2;g?_BgKfV!QsF`juGo<CbF9^u$IHT6iy`f2RF
z4A@Ew^#aTD@o{82-3cjTxKB01s!WRCN}fkQ#nh;qT#aQ}iTJfmh9IHEBAqSt+ww#o
zLZ*&-^WC&l%Uaxdx6tACDP7E1*%!h}e9OV;zVnG)0@&6t(v$w=Y9bzS@37SgR+*#@
z8pH52)QR}zM~AKW;XKKE&k@gn=Y&Hkr00W7KvCzqJlV%0e&wn`<D$o#PDj|m_5Jx?
z>jAGtU&;<r_|^)8N{`p^tG~X@={QiPeF0K`yJa(EwLXHehsgY7h!9`qOF`tk@7kxm
z_LdDO8rfqQmWuKQ2JXhkJB+KnKk8heIz7dJuw9FYh?wT^vp((8rRO{peba)>7rn4C
zUla9^ceTBFQR_a)5cAY!L;;`@*MM>ak#X_7f=-7K)}!f~px_Ia568r-zdz2jw)ZW&
zpyoIJhGC+SN7h~S{tabRGm|Hy@%&KLbEnS7k<A8?nGXUv!s)nP-B<ALD8bnHZ+tD8
zehWdoqFnxgJ2#;H0Z7^Xr6=Wz8|!P9RxoYt<@hN(yX7<a?|>Rrk78<xU%#*%2n`g)
z7Cxt7vTm53KTj*~J(t7M#B*i#`5D(IBFyyk4=uC2hXYtQR_7aI1atwe3Gx(gx_*RH
z9~h=RjT9hEO-;Q?5T8-&_nJO=u{W+JnrM)IeVWzgLgl1@j7;zQTUU89s$>X6A~8`V
zNEzUa8Dz-QCOK>*(s72N05{q82q;fcx760EULqQTfby2cV7X_H6h8kZi|EbOp)2CL
zRTmy8-_EnD7+sur`_wp_#m5hzgM+aGN`afQK#eUkZ(+>yFi?}z28bg-;=%yF6Z{FU
zH3bwB55*VSzNA2XTH_npv~Nq5bLx0VPIZJM3^=l^d|?&oU#JrcM1~U%=BB%L$8s}N
z#RC<K4dlJ4wX@pV_Tz8vz8;K7=P#8Ztg^nivg(GPeb3<7mfB>OaY&LJTUrF7X3}?8
z+WwAF`M5o)@*{#L@H;ua7@XN>edP$VhzVuQEdCzAl+#B}5CD2x4-i%3q$?}41}efg
zqoUvo?^=h7(D$xZSXcQ?A9y)w3f<-uRaRlCyMCk&;K$t=iU3nQ3zW=y0!`>zW}6)c
zm()l3EFT1}6W8gJeIoJ{0&0spJ`_4;Z$rO2$PIdxQ3{izd$O3fzeNQ-G%IvqpMN~_
zmi(f#mUF`z-9h>E2)cyCc-OVX30pQr+~{#{K(){I+GLLkqpW$g)@=@9XaBL+w<Dv-
z$v?N>DjjO{*9fOGZtnnhjWQaVQ(ct>N=wVoKr$9_>r@7b5qO+Q5>MQ0^QtGSMBz3b
z#Lv$k=Qt|z@K{F~ZZa!M!OE}VR3@L2&jPPJ{tFp5^IEe=J`%m!$P<)fsZBiw3j-)@
zA-fxum9ZAfd<mhY(eZ7<kFT1wFI@XiuO=M0o`-dEa?(l)OWV+2yEa}Nc;}Xlpk8tq
zPzpq;HVdWIJ!XWJPtw8fH{R^<Tlz5DEcnG&nBh|3_flzl@4f^vu_F{J171|IW9`DQ
z!>Rx?yEMl0A~#3p-l@xNN$l2CDW%N09%HZBd+*eGH0pk??|)i$a)dtCB$~B+QLari
z->PDTOq|y8+k8>!d3*%AiYq?0H!Vc$RNg_XH8v>x+iKW{xCaSjo<(ta;wpi^M9)s^
zNWkhxP7Zokl$RzorWr*aQx8mX3gM3b(uM6eO9G?7+bzzkZ0||cJCi*0yC>hjHS+M#
zM=iB1EVx4iGcqz#bZ?#4ZB#ur|ABzggTZPoFjW~;$8v=yQ^Ph!8*cf0y2jd3?l}{#
zF7N-{eV{tPLP;aR2dI*tW@;}e6d5j3P^wz)w51JEQ(xg%3G*`3p6KFfWN)O%luT7$
zQ}4PSyQJSXL(S6ShvlC1{n}pwkI(b@HcCzACWkwjmiw+IC<t<N;-O#|-eCSB-jlsG
zqZY3sC*gGW$6`W_5Ht}bfo;iPqG*h2Sf1(Yk)oCE<mftU?|uU(Ql6D0I@Ov2eOV#D
z_R^NiOeRzyRlyNnEslSt;5rf;#$y$zsyCcee_xZ965Na_@$5)lhjDhLQa9(;I9eZ=
zs1huS-}ERv>CE&HUZ+Qv8VJz-*QI)EsY5wtg;b>4YYKR_=pL*j+~tUHRP{s7mf(Yk
ziLRsG(wzmm1sBMvrhQ&1gi1#0BrZxp{m!P&b%feI=tqg?6)M>~!`5`E+|G@}Ym~oF
zRq4F>5k)uQ;yB?ISYBOZ9!cXqc1Y#sP;c^58*m`@GrhS&W(9<TIj`?!NeTgu&agpr
zsp3pIg=Nq{rLUCi(eY$R-)kxHyJgncm5X5N8qBbAaniLVdn(BQloC|1in1$}DW=9T
zn+%#i`~JB`NL}!W^d-y1hbUa%t1i1PEKWyAOpMO5@AJWs6nLp+LPCsca)MufsH=c}
zWC5d)c@=h|nGdMv0$<bBBaR8sA3vV+cq8n?^G9I@UauMhK1Oy+f!lLaH~FYaQo7`E
zP&WS6>bWlEEN4o?qjUq6=F%_6Vz>}vF&v-9j&UKzqfQ(WWJHJp-wduMRJ=SS)#q^7
z`x-Sfx9Qy5Y(Hv(Z}DWt;0vIxR;&ltIlGiaL4mgZ>WeOM1&XAu?~DyQE3;~UkkcwQ
XJq;bVy$}54o=oTL1<gEl+pGT%SmM;#

literal 0
HcmV?d00001


From 3081d6629af6b41895df8d5a80ca754b0d198c85 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Tue, 6 Aug 2024 17:06:47 +0900
Subject: [PATCH 05/72] chore: fix docs

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../docs/distortion-corrector.md                                | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md
index eaccf23a2a444..05b76e49511f8 100644
--- a/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md
+++ b/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md
@@ -49,7 +49,7 @@ ros2 launch autoware_pointcloud_preprocessor distortion_corrector.launch.xml
 - The node requires time synchronization between the topics from lidars, twist, and IMU.
 - If you want to use a 3D distortion corrector without IMU, please check that the linear and angular velocity fields of your twist message are not empty.
 - The node updates the per-point azimuth and distance values based on the undistorted XYZ coordinates when the input point cloud is in the sensor frame and the `sensor_azimuth_coordinate` parameter is set to either `velodyne` or `hesai`. The azimuth values are calculated using the `cv::fastAtan2` function. Please note that updating the azimuth and distance fields increases the execution time by approximately 13%. Additionally, due to the `cv::fastAtan2` algorithm's accuracy of about 0.3 degrees, there is a **possibility of changing beam order for high azimuth resolution LiDAR**.
-- Currently, `sensor_azimuth_coordinate` only support `velodyne` and `hesai`. The `velodyne` and `hesai` coordinate are illustrated below. If a LiDAR uses the same coordinates as velodyne or hesai, users can use them as well.
+- Currently, `sensor_azimuth_coordinate` only supports `velodyne` and `hesai`. The `velodyne` and `hesai` coordinates are illustrated below. If a LiDAR uses the same coordinates as `velodyne` or `hesai`, users can use them as well.
 
 <table>
   <tr>

From 94551b67e9f561a28a3d8fac8a81e40cc44231c6 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Fri, 9 Aug 2024 11:46:11 +0900
Subject: [PATCH 06/72] feat: conversion formula implementation for degree,
 still need to change to rad

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector_node.param.yaml      |   2 +-
 .../distortion_corrector.hpp                  |  17 +-
 .../distortion_corrector_node.hpp             |   2 +-
 .../distortion_corrector_node.launch.xml      |   2 +-
 .../distortion_corrector_node.schema.json     |  10 +-
 .../distortion_corrector.cpp                  |  84 ++++-
 .../distortion_corrector_node.cpp             |   5 +-
 .../test/test_distortion_corrector_node.cpp   | 287 ++++++++++--------
 8 files changed, 252 insertions(+), 157 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml
index 8c160ecdae23d..c01317aadd3a8 100644
--- a/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml
+++ b/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml
@@ -3,4 +3,4 @@
     base_frame: base_link
     use_imu: true
     use_3d_distortion_correction: false
-    sensor_azimuth_coordinate: ""
+    update_azimuth_and_distance: true
diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
index fd26164e1660d..dd99300b1891c 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -45,6 +45,7 @@
 #include <deque>
 #include <memory>
 #include <string>
+#include <utility>
 
 namespace autoware::pointcloud_preprocessor
 {
@@ -65,8 +66,7 @@ class DistortionCorrectorBase
     const std::string & base_frame, const std::string & lidar_frame) = 0;
   virtual void initialize() = 0;
   virtual void undistortPointCloud(
-    bool use_imu, std::string sensor_azimuth_coordinate,
-    sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
+    bool use_imu, bool update_azimuth_and_distance, sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
 };
 
 template <class T>
@@ -83,6 +83,13 @@ class DistortionCorrector : public DistortionCorrectorBase
   std::deque<geometry_msgs::msg::TwistStamped> twist_queue_;
   std::deque<geometry_msgs::msg::Vector3Stamped> angular_velocity_queue_;
 
+  // Equation of converion between sensor azimuth coordinates and cartesian coordinates:
+  // sensor azimuth coordinates = a + b * cartesian coordinates;
+  // a is restricted to be a multiple of 90, and b is restricted to be 1 or -1.
+  bool first_time_{true};
+  float a_{0};
+  float b_{1};
+
   void getIMUTransformation(const std::string & base_frame, const std::string & imu_frame);
   void enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg);
   void getTwistAndIMUIterator(
@@ -117,8 +124,12 @@ class DistortionCorrector : public DistortionCorrectorBase
   void processIMUMessage(
     const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override;
   void undistortPointCloud(
-    bool use_imu, std::string sensor_azimuth_coordinate,
+    bool use_imu, bool update_azimuth_and_distance,
     sensor_msgs::msg::PointCloud2 & pointcloud) override;
+  std::pair<float, float> getAzimuthConversion(
+    float point1_azimuth_rad, float point1_cartesian_deg, float point2_azimuth_rad,
+    float point2_cartesian_deg);
+
   bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud);
 };
 
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 43ef2d0dc5d96..4d5f775341f3f 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
@@ -52,7 +52,7 @@ class DistortionCorrectorComponent : public rclcpp::Node
   std::string base_frame_;
   bool use_imu_;
   bool use_3d_distortion_correction_;
-  std::string sensor_azimuth_coordinate_;
+  bool update_azimuth_and_distance_;
 
   std::unique_ptr<DistortionCorrectorBase> distortion_corrector_;
 
diff --git a/sensing/autoware_pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml
index bde9ca9e59ddd..62edeb6373232 100644
--- a/sensing/autoware_pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml
+++ b/sensing/autoware_pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml
@@ -1,5 +1,5 @@
 <launch>
-  <arg name="input/pointcloud" default="/sensing/lidar/top/mirror_cropped/pointcloud_ex"/>
+  <arg name="input/pointcloud" default="/sensing/lidar/top/pointcloud_raw_ex"/>
   <arg name="input/twist" default="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
   <arg name="input/imu" default="/sensing/imu/imu_data"/>
   <arg name="output/pointcloud" default="/sensing/lidar/top/rectified/pointcloud_ex"/>
diff --git a/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json
index 2e17ef6b66193..097453afdea21 100644
--- a/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json
+++ b/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json
@@ -21,17 +21,17 @@
           "description": "Use 3d distortion correction algorithm, otherwise, use 2d distortion correction algorithm.",
           "default": "false"
         },
-        "sensor_azimuth_coordinate": {
-          "type": "string",
-          "description": "The azimuth coordinate of sensor. Currently support Hesai and Velodyne.",
-          "default": ""
+        "update_azimuth_and_distance": {
+          "type": "boolean",
+          "description": "Update the azimuth and distance value of each points.",
+          "default": "false"
         }
       },
       "required": [
         "base_frame",
         "use_imu",
         "use_3d_distortion_correction",
-        "sensor_azimuth_coordinate"
+        "update_azimuth_and_distance"
       ]
     }
   },
diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index b3cb2cbba7fbd..f040edeba8c2d 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -187,9 +187,55 @@ bool DistortionCorrector<T>::isInputValid(sensor_msgs::msg::PointCloud2 & pointc
   return true;
 }
 
+template <class T>
+std::pair<float, float> DistortionCorrector<T>::getAzimuthConversion(
+  float point1_azimuth_rad, float point1_cartesian_deg, float point2_azimuth_rad,
+  float point2_cartesian_deg)
+{
+  float point1_azimuth_deg = point1_azimuth_rad * 180 / M_PI;
+  float point2_azimuth_deg = point2_azimuth_rad * 180 / M_PI;
+
+  float b =
+    (point2_azimuth_deg - point1_azimuth_deg) / (point2_cartesian_deg - point1_cartesian_deg);
+  float a = point1_azimuth_deg - b * point1_cartesian_deg;
+
+  bool error_flag = false;
+  // Check if b can be adjusted to 1 or -1
+  if (std::abs(b - 1) <= 0.1) {
+    b = 1;
+  } else if (std::abs(b + 1) <= 0.1) {
+    b = -1;
+  } else {
+    error_flag = true;
+  }
+
+  // Check if a can be adjusted to a multiple of 90
+  int nearest_multiple_of_90 = static_cast<int>(round(a / 90.0)) * 90;
+  if (std::abs(a - nearest_multiple_of_90) <= 5) {
+    a = nearest_multiple_of_90;
+    if (a > 360) {
+      a -= 360;
+    } else if (a < -360) {
+      a += 360;
+    }
+  } else {
+    error_flag = true;
+  }
+
+  if (error_flag) {
+    a = 0;
+    b = 1;
+    std::cerr << "ERROR!!!" << std::endl;
+    // print error message!!!!
+  }
+
+  std::cout << "Adjusted a: " << a << " Adjusted b: " << b << std::endl;
+  return {a, b};
+}
+
 template <class T>
 void DistortionCorrector<T>::undistortPointCloud(
-  bool use_imu, std::string sensor_azimuth_coordinate, sensor_msgs::msg::PointCloud2 & pointcloud)
+  bool use_imu, bool update_azimuth_and_distance, sensor_msgs::msg::PointCloud2 & pointcloud)
 {
   if (!isInputValid(pointcloud)) return;
 
@@ -257,24 +303,38 @@ void DistortionCorrector<T>::undistortPointCloud(
 
     float time_offset = static_cast<float>(global_point_stamp - prev_time_stamp_sec);
 
+    std::cout << "before" << std::endl;
+    std::cout << " *it_x: " << *it_x << " *it_y: " << *it_y << " *it_azimuth: " << *it_azimuth
+              << std::endl;
+
     // Undistort a single point based on the strategy
     undistortPoint(it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid);
 
-    if (!sensor_azimuth_coordinate.empty() && pointcloudTransformNeeded()) {
+    if (update_azimuth_and_distance && pointcloudTransformNeeded()) {
       // Input frame should be in the sensor frame.
       *it_distance = sqrt(*it_x * *it_x + *it_y * *it_y + *it_z * *it_z);
-      float cartesian_coordinate_azimuth = cv::fastAtan2(*it_y, *it_x);
-      if (sensor_azimuth_coordinate == "velodyne") {
-        *it_azimuth = (360 - cartesian_coordinate_azimuth) * M_PI / 180;
-      } else if (sensor_azimuth_coordinate == "hesai") {
-        *it_azimuth = (90 - cartesian_coordinate_azimuth) < 0
-                        ? (90 - cartesian_coordinate_azimuth + 360) * M_PI / 180
-                        : (90 - cartesian_coordinate_azimuth) * M_PI / 180;
-      } else {
-        throw std::runtime_error(
-          sensor_azimuth_coordinate + " azimuth coordinate is not supported");
+
+      if (first_time_) {
+        // std::cout << "*it_x: " << *it_x << " *it_y: " << *it_y << " *it_azimuth: " << *it_azimuth
+        // << std::endl;
+        auto next_azimuth = it_azimuth + 1;
+        auto next_x = it_x + 1;
+        auto next_y = it_y + 1;
+        // std::cout << "after" << std::endl;
+        // std::cout << "*it_x: " << *it_x << " *it_y: " << *it_y << " *it_azimuth: " << *it_azimuth
+        // << std::endl; std::cout << "*next_x: " << *next_x << " *next_y: " << *next_y << "
+        // *next_azimuth: " << *next_azimuth << std::endl;
+        std::tie(a_, b_) = getAzimuthConversion(
+          *it_azimuth, cv::fastAtan2(*it_y, *it_x), *next_azimuth, cv::fastAtan2(*next_y, *next_x));
+        first_time_ = false;
       }
 
+      float cartesian_coordinate_azimuth = cv::fastAtan2(*it_y, *it_x);
+      float updated_azimuth = (a_ + b_ * cartesian_coordinate_azimuth) * M_PI / 180;
+      *it_azimuth = updated_azimuth;
+      // std::cout << "after" << std::endl;
+      // std::cout << " *it_x: " << *it_x << " *it_y: " << *it_y << " *it_azimuth: " << *it_azimuth
+      // << std::endl;
       ++it_azimuth;
       ++it_distance;
     }
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 58c3ca330a73b..97ace13686b0d 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
@@ -35,7 +35,7 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt
   base_frame_ = declare_parameter<std::string>("base_frame");
   use_imu_ = declare_parameter<bool>("use_imu");
   use_3d_distortion_correction_ = declare_parameter<bool>("use_3d_distortion_correction");
-  sensor_azimuth_coordinate_ = declare_parameter<std::string>("sensor_azimuth_coordinate");
+  update_azimuth_and_distance_ = declare_parameter<bool>("update_azimuth_and_distance");
 
   // Publisher
   {
@@ -93,7 +93,8 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointclou
 
   distortion_corrector_->setPointCloudTransform(base_frame_, pointcloud_msg->header.frame_id);
   distortion_corrector_->initialize();
-  distortion_corrector_->undistortPointCloud(use_imu_, sensor_azimuth_coordinate_, *pointcloud_msg);
+  distortion_corrector_->undistortPointCloud(
+    use_imu_, update_azimuth_and_distance_, *pointcloud_msg);
 
   if (debug_publisher_) {
     auto pipeline_latency_ms =
diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index 0c496b0273153..4681020e60327 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -170,7 +170,7 @@ class DistortionCorrectorTest : public ::testing::Test
   }
 
   sensor_msgs::msg::PointCloud2 generatePointCloudMsg(
-    bool generate_points, bool is_lidar_frame, rclcpp::Time stamp)
+    bool generate_points, bool is_lidar_frame, std::string vendor, rclcpp::Time stamp)
   {
     sensor_msgs::msg::PointCloud2 pointcloud_msg;
     pointcloud_msg.header.stamp = stamp;
@@ -181,16 +181,16 @@ class DistortionCorrectorTest : public ::testing::Test
 
     if (generate_points) {
       std::array<Eigen::Vector3f, number_of_points_> points = {{
-        Eigen::Vector3f(10.0f, 0.0f, 0.0f),   // point 1
-        Eigen::Vector3f(0.0f, 10.0f, 0.0f),   // point 2
-        Eigen::Vector3f(0.0f, 0.0f, 10.0f),   // point 3
-        Eigen::Vector3f(20.0f, 0.0f, 0.0f),   // point 4
-        Eigen::Vector3f(0.0f, 20.0f, 0.0f),   // point 5
-        Eigen::Vector3f(0.0f, 0.0f, 20.0f),   // point 6
-        Eigen::Vector3f(30.0f, 0.0f, 0.0f),   // point 7
-        Eigen::Vector3f(0.0f, 30.0f, 0.0f),   // point 8
-        Eigen::Vector3f(0.0f, 0.0f, 30.0f),   // point 9
-        Eigen::Vector3f(10.0f, 10.0f, 10.0f)  // point 10
+        Eigen::Vector3f(10.0f, 0.0f, 1.0f),   // point 1
+        Eigen::Vector3f(5.0f, -5.0f, 2.0f),   // point 2
+        Eigen::Vector3f(0.0f, -10.0f, 3.0f),  // point 3
+        Eigen::Vector3f(-5.0f, -5.0f, 4.0f),  // point 4
+        Eigen::Vector3f(-10.0f, 0.0f, 5.0f),  // point 5
+        Eigen::Vector3f(-5.0f, 5.0f, -5.0f),  // point 6
+        Eigen::Vector3f(0.0f, 10.0f, -4.0f),  // point 7
+        Eigen::Vector3f(5.0f, 5.0f, -3.0f),   // point 8
+        Eigen::Vector3f(8.0f, 3.0f, -2.0f),   // point 9
+        Eigen::Vector3f(9.0f, 1.0f, -1.0f)    // point 10
       }};
 
       // Generate timestamps for the points
@@ -219,7 +219,26 @@ class DistortionCorrectorTest : public ::testing::Test
         *iter_x = points[i].x();
         *iter_y = points[i].y();
         *iter_z = points[i].z();
-        *iter_azimuth = std::atan2(points[i].y(), points[i].x());
+
+        if (vendor == "velodyne") {
+          float cartesian_deg = std::atan2(points[i].y(), points[i].x()) * 180 / M_PI;
+          if (cartesian_deg < 0) cartesian_deg += 360;
+          float velodyne_deg = 360 - cartesian_deg;
+          if (velodyne_deg == 360) velodyne_deg = 0;
+          *iter_azimuth = velodyne_deg * M_PI / 180;
+        } else if (vendor == "hesai") {
+          float cartesian_deg = std::atan2(points[i].y(), points[i].x()) * 180 / M_PI;
+          if (cartesian_deg < 0) cartesian_deg += 360;
+          float hesai_deg = 90 - cartesian_deg < 0 ? 90 - cartesian_deg + 360 : 90 - cartesian_deg;
+          if (hesai_deg == 360) hesai_deg = 0;
+          *iter_azimuth = hesai_deg * M_PI / 180;
+        } else {  // empty string
+          *iter_azimuth = std::atan2(points[i].y(), points[i].x());
+        }
+
+        std::cout << "point " << i << ": "
+                  << "x : " << *iter_x << " y: " << *iter_y << " azimuth: " << *iter_azimuth
+                  << std::endl;
         *iter_distance = points[i].norm();
         *iter_t = timestamps[i];
         ++iter_x;
@@ -282,8 +301,10 @@ class DistortionCorrectorTest : public ::testing::Test
   static constexpr int twist_msgs_interval_ms_{24};
   static constexpr int imu_msgs_interval_ms_{27};
 
+  static constexpr double epsilon = 1e-5;
+
   // for debugging or regenerating the ground truth point cloud
-  bool debug_{false};
+  bool debug_{true};
 };
 
 TEST_F(DistortionCorrectorTest, TestProcessTwistMessage)
@@ -314,7 +335,7 @@ TEST_F(DistortionCorrectorTest, TestIsInputValid)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
 
   // input normal pointcloud without twist
-  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp);
+  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, "", timestamp);
   bool result = distortion_corrector_2d_->isInputValid(pointcloud);
   EXPECT_FALSE(result);
 
@@ -322,12 +343,12 @@ TEST_F(DistortionCorrectorTest, TestIsInputValid)
   auto twist_msg = generateTwistMsg(twist_linear_x_, twist_angular_z_, timestamp);
   distortion_corrector_2d_->processTwistMessage(twist_msg);
 
-  pointcloud = generatePointCloudMsg(true, false, timestamp);
+  pointcloud = generatePointCloudMsg(true, false, "", timestamp);
   result = distortion_corrector_2d_->isInputValid(pointcloud);
   EXPECT_TRUE(result);
 
   // input empty pointcloud
-  pointcloud = generatePointCloudMsg(false, false, timestamp);
+  pointcloud = generatePointCloudMsg(false, false, "", timestamp);
   result = distortion_corrector_2d_->isInputValid(pointcloud);
   EXPECT_FALSE(result);
 }
@@ -357,11 +378,11 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyTwist)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   // Generate the point cloud message
-  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp);
+  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, "", timestamp);
 
   // Process empty twist queue
   distortion_corrector_2d_->initialize();
-  distortion_corrector_2d_->undistortPointCloud(false, "", pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(false, false, pointcloud);
 
   // Verify the point cloud is not changed
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
@@ -369,12 +390,11 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyTwist)
   sensor_msgs::PointCloud2ConstIterator<float> iter_z(pointcloud, "z");
 
   std::array<Eigen::Vector3f, number_of_points_> expected_pointcloud = {
-    {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 10.0f, 0.0f),
-     Eigen::Vector3f(0.0f, 0.0f, 10.0f), Eigen::Vector3f(20.0f, 0.0f, 0.0f),
-     Eigen::Vector3f(0.0f, 20.0f, 0.0f), Eigen::Vector3f(0.0f, 0.0f, 20.0f),
-     Eigen::Vector3f(30.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 30.0f, 0.0f),
-     Eigen::Vector3f(0.0f, 0.0f, 30.0f), Eigen::Vector3f(10.0f, 10.0f, 10.0f)}};
-
+    {Eigen::Vector3f(10.0f, 0.0f, 1.0f), Eigen::Vector3f(5.0f, -5.0f, 2.0f),
+     Eigen::Vector3f(0.0f, -10.0f, 3.0f), Eigen::Vector3f(-5.0f, -5.0f, 4.0f),
+     Eigen::Vector3f(-10.0f, 0.0f, 5.0f), Eigen::Vector3f(-5.0f, 5.0f, -5.0f),
+     Eigen::Vector3f(0.0f, 10.0f, -4.0f), Eigen::Vector3f(5.0f, 5.0f, -3.0f),
+     Eigen::Vector3f(8.0f, 3.0f, -2.0f), Eigen::Vector3f(9.0f, 1.0f, -1.0f)}};
   size_t i = 0;
   std::ostringstream oss;
   oss << "Expected pointcloud:\n";
@@ -400,11 +420,12 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyPointCloud)
     distortion_corrector_2d_->processTwistMessage(twist_msg);
   }
   // Generate an empty point cloud message
-  sensor_msgs::msg::PointCloud2 empty_pointcloud = generatePointCloudMsg(false, false, timestamp);
+  sensor_msgs::msg::PointCloud2 empty_pointcloud =
+    generatePointCloudMsg(false, false, "", timestamp);
 
   // Process empty point cloud
   distortion_corrector_2d_->initialize();
-  distortion_corrector_2d_->undistortPointCloud(true, "", empty_pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(true, false, empty_pointcloud);
 
   // Verify the point cloud is still empty
   EXPECT_EQ(empty_pointcloud.width, 0);
@@ -415,7 +436,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp);
+  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, "", timestamp);
 
   // Generate and process multiple twist messages
   auto twist_msgs = generateTwistMsgs(timestamp);
@@ -426,19 +447,19 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink)
   // Test using only twist
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_2d_->undistortPointCloud(false, "", pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(false, false, pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
   sensor_msgs::PointCloud2ConstIterator<float> iter_z(pointcloud, "z");
 
   // Expected undistorted point cloud values
-  std::array<Eigen::Vector3f, 10> expected_pointcloud = {
-    {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.117124f, 10.0f, 0.0f),
-     Eigen::Vector3f(0.26f, 0.000135182f, 10.0f), Eigen::Vector3f(20.4f, 0.0213818f, 0.0f),
-     Eigen::Vector3f(0.50932f, 20.0005f, 0.0f), Eigen::Vector3f(0.699999f, 0.000819721f, 20.0f),
-     Eigen::Vector3f(30.8599f, 0.076f, 0.0f), Eigen::Vector3f(0.947959f, 30.0016f, 0.0f),
-     Eigen::Vector3f(1.22f, 0.00244382f, 30.0f), Eigen::Vector3f(11.3568f, 10.0463f, 10.0f)}};
+  std::array<Eigen::Vector3f, number_of_points_> expected_pointcloud = {
+    {Eigen::Vector3f(10.0f, 0.0f, 1.0f), Eigen::Vector3f(5.12144f, -4.99853f, 2.0f),
+     Eigen::Vector3f(0.266711f, -9.99986f, 3.0f), Eigen::Vector3f(-4.59472f, -5.00498f, 4.0f),
+     Eigen::Vector3f(-9.45999f, -0.0148422f, 5.0f), Eigen::Vector3f(-4.31006f, 4.99074f, -5.0f),
+     Eigen::Vector3f(0.835072f, 10.0012f, -4.0f), Eigen::Vector3f(6.02463f, 5.0171f, -3.0f),
+     Eigen::Vector3f(9.20872f, 3.03234f, -2.0f), Eigen::Vector3f(10.3956f, 1.04204f, -1.0f)}};
 
   // Verify each point in the undistorted point cloud
   size_t i = 0;
@@ -461,7 +482,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp);
+  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, "", timestamp);
 
   // Generate and process multiple twist messages
   auto twist_msgs = generateTwistMsgs(timestamp);
@@ -477,19 +498,19 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink)
 
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_2d_->undistortPointCloud(true, "", pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(true, false, pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
   sensor_msgs::PointCloud2ConstIterator<float> iter_z(pointcloud, "z");
 
   // Expected undistorted point cloud values
-  std::array<Eigen::Vector3f, 10> expected_pointcloud = {
-    {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.122876f, 9.99996f, 0.0f),
-     Eigen::Vector3f(0.26f, -0.000115049f, 10.0f), Eigen::Vector3f(20.4f, -0.0174931f, 0.0f),
-     Eigen::Vector3f(0.56301f, 19.9996f, 0.0f), Eigen::Vector3f(0.7f, -0.000627014f, 20.0f),
-     Eigen::Vector3f(30.86f, -0.052675f, 0.0f), Eigen::Vector3f(1.1004f, 29.9987f, 0.0f),
-     Eigen::Vector3f(1.22f, -0.00166245f, 30.0f), Eigen::Vector3f(11.4249f, 9.97293f, 10.0f)}};
+  std::array<Eigen::Vector3f, number_of_points_> expected_pointcloud = {
+    {Eigen::Vector3f(10.0f, 0.0f, 1.0f), Eigen::Vector3f(5.11856f, -5.00147f, 2.0f),
+     Eigen::Vector3f(0.254248f, -10.0001f, 3.0f), Eigen::Vector3f(-4.60431f, -4.99592f, 4.0f),
+     Eigen::Vector3f(-9.45999f, 0.0111079f, 5.0f), Eigen::Vector3f(-4.2928f, 5.00656f, -5.0f),
+     Eigen::Vector3f(0.877257f, 9.99908f, -4.0f), Eigen::Vector3f(6.05006f, 4.98867f, -3.0f),
+     Eigen::Vector3f(9.22659f, 2.98069f, -2.0f), Eigen::Vector3f(10.4025f, 0.975451f, -1.0f)}};
 
   // Verify each point in the undistorted point cloud
   size_t i = 0;
@@ -512,7 +533,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, timestamp);
+  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, "", timestamp);
 
   // Generate and process multiple twist messages
   auto twist_msgs = generateTwistMsgs(timestamp);
@@ -528,24 +549,22 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame)
 
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
-  distortion_corrector_2d_->undistortPointCloud(true, "", pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(true, false, pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
   sensor_msgs::PointCloud2ConstIterator<float> iter_z(pointcloud, "z");
 
   // Expected undistorted point cloud values
-  std::array<Eigen::Vector3f, 10> expected_pointcloud = {
-    {Eigen::Vector3f(10.0f, -1.77636e-15f, -4.44089e-16f),
-     Eigen::Vector3f(0.049989f, 10.0608f, 0.0924992f),
-     Eigen::Vector3f(0.106107f, 0.130237f, 10.1986f),
-     Eigen::Vector3f(20.1709f, 0.210011f, 0.32034f),
-     Eigen::Vector3f(0.220674f, 20.2734f, 0.417974f),
-     Eigen::Vector3f(0.274146f, 0.347043f, 20.5341f),
-     Eigen::Vector3f(30.3673f, 0.457564f, 0.700818f),
-     Eigen::Vector3f(0.418014f, 30.5259f, 0.807963f),
-     Eigen::Vector3f(0.464088f, 0.600081f, 30.9292f),
-     Eigen::Vector3f(10.5657f, 10.7121f, 11.094f)}};
+  std::array<Eigen::Vector3f, number_of_points_> expected_pointcloud = {
+    {Eigen::Vector3f(10.0f, -2.63462e-08f, 1.0f), Eigen::Vector3f(5.05137f, -4.93869f, 2.09267f),
+     Eigen::Vector3f(0.112092f, -9.86876f, 3.19715f),
+     Eigen::Vector3f(-4.83021f, -4.80022f, 4.30059f),
+     Eigen::Vector3f(-9.7743f, 0.266927f, 5.40228f),
+     Eigen::Vector3f(-4.69786f, 5.35289f, -4.47032f),
+     Eigen::Vector3f(0.365836f, 10.4368f, -3.33969f),
+     Eigen::Vector3f(5.44511f, 5.53184f, -2.19585f), Eigen::Vector3f(8.52226f, 3.62536f, -1.0538f),
+     Eigen::Vector3f(9.59921f, 1.71784f, 0.0862978f)}};
 
   // Verify each point in the undistorted point cloud
   size_t i = 0;
@@ -568,7 +587,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp);
+  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, "", timestamp);
 
   // Generate and process multiple twist messages
   auto twist_msgs = generateTwistMsgs(timestamp);
@@ -579,19 +598,19 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink)
   // Test using only twist
   distortion_corrector_3d_->initialize();
   distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_3d_->undistortPointCloud(false, "", pointcloud);
+  distortion_corrector_3d_->undistortPointCloud(false, false, pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
   sensor_msgs::PointCloud2ConstIterator<float> iter_z(pointcloud, "z");
 
   // Expected undistorted point cloud values
-  std::array<Eigen::Vector3f, 10> expected_pointcloud = {
-    {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.117f, 10.0f, 0.0f),
-     Eigen::Vector3f(0.26f, 9.27035e-05f, 10.0f), Eigen::Vector3f(20.4f, 0.0222176f, 0.0f),
-     Eigen::Vector3f(0.51f, 20.0004f, 0.0f), Eigen::Vector3f(0.7f, 0.000706573f, 20.0f),
-     Eigen::Vector3f(30.8599f, 0.0760946f, 0.0f), Eigen::Vector3f(0.946998f, 30.0015f, 0.0f),
-     Eigen::Vector3f(1.22f, 0.00234201f, 30.0f), Eigen::Vector3f(11.3569f, 10.046f, 10.0f)}};
+  std::array<Eigen::Vector3f, number_of_points_> expected_pointcloud = {
+    {Eigen::Vector3f(10.0f, 0.0f, 1.0f), Eigen::Vector3f(5.1215f, -4.99848f, 2.0f),
+     Eigen::Vector3f(0.267f, -9.99991f, 3.0f), Eigen::Vector3f(-4.5945f, -5.00528f, 4.0f),
+     Eigen::Vector3f(-9.45999f, -0.0146016f, 5.0f), Eigen::Vector3f(-4.30999f, 4.9907f, -5.0f),
+     Eigen::Vector3f(0.834999f, 10.0011f, -4.0f), Eigen::Vector3f(6.02447f, 5.01714f, -3.0f),
+     Eigen::Vector3f(9.20884f, 3.03192f, -2.0f), Eigen::Vector3f(10.3956f, 1.04182f, -1.0f)}};
 
   // Verify each point in the undistorted point cloud
   size_t i = 0;
@@ -614,7 +633,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp);
+  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, "", timestamp);
 
   // Generate and process multiple twist messages
   auto twist_msgs = generateTwistMsgs(timestamp);
@@ -630,23 +649,22 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink)
 
   distortion_corrector_3d_->initialize();
   distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_3d_->undistortPointCloud(true, "", pointcloud);
+  distortion_corrector_3d_->undistortPointCloud(true, false, pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
   sensor_msgs::PointCloud2ConstIterator<float> iter_z(pointcloud, "z");
 
   // Expected undistorted point cloud values
-  std::array<Eigen::Vector3f, 10> expected_pointcloud = {
-    {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.123015f, 9.99998f, 0.00430552f),
-     Eigen::Vector3f(0.266103f, -0.00895269f, 9.99992f),
-     Eigen::Vector3f(20.4f, -0.0176539f, -0.0193392f),
-     Eigen::Vector3f(0.563265f, 19.9997f, 0.035628f),
-     Eigen::Vector3f(0.734597f, -0.046068f, 19.9993f),
-     Eigen::Vector3f(30.8599f, -0.0517931f, -0.0658165f),
-     Eigen::Vector3f(1.0995f, 29.9989f, 0.0956997f),
-     Eigen::Vector3f(1.31283f, -0.113544f, 29.9977f),
-     Eigen::Vector3f(11.461f, 9.93096f, 10.0035f)}};
+  std::array<Eigen::Vector3f, number_of_points_> expected_pointcloud = {
+    {Eigen::Vector3f(10.0f, 0.0f, 1.0f), Eigen::Vector3f(5.11902f, -5.00239f, 1.9965f),
+     Eigen::Vector3f(0.255945f, -10.0027f, 2.99104f),
+     Eigen::Vector3f(-4.60055f, -5.00116f, 3.99787f),
+     Eigen::Vector3f(-9.45347f, 0.00231253f, 5.01268f),
+     Eigen::Vector3f(-4.30145f, 5.01808f, -4.98054f),
+     Eigen::Vector3f(0.868468f, 10.0103f, -3.97336f),
+     Eigen::Vector3f(6.04213f, 4.99888f, -2.99811f), Eigen::Vector3f(9.22048f, 2.98838f, -2.01552f),
+     Eigen::Vector3f(10.3988f, 0.980287f, -1.03088f)}};
 
   // Verify each point in the undistorted point cloud
   size_t i = 0;
@@ -669,7 +687,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, timestamp);
+  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, "", timestamp);
 
   // Generate and process multiple twist messages
   auto twist_msgs = generateTwistMsgs(timestamp);
@@ -685,7 +703,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame)
 
   distortion_corrector_3d_->initialize();
   distortion_corrector_3d_->setPointCloudTransform("base_link", "lidar_top");
-  distortion_corrector_3d_->undistortPointCloud(true, "", pointcloud);
+  distortion_corrector_3d_->undistortPointCloud(true, false, pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
@@ -693,15 +711,13 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame)
 
   // Expected undistorted point cloud values
   std::array<Eigen::Vector3f, 10> expected_pointcloud = {
-    {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.046484f, 10.0622f, 0.098484f),
-     Eigen::Vector3f(0.107595f, 0.123767f, 10.2026f),
-     Eigen::Vector3f(20.1667f, 0.22465f, 0.313351f),
-     Eigen::Vector3f(0.201149f, 20.2784f, 0.464665f),
-     Eigen::Vector3f(0.290531f, 0.303489f, 20.5452f),
-     Eigen::Vector3f(30.3598f, 0.494116f, 0.672914f),
-     Eigen::Vector3f(0.375848f, 30.5336f, 0.933633f),
-     Eigen::Vector3f(0.510001f, 0.479651f, 30.9493f),
-     Eigen::Vector3f(10.5629f, 10.6855f, 11.1461f)}};
+    {Eigen::Vector3f(10.0f, 4.76837e-07f, 1.0f), Eigen::Vector3f(5.05124f, -4.93713f, 2.09134f),
+     Eigen::Vector3f(0.114215f, -9.86881f, 3.19198f),
+     Eigen::Vector3f(-4.82904f, -4.80461f, 4.30305f),
+     Eigen::Vector3f(-9.77529f, 0.255465f, 5.41964f), Eigen::Vector3f(-4.71431f, 5.36722f, -4.441f),
+     Eigen::Vector3f(0.342873f, 10.4561f, -3.29519f), Eigen::Vector3f(5.426f, 5.55628f, -2.16926f),
+     Eigen::Vector3f(8.50523f, 3.65298f, -1.03512f),
+     Eigen::Vector3f(9.58544f, 1.74465f, 0.0959219f)}};
 
   // Verify each point in the undistorted point cloud
   size_t i = 0;
@@ -723,8 +739,10 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame)
 TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 test2d_pointcloud = generatePointCloudMsg(true, false, timestamp);
-  sensor_msgs::msg::PointCloud2 test3d_pointcloud = generatePointCloudMsg(true, false, timestamp);
+  sensor_msgs::msg::PointCloud2 test2d_pointcloud =
+    generatePointCloudMsg(true, false, "", timestamp);
+  sensor_msgs::msg::PointCloud2 test3d_pointcloud =
+    generatePointCloudMsg(true, false, "", timestamp);
 
   // Generate and process a single twist message with constant linear velocity
   auto twist_msg = generateTwistMsg(1.0, 0.0, timestamp);
@@ -732,16 +750,16 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion)
   distortion_corrector_2d_->processTwistMessage(twist_msg);
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_2d_->undistortPointCloud(false, "", test2d_pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(false, false, test2d_pointcloud);
 
   distortion_corrector_3d_->processTwistMessage(twist_msg);
   distortion_corrector_3d_->initialize();
   distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_3d_->undistortPointCloud(false, "", test3d_pointcloud);
+  distortion_corrector_3d_->undistortPointCloud(false, false, test3d_pointcloud);
 
   // Generate expected point cloud for testing
   sensor_msgs::msg::PointCloud2 expected_pointcloud_msg =
-    generatePointCloudMsg(true, false, timestamp);
+    generatePointCloudMsg(true, false, "", timestamp);
 
   // Calculate expected point cloud values based on constant linear motion
   double velocity = 1.0;  // 1 m/s linear velocity
@@ -810,8 +828,10 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion)
 TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 test2d_pointcloud = generatePointCloudMsg(true, false, timestamp);
-  sensor_msgs::msg::PointCloud2 test3d_pointcloud = generatePointCloudMsg(true, false, timestamp);
+  sensor_msgs::msg::PointCloud2 test2d_pointcloud =
+    generatePointCloudMsg(true, false, "", timestamp);
+  sensor_msgs::msg::PointCloud2 test3d_pointcloud =
+    generatePointCloudMsg(true, false, "", timestamp);
 
   // Generate and process a single twist message with constant angular velocity
   auto twist_msg = generateTwistMsg(0.0, 0.1, timestamp);
@@ -819,16 +839,16 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion)
   distortion_corrector_2d_->processTwistMessage(twist_msg);
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_2d_->undistortPointCloud(false, "", test2d_pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(false, false, test2d_pointcloud);
 
   distortion_corrector_3d_->processTwistMessage(twist_msg);
   distortion_corrector_3d_->initialize();
   distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_3d_->undistortPointCloud(false, "", test3d_pointcloud);
+  distortion_corrector_3d_->undistortPointCloud(false, false, test3d_pointcloud);
 
   // Generate expected point cloud for testing
   sensor_msgs::msg::PointCloud2 expected_pointcloud_msg =
-    generatePointCloudMsg(true, false, timestamp);
+    generatePointCloudMsg(true, false, "", timestamp);
 
   // Calculate expected point cloud values based on constant rotational motion
   double angular_velocity = 0.1;  // 0.1 rad/s rotational velocity
@@ -916,7 +936,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdateAzimuthAndDistan
   // Generate the point cloud message in base_link
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   sensor_msgs::msg::PointCloud2 pointcloud_base_link =
-    generatePointCloudMsg(true, false, timestamp);
+    generatePointCloudMsg(true, false, "", timestamp);
 
   // Generate and process multiple twist messages
   auto twist_msgs = generateTwistMsgs(timestamp);
@@ -933,10 +953,10 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdateAzimuthAndDistan
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
   // set the azimuth coordinate system
-  distortion_corrector_2d_->undistortPointCloud(true, "velodyne", pointcloud_base_link);
+  distortion_corrector_2d_->undistortPointCloud(true, true, pointcloud_base_link);
 
   sensor_msgs::msg::PointCloud2 original_pointcloud_base_link =
-    generatePointCloudMsg(true, false, timestamp);
+    generatePointCloudMsg(true, false, "", timestamp);
 
   sensor_msgs::PointCloud2ConstIterator<float> test_iter_azimuth_base_link(
     pointcloud_base_link, "azimuth");
@@ -969,7 +989,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdateAzimuthAndDistan
   }
 
   // Generate the point cloud message in sensor frame
-  sensor_msgs::msg::PointCloud2 pointcloud_lidar_top = generatePointCloudMsg(true, true, timestamp);
+  sensor_msgs::msg::PointCloud2 pointcloud_lidar_top =
+    generatePointCloudMsg(true, true, "", timestamp);
 
   // Generate and process multiple twist messages
   twist_msgs = generateTwistMsgs(timestamp);
@@ -986,10 +1007,10 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdateAzimuthAndDistan
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
   // set the empty coordinate system
-  distortion_corrector_2d_->undistortPointCloud(true, "", pointcloud_lidar_top);
+  distortion_corrector_2d_->undistortPointCloud(true, false, pointcloud_lidar_top);
 
   sensor_msgs::msg::PointCloud2 original_pointcloud_lidar_top =
-    generatePointCloudMsg(true, true, timestamp);
+    generatePointCloudMsg(true, true, "", timestamp);
 
   sensor_msgs::PointCloud2ConstIterator<float> test_iter_azimuth_lidar_top(
     pointcloud_lidar_top, "azimuth");
@@ -1027,7 +1048,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
 {
   // Generate the point cloud message in sensor frame
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, timestamp);
+  sensor_msgs::msg::PointCloud2 pointcloud =
+    generatePointCloudMsg(true, true, "velodyne", timestamp);
 
   // Generate and process multiple twist messages
   auto twist_msgs = generateTwistMsgs(timestamp);
@@ -1044,25 +1066,26 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
   // set the azimuth coordinate system
-  distortion_corrector_2d_->undistortPointCloud(true, "velodyne", pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(true, true, pointcloud);
 
-  sensor_msgs::msg::PointCloud2 original_pointcloud = generatePointCloudMsg(true, false, timestamp);
+  sensor_msgs::msg::PointCloud2 original_pointcloud =
+    generatePointCloudMsg(true, true, "velodyne", timestamp);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_azimuth(pointcloud, "azimuth");
   sensor_msgs::PointCloud2ConstIterator<float> iter_distance(pointcloud, "distance");
 
   // Expected undistorted azimuth and distance values
   std::array<std::array<float, 2>, 10> expected_distance_azimuth = {{
-    {0.0f, 10.0f},         // points: (10.0f, -1.77636e-15f, -4.44089e-16f)
-    {4.71736f, 10.0614f},  // points: (0.049989f, 10.0608f, 0.0924992f)
-    {5.39603f, 10.2f},     // points: (0.106107f, 0.130237f, 10.1986f)
-    {6.27278f, 20.1745f},  // points: (20.1709f, 0.210011f, 0.32034f)
-    {4.72327f, 20.2789f},  // points: (0.220674f, 20.2734f, 0.417974f)
-    {5.38091f, 20.5389f},  // points: (0.274146f, 0.347043f, 20.5341f)
-    {6.26812f, 30.3789f},  // points: (30.3673f, 0.457564f, 0.700818f)
-    {4.72608f, 30.5395f},  // points: (0.418014f, 30.5259f, 0.807963f)
-    {5.3706f, 30.9385f},   // points: (0.464088f, 0.600081f, 30.9292f)
-    {5.49086f, 18.6938f}   // points: (10.5657f, 10.7121f, 11.094f)
+    {0.0f, 10.0499f},      // points: (10.0f, -1.77636e-15f, -4.44089e-16f)
+    {0.77413f, 7.36792f},  // points: (0.049989f, 10.0608f, 0.0924992f)
+    {1.55944f, 10.3743f},  // points: (0.106107f, 0.130237f, 10.1986f)
+    {2.35942f, 8.05408f},  // points: (20.1709f, 0.210011f, 0.32034f)
+    {3.16889f, 11.1711f},  // points: (0.220674f, 20.2734f, 0.417974f)
+    {3.99196f, 8.40875f},  // points: (0.274146f, 0.347043f, 20.5341f)
+    {4.74742f, 10.9642f},  // points: (30.3673f, 0.457564f, 0.700818f)
+    {5.48985f, 8.06673f},  // points: (0.418014f, 30.5259f, 0.807963f)
+    {5.8809f, 9.32108f},   // points: (0.464088f, 0.600081f, 30.9292f)
+    {6.10611f, 9.75209f}   // points: (10.5657f, 10.7121f, 11.094f)
   }};
 
   size_t i = 0;
@@ -1072,7 +1095,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   for (; iter_azimuth != iter_azimuth.end(); ++iter_azimuth, ++iter_distance, ++i) {
     oss << "Point " << i << " - Output azimuth and distance: (" << *iter_azimuth << ", "
         << *iter_distance << ")"
-        << " vs Original azimuth and distance: (" << expected_distance_azimuth[i][0] << ", "
+        << " vs Expected azimuth and distance: (" << expected_distance_azimuth[i][0] << ", "
         << expected_distance_azimuth[i][1] << ")\n";
 
     EXPECT_NEAR(*iter_azimuth, expected_distance_azimuth[i][0], standard_tolerance_);
@@ -1088,7 +1111,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
 {
   // Generate the point cloud message in sensor frame
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, timestamp);
+  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, "hesai", timestamp);
 
   // Generate and process multiple twist messages
   auto twist_msgs = generateTwistMsgs(timestamp);
@@ -1105,26 +1128,26 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
   // set the azimuth coordinate system
-  distortion_corrector_2d_->undistortPointCloud(true, "hesai", pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(true, true, pointcloud);
 
-  sensor_msgs::msg::PointCloud2 original_pointcloud = generatePointCloudMsg(true, false, timestamp);
+  sensor_msgs::msg::PointCloud2 original_pointcloud =
+    generatePointCloudMsg(true, true, "hesai", timestamp);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_azimuth(pointcloud, "azimuth");
   sensor_msgs::PointCloud2ConstIterator<float> iter_distance(pointcloud, "distance");
 
   // Expected undistorted azimuth and distance values
-  std::array<std::array<float, 2>, 10> expected_distance_azimuth = {{
-    {1.5708f, 10.0f},         // points: (10.0f, -1.77636e-15f, -4.44089e-16f)
-    {0.00496759f, 10.0614f},  // points: (0.049989f, 10.0608f, 0.0924992f)
-    {0.683643f, 10.2f},       // points: (0.106107f, 0.130237f, 10.1986f)
-    {1.56039f, 20.1745f},     // points: (20.1709f, 0.210011f, 0.32034f)
-    {0.0108822f, 20.2789f},   // points: (0.220674f, 20.2734f, 0.417974f)
-    {0.668525f, 20.5389f},    // points: (0.274146f, 0.347043f, 20.5341f)
-    {1.55573f, 30.3789f},     // points: (30.3673f, 0.457564f, 0.700818f)
-    {0.01369f, 30.5395f},     // points: (0.418014f, 30.5259f, 0.807963f)
-    {0.658213f, 30.9385f},    // points: (0.464088f, 0.600081f, 30.9292f)
-    {0.778473f, 18.6938f}     // points: (10.5657f, 10.7121f, 11.094f)
-  }};
+  std::array<std::array<float, 2>, 10> expected_distance_azimuth = {
+    {{-4.71239f, 10.0499f},
+     {-3.93826f, 7.36792f},
+     {-3.15295f, 10.3743f},
+     {-2.35297f, 8.05408f},
+     {-1.5435f, 11.1711f},
+     {-0.720431f, 8.40875f},
+     {0.0350311f, 10.9642f},
+     {0.777465f, 8.06673f},
+     {1.16851f, 9.32108f},
+     {1.39372f, 9.75209f}}};
 
   size_t i = 0;
   std::ostringstream oss;
@@ -1133,7 +1156,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   for (; iter_azimuth != iter_azimuth.end(); ++iter_azimuth, ++iter_distance, ++i) {
     oss << "Point " << i << " - Output azimuth and distance: (" << *iter_azimuth << ", "
         << *iter_distance << ")"
-        << " vs Original azimuth and distance: (" << expected_distance_azimuth[i][0] << ", "
+        << " vs Expected azimuth and distance: (" << expected_distance_azimuth[i][0] << ", "
         << expected_distance_azimuth[i][1] << ")\n";
 
     EXPECT_NEAR(*iter_azimuth, expected_distance_azimuth[i][0], standard_tolerance_);

From 9ac26896b6ba9ab85868ca2282286a51cbbeb713 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Tue, 13 Aug 2024 16:23:42 +0900
Subject: [PATCH 07/72] chore: fix tests for AzimuthConversionExists function

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector.hpp                  |  17 ++-
 .../distortion_corrector_node.hpp             |   2 +
 .../distortion_corrector.cpp                  | 133 +++++++++---------
 .../distortion_corrector_node.cpp             |  20 ++-
 .../test/test_distortion_corrector_node.cpp   |  57 +++++---
 5 files changed, 138 insertions(+), 91 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
index dd99300b1891c..309e7c2e2d3f6 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -45,7 +45,7 @@
 #include <deque>
 #include <memory>
 #include <string>
-#include <utility>
+#include <tuple>
 
 namespace autoware::pointcloud_preprocessor
 {
@@ -65,8 +65,10 @@ class DistortionCorrectorBase
   virtual void setPointCloudTransform(
     const std::string & base_frame, const std::string & lidar_frame) = 0;
   virtual void initialize() = 0;
+  virtual bool AzimuthConversionExists(sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
   virtual void undistortPointCloud(
-    bool use_imu, bool update_azimuth_and_distance, sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
+    bool use_imu, bool can_update_azimuth_and_distance,
+    sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
 };
 
 template <class T>
@@ -87,8 +89,11 @@ class DistortionCorrector : public DistortionCorrectorBase
   // sensor azimuth coordinates = a + b * cartesian coordinates;
   // a is restricted to be a multiple of 90, and b is restricted to be 1 or -1.
   bool first_time_{true};
-  float a_{0};
-  float b_{1};
+  bool success_{true};
+  float threshold_a_{0.087f};  // 5 / 180 * M_PI
+  float threshold_b_{0.1f};
+  float adjusted_a_{0};
+  float adjusted_b_{1};
 
   void getIMUTransformation(const std::string & base_frame, const std::string & imu_frame);
   void enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg);
@@ -126,9 +131,7 @@ class DistortionCorrector : public DistortionCorrectorBase
   void undistortPointCloud(
     bool use_imu, bool update_azimuth_and_distance,
     sensor_msgs::msg::PointCloud2 & pointcloud) override;
-  std::pair<float, float> getAzimuthConversion(
-    float point1_azimuth_rad, float point1_cartesian_deg, float point2_azimuth_rad,
-    float point2_cartesian_deg);
+  bool AzimuthConversionExists(sensor_msgs::msg::PointCloud2 & pointcloud) override;
 
   bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud);
 };
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 4d5f775341f3f..a7df01767d780 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
@@ -54,6 +54,8 @@ class DistortionCorrectorComponent : public rclcpp::Node
   bool use_3d_distortion_correction_;
   bool update_azimuth_and_distance_;
 
+  bool can_update_azimuth_and_distance_{false};
+
   std::unique_ptr<DistortionCorrectorBase> distortion_corrector_;
 
   void onPointCloud(PointCloud2::UniquePtr points_msg);
diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index f040edeba8c2d..a0de2fc0e399c 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -188,54 +188,79 @@ bool DistortionCorrector<T>::isInputValid(sensor_msgs::msg::PointCloud2 & pointc
 }
 
 template <class T>
-std::pair<float, float> DistortionCorrector<T>::getAzimuthConversion(
-  float point1_azimuth_rad, float point1_cartesian_deg, float point2_azimuth_rad,
-  float point2_cartesian_deg)
+bool DistortionCorrector<T>::AzimuthConversionExists(sensor_msgs::msg::PointCloud2 & pointcloud)
 {
-  float point1_azimuth_deg = point1_azimuth_rad * 180 / M_PI;
-  float point2_azimuth_deg = point2_azimuth_rad * 180 / M_PI;
-
-  float b =
-    (point2_azimuth_deg - point1_azimuth_deg) / (point2_cartesian_deg - point1_cartesian_deg);
-  float a = point1_azimuth_deg - b * point1_cartesian_deg;
-
-  bool error_flag = false;
-  // Check if b can be adjusted to 1 or -1
-  if (std::abs(b - 1) <= 0.1) {
-    b = 1;
-  } else if (std::abs(b + 1) <= 0.1) {
-    b = -1;
+  if (!isInputValid(pointcloud)) return false;
+
+  sensor_msgs::PointCloud2Iterator<float> it_x(pointcloud, "x");
+  sensor_msgs::PointCloud2Iterator<float> it_y(pointcloud, "y");
+  sensor_msgs::PointCloud2Iterator<float> it_azimuth(pointcloud, "azimuth");
+
+  auto next_it_x = it_x;
+  auto next_it_y = it_y;
+  auto next_it_azimuth = it_azimuth;
+
+  if (it_x != it_x.end() && it_x + 1 != it_x.end()) {
+    next_it_x = it_x + 1;
+    next_it_y = it_y + 1;
+    next_it_azimuth = it_azimuth + 1;
   } else {
-    error_flag = true;
+    return false;
   }
 
-  // Check if a can be adjusted to a multiple of 90
-  int nearest_multiple_of_90 = static_cast<int>(round(a / 90.0)) * 90;
-  if (std::abs(a - nearest_multiple_of_90) <= 5) {
-    a = nearest_multiple_of_90;
-    if (a > 360) {
-      a -= 360;
-    } else if (a < -360) {
-      a += 360;
+  for (; next_it_x != it_x.end();
+       ++it_x, ++it_y, ++it_azimuth, ++next_it_x, ++next_it_y, ++next_it_azimuth) {
+    auto current_cartesian_rad = cv::fastAtan2(*it_y, *it_x) * M_PI / 180;
+    auto next_cartesian_rad = cv::fastAtan2(*next_it_y, *next_it_x) * M_PI / 180;
+
+    // If the angle exceeds 180 degrees, it may cross the 0-degree axis,
+    // which could disrupt the calculation of the formula.
+    if (
+      *next_it_azimuth - *it_azimuth >= M_PI ||
+      next_cartesian_rad - current_cartesian_rad >= M_PI) {
+      continue;
     }
-  } else {
-    error_flag = true;
-  }
 
-  if (error_flag) {
-    a = 0;
-    b = 1;
-    std::cerr << "ERROR!!!" << std::endl;
-    // print error message!!!!
-  }
+    float b = (*next_it_azimuth - *it_azimuth) / (next_cartesian_rad - current_cartesian_rad);
+    float a = *it_azimuth - b * current_cartesian_rad;
+
+    adjusted_b_ = b;
+    adjusted_a_ = a;
+
+    // Check if 'b' can be adjusted to 1 or -1
+    if (std::abs(b - 1.0f) <= threshold_b_) {
+      adjusted_b_ = 1.0f;
+    } else if (std::abs(b + 1.0f) <= threshold_b_) {
+      adjusted_b_ = -1.0f;
+    } else {
+      continue;
+    }
+
+    // Check if 'a' can be adjusted to a multiple of π/2
+    int multiple_of_90_degrees = std::round(a / (M_PI / 2));
+    if (std::abs(a - multiple_of_90_degrees * (M_PI / 2)) > threshold_a_) {
+      continue;
+    }
+
+    if (multiple_of_90_degrees < 0) {
+      multiple_of_90_degrees += 4;
+    } else if (multiple_of_90_degrees > 4) {
+      multiple_of_90_degrees -= 4;
+    }
+
+    adjusted_a_ = multiple_of_90_degrees * (M_PI / 2);
 
-  std::cout << "Adjusted a: " << a << " Adjusted b: " << b << std::endl;
-  return {a, b};
+    std::cout << "adjusted_a_: " << adjusted_a_ << std::endl;
+    std::cout << "adjusted_b_: " << adjusted_b_ << std::endl;
+
+    return true;
+  }
+  return false;
 }
 
 template <class T>
 void DistortionCorrector<T>::undistortPointCloud(
-  bool use_imu, bool update_azimuth_and_distance, sensor_msgs::msg::PointCloud2 & pointcloud)
+  bool use_imu, bool can_update_azimuth_and_distance, sensor_msgs::msg::PointCloud2 & pointcloud)
 {
   if (!isInputValid(pointcloud)) return;
 
@@ -303,38 +328,18 @@ void DistortionCorrector<T>::undistortPointCloud(
 
     float time_offset = static_cast<float>(global_point_stamp - prev_time_stamp_sec);
 
-    std::cout << "before" << std::endl;
-    std::cout << " *it_x: " << *it_x << " *it_y: " << *it_y << " *it_azimuth: " << *it_azimuth
-              << std::endl;
-
     // Undistort a single point based on the strategy
     undistortPoint(it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid);
 
-    if (update_azimuth_and_distance && pointcloudTransformNeeded()) {
-      // Input frame should be in the sensor frame.
+    if (can_update_azimuth_and_distance && pointcloudTransformNeeded()) {
+      float cartesian_coordinate_azimuth = cv::fastAtan2(*it_y, *it_x) * M_PI / 180;
+      float updated_azimuth = adjusted_a_ + adjusted_b_ * cartesian_coordinate_azimuth;
+      // if(updated_azimuth < 0) {
+      //   //TODO(vivid):
+      // }
+      *it_azimuth = updated_azimuth;
       *it_distance = sqrt(*it_x * *it_x + *it_y * *it_y + *it_z * *it_z);
 
-      if (first_time_) {
-        // std::cout << "*it_x: " << *it_x << " *it_y: " << *it_y << " *it_azimuth: " << *it_azimuth
-        // << std::endl;
-        auto next_azimuth = it_azimuth + 1;
-        auto next_x = it_x + 1;
-        auto next_y = it_y + 1;
-        // std::cout << "after" << std::endl;
-        // std::cout << "*it_x: " << *it_x << " *it_y: " << *it_y << " *it_azimuth: " << *it_azimuth
-        // << std::endl; std::cout << "*next_x: " << *next_x << " *next_y: " << *next_y << "
-        // *next_azimuth: " << *next_azimuth << std::endl;
-        std::tie(a_, b_) = getAzimuthConversion(
-          *it_azimuth, cv::fastAtan2(*it_y, *it_x), *next_azimuth, cv::fastAtan2(*next_y, *next_x));
-        first_time_ = false;
-      }
-
-      float cartesian_coordinate_azimuth = cv::fastAtan2(*it_y, *it_x);
-      float updated_azimuth = (a_ + b_ * cartesian_coordinate_azimuth) * M_PI / 180;
-      *it_azimuth = updated_azimuth;
-      // std::cout << "after" << std::endl;
-      // std::cout << " *it_x: " << *it_x << " *it_y: " << *it_y << " *it_azimuth: " << *it_azimuth
-      // << std::endl;
       ++it_azimuth;
       ++it_distance;
     }
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 97ace13686b0d..df3af81902e3a 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
@@ -93,8 +93,26 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointclou
 
   distortion_corrector_->setPointCloudTransform(base_frame_, pointcloud_msg->header.frame_id);
   distortion_corrector_->initialize();
+
+  if (update_azimuth_and_distance_ && !can_update_azimuth_and_distance_) {
+    can_update_azimuth_and_distance_ =
+      distortion_corrector_->AzimuthConversionExists(*pointcloud_msg);
+    if (can_update_azimuth_and_distance_) {
+      RCLCPP_INFO(
+        this->get_logger(),
+        "Success to get the conversion formula between cartesian coordinates and LiDAR azimuth "
+        "coordinates");
+    } else {
+      RCLCPP_ERROR(
+        this->get_logger(),
+        "Failed to get the conversion formula between cartesian coordinates and LiDAR azimuth "
+        "coordinates. Please check the LiDAR azimuth coordinate. This pointcloud will not update "
+        "azimuth and distance");
+    }
+  }
+
   distortion_corrector_->undistortPointCloud(
-    use_imu_, update_azimuth_and_distance_, *pointcloud_msg);
+    use_imu_, can_update_azimuth_and_distance_, *pointcloud_msg);
 
   if (debug_publisher_) {
     auto pipeline_latency_ms =
diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index 4681020e60327..3f7dd5f560160 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -236,9 +236,6 @@ class DistortionCorrectorTest : public ::testing::Test
           *iter_azimuth = std::atan2(points[i].y(), points[i].x());
         }
 
-        std::cout << "point " << i << ": "
-                  << "x : " << *iter_x << " y: " << *iter_y << " azimuth: " << *iter_azimuth
-                  << std::endl;
         *iter_distance = points[i].norm();
         *iter_t = timestamps[i];
         ++iter_x;
@@ -931,7 +928,6 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdateAzimuthAndDistan
 {
   // Test the case when the cloud will not update the azimuth and distance values
   // 1. when pointcloud is in the base_link
-  // 2. when user didn't specify the sensor azimuth coordinate
 
   // Generate the point cloud message in base_link
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -952,8 +948,11 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdateAzimuthAndDistan
 
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
-  // set the azimuth coordinate system
-  distortion_corrector_2d_->undistortPointCloud(true, true, pointcloud_base_link);
+  auto can_update_azimuth_and_distance =
+    distortion_corrector_2d_->AzimuthConversionExists(pointcloud_base_link);
+
+  distortion_corrector_2d_->undistortPointCloud(
+    true, can_update_azimuth_and_distance, pointcloud_base_link);
 
   sensor_msgs::msg::PointCloud2 original_pointcloud_base_link =
     generatePointCloudMsg(true, false, "", timestamp);
@@ -988,6 +987,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdateAzimuthAndDistan
     RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str());
   }
 
+  // Test the case when the cloud will not update the azimuth and distance values
+  // 2. when "can_update_azimuth_and_distance" is false
+
   // Generate the point cloud message in sensor frame
   sensor_msgs::msg::PointCloud2 pointcloud_lidar_top =
     generatePointCloudMsg(true, true, "", timestamp);
@@ -1007,7 +1009,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdateAzimuthAndDistan
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
   // set the empty coordinate system
-  distortion_corrector_2d_->undistortPointCloud(true, false, pointcloud_lidar_top);
+  can_update_azimuth_and_distance = false;
+  distortion_corrector_2d_->undistortPointCloud(
+    true, can_update_azimuth_and_distance, pointcloud_lidar_top);
 
   sensor_msgs::msg::PointCloud2 original_pointcloud_lidar_top =
     generatePointCloudMsg(true, true, "", timestamp);
@@ -1066,7 +1070,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
   // set the azimuth coordinate system
-  distortion_corrector_2d_->undistortPointCloud(true, true, pointcloud);
+  auto can_update_azimuth_and_distance =
+    distortion_corrector_2d_->AzimuthConversionExists(pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(true, can_update_azimuth_and_distance, pointcloud);
 
   sensor_msgs::msg::PointCloud2 original_pointcloud =
     generatePointCloudMsg(true, true, "velodyne", timestamp);
@@ -1076,16 +1082,16 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
 
   // Expected undistorted azimuth and distance values
   std::array<std::array<float, 2>, 10> expected_distance_azimuth = {{
-    {0.0f, 10.0499f},      // points: (10.0f, -1.77636e-15f, -4.44089e-16f)
-    {0.77413f, 7.36792f},  // points: (0.049989f, 10.0608f, 0.0924992f)
-    {1.55944f, 10.3743f},  // points: (0.106107f, 0.130237f, 10.1986f)
-    {2.35942f, 8.05408f},  // points: (20.1709f, 0.210011f, 0.32034f)
-    {3.16889f, 11.1711f},  // points: (0.220674f, 20.2734f, 0.417974f)
-    {3.99196f, 8.40875f},  // points: (0.274146f, 0.347043f, 20.5341f)
-    {4.74742f, 10.9642f},  // points: (30.3673f, 0.457564f, 0.700818f)
-    {5.48985f, 8.06673f},  // points: (0.418014f, 30.5259f, 0.807963f)
-    {5.8809f, 9.32108f},   // points: (0.464088f, 0.600081f, 30.9292f)
-    {6.10611f, 9.75209f}   // points: (10.5657f, 10.7121f, 11.094f)
+    {0.0f, 10.0499f},      // points: (10.0f, -2.63462e-08f, 1.0f)
+    {0.77413f, 7.36792f},  // points: (5.05137f, -4.93869f, 2.09267f)
+    {1.55944f, 10.3743f},  // points: (0.112092f, -9.86876f, 3.19715f)
+    {2.35942f, 8.05408f},  // points: (-4.83021f, -4.80022f, 4.30059f)
+    {3.16889f, 11.1711f},  // points: (-9.7743f, 0.266927f, 5.40228f)
+    {3.99196f, 8.40875f},  // points: (-4.69786f, 5.35289f, -4.47032f)
+    {4.74742f, 10.9642f},  // points: (0.365836f, 10.4368f, -3.33969f)
+    {5.48985f, 8.06673f},  // points: (5.44511f, 5.53184f, -2.19585f)
+    {5.8809f, 9.32108f},   // points: (8.52226f, 3.62536f, -1.0538f)
+    {6.10611f, 9.75209f}   // points: (9.59921f, 1.71784f, 0.0862978f)
   }};
 
   size_t i = 0;
@@ -1128,7 +1134,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
   // set the azimuth coordinate system
-  distortion_corrector_2d_->undistortPointCloud(true, true, pointcloud);
+  auto can_update_azimuth_and_distance =
+    distortion_corrector_2d_->AzimuthConversionExists(pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(true, can_update_azimuth_and_distance, pointcloud);
 
   sensor_msgs::msg::PointCloud2 original_pointcloud =
     generatePointCloudMsg(true, true, "hesai", timestamp);
@@ -1168,6 +1176,17 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   }
 }
 
+TEST_F(DistortionCorrectorTest, TestAzimuthConversionExists)
+{
+  // test empty pointcloud
+  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  sensor_msgs::msg::PointCloud2 empty_pointcloud =
+    generatePointCloudMsg(false, false, "", timestamp);
+  EXPECT_FALSE(distortion_corrector_2d_->AzimuthConversionExists(empty_pointcloud));
+
+  // test velodyne pointcloud (x-axis: 0 degree, y-axis: 270 degree)
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

From 0990c023ebb5529aa2f0da063e16ad871bfb7b47 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Wed, 14 Aug 2024 15:48:57 +0900
Subject: [PATCH 08/72] feat: add fastatan to utils

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../universe_utils/math/trigonometry.hpp      |   2 +
 .../src/math/trigonometry.cpp                 |  30 ++
 .../test/src/math/test_trigonometry.cpp       |  25 ++
 .../distortion_corrector.hpp                  |   8 +-
 .../distortion_corrector.cpp                  |  57 ++--
 .../test/test_distortion_corrector_node.cpp   | 264 +++++++++++++-----
 6 files changed, 296 insertions(+), 90 deletions(-)

diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp
index 19a59523c7f08..77be77909c4da 100644
--- a/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp
+++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp
@@ -26,6 +26,8 @@ float cos(float radian);
 
 std::pair<float, float> sin_and_cos(float radian);
 
+float opencv_fastAtan2(float dy, float dx);
+
 }  // namespace autoware::universe_utils
 
 #endif  // AUTOWARE__UNIVERSE_UTILS__MATH__TRIGONOMETRY_HPP_
diff --git a/common/autoware_universe_utils/src/math/trigonometry.cpp b/common/autoware_universe_utils/src/math/trigonometry.cpp
index 586b9075ba6d5..02dda7a396a8a 100644
--- a/common/autoware_universe_utils/src/math/trigonometry.cpp
+++ b/common/autoware_universe_utils/src/math/trigonometry.cpp
@@ -72,4 +72,34 @@ std::pair<float, float> sin_and_cos(float radian)
   }
 }
 
+static const float atan2_p1 =
+  0.9997878412794807f * static_cast<float>(180) / autoware::universe_utils::pi;
+static const float atan2_p3 =
+  -0.3258083974640975f * static_cast<float>(180) / autoware::universe_utils::pi;
+static const float atan2_p5 =
+  0.1555786518463281f * static_cast<float>(180) / autoware::universe_utils::pi;
+static const float atan2_p7 =
+  -0.04432655554792128f * static_cast<float>(180) / autoware::universe_utils::pi;
+static const float atan2_DBL_EPSILON = 2.2204460492503131e-016f;
+
+float opencv_fastAtan2(float dy, float dx)
+{
+  float ax = std::abs(dx), ay = std::abs(dy);
+  float a, c, c2;
+  if (ax >= ay) {
+    c = ay / (ax + atan2_DBL_EPSILON);
+    c2 = c * c;
+    a = (((atan2_p7 * c2 + atan2_p5) * c2 + atan2_p3) * c2 + atan2_p1) * c;
+  } else {
+    c = ax / (ay + atan2_DBL_EPSILON);
+    c2 = c * c;
+    a = 90.f - (((atan2_p7 * c2 + atan2_p5) * c2 + atan2_p3) * c2 + atan2_p1) * c;
+  }
+  if (dx < 0) a = 180.f - a;
+  if (dy < 0) a = 360.f - a;
+
+  a = a * autoware::universe_utils::pi / 180.f;
+  return a;
+}
+
 }  // namespace autoware::universe_utils
diff --git a/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp
index b55b27a34a6ac..cba601ba7894f 100644
--- a/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp
+++ b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp
@@ -50,3 +50,28 @@ TEST(trigonometry, sin_and_cos)
     EXPECT_TRUE(std::abs(std::cos(x * static_cast<float>(i)) - sin_and_cos.second) < 10e-7);
   }
 }
+
+float normalize_angle(double angle)
+{
+  if (angle < 0) {
+    return angle + 2 * autoware::universe_utils::pi;
+  }
+  return angle;
+}
+
+TEST(trigonometry, opencv_fastAtan2)
+{
+  for (int i = 0; i < 100; ++i) {
+    // Generate random x and y between -10 and 10
+    float x = static_cast<float>(std::rand()) / RAND_MAX * 20.0 - 10.0;
+    float y = static_cast<float>(std::rand()) / RAND_MAX * 20.0 - 10.0;
+
+    float fast_atan = autoware::universe_utils::opencv_fastAtan2(y, x);
+    float std_atan = normalize_angle(std::atan2(y, x));
+
+    // 0.3 degree accuracy
+    ASSERT_NEAR(fast_atan, std_atan, 6e-3)
+      << "Test failed for input (" << y << ", " << x << "): "
+      << "fastatan2 = " << fast_atan << ", std::atan2 = " << std_atan;
+  }
+}
diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
index 309e7c2e2d3f6..3a3ee1e1df06f 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -36,8 +36,6 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
 
-#include <opencv2/opencv.hpp>
-
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/buffer_interface.h>
 #include <tf2_ros/transform_listener.h>
@@ -66,6 +64,7 @@ class DistortionCorrectorBase
     const std::string & base_frame, const std::string & lidar_frame) = 0;
   virtual void initialize() = 0;
   virtual bool AzimuthConversionExists(sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
+  virtual std::tuple<float, float> getConversion() = 0;
   virtual void undistortPointCloud(
     bool use_imu, bool can_update_azimuth_and_distance,
     sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
@@ -92,8 +91,8 @@ class DistortionCorrector : public DistortionCorrectorBase
   bool success_{true};
   float threshold_a_{0.087f};  // 5 / 180 * M_PI
   float threshold_b_{0.1f};
-  float adjusted_a_{0};
-  float adjusted_b_{1};
+  float a_{0};
+  float b_{1};
 
   void getIMUTransformation(const std::string & base_frame, const std::string & imu_frame);
   void enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg);
@@ -132,6 +131,7 @@ class DistortionCorrector : public DistortionCorrectorBase
     bool use_imu, bool update_azimuth_and_distance,
     sensor_msgs::msg::PointCloud2 & pointcloud) override;
   bool AzimuthConversionExists(sensor_msgs::msg::PointCloud2 & pointcloud) override;
+  std::tuple<float, float> getConversion() override;
 
   bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud);
 };
diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index a0de2fc0e399c..42cef560cc8c5 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -15,6 +15,7 @@
 #include "autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp"
 
 #include "autoware/pointcloud_preprocessor/utility/memory.hpp"
+#include "autoware/universe_utils/math/constants.hpp"
 
 #include <autoware/universe_utils/math/trigonometry.hpp>
 #include <tf2_eigen/tf2_eigen.hpp>
@@ -155,7 +156,7 @@ bool DistortionCorrector<T>::isInputValid(sensor_msgs::msg::PointCloud2 & pointc
   if (pointcloud.data.empty() || twist_queue_.empty()) {
     RCLCPP_WARN_STREAM_THROTTLE(
       node_->get_logger(), *node_->get_clock(), 10000 /* ms */,
-      "input pointcloud or twist_queue_ is empty.");
+      "Input pointcloud or twist_queue_ is empty.");
     return false;
   }
 
@@ -205,59 +206,70 @@ bool DistortionCorrector<T>::AzimuthConversionExists(sensor_msgs::msg::PointClou
     next_it_y = it_y + 1;
     next_it_azimuth = it_azimuth + 1;
   } else {
+    RCLCPP_WARN(
+      node_->get_logger(),
+      "Current point cloud only has a single point cloud. Could not calculate the formula.");
     return false;
   }
 
   for (; next_it_x != it_x.end();
        ++it_x, ++it_y, ++it_azimuth, ++next_it_x, ++next_it_y, ++next_it_azimuth) {
-    auto current_cartesian_rad = cv::fastAtan2(*it_y, *it_x) * M_PI / 180;
-    auto next_cartesian_rad = cv::fastAtan2(*next_it_y, *next_it_x) * M_PI / 180;
+    auto current_cartesian_rad = autoware::universe_utils::opencv_fastAtan2(*it_y, *it_x);
+    auto next_cartesian_rad = autoware::universe_utils::opencv_fastAtan2(*next_it_y, *next_it_x);
 
     // If the angle exceeds 180 degrees, it may cross the 0-degree axis,
     // which could disrupt the calculation of the formula.
     if (
-      *next_it_azimuth - *it_azimuth >= M_PI ||
-      next_cartesian_rad - current_cartesian_rad >= M_PI) {
+      abs(*next_it_azimuth - *it_azimuth) >= autoware::universe_utils::pi ||
+      abs(next_cartesian_rad - current_cartesian_rad) >= autoware::universe_utils::pi) {
+      RCLCPP_WARN(
+        node_->get_logger(),
+        "Angle between two points exceeds 180 degrees. Iterate to next point ...");
       continue;
     }
 
     float b = (*next_it_azimuth - *it_azimuth) / (next_cartesian_rad - current_cartesian_rad);
     float a = *it_azimuth - b * current_cartesian_rad;
 
-    adjusted_b_ = b;
-    adjusted_a_ = a;
-
     // Check if 'b' can be adjusted to 1 or -1
     if (std::abs(b - 1.0f) <= threshold_b_) {
-      adjusted_b_ = 1.0f;
+      b_ = 1.0f;
     } else if (std::abs(b + 1.0f) <= threshold_b_) {
-      adjusted_b_ = -1.0f;
+      b_ = -1.0f;
     } else {
+      RCLCPP_WARN(
+        node_->get_logger(),
+        "Angle between two points exceeds 180 degrees. Iterate to next point ...");
+      std::cout << "b: " << b << "should be close to 1 or -1" << std::endl;
       continue;
     }
 
     // Check if 'a' can be adjusted to a multiple of π/2
-    int multiple_of_90_degrees = std::round(a / (M_PI / 2));
-    if (std::abs(a - multiple_of_90_degrees * (M_PI / 2)) > threshold_a_) {
+    int multiple_of_90_degrees = std::round(a / (autoware::universe_utils::pi / 2));
+    if (std::abs(a - multiple_of_90_degrees * (autoware::universe_utils::pi / 2)) > threshold_a_) {
       continue;
     }
 
+    // Limit the range of a in [0, 360]
     if (multiple_of_90_degrees < 0) {
       multiple_of_90_degrees += 4;
     } else if (multiple_of_90_degrees > 4) {
       multiple_of_90_degrees -= 4;
     }
 
-    adjusted_a_ = multiple_of_90_degrees * (M_PI / 2);
-
-    std::cout << "adjusted_a_: " << adjusted_a_ << std::endl;
-    std::cout << "adjusted_b_: " << adjusted_b_ << std::endl;
+    a_ = multiple_of_90_degrees * (autoware::universe_utils::pi / 2);
 
     return true;
   }
   return false;
 }
 
+template <class T>
+std::tuple<float, float> DistortionCorrector<T>::getConversion()
+{
+  return std::make_tuple(a_, b_);
+}
+
 template <class T>
 void DistortionCorrector<T>::undistortPointCloud(
   bool use_imu, bool can_update_azimuth_and_distance, sensor_msgs::msg::PointCloud2 & pointcloud)
@@ -332,11 +344,14 @@ void DistortionCorrector<T>::undistortPointCloud(
     undistortPoint(it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid);
 
     if (can_update_azimuth_and_distance && pointcloudTransformNeeded()) {
-      float cartesian_coordinate_azimuth = cv::fastAtan2(*it_y, *it_x) * M_PI / 180;
-      float updated_azimuth = adjusted_a_ + adjusted_b_ * cartesian_coordinate_azimuth;
-      // if(updated_azimuth < 0) {
-      //   //TODO(vivid):
-      // }
+      float cartesian_coordinate_azimuth = autoware::universe_utils::opencv_fastAtan2(*it_y, *it_x);
+      float updated_azimuth = a_ + b_ * cartesian_coordinate_azimuth;
+      if (updated_azimuth < 0) {
+        updated_azimuth += autoware::universe_utils::pi * 2;
+      } else if (updated_azimuth > 2 * autoware::universe_utils::pi) {
+        updated_azimuth -= autoware::universe_utils::pi * 2;
+      }
+
       *it_azimuth = updated_azimuth;
       *it_distance = sqrt(*it_x * *it_x + *it_y * *it_y + *it_z * *it_z);
 
diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index 3f7dd5f560160..962acb8edc582 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -26,6 +26,7 @@
 // 10.09, 10.117, 10.144, 10.171, 10.198, 10.225
 
 #include "autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp"
+#include "autoware/universe_utils/math/constants.hpp"
 #include "autoware/universe_utils/math/trigonometry.hpp"
 
 #include <rclcpp/rclcpp.hpp>
@@ -170,7 +171,9 @@ class DistortionCorrectorTest : public ::testing::Test
   }
 
   sensor_msgs::msg::PointCloud2 generatePointCloudMsg(
-    bool generate_points, bool is_lidar_frame, std::string vendor, rclcpp::Time stamp)
+    bool generate_points, bool is_lidar_frame, std::string vendor, rclcpp::Time stamp,
+    bool use_default_pointcloud, std::vector<Eigen::Vector3f> defined_points,
+    std::vector<float> defined_azimuths)
   {
     sensor_msgs::msg::PointCloud2 pointcloud_msg;
     pointcloud_msg.header.stamp = stamp;
@@ -180,21 +183,54 @@ class DistortionCorrectorTest : public ::testing::Test
     pointcloud_msg.is_bigendian = false;
 
     if (generate_points) {
-      std::array<Eigen::Vector3f, number_of_points_> points = {{
-        Eigen::Vector3f(10.0f, 0.0f, 1.0f),   // point 1
-        Eigen::Vector3f(5.0f, -5.0f, 2.0f),   // point 2
-        Eigen::Vector3f(0.0f, -10.0f, 3.0f),  // point 3
-        Eigen::Vector3f(-5.0f, -5.0f, 4.0f),  // point 4
-        Eigen::Vector3f(-10.0f, 0.0f, 5.0f),  // point 5
-        Eigen::Vector3f(-5.0f, 5.0f, -5.0f),  // point 6
-        Eigen::Vector3f(0.0f, 10.0f, -4.0f),  // point 7
-        Eigen::Vector3f(5.0f, 5.0f, -3.0f),   // point 8
-        Eigen::Vector3f(8.0f, 3.0f, -2.0f),   // point 9
-        Eigen::Vector3f(9.0f, 1.0f, -1.0f)    // point 10
-      }};
+      std::vector<Eigen::Vector3f> points;
+      std::vector<float> azimuths;
+
+      if (use_default_pointcloud) {
+        std::vector<Eigen::Vector3f> default_points = {{
+          Eigen::Vector3f(10.0f, 0.0f, 1.0f),   // point 1
+          Eigen::Vector3f(5.0f, -5.0f, 2.0f),   // point 2
+          Eigen::Vector3f(0.0f, -10.0f, 3.0f),  // point 3
+          Eigen::Vector3f(-5.0f, -5.0f, 4.0f),  // point 4
+          Eigen::Vector3f(-10.0f, 0.0f, 5.0f),  // point 5
+          Eigen::Vector3f(-5.0f, 5.0f, -5.0f),  // point 6
+          Eigen::Vector3f(0.0f, 10.0f, -4.0f),  // point 7
+          Eigen::Vector3f(5.0f, 5.0f, -3.0f),   // point 8
+          Eigen::Vector3f(8.0f, 3.0f, -2.0f),   // point 9
+          Eigen::Vector3f(9.0f, 1.0f, -1.0f)    // point 10
+        }};
+
+        std::vector<float> default_azimuths;
+        for (const auto & point : default_points) {
+          if (vendor == "velodyne") {
+            float cartesian_deg =
+              std::atan2(point.y(), point.x()) * 180 / autoware::universe_utils::pi;
+            if (cartesian_deg < 0) cartesian_deg += 360;
+            float velodyne_deg = 360 - cartesian_deg;
+            if (velodyne_deg == 360) velodyne_deg = 0;
+            default_azimuths.push_back(velodyne_deg * autoware::universe_utils::pi / 180);
+          } else if (vendor == "hesai") {
+            float cartesian_deg =
+              std::atan2(point.y(), point.x()) * 180 / autoware::universe_utils::pi;
+            if (cartesian_deg < 0) cartesian_deg += 360;
+            float hesai_deg =
+              90 - cartesian_deg < 0 ? 90 - cartesian_deg + 360 : 90 - cartesian_deg;
+            if (hesai_deg == 360) hesai_deg = 0;
+            default_azimuths.push_back(hesai_deg * autoware::universe_utils::pi / 180);
+          } else {  // empty string
+            default_azimuths.push_back(std::atan2(point.y(), point.x()));
+          }
+        }
+
+        points = default_points;
+        azimuths = default_azimuths;
+      } else {
+        points = defined_points;
+        azimuths = defined_azimuths;
+      }
 
       // Generate timestamps for the points
-      std::vector<std::uint32_t> timestamps = generatePointTimestamps(stamp, number_of_points_);
+      std::vector<std::uint32_t> timestamps = generatePointTimestamps(stamp, points.size());
 
       sensor_msgs::PointCloud2Modifier modifier(pointcloud_msg);
       modifier.setPointCloud2Fields(
@@ -206,7 +242,7 @@ class DistortionCorrectorTest : public ::testing::Test
         sensor_msgs::msg::PointField::FLOAT32, "distance", 1, sensor_msgs::msg::PointField::FLOAT32,
         "time_stamp", 1, sensor_msgs::msg::PointField::UINT32);
 
-      modifier.resize(number_of_points_);
+      modifier.resize(points.size());
 
       sensor_msgs::PointCloud2Iterator<float> iter_x(pointcloud_msg, "x");
       sensor_msgs::PointCloud2Iterator<float> iter_y(pointcloud_msg, "y");
@@ -215,27 +251,12 @@ class DistortionCorrectorTest : public ::testing::Test
       sensor_msgs::PointCloud2Iterator<float> iter_distance(pointcloud_msg, "distance");
       sensor_msgs::PointCloud2Iterator<std::uint32_t> iter_t(pointcloud_msg, "time_stamp");
 
-      for (size_t i = 0; i < number_of_points_; ++i) {
+      for (size_t i = 0; i < points.size(); ++i) {
         *iter_x = points[i].x();
         *iter_y = points[i].y();
         *iter_z = points[i].z();
 
-        if (vendor == "velodyne") {
-          float cartesian_deg = std::atan2(points[i].y(), points[i].x()) * 180 / M_PI;
-          if (cartesian_deg < 0) cartesian_deg += 360;
-          float velodyne_deg = 360 - cartesian_deg;
-          if (velodyne_deg == 360) velodyne_deg = 0;
-          *iter_azimuth = velodyne_deg * M_PI / 180;
-        } else if (vendor == "hesai") {
-          float cartesian_deg = std::atan2(points[i].y(), points[i].x()) * 180 / M_PI;
-          if (cartesian_deg < 0) cartesian_deg += 360;
-          float hesai_deg = 90 - cartesian_deg < 0 ? 90 - cartesian_deg + 360 : 90 - cartesian_deg;
-          if (hesai_deg == 360) hesai_deg = 0;
-          *iter_azimuth = hesai_deg * M_PI / 180;
-        } else {  // empty string
-          *iter_azimuth = std::atan2(points[i].y(), points[i].x());
-        }
-
+        *iter_azimuth = azimuths[i];
         *iter_distance = points[i].norm();
         *iter_t = timestamps[i];
         ++iter_x;
@@ -332,7 +353,8 @@ TEST_F(DistortionCorrectorTest, TestIsInputValid)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
 
   // input normal pointcloud without twist
-  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, "", timestamp);
+  sensor_msgs::msg::PointCloud2 pointcloud =
+    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
   bool result = distortion_corrector_2d_->isInputValid(pointcloud);
   EXPECT_FALSE(result);
 
@@ -340,12 +362,12 @@ TEST_F(DistortionCorrectorTest, TestIsInputValid)
   auto twist_msg = generateTwistMsg(twist_linear_x_, twist_angular_z_, timestamp);
   distortion_corrector_2d_->processTwistMessage(twist_msg);
 
-  pointcloud = generatePointCloudMsg(true, false, "", timestamp);
+  pointcloud = generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
   result = distortion_corrector_2d_->isInputValid(pointcloud);
   EXPECT_TRUE(result);
 
   // input empty pointcloud
-  pointcloud = generatePointCloudMsg(false, false, "", timestamp);
+  pointcloud = generatePointCloudMsg(false, false, "", timestamp, true, {}, {});
   result = distortion_corrector_2d_->isInputValid(pointcloud);
   EXPECT_FALSE(result);
 }
@@ -375,7 +397,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyTwist)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   // Generate the point cloud message
-  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, "", timestamp);
+  sensor_msgs::msg::PointCloud2 pointcloud =
+    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
 
   // Process empty twist queue
   distortion_corrector_2d_->initialize();
@@ -418,7 +441,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyPointCloud)
   }
   // Generate an empty point cloud message
   sensor_msgs::msg::PointCloud2 empty_pointcloud =
-    generatePointCloudMsg(false, false, "", timestamp);
+    generatePointCloudMsg(false, false, "", timestamp, true, {}, {});
 
   // Process empty point cloud
   distortion_corrector_2d_->initialize();
@@ -433,7 +456,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, "", timestamp);
+  sensor_msgs::msg::PointCloud2 pointcloud =
+    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
   auto twist_msgs = generateTwistMsgs(timestamp);
@@ -479,7 +503,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, "", timestamp);
+  sensor_msgs::msg::PointCloud2 pointcloud =
+    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
   auto twist_msgs = generateTwistMsgs(timestamp);
@@ -530,7 +555,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, "", timestamp);
+  sensor_msgs::msg::PointCloud2 pointcloud =
+    generatePointCloudMsg(true, true, "", timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
   auto twist_msgs = generateTwistMsgs(timestamp);
@@ -584,7 +610,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, "", timestamp);
+  sensor_msgs::msg::PointCloud2 pointcloud =
+    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
   auto twist_msgs = generateTwistMsgs(timestamp);
@@ -630,7 +657,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, "", timestamp);
+  sensor_msgs::msg::PointCloud2 pointcloud =
+    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
   auto twist_msgs = generateTwistMsgs(timestamp);
@@ -684,7 +712,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, "", timestamp);
+  sensor_msgs::msg::PointCloud2 pointcloud =
+    generatePointCloudMsg(true, true, "", timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
   auto twist_msgs = generateTwistMsgs(timestamp);
@@ -737,9 +766,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   sensor_msgs::msg::PointCloud2 test2d_pointcloud =
-    generatePointCloudMsg(true, false, "", timestamp);
+    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
   sensor_msgs::msg::PointCloud2 test3d_pointcloud =
-    generatePointCloudMsg(true, false, "", timestamp);
+    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
 
   // Generate and process a single twist message with constant linear velocity
   auto twist_msg = generateTwistMsg(1.0, 0.0, timestamp);
@@ -756,7 +785,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion)
 
   // Generate expected point cloud for testing
   sensor_msgs::msg::PointCloud2 expected_pointcloud_msg =
-    generatePointCloudMsg(true, false, "", timestamp);
+    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
 
   // Calculate expected point cloud values based on constant linear motion
   double velocity = 1.0;  // 1 m/s linear velocity
@@ -826,9 +855,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   sensor_msgs::msg::PointCloud2 test2d_pointcloud =
-    generatePointCloudMsg(true, false, "", timestamp);
+    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
   sensor_msgs::msg::PointCloud2 test3d_pointcloud =
-    generatePointCloudMsg(true, false, "", timestamp);
+    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
 
   // Generate and process a single twist message with constant angular velocity
   auto twist_msg = generateTwistMsg(0.0, 0.1, timestamp);
@@ -845,7 +874,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion)
 
   // Generate expected point cloud for testing
   sensor_msgs::msg::PointCloud2 expected_pointcloud_msg =
-    generatePointCloudMsg(true, false, "", timestamp);
+    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
 
   // Calculate expected point cloud values based on constant rotational motion
   double angular_velocity = 0.1;  // 0.1 rad/s rotational velocity
@@ -932,7 +961,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdateAzimuthAndDistan
   // Generate the point cloud message in base_link
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   sensor_msgs::msg::PointCloud2 pointcloud_base_link =
-    generatePointCloudMsg(true, false, "", timestamp);
+    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
   auto twist_msgs = generateTwistMsgs(timestamp);
@@ -955,7 +984,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdateAzimuthAndDistan
     true, can_update_azimuth_and_distance, pointcloud_base_link);
 
   sensor_msgs::msg::PointCloud2 original_pointcloud_base_link =
-    generatePointCloudMsg(true, false, "", timestamp);
+    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
 
   sensor_msgs::PointCloud2ConstIterator<float> test_iter_azimuth_base_link(
     pointcloud_base_link, "azimuth");
@@ -992,7 +1021,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdateAzimuthAndDistan
 
   // Generate the point cloud message in sensor frame
   sensor_msgs::msg::PointCloud2 pointcloud_lidar_top =
-    generatePointCloudMsg(true, true, "", timestamp);
+    generatePointCloudMsg(true, true, "", timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
   twist_msgs = generateTwistMsgs(timestamp);
@@ -1014,7 +1043,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdateAzimuthAndDistan
     true, can_update_azimuth_and_distance, pointcloud_lidar_top);
 
   sensor_msgs::msg::PointCloud2 original_pointcloud_lidar_top =
-    generatePointCloudMsg(true, true, "", timestamp);
+    generatePointCloudMsg(true, true, "", timestamp, true, {}, {});
 
   sensor_msgs::PointCloud2ConstIterator<float> test_iter_azimuth_lidar_top(
     pointcloud_lidar_top, "azimuth");
@@ -1053,7 +1082,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   // Generate the point cloud message in sensor frame
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, true, "velodyne", timestamp);
+    generatePointCloudMsg(true, true, "velodyne", timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
   auto twist_msgs = generateTwistMsgs(timestamp);
@@ -1075,7 +1104,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   distortion_corrector_2d_->undistortPointCloud(true, can_update_azimuth_and_distance, pointcloud);
 
   sensor_msgs::msg::PointCloud2 original_pointcloud =
-    generatePointCloudMsg(true, true, "velodyne", timestamp);
+    generatePointCloudMsg(true, true, "velodyne", timestamp, true, {}, {});
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_azimuth(pointcloud, "azimuth");
   sensor_msgs::PointCloud2ConstIterator<float> iter_distance(pointcloud, "distance");
@@ -1117,7 +1146,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
 {
   // Generate the point cloud message in sensor frame
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, "hesai", timestamp);
+  sensor_msgs::msg::PointCloud2 pointcloud =
+    generatePointCloudMsg(true, true, "hesai", timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
   auto twist_msgs = generateTwistMsgs(timestamp);
@@ -1139,19 +1169,19 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   distortion_corrector_2d_->undistortPointCloud(true, can_update_azimuth_and_distance, pointcloud);
 
   sensor_msgs::msg::PointCloud2 original_pointcloud =
-    generatePointCloudMsg(true, true, "hesai", timestamp);
+    generatePointCloudMsg(true, true, "hesai", timestamp, true, {}, {});
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_azimuth(pointcloud, "azimuth");
   sensor_msgs::PointCloud2ConstIterator<float> iter_distance(pointcloud, "distance");
 
   // Expected undistorted azimuth and distance values
   std::array<std::array<float, 2>, 10> expected_distance_azimuth = {
-    {{-4.71239f, 10.0499f},
-     {-3.93826f, 7.36792f},
-     {-3.15295f, 10.3743f},
-     {-2.35297f, 8.05408f},
-     {-1.5435f, 11.1711f},
-     {-0.720431f, 8.40875f},
+    {{1.5708f, 10.0499f},
+     {2.34493f, 7.36792f},
+     {3.13024f, 10.3743f},
+     {3.93021f, 8.05408f},
+     {4.73969f, 11.1711f},
+     {5.56275f, 8.40875f},
      {0.0350311f, 10.9642f},
      {0.777465f, 8.06673f},
      {1.16851f, 9.32108f},
@@ -1176,15 +1206,119 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   }
 }
 
-TEST_F(DistortionCorrectorTest, TestAzimuthConversionExists)
+TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsEmptyPointlcoud)
 {
   // test empty pointcloud
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  // Generate and process multiple twist messages
+  auto twist_msgs = generateTwistMsgs(timestamp);
+  for (const auto & twist_msg : twist_msgs) {
+    distortion_corrector_2d_->processTwistMessage(twist_msg);
+  }
   sensor_msgs::msg::PointCloud2 empty_pointcloud =
-    generatePointCloudMsg(false, false, "", timestamp);
+    generatePointCloudMsg(false, false, "", timestamp, true, {}, {});
   EXPECT_FALSE(distortion_corrector_2d_->AzimuthConversionExists(empty_pointcloud));
+}
 
+TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsVelodynePointlcoud)
+{
   // test velodyne pointcloud (x-axis: 0 degree, y-axis: 270 degree)
+  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  // Generate and process multiple twist messages
+  auto twist_msgs = generateTwistMsgs(timestamp);
+  for (const auto & twist_msg : twist_msgs) {
+    distortion_corrector_2d_->processTwistMessage(twist_msg);
+  }
+  std::vector<Eigen::Vector3f> velodyne_points = {
+    Eigen::Vector3f(0.0f, 0.0f, 0.0f),
+    Eigen::Vector3f(1.0f, -1.0f, 1.0f),
+    Eigen::Vector3f(0.0f, -2.0f, 1.0f),
+  };
+  std::vector<float> velodyne_azimuths = {
+    0.0f, autoware::universe_utils::pi / 4, autoware::universe_utils::pi / 2};
+  sensor_msgs::msg::PointCloud2 velodyne_pointcloud =
+    generatePointCloudMsg(true, true, "", timestamp, false, velodyne_points, velodyne_azimuths);
+  EXPECT_TRUE(distortion_corrector_2d_->AzimuthConversionExists(velodyne_pointcloud));
+
+  auto [a, b] = distortion_corrector_2d_->getConversion();
+  EXPECT_EQ(b, -1);
+  EXPECT_NEAR(a, autoware::universe_utils::pi * 2, standard_tolerance_);
+}
+
+TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsHesaiPointlcoud)
+{
+  // test hesai pointcloud (x-axis: 90 degree, y-axis: 0 degree)
+  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  // Generate and process multiple twist messages
+  auto twist_msgs = generateTwistMsgs(timestamp);
+  for (const auto & twist_msg : twist_msgs) {
+    distortion_corrector_2d_->processTwistMessage(twist_msg);
+  }
+  std::vector<Eigen::Vector3f> hesai_points = {
+    Eigen::Vector3f(0.0f, 0.0f, 0.0f),
+    Eigen::Vector3f(1.0f, -1.0f, 1.0f),
+    Eigen::Vector3f(0.0f, -2.0f, 1.0f),
+  };
+  std::vector<float> hesai_azimuths = {
+    autoware::universe_utils::pi / 2, autoware::universe_utils::pi * 3 / 4,
+    autoware::universe_utils::pi};
+  sensor_msgs::msg::PointCloud2 hesai_pointcloud =
+    generatePointCloudMsg(true, true, "", timestamp, false, hesai_points, hesai_azimuths);
+  EXPECT_TRUE(distortion_corrector_2d_->AzimuthConversionExists(hesai_pointcloud));
+
+  auto [a, b] = distortion_corrector_2d_->getConversion();
+  EXPECT_EQ(b, -1);
+  EXPECT_NEAR(a, autoware::universe_utils::pi / 2, standard_tolerance_);
+}
+
+TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsCartesianPointlcoud)
+{
+  // test pointcloud that use cartesian coordinate for azimuth (x-axis: 0 degree, y-axis: 90 degree)
+  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  // Generate and process multiple twist messages
+  auto twist_msgs = generateTwistMsgs(timestamp);
+  for (const auto & twist_msg : twist_msgs) {
+    distortion_corrector_2d_->processTwistMessage(twist_msg);
+  }
+  std::vector<Eigen::Vector3f> cartesian_points = {
+    Eigen::Vector3f(0.0f, 0.0f, 0.0f),
+    Eigen::Vector3f(1.0f, 1.0f, 1.0f),
+    Eigen::Vector3f(0.0f, 2.0f, 1.0f),
+  };
+  std::vector<float> cartesian_azimuths = {
+    0, autoware::universe_utils::pi / 4, autoware::universe_utils::pi / 2};
+  sensor_msgs::msg::PointCloud2 cartesian_pointcloud =
+    generatePointCloudMsg(true, true, "", timestamp, false, cartesian_points, cartesian_azimuths);
+  EXPECT_TRUE(distortion_corrector_2d_->AzimuthConversionExists(cartesian_pointcloud));
+
+  auto [a, b] = distortion_corrector_2d_->getConversion();
+  EXPECT_EQ(b, 1);
+  EXPECT_NEAR(a, 0, standard_tolerance_);
+}
+
+TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsRandomPointlcoud1)
+{
+  // test pointcloud that use coordinate (x-axis: 270 degree, y-axis: 0 degree)
+  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  // Generate and process multiple twist messages
+  auto twist_msgs = generateTwistMsgs(timestamp);
+  for (const auto & twist_msg : twist_msgs) {
+    distortion_corrector_2d_->processTwistMessage(twist_msg);
+  }
+  std::vector<Eigen::Vector3f> points = {
+    Eigen::Vector3f(0.0f, 1.0f, 0.0f),
+    Eigen::Vector3f(2.0f, 0.0f, 1.0f),
+    Eigen::Vector3f(1.0f, 1.0f, 1.0f),
+  };
+  std::vector<float> azimuths = {
+    0, autoware::universe_utils::pi * 3 / 2, autoware::universe_utils::pi * 7 / 4};
+  sensor_msgs::msg::PointCloud2 pointcloud =
+    generatePointCloudMsg(true, true, "", timestamp, false, points, azimuths);
+  EXPECT_TRUE(distortion_corrector_2d_->AzimuthConversionExists(pointcloud));
+
+  auto [a, b] = distortion_corrector_2d_->getConversion();
+  EXPECT_EQ(b, 1);
+  EXPECT_NEAR(a, autoware::universe_utils::pi * 3 / 2, standard_tolerance_);
 }
 
 int main(int argc, char ** argv)

From 6796341c0e3a8f8319087dd7f54f0149a3b4d1e8 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Wed, 14 Aug 2024 16:15:18 +0900
Subject: [PATCH 09/72] feat: remove seperate sin, cos and use sin_and_cos
 function

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector/distortion_corrector.cpp   | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index 42cef560cc8c5..f07b815855f67 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -458,14 +458,15 @@ inline void DistortionCorrector2D::undistortPointImplementation(
     point_tf_ = tf2_lidar_to_base_link_ * point_tf_;
   }
   theta_ += w * time_offset;
+  auto [sin_half_theta, cos_half_theta] = autoware::universe_utils::sin_and_cos(theta_ * 0.5f);
+  auto [sin_theta, cos_theta] = autoware::universe_utils::sin_and_cos(theta_);
+
   baselink_quat_.setValue(
-    0, 0, autoware::universe_utils::sin(theta_ * 0.5f),
-    autoware::universe_utils::cos(
-      theta_ *
-      0.5f));  // baselink_quat.setRPY(0.0, 0.0, theta); (Note that the value is slightly different)
+    0, 0, sin_half_theta, cos_half_theta);  // baselink_quat.setRPY(0.0, 0.0, theta); (Note that the
+                                            // value is slightly different)
   const float dis = v * time_offset;
-  x_ += dis * autoware::universe_utils::cos(theta_);
-  y_ += dis * autoware::universe_utils::sin(theta_);
+  x_ += dis * cos_theta;
+  y_ += dis * sin_theta;
 
   baselink_tf_odom_.setOrigin(tf2::Vector3(x_, y_, 0.0));
   baselink_tf_odom_.setRotation(baselink_quat_);

From afd0fda343b0322b17da049dfdbfd0c023cab5d7 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Wed, 14 Aug 2024 16:27:12 +0900
Subject: [PATCH 10/72] chore: fix readme

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../docs/distortion-corrector.md                         | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md
index 05b76e49511f8..81bcfdd50b42f 100644
--- a/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md
+++ b/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md
@@ -48,8 +48,11 @@ ros2 launch autoware_pointcloud_preprocessor distortion_corrector.launch.xml
 
 - The node requires time synchronization between the topics from lidars, twist, and IMU.
 - If you want to use a 3D distortion corrector without IMU, please check that the linear and angular velocity fields of your twist message are not empty.
-- The node updates the per-point azimuth and distance values based on the undistorted XYZ coordinates when the input point cloud is in the sensor frame and the `sensor_azimuth_coordinate` parameter is set to either `velodyne` or `hesai`. The azimuth values are calculated using the `cv::fastAtan2` function. Please note that updating the azimuth and distance fields increases the execution time by approximately 13%. Additionally, due to the `cv::fastAtan2` algorithm's accuracy of about 0.3 degrees, there is a **possibility of changing beam order for high azimuth resolution LiDAR**.
-- Currently, `sensor_azimuth_coordinate` only supports `velodyne` and `hesai`. The `velodyne` and `hesai` coordinates are illustrated below. If a LiDAR uses the same coordinates as `velodyne` or `hesai`, users can use them as well.
+- The node updates the per-point azimuth and distance values based on the undistorted XYZ coordinates when the input point cloud is in the sensor frame (not in the `base_link`) and the `update_azimuth_and_distance` parameter is set to `true`. The azimuth values are calculated using the `cv::fastAtan2` function. Please note that updating the azimuth and distance fields increases the execution time by approximately 13%. Additionally, due to the `cv::fastAtan2` algorithm's accuracy of about 0.3 degrees, there is a **possibility of changing beam order for high azimuth resolution LiDAR**.
+- LiDARs from different vendors have different azimuth coordinates, as shown in the images below. Currently, the coordinate systems listed below have been tested, and the node will update the azimuth based on the input coordinate system.
+  - `velodyne`: (x: 0 degrees, y: 270 degrees)
+  - `hesai`: (x: 90 degrees, y: 0 degrees)
+  - `others`: (x: 0 degrees, y: 90 degrees) and (x: 270 degrees, y: 0 degrees)
 
 <table>
   <tr>
@@ -57,7 +60,7 @@ ros2 launch autoware_pointcloud_preprocessor distortion_corrector.launch.xml
     <td><img src="./image/hesai.drawio.png" alt="hesai azimuth coordinate"></td>
    </tr>
    <tr>
-    <td><p style="text-align: center;">Velodyne azimuth coordinate.</p></td>
+    <td><p style="text-align: center;">Velodyne azimuth coordinate</p></td>
     <td><p style="text-align: center;">Hesai azimuth coordinate</p></td>
   </tr>
 </table>

From 9809a4b0273e3aaa5539b65313ebd1ea4172dd43 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Wed, 14 Aug 2024 16:34:36 +0900
Subject: [PATCH 11/72] chore: fix some grammar errors

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector.hpp                  | 10 +++----
 .../distortion_corrector.cpp                  |  4 +--
 .../distortion_corrector_node.cpp             |  2 +-
 .../test/test_distortion_corrector_node.cpp   | 26 +++++++++----------
 4 files changed, 20 insertions(+), 22 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
index 3a3ee1e1df06f..2510817cf02e8 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -63,7 +63,7 @@ class DistortionCorrectorBase
   virtual void setPointCloudTransform(
     const std::string & base_frame, const std::string & lidar_frame) = 0;
   virtual void initialize() = 0;
-  virtual bool AzimuthConversionExists(sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
+  virtual bool azimuthConversionExists(sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
   virtual std::tuple<float, float> getConversion() = 0;
   virtual void undistortPointCloud(
     bool use_imu, bool can_update_azimuth_and_distance,
@@ -87,12 +87,10 @@ class DistortionCorrector : public DistortionCorrectorBase
   // Equation of converion between sensor azimuth coordinates and cartesian coordinates:
   // sensor azimuth coordinates = a + b * cartesian coordinates;
   // a is restricted to be a multiple of 90, and b is restricted to be 1 or -1.
-  bool first_time_{true};
-  bool success_{true};
-  float threshold_a_{0.087f};  // 5 / 180 * M_PI
-  float threshold_b_{0.1f};
   float a_{0};
   float b_{1};
+  float threshold_a_{0.087f};  // 5 / 180 * M_PI
+  float threshold_b_{0.1f};
 
   void getIMUTransformation(const std::string & base_frame, const std::string & imu_frame);
   void enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg);
@@ -130,7 +128,7 @@ class DistortionCorrector : public DistortionCorrectorBase
   void undistortPointCloud(
     bool use_imu, bool update_azimuth_and_distance,
     sensor_msgs::msg::PointCloud2 & pointcloud) override;
-  bool AzimuthConversionExists(sensor_msgs::msg::PointCloud2 & pointcloud) override;
+  bool azimuthConversionExists(sensor_msgs::msg::PointCloud2 & pointcloud) override;
   std::tuple<float, float> getConversion() override;
 
   bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud);
diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index f07b815855f67..e861da1ea7e7d 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -189,7 +189,7 @@ bool DistortionCorrector<T>::isInputValid(sensor_msgs::msg::PointCloud2 & pointc
 }
 
 template <class T>
-bool DistortionCorrector<T>::AzimuthConversionExists(sensor_msgs::msg::PointCloud2 & pointcloud)
+bool DistortionCorrector<T>::azimuthConversionExists(sensor_msgs::msg::PointCloud2 & pointcloud)
 {
   if (!isInputValid(pointcloud)) return false;
 
@@ -208,7 +208,7 @@ bool DistortionCorrector<T>::AzimuthConversionExists(sensor_msgs::msg::PointClou
   } else {
     RCLCPP_WARN(
       node_->get_logger(),
-      "Current point cloud only has a single point cloud. Could not calculate the formula.");
+      "Current point cloud only has a single point. Could not calculate the formula.");
     return false;
   }
 
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 df3af81902e3a..4a088d4f567af 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
@@ -96,7 +96,7 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointclou
 
   if (update_azimuth_and_distance_ && !can_update_azimuth_and_distance_) {
     can_update_azimuth_and_distance_ =
-      distortion_corrector_->AzimuthConversionExists(*pointcloud_msg);
+      distortion_corrector_->azimuthConversionExists(*pointcloud_msg);
     if (can_update_azimuth_and_distance_) {
       RCLCPP_INFO(
         this->get_logger(),
diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index 962acb8edc582..7b3a5f95a6051 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -978,7 +978,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdateAzimuthAndDistan
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
   auto can_update_azimuth_and_distance =
-    distortion_corrector_2d_->AzimuthConversionExists(pointcloud_base_link);
+    distortion_corrector_2d_->azimuthConversionExists(pointcloud_base_link);
 
   distortion_corrector_2d_->undistortPointCloud(
     true, can_update_azimuth_and_distance, pointcloud_base_link);
@@ -1100,7 +1100,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
   // set the azimuth coordinate system
   auto can_update_azimuth_and_distance =
-    distortion_corrector_2d_->AzimuthConversionExists(pointcloud);
+    distortion_corrector_2d_->azimuthConversionExists(pointcloud);
   distortion_corrector_2d_->undistortPointCloud(true, can_update_azimuth_and_distance, pointcloud);
 
   sensor_msgs::msg::PointCloud2 original_pointcloud =
@@ -1165,7 +1165,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
   // set the azimuth coordinate system
   auto can_update_azimuth_and_distance =
-    distortion_corrector_2d_->AzimuthConversionExists(pointcloud);
+    distortion_corrector_2d_->azimuthConversionExists(pointcloud);
   distortion_corrector_2d_->undistortPointCloud(true, can_update_azimuth_and_distance, pointcloud);
 
   sensor_msgs::msg::PointCloud2 original_pointcloud =
@@ -1206,7 +1206,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   }
 }
 
-TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsEmptyPointlcoud)
+TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsEmptyPointcloud)
 {
   // test empty pointcloud
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1217,10 +1217,10 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsEmptyPointlcoud)
   }
   sensor_msgs::msg::PointCloud2 empty_pointcloud =
     generatePointCloudMsg(false, false, "", timestamp, true, {}, {});
-  EXPECT_FALSE(distortion_corrector_2d_->AzimuthConversionExists(empty_pointcloud));
+  EXPECT_FALSE(distortion_corrector_2d_->azimuthConversionExists(empty_pointcloud));
 }
 
-TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsVelodynePointlcoud)
+TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsVelodynePointcloud)
 {
   // test velodyne pointcloud (x-axis: 0 degree, y-axis: 270 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1238,14 +1238,14 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsVelodynePointlcoud)
     0.0f, autoware::universe_utils::pi / 4, autoware::universe_utils::pi / 2};
   sensor_msgs::msg::PointCloud2 velodyne_pointcloud =
     generatePointCloudMsg(true, true, "", timestamp, false, velodyne_points, velodyne_azimuths);
-  EXPECT_TRUE(distortion_corrector_2d_->AzimuthConversionExists(velodyne_pointcloud));
+  EXPECT_TRUE(distortion_corrector_2d_->azimuthConversionExists(velodyne_pointcloud));
 
   auto [a, b] = distortion_corrector_2d_->getConversion();
   EXPECT_EQ(b, -1);
   EXPECT_NEAR(a, autoware::universe_utils::pi * 2, standard_tolerance_);
 }
 
-TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsHesaiPointlcoud)
+TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsHesaiPointcloud)
 {
   // test hesai pointcloud (x-axis: 90 degree, y-axis: 0 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1264,14 +1264,14 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsHesaiPointlcoud)
     autoware::universe_utils::pi};
   sensor_msgs::msg::PointCloud2 hesai_pointcloud =
     generatePointCloudMsg(true, true, "", timestamp, false, hesai_points, hesai_azimuths);
-  EXPECT_TRUE(distortion_corrector_2d_->AzimuthConversionExists(hesai_pointcloud));
+  EXPECT_TRUE(distortion_corrector_2d_->azimuthConversionExists(hesai_pointcloud));
 
   auto [a, b] = distortion_corrector_2d_->getConversion();
   EXPECT_EQ(b, -1);
   EXPECT_NEAR(a, autoware::universe_utils::pi / 2, standard_tolerance_);
 }
 
-TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsCartesianPointlcoud)
+TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsCartesianPointcloud)
 {
   // test pointcloud that use cartesian coordinate for azimuth (x-axis: 0 degree, y-axis: 90 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1289,14 +1289,14 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsCartesianPointlcoud)
     0, autoware::universe_utils::pi / 4, autoware::universe_utils::pi / 2};
   sensor_msgs::msg::PointCloud2 cartesian_pointcloud =
     generatePointCloudMsg(true, true, "", timestamp, false, cartesian_points, cartesian_azimuths);
-  EXPECT_TRUE(distortion_corrector_2d_->AzimuthConversionExists(cartesian_pointcloud));
+  EXPECT_TRUE(distortion_corrector_2d_->azimuthConversionExists(cartesian_pointcloud));
 
   auto [a, b] = distortion_corrector_2d_->getConversion();
   EXPECT_EQ(b, 1);
   EXPECT_NEAR(a, 0, standard_tolerance_);
 }
 
-TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsRandomPointlcoud1)
+TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsRandomPointcloud1)
 {
   // test pointcloud that use coordinate (x-axis: 270 degree, y-axis: 0 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1314,7 +1314,7 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsRandomPointlcoud1)
     0, autoware::universe_utils::pi * 3 / 2, autoware::universe_utils::pi * 7 / 4};
   sensor_msgs::msg::PointCloud2 pointcloud =
     generatePointCloudMsg(true, true, "", timestamp, false, points, azimuths);
-  EXPECT_TRUE(distortion_corrector_2d_->AzimuthConversionExists(pointcloud));
+  EXPECT_TRUE(distortion_corrector_2d_->azimuthConversionExists(pointcloud));
 
   auto [a, b] = distortion_corrector_2d_->getConversion();
   EXPECT_EQ(b, 1);

From 9aeac77811851c21e2247d205c065e85523e4e18 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Wed, 14 Aug 2024 16:42:29 +0900
Subject: [PATCH 12/72] chore: fix spell error

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../autoware_universe_utils/test/src/math/test_trigonometry.cpp | 2 +-
 .../distortion_corrector/distortion_corrector.hpp               | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp
index cba601ba7894f..0b21d2701b3af 100644
--- a/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp
+++ b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp
@@ -72,6 +72,6 @@ TEST(trigonometry, opencv_fastAtan2)
     // 0.3 degree accuracy
     ASSERT_NEAR(fast_atan, std_atan, 6e-3)
       << "Test failed for input (" << y << ", " << x << "): "
-      << "fastatan2 = " << fast_atan << ", std::atan2 = " << std_atan;
+      << "fast atan2 = " << fast_atan << ", std::atan2 = " << std_atan;
   }
 }
diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
index 2510817cf02e8..f5811878af057 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -84,7 +84,7 @@ class DistortionCorrector : public DistortionCorrectorBase
   std::deque<geometry_msgs::msg::TwistStamped> twist_queue_;
   std::deque<geometry_msgs::msg::Vector3Stamped> angular_velocity_queue_;
 
-  // Equation of converion between sensor azimuth coordinates and cartesian coordinates:
+  // Equation for the conversion between sensor azimuth coordinates and Cartesian coordinates:
   // sensor azimuth coordinates = a + b * cartesian coordinates;
   // a is restricted to be a multiple of 90, and b is restricted to be 1 or -1.
   float a_{0};

From b5dfa853e76901f783692dc8b0352f1c61308dd2 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Wed, 14 Aug 2024 17:30:20 +0900
Subject: [PATCH 13/72] chore: set debug mode to false

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../test/test_distortion_corrector_node.cpp                     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index 7b3a5f95a6051..46f5f7fa4abcf 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -322,7 +322,7 @@ class DistortionCorrectorTest : public ::testing::Test
   static constexpr double epsilon = 1e-5;
 
   // for debugging or regenerating the ground truth point cloud
-  bool debug_{true};
+  bool debug_{false};
 };
 
 TEST_F(DistortionCorrectorTest, TestProcessTwistMessage)

From d57f751e16064705b36c2854eda8e565db4f4018 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Mon, 19 Aug 2024 16:19:32 +0900
Subject: [PATCH 14/72] chore: set update_azimuth_and_distance default value to
 false

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../config/distortion_corrector_node.param.yaml                 | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml
index c01317aadd3a8..c558756f5eed3 100644
--- a/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml
+++ b/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml
@@ -3,4 +3,4 @@
     base_frame: base_link
     use_imu: true
     use_3d_distortion_correction: false
-    update_azimuth_and_distance: true
+    update_azimuth_and_distance: false

From 8703a23e268d4da2a1118f63c636192fdbc78634 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Mon, 19 Aug 2024 16:57:50 +0900
Subject: [PATCH 15/72] chore: update readme

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../docs/distortion-corrector.md                               | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md
index 81bcfdd50b42f..a61eb24d26066 100644
--- a/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md
+++ b/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md
@@ -48,7 +48,8 @@ ros2 launch autoware_pointcloud_preprocessor distortion_corrector.launch.xml
 
 - The node requires time synchronization between the topics from lidars, twist, and IMU.
 - If you want to use a 3D distortion corrector without IMU, please check that the linear and angular velocity fields of your twist message are not empty.
-- The node updates the per-point azimuth and distance values based on the undistorted XYZ coordinates when the input point cloud is in the sensor frame (not in the `base_link`) and the `update_azimuth_and_distance` parameter is set to `true`. The azimuth values are calculated using the `cv::fastAtan2` function. Please note that updating the azimuth and distance fields increases the execution time by approximately 13%. Additionally, due to the `cv::fastAtan2` algorithm's accuracy of about 0.3 degrees, there is a **possibility of changing beam order for high azimuth resolution LiDAR**.
+- The node updates the per-point azimuth and distance values based on the undistorted XYZ coordinates when the input point cloud is in the sensor frame (not in the `base_link`) and the `update_azimuth_and_distance` parameter is set to `true`. The azimuth values are calculated using the `cv::fastAtan2` function.
+- Please note that updating the azimuth and distance fields increases the execution time by approximately 20%. Additionally, due to the `cv::fastAtan2` algorithm's accuracy of about 0.3 degrees, there is a **possibility of changing beam order for high azimuth resolution LiDAR**.
 - LiDARs from different vendors have different azimuth coordinates, as shown in the images below. Currently, the coordinate systems listed below have been tested, and the node will update the azimuth based on the input coordinate system.
   - `velodyne`: (x: 0 degrees, y: 270 degrees)
   - `hesai`: (x: 90 degrees, y: 0 degrees)

From f509d7d84d5fa92aeb7e6357da3446c803383270 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Wed, 4 Sep 2024 13:37:56 +0900
Subject: [PATCH 16/72] chore: remove cout

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../src/distortion_corrector/distortion_corrector.cpp            | 1 -
 1 file changed, 1 deletion(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index 85d8065204e92..a6e32ba15af22 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -240,7 +240,6 @@ bool DistortionCorrector<T>::azimuthConversionExists(sensor_msgs::msg::PointClou
       RCLCPP_WARN(
         node_->get_logger(),
         "Angle between two points exceeds 180 degrees. Iterate to next point ...");
-      std::cout << "b: " << b << "should be close to 1 or -1" << std::endl;
       continue;
     }
 

From 62434faf959d7324e0d826f4e2b44ee84a3faa1d Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Wed, 4 Sep 2024 14:30:56 +0900
Subject: [PATCH 17/72] chore: add opencv license

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 common/autoware_universe_utils/LICENSE        | 202 ++++++++++++++++++
 .../src/math/trigonometry.cpp                 |   8 +
 2 files changed, 210 insertions(+)
 create mode 100644 common/autoware_universe_utils/LICENSE

diff --git a/common/autoware_universe_utils/LICENSE b/common/autoware_universe_utils/LICENSE
new file mode 100644
index 0000000000000..d645695673349
--- /dev/null
+++ b/common/autoware_universe_utils/LICENSE
@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
diff --git a/common/autoware_universe_utils/src/math/trigonometry.cpp b/common/autoware_universe_utils/src/math/trigonometry.cpp
index 02dda7a396a8a..e835c2c821ae8 100644
--- a/common/autoware_universe_utils/src/math/trigonometry.cpp
+++ b/common/autoware_universe_utils/src/math/trigonometry.cpp
@@ -72,6 +72,14 @@ std::pair<float, float> sin_and_cos(float radian)
   }
 }
 
+// This code is modified from part of OpenCV project
+// (https://github.com/opencv/opencv/blob/4.x/modules/core/src/mathfuncs_core.simd.hpp). It is
+// subject to the license terms in the LICENSE file found in the top-level directory of this
+// distribution and at http://opencv.org/license.html.
+
+// Modification:
+// 1. use autoware defined PI
+// 2. output of the function change from degrees to radians.
 static const float atan2_p1 =
   0.9997878412794807f * static_cast<float>(180) / autoware::universe_utils::pi;
 static const float atan2_p3 =

From a3c977b5b7772cb6e83e183e80ec9c9b2894f48e Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Wed, 4 Sep 2024 14:34:13 +0900
Subject: [PATCH 18/72] chore: fix grammar error

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 common/autoware_universe_utils/src/math/trigonometry.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/common/autoware_universe_utils/src/math/trigonometry.cpp b/common/autoware_universe_utils/src/math/trigonometry.cpp
index e835c2c821ae8..35b0dc556cc03 100644
--- a/common/autoware_universe_utils/src/math/trigonometry.cpp
+++ b/common/autoware_universe_utils/src/math/trigonometry.cpp
@@ -79,7 +79,7 @@ std::pair<float, float> sin_and_cos(float radian)
 
 // Modification:
 // 1. use autoware defined PI
-// 2. output of the function change from degrees to radians.
+// 2. output of the function is changed from degrees to radians.
 static const float atan2_p1 =
   0.9997878412794807f * static_cast<float>(180) / autoware::universe_utils::pi;
 static const float atan2_p3 =

From 7288042125df7f28b9718c23dca885576703de72 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Mon, 9 Sep 2024 03:43:45 +0000
Subject: [PATCH 19/72] style(pre-commit): autofix

---
 .../schema/distortion_corrector_node.schema.json          | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json
index cbc918f07236a..78deb91b0c899 100644
--- a/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json
+++ b/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json
@@ -32,7 +32,13 @@
           "default": false
         }
       },
-      "required": ["base_frame", "use_imu", "use_3d_distortion_correction", "update_azimuth_and_distance", "has_static_tf_only"]
+      "required": [
+        "base_frame",
+        "use_imu",
+        "use_3d_distortion_correction",
+        "update_azimuth_and_distance",
+        "has_static_tf_only"
+      ]
     }
   },
   "properties": {

From 7e1f510b89f2b0425547cef9ff85b228f1f62437 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Mon, 9 Sep 2024 12:46:25 +0900
Subject: [PATCH 20/72] chore: add runtime error when azimuth conversion failed

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../src/distortion_corrector/distortion_corrector_node.cpp     | 3 +++
 1 file changed, 3 insertions(+)

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 1160b1b4e3d6e..7330438fe7afa 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
@@ -110,6 +110,9 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointclou
         "Failed to get the conversion formula between cartesian coordinates and LiDAR azimuth "
         "coordinates. Please check the LiDAR azimuth coordinate. This pointcloud will not update "
         "azimuth and distance");
+      throw std::runtime_error(
+        "Couldn't get the conversion formula between cartesian coordinates and LiDAR azimuth "
+        "coordinates");
     }
   }
 

From b156fb59cee6536173178bd15941e4f8d7fa0fe6 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Mon, 9 Sep 2024 12:53:01 +0900
Subject: [PATCH 21/72] chore: change default pointcloud

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../launch/distortion_corrector_node.launch.xml                 | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml
index 62edeb6373232..bde9ca9e59ddd 100644
--- a/sensing/autoware_pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml
+++ b/sensing/autoware_pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml
@@ -1,5 +1,5 @@
 <launch>
-  <arg name="input/pointcloud" default="/sensing/lidar/top/pointcloud_raw_ex"/>
+  <arg name="input/pointcloud" default="/sensing/lidar/top/mirror_cropped/pointcloud_ex"/>
   <arg name="input/twist" default="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
   <arg name="input/imu" default="/sensing/imu/imu_data"/>
   <arg name="output/pointcloud" default="/sensing/lidar/top/rectified/pointcloud_ex"/>

From 0a1f4c90eb5ce5b96660abb40e8a70c59f887055 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Mon, 9 Sep 2024 13:21:42 +0900
Subject: [PATCH 22/72] chore: change function name

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector/distortion_corrector.hpp      |  8 ++++----
 .../src/distortion_corrector/distortion_corrector.cpp  |  4 ++--
 .../test/test_distortion_corrector_node.cpp            | 10 +++++-----
 3 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
index f49acde18a863..c271a65043075 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -53,8 +53,8 @@ class DistortionCorrectorBase
 public:
   virtual bool pointcloudTransformExists() = 0;
   virtual bool pointcloudTransformNeeded() = 0;
-  virtual std::deque<geometry_msgs::msg::TwistStamped> get_twist_queue() = 0;
-  virtual std::deque<geometry_msgs::msg::Vector3Stamped> get_angular_velocity_queue() = 0;
+  virtual std::deque<geometry_msgs::msg::TwistStamped> getTwistQueue() = 0;
+  virtual std::deque<geometry_msgs::msg::Vector3Stamped> getAngularVelocityQueue() = 0;
 
   virtual void processTwistMessage(
     const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) = 0;
@@ -119,8 +119,8 @@ class DistortionCorrector : public DistortionCorrectorBase
   }
   bool pointcloudTransformExists();
   bool pointcloudTransformNeeded();
-  std::deque<geometry_msgs::msg::TwistStamped> get_twist_queue();
-  std::deque<geometry_msgs::msg::Vector3Stamped> get_angular_velocity_queue();
+  std::deque<geometry_msgs::msg::TwistStamped> getTwistQueue();
+  std::deque<geometry_msgs::msg::Vector3Stamped> getAngularVelocityQueue();
   void processTwistMessage(
     const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override;
 
diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index 162c73f40d7d1..d9347ae30a0af 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -38,13 +38,13 @@ bool DistortionCorrector<T>::pointcloudTransformNeeded()
 }
 
 template <class T>
-std::deque<geometry_msgs::msg::TwistStamped> DistortionCorrector<T>::get_twist_queue()
+std::deque<geometry_msgs::msg::TwistStamped> DistortionCorrector<T>::getTwistQueue()
 {
   return twist_queue_;
 }
 
 template <class T>
-std::deque<geometry_msgs::msg::Vector3Stamped> DistortionCorrector<T>::get_angular_velocity_queue()
+std::deque<geometry_msgs::msg::Vector3Stamped> DistortionCorrector<T>::getAngularVelocityQueue()
 {
   return angular_velocity_queue_;
 }
diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index d062e2c787bd3..7b6fcfe2eb23f 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -331,9 +331,9 @@ TEST_F(DistortionCorrectorTest, TestProcessTwistMessage)
   auto twist_msg = generateTwistMsg(twist_linear_x_, twist_angular_z_, timestamp);
   distortion_corrector_2d_->processTwistMessage(twist_msg);
 
-  ASSERT_FALSE(distortion_corrector_2d_->get_twist_queue().empty());
-  EXPECT_EQ(distortion_corrector_2d_->get_twist_queue().front().twist.linear.x, twist_linear_x_);
-  EXPECT_EQ(distortion_corrector_2d_->get_twist_queue().front().twist.angular.z, twist_angular_z_);
+  ASSERT_FALSE(distortion_corrector_2d_->getTwistQueue().empty());
+  EXPECT_EQ(distortion_corrector_2d_->getTwistQueue().front().twist.linear.x, twist_linear_x_);
+  EXPECT_EQ(distortion_corrector_2d_->getTwistQueue().front().twist.angular.z, twist_angular_z_);
 }
 
 TEST_F(DistortionCorrectorTest, TestProcessIMUMessage)
@@ -342,9 +342,9 @@ TEST_F(DistortionCorrectorTest, TestProcessIMUMessage)
   auto imu_msg = generateImuMsg(imu_angular_x_, imu_angular_y_, imu_angular_z_, timestamp);
   distortion_corrector_2d_->processIMUMessage("base_link", imu_msg);
 
-  ASSERT_FALSE(distortion_corrector_2d_->get_angular_velocity_queue().empty());
+  ASSERT_FALSE(distortion_corrector_2d_->getAngularVelocityQueue().empty());
   EXPECT_NEAR(
-    distortion_corrector_2d_->get_angular_velocity_queue().front().vector.z, -0.03159,
+    distortion_corrector_2d_->getAngularVelocityQueue().front().vector.z, -0.03159,
     standard_tolerance_);
 }
 

From f11e5295e2523844282672822be2c4bd888a2bc7 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Mon, 9 Sep 2024 14:36:29 +0900
Subject: [PATCH 23/72] chore: move variables to structure

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector.hpp                  | 23 ++++++++-----
 .../distortion_corrector.cpp                  | 33 ++++++++++---------
 .../test/test_distortion_corrector_node.cpp   | 24 +++++++-------
 3 files changed, 44 insertions(+), 36 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
index c271a65043075..cc3e5f98ed375 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -48,6 +48,17 @@
 namespace autoware::pointcloud_preprocessor
 {
 
+struct AngleConversion
+{
+  // Equation for the conversion between sensor azimuth coordinates and Cartesian coordinates:
+  // sensor azimuth coordinates = offset_rad_ + sign_ * cartesian coordinates;
+  // offset_rad_ is restricted to be a multiple of 90, and sign_ is restricted to be 1 or -1.
+  float offset_rad_{0};
+  float sign_{1};
+  float offset_rad_threshold_{0.087f};  // (5 / 180) * M_PI
+  float sign_threshold_{0.1f};
+};
+
 class DistortionCorrectorBase
 {
 public:
@@ -84,13 +95,7 @@ class DistortionCorrector : public DistortionCorrectorBase
   std::deque<geometry_msgs::msg::TwistStamped> twist_queue_;
   std::deque<geometry_msgs::msg::Vector3Stamped> angular_velocity_queue_;
 
-  // Equation for the conversion between sensor azimuth coordinates and Cartesian coordinates:
-  // sensor azimuth coordinates = a + b * cartesian coordinates;
-  // a is restricted to be a multiple of 90, and b is restricted to be 1 or -1.
-  float a_{0};
-  float b_{1};
-  float threshold_a_{0.087f};  // 5 / 180 * M_PI
-  float threshold_b_{0.1f};
+  AngleConversion angle_conversion_;
 
   void getIMUTransformation(const std::string & base_frame, const std::string & imu_frame);
   void enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg);
@@ -117,8 +122,8 @@ class DistortionCorrector : public DistortionCorrectorBase
     managed_tf_buffer_ =
       std::make_unique<autoware::universe_utils::ManagedTransformBuffer>(node, has_static_tf_only);
   }
-  bool pointcloudTransformExists();
-  bool pointcloudTransformNeeded();
+  bool pointcloudTransformExists() override;
+  bool pointcloudTransformNeeded() override;
   std::deque<geometry_msgs::msg::TwistStamped> getTwistQueue();
   std::deque<geometry_msgs::msg::Vector3Stamped> getAngularVelocityQueue();
   void processTwistMessage(
diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index d9347ae30a0af..3d6203a4b26e8 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -228,14 +228,14 @@ bool DistortionCorrector<T>::azimuthConversionExists(sensor_msgs::msg::PointClou
       continue;
     }
 
-    float b = (*next_it_azimuth - *it_azimuth) / (next_cartesian_rad - current_cartesian_rad);
-    float a = *it_azimuth - b * current_cartesian_rad;
-
-    // Check if 'b' can be adjusted to 1 or -1
-    if (std::abs(b - 1.0f) <= threshold_b_) {
-      b_ = 1.0f;
-    } else if (std::abs(b + 1.0f) <= threshold_b_) {
-      b_ = -1.0f;
+    float sign = (*next_it_azimuth - *it_azimuth) / (next_cartesian_rad - current_cartesian_rad);
+    float offset_rad = *it_azimuth - sign * current_cartesian_rad;
+
+    // Check if 'sign' can be adjusted to 1 or -1
+    if (std::abs(sign - 1.0f) <= angle_conversion_.sign_threshold_) {
+      angle_conversion_.sign_ = 1.0f;
+    } else if (std::abs(sign + 1.0f) <= angle_conversion_.sign_threshold_) {
+      angle_conversion_.sign_ = -1.0f;
     } else {
       RCLCPP_WARN(
         node_->get_logger(),
@@ -243,20 +243,22 @@ bool DistortionCorrector<T>::azimuthConversionExists(sensor_msgs::msg::PointClou
       continue;
     }
 
-    // Check if 'a' can be adjusted to a multiple of π/2
-    int multiple_of_90_degrees = std::round(a / (autoware::universe_utils::pi / 2));
-    if (std::abs(a - multiple_of_90_degrees * (autoware::universe_utils::pi / 2)) > threshold_a_) {
+    // Check if 'offset_rad' can be adjusted to offset_rad multiple of π/2
+    int multiple_of_90_degrees = std::round(offset_rad / (autoware::universe_utils::pi / 2));
+    if (
+      std::abs(offset_rad - multiple_of_90_degrees * (autoware::universe_utils::pi / 2)) >
+      angle_conversion_.offset_rad_threshold_) {
       continue;
     }
 
-    // Limit the range of a in [0, 360]
+    // Limit the range of offset_rad in [0, 360]
     if (multiple_of_90_degrees < 0) {
       multiple_of_90_degrees += 4;
     } else if (multiple_of_90_degrees > 4) {
       multiple_of_90_degrees -= 4;
     }
 
-    a_ = multiple_of_90_degrees * (autoware::universe_utils::pi / 2);
+    angle_conversion_.offset_rad_ = multiple_of_90_degrees * (autoware::universe_utils::pi / 2);
 
     return true;
   }
@@ -266,7 +268,7 @@ bool DistortionCorrector<T>::azimuthConversionExists(sensor_msgs::msg::PointClou
 template <class T>
 std::tuple<float, float> DistortionCorrector<T>::getConversion()
 {
-  return std::make_tuple(a_, b_);
+  return std::make_tuple(angle_conversion_.offset_rad_, angle_conversion_.sign_);
 }
 
 template <class T>
@@ -341,7 +343,8 @@ void DistortionCorrector<T>::undistortPointCloud(
 
     if (can_update_azimuth_and_distance && pointcloudTransformNeeded()) {
       float cartesian_coordinate_azimuth = autoware::universe_utils::opencv_fastAtan2(*it_y, *it_x);
-      float updated_azimuth = a_ + b_ * cartesian_coordinate_azimuth;
+      float updated_azimuth =
+        angle_conversion_.offset_rad_ + angle_conversion_.sign_ * cartesian_coordinate_azimuth;
       if (updated_azimuth < 0) {
         updated_azimuth += autoware::universe_utils::pi * 2;
       } else if (updated_azimuth > 2 * autoware::universe_utils::pi) {
diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index 7b6fcfe2eb23f..5556b92b91004 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -1240,9 +1240,9 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsVelodynePointcloud)
     generatePointCloudMsg(true, true, "", timestamp, false, velodyne_points, velodyne_azimuths);
   EXPECT_TRUE(distortion_corrector_2d_->azimuthConversionExists(velodyne_pointcloud));
 
-  auto [a, b] = distortion_corrector_2d_->getConversion();
-  EXPECT_EQ(b, -1);
-  EXPECT_NEAR(a, autoware::universe_utils::pi * 2, standard_tolerance_);
+  auto [rad_offset, sign] = distortion_corrector_2d_->getConversion();
+  EXPECT_EQ(sign, -1);
+  EXPECT_NEAR(rad_offset, autoware::universe_utils::pi * 2, standard_tolerance_);
 }
 
 TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsHesaiPointcloud)
@@ -1266,9 +1266,9 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsHesaiPointcloud)
     generatePointCloudMsg(true, true, "", timestamp, false, hesai_points, hesai_azimuths);
   EXPECT_TRUE(distortion_corrector_2d_->azimuthConversionExists(hesai_pointcloud));
 
-  auto [a, b] = distortion_corrector_2d_->getConversion();
-  EXPECT_EQ(b, -1);
-  EXPECT_NEAR(a, autoware::universe_utils::pi / 2, standard_tolerance_);
+  auto [rad_offset, sign] = distortion_corrector_2d_->getConversion();
+  EXPECT_EQ(sign, -1);
+  EXPECT_NEAR(rad_offset, autoware::universe_utils::pi / 2, standard_tolerance_);
 }
 
 TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsCartesianPointcloud)
@@ -1291,9 +1291,9 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsCartesianPointcloud)
     generatePointCloudMsg(true, true, "", timestamp, false, cartesian_points, cartesian_azimuths);
   EXPECT_TRUE(distortion_corrector_2d_->azimuthConversionExists(cartesian_pointcloud));
 
-  auto [a, b] = distortion_corrector_2d_->getConversion();
-  EXPECT_EQ(b, 1);
-  EXPECT_NEAR(a, 0, standard_tolerance_);
+  auto [rad_offset, sign] = distortion_corrector_2d_->getConversion();
+  EXPECT_EQ(sign, 1);
+  EXPECT_NEAR(rad_offset, 0, standard_tolerance_);
 }
 
 TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsRandomPointcloud1)
@@ -1316,9 +1316,9 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsRandomPointcloud1)
     generatePointCloudMsg(true, true, "", timestamp, false, points, azimuths);
   EXPECT_TRUE(distortion_corrector_2d_->azimuthConversionExists(pointcloud));
 
-  auto [a, b] = distortion_corrector_2d_->getConversion();
-  EXPECT_EQ(b, 1);
-  EXPECT_NEAR(a, autoware::universe_utils::pi * 3 / 2, standard_tolerance_);
+  auto [rad_offset, sign] = distortion_corrector_2d_->getConversion();
+  EXPECT_EQ(sign, 1);
+  EXPECT_NEAR(rad_offset, autoware::universe_utils::pi * 3 / 2, standard_tolerance_);
 }
 
 int main(int argc, char ** argv)

From f0609f51bde7f99f34237fcf2bc21178020d8d4a Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Mon, 9 Sep 2024 14:40:22 +0900
Subject: [PATCH 24/72] chore: add random seed

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../autoware_universe_utils/test/src/math/test_trigonometry.cpp  | 1 +
 1 file changed, 1 insertion(+)

diff --git a/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp
index 0b21d2701b3af..4ffaeaf6f4ca5 100644
--- a/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp
+++ b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp
@@ -63,6 +63,7 @@ TEST(trigonometry, opencv_fastAtan2)
 {
   for (int i = 0; i < 100; ++i) {
     // Generate random x and y between -10 and 10
+    std::srand(0);
     float x = static_cast<float>(std::rand()) / RAND_MAX * 20.0 - 10.0;
     float y = static_cast<float>(std::rand()) / RAND_MAX * 20.0 - 10.0;
 

From ec4172eaa59c49e60fa0ea0902e1e8041df40706 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Mon, 9 Sep 2024 17:53:05 +0900
Subject: [PATCH 25/72] chore: rewrite get conversion function

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector.hpp                  |  4 +--
 .../distortion_corrector.cpp                  |  4 +--
 .../test/test_distortion_corrector_node.cpp   | 25 ++++++++++---------
 3 files changed, 17 insertions(+), 16 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
index cc3e5f98ed375..0caddff956717 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -75,7 +75,7 @@ class DistortionCorrectorBase
     const std::string & base_frame, const std::string & lidar_frame) = 0;
   virtual void initialize() = 0;
   virtual bool azimuthConversionExists(sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
-  virtual std::tuple<float, float> getConversion() = 0;
+  virtual AngleConversion getAngleConversion() = 0;
   virtual void undistortPointCloud(
     bool use_imu, bool can_update_azimuth_and_distance,
     sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
@@ -135,7 +135,7 @@ class DistortionCorrector : public DistortionCorrectorBase
     bool use_imu, bool update_azimuth_and_distance,
     sensor_msgs::msg::PointCloud2 & pointcloud) override;
   bool azimuthConversionExists(sensor_msgs::msg::PointCloud2 & pointcloud) override;
-  std::tuple<float, float> getConversion() override;
+  AngleConversion getAngleConversion() override;
 
   bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud);
 };
diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index 3d6203a4b26e8..7420077cf0160 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -266,9 +266,9 @@ bool DistortionCorrector<T>::azimuthConversionExists(sensor_msgs::msg::PointClou
 }
 
 template <class T>
-std::tuple<float, float> DistortionCorrector<T>::getConversion()
+AngleConversion DistortionCorrector<T>::getAngleConversion()
 {
-  return std::make_tuple(angle_conversion_.offset_rad_, angle_conversion_.sign_);
+  return angle_conversion_;
 }
 
 template <class T>
diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index 5556b92b91004..a4a06b327eccd 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -1240,9 +1240,9 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsVelodynePointcloud)
     generatePointCloudMsg(true, true, "", timestamp, false, velodyne_points, velodyne_azimuths);
   EXPECT_TRUE(distortion_corrector_2d_->azimuthConversionExists(velodyne_pointcloud));
 
-  auto [rad_offset, sign] = distortion_corrector_2d_->getConversion();
-  EXPECT_EQ(sign, -1);
-  EXPECT_NEAR(rad_offset, autoware::universe_utils::pi * 2, standard_tolerance_);
+  auto angle_conversion = distortion_corrector_2d_->getAngleConversion();
+  EXPECT_EQ(angle_conversion.sign_, -1);
+  EXPECT_NEAR(angle_conversion.offset_rad_, autoware::universe_utils::pi * 2, standard_tolerance_);
 }
 
 TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsHesaiPointcloud)
@@ -1266,9 +1266,9 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsHesaiPointcloud)
     generatePointCloudMsg(true, true, "", timestamp, false, hesai_points, hesai_azimuths);
   EXPECT_TRUE(distortion_corrector_2d_->azimuthConversionExists(hesai_pointcloud));
 
-  auto [rad_offset, sign] = distortion_corrector_2d_->getConversion();
-  EXPECT_EQ(sign, -1);
-  EXPECT_NEAR(rad_offset, autoware::universe_utils::pi / 2, standard_tolerance_);
+  auto angle_conversion = distortion_corrector_2d_->getAngleConversion();
+  EXPECT_EQ(angle_conversion.sign_, -1);
+  EXPECT_NEAR(angle_conversion.offset_rad_, autoware::universe_utils::pi / 2, standard_tolerance_);
 }
 
 TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsCartesianPointcloud)
@@ -1291,9 +1291,9 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsCartesianPointcloud)
     generatePointCloudMsg(true, true, "", timestamp, false, cartesian_points, cartesian_azimuths);
   EXPECT_TRUE(distortion_corrector_2d_->azimuthConversionExists(cartesian_pointcloud));
 
-  auto [rad_offset, sign] = distortion_corrector_2d_->getConversion();
-  EXPECT_EQ(sign, 1);
-  EXPECT_NEAR(rad_offset, 0, standard_tolerance_);
+  auto angle_conversion = distortion_corrector_2d_->getAngleConversion();
+  EXPECT_EQ(angle_conversion.sign_, 1);
+  EXPECT_NEAR(angle_conversion.offset_rad_, 0, standard_tolerance_);
 }
 
 TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsRandomPointcloud1)
@@ -1316,9 +1316,10 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsRandomPointcloud1)
     generatePointCloudMsg(true, true, "", timestamp, false, points, azimuths);
   EXPECT_TRUE(distortion_corrector_2d_->azimuthConversionExists(pointcloud));
 
-  auto [rad_offset, sign] = distortion_corrector_2d_->getConversion();
-  EXPECT_EQ(sign, 1);
-  EXPECT_NEAR(rad_offset, autoware::universe_utils::pi * 3 / 2, standard_tolerance_);
+  auto angle_conversion = distortion_corrector_2d_->getAngleConversion();
+  EXPECT_EQ(angle_conversion.sign_, 1);
+  EXPECT_NEAR(
+    angle_conversion.offset_rad_, autoware::universe_utils::pi * 3 / 2, standard_tolerance_);
 }
 
 int main(int argc, char ** argv)

From 427a0ce2019d10eb346e34f9b0a50224a1415ecd Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Tue, 10 Sep 2024 13:55:25 +0900
Subject: [PATCH 26/72] chore: fix opencv fast atan2 function

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../universe_utils/math/trigonometry.hpp      |  2 +-
 .../src/math/trigonometry.cpp                 | 24 ++++++++++++++-----
 .../test/src/math/test_trigonometry.cpp       |  4 ++--
 .../distortion_corrector.cpp                  |  7 +++---
 4 files changed, 25 insertions(+), 12 deletions(-)

diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp
index 77be77909c4da..184538f158c3c 100644
--- a/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp
+++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp
@@ -26,7 +26,7 @@ float cos(float radian);
 
 std::pair<float, float> sin_and_cos(float radian);
 
-float opencv_fastAtan2(float dy, float dx);
+float opencv_fast_atan2(float dy, float dx);
 
 }  // namespace autoware::universe_utils
 
diff --git a/common/autoware_universe_utils/src/math/trigonometry.cpp b/common/autoware_universe_utils/src/math/trigonometry.cpp
index 35b0dc556cc03..82785d1187220 100644
--- a/common/autoware_universe_utils/src/math/trigonometry.cpp
+++ b/common/autoware_universe_utils/src/math/trigonometry.cpp
@@ -80,6 +80,8 @@ std::pair<float, float> sin_and_cos(float radian)
 // Modification:
 // 1. use autoware defined PI
 // 2. output of the function is changed from degrees to radians.
+namespace detail_fast_atan2
+{
 static const float atan2_p1 =
   0.9997878412794807f * static_cast<float>(180) / autoware::universe_utils::pi;
 static const float atan2_p3 =
@@ -89,19 +91,29 @@ static const float atan2_p5 =
 static const float atan2_p7 =
   -0.04432655554792128f * static_cast<float>(180) / autoware::universe_utils::pi;
 static const float atan2_DBL_EPSILON = 2.2204460492503131e-016f;
+}  // namespace detail_fast_atan2
 
-float opencv_fastAtan2(float dy, float dx)
+float opencv_fast_atan2(float dy, float dx)
 {
-  float ax = std::abs(dx), ay = std::abs(dy);
+  float ax = std::abs(dx);
+  float ay = std::abs(dy);
   float a, c, c2;
   if (ax >= ay) {
-    c = ay / (ax + atan2_DBL_EPSILON);
+    c = ay / (ax + detail_fast_atan2::atan2_DBL_EPSILON);
     c2 = c * c;
-    a = (((atan2_p7 * c2 + atan2_p5) * c2 + atan2_p3) * c2 + atan2_p1) * c;
+    a = (((detail_fast_atan2::atan2_p7 * c2 + detail_fast_atan2::atan2_p5) * c2 +
+          detail_fast_atan2::atan2_p3) *
+           c2 +
+         detail_fast_atan2::atan2_p1) *
+        c;
   } else {
-    c = ax / (ay + atan2_DBL_EPSILON);
+    c = ax / (ay + detail_fast_atan2::atan2_DBL_EPSILON);
     c2 = c * c;
-    a = 90.f - (((atan2_p7 * c2 + atan2_p5) * c2 + atan2_p3) * c2 + atan2_p1) * c;
+    a = 90.f - (((detail_fast_atan2::atan2_p7 * c2 + detail_fast_atan2::atan2_p5) * c2 +
+                 detail_fast_atan2::atan2_p3) *
+                  c2 +
+                detail_fast_atan2::atan2_p1) *
+                 c;
   }
   if (dx < 0) a = 180.f - a;
   if (dy < 0) a = 360.f - a;
diff --git a/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp
index 4ffaeaf6f4ca5..be33f340ba706 100644
--- a/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp
+++ b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp
@@ -59,7 +59,7 @@ float normalize_angle(double angle)
   return angle;
 }
 
-TEST(trigonometry, opencv_fastAtan2)
+TEST(trigonometry, opencv_fast_atan2)
 {
   for (int i = 0; i < 100; ++i) {
     // Generate random x and y between -10 and 10
@@ -67,7 +67,7 @@ TEST(trigonometry, opencv_fastAtan2)
     float x = static_cast<float>(std::rand()) / RAND_MAX * 20.0 - 10.0;
     float y = static_cast<float>(std::rand()) / RAND_MAX * 20.0 - 10.0;
 
-    float fast_atan = autoware::universe_utils::opencv_fastAtan2(y, x);
+    float fast_atan = autoware::universe_utils::opencv_fast_atan2(y, x);
     float std_atan = normalize_angle(std::atan2(y, x));
 
     // 0.3 degree accuracy
diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index 7420077cf0160..d1509765cd0da 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -214,8 +214,8 @@ bool DistortionCorrector<T>::azimuthConversionExists(sensor_msgs::msg::PointClou
 
   for (; next_it_x != it_x.end();
        ++it_x, ++it_y, ++it_azimuth, ++next_it_x, ++next_it_y, ++next_it_azimuth) {
-    auto current_cartesian_rad = autoware::universe_utils::opencv_fastAtan2(*it_y, *it_x);
-    auto next_cartesian_rad = autoware::universe_utils::opencv_fastAtan2(*next_it_y, *next_it_x);
+    auto current_cartesian_rad = autoware::universe_utils::opencv_fast_atan2(*it_y, *it_x);
+    auto next_cartesian_rad = autoware::universe_utils::opencv_fast_atan2(*next_it_y, *next_it_x);
 
     // If the angle exceeds 180 degrees, it may cross the 0-degree axis,
     // which could disrupt the calculation of the formula.
@@ -342,7 +342,8 @@ void DistortionCorrector<T>::undistortPointCloud(
     undistortPoint(it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid);
 
     if (can_update_azimuth_and_distance && pointcloudTransformNeeded()) {
-      float cartesian_coordinate_azimuth = autoware::universe_utils::opencv_fastAtan2(*it_y, *it_x);
+      float cartesian_coordinate_azimuth =
+        autoware::universe_utils::opencv_fast_atan2(*it_y, *it_x);
       float updated_azimuth =
         angle_conversion_.offset_rad_ + angle_conversion_.sign_ * cartesian_coordinate_azimuth;
       if (updated_azimuth < 0) {

From 25253c3a5a30c203ec2421a618bdd7628a5cdfee Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Tue, 10 Sep 2024 14:05:46 +0900
Subject: [PATCH 27/72] chore: fix schema description

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../schema/distortion_corrector_node.schema.json                | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json
index 78deb91b0c899..091695716c610 100644
--- a/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json
+++ b/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json
@@ -23,7 +23,7 @@
         },
         "update_azimuth_and_distance": {
           "type": "boolean",
-          "description": "Update the azimuth and distance value of each points.",
+          "description": "Flag to update the azimuth and distance values of each point after undistortion. If set to false, the azimuth and distance values will remain unchanged after undistortion, resulting in a mismatch with the updated x, y, z coordinates.",
           "default": "false"
         },
         "has_static_tf_only": {

From ed3efeeedc4e707f6b08d4c37b069dc9edd33463 Mon Sep 17 00:00:00 2001
From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com>
Date: Tue, 10 Sep 2024 14:10:15 +0900
Subject: [PATCH 28/72] Update
 sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp

Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com>
---
 .../test/test_distortion_corrector_node.cpp                     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index a4a06b327eccd..ef8433ba9a95d 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -953,7 +953,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion)
   }
 }
 
-TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdateAzimuthAndDistance)
+TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdatingAzimuthAndDistance)
 {
   // Test the case when the cloud will not update the azimuth and distance values
   // 1. when pointcloud is in the base_link

From 031bc2c616396c9d5e0e45d63d30fa312f41fcf3 Mon Sep 17 00:00:00 2001
From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com>
Date: Tue, 10 Sep 2024 14:10:38 +0900
Subject: [PATCH 29/72] Update
 sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp

Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com>
---
 .../test/test_distortion_corrector_node.cpp                     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index ef8433ba9a95d..7ddf3163121a4 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -956,7 +956,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion)
 TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdatingAzimuthAndDistance)
 {
   // Test the case when the cloud will not update the azimuth and distance values
-  // 1. when pointcloud is in the base_link
+  // 1. when pointcloud is in base_link
 
   // Generate the point cloud message in base_link
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);

From a1f076c0429c5cdf4ac16890941c72f9c9a2f96e Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Tue, 10 Sep 2024 14:58:10 +0900
Subject: [PATCH 30/72] chore: move code to function for readability

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../test/test_distortion_corrector_node.cpp   | 81 +++++++++++--------
 1 file changed, 46 insertions(+), 35 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index 7ddf3163121a4..a8c729f858163 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -170,6 +170,51 @@ class DistortionCorrectorTest : public ::testing::Test
     return imu_msgs;
   }
 
+  std::tuple<std::vector<Eigen::Vector3f>, std::vector<float>> generate_default_pointcloud(
+    std::string vendor)
+  {
+    // Generate all combinations of signs { -, 0, + } x { -, 0, + } for x and y.
+    std::vector<Eigen::Vector3f> default_points = {{
+      Eigen::Vector3f(10.0f, 0.0f, 1.0f),   // point 1
+      Eigen::Vector3f(5.0f, -5.0f, 2.0f),   // point 2
+      Eigen::Vector3f(0.0f, -10.0f, 3.0f),  // point 3
+      Eigen::Vector3f(-5.0f, -5.0f, 4.0f),  // point 4
+      Eigen::Vector3f(-10.0f, 0.0f, 5.0f),  // point 5
+      Eigen::Vector3f(-5.0f, 5.0f, -5.0f),  // point 6
+      Eigen::Vector3f(0.0f, 10.0f, -4.0f),  // point 7
+      Eigen::Vector3f(5.0f, 5.0f, -3.0f),   // point 8
+      Eigen::Vector3f(8.0f, 3.0f, -2.0f),   // point 9
+      Eigen::Vector3f(9.0f, 1.0f, -1.0f)    // point 10
+    }};
+
+    std::vector<float> default_azimuths;
+    for (const auto & point : default_points) {
+      if (vendor == "velodyne") {
+        // velodyne coordinates: x-axis is 0 degrees, y-axis is 270 degrees, angle increase in
+        // clockwise direction
+        float cartesian_deg = std::atan2(point.y(), point.x()) * 180 / autoware::universe_utils::pi;
+        if (cartesian_deg < 0) cartesian_deg += 360;
+        float velodyne_deg = 360 - cartesian_deg;
+        if (velodyne_deg == 360) velodyne_deg = 0;
+        default_azimuths.push_back(velodyne_deg * autoware::universe_utils::pi / 180);
+      } else if (vendor == "hesai") {
+        // hesai coordinates: y-axis is 0 degrees, x-axis is 90 degrees, angle increase in clockwise
+        // direction
+        float cartesian_deg = std::atan2(point.y(), point.x()) * 180 / autoware::universe_utils::pi;
+        if (cartesian_deg < 0) cartesian_deg += 360;
+        float hesai_deg = 90 - cartesian_deg < 0 ? 90 - cartesian_deg + 360 : 90 - cartesian_deg;
+        if (hesai_deg == 360) hesai_deg = 0;
+        default_azimuths.push_back(hesai_deg * autoware::universe_utils::pi / 180);
+      } else {
+        // Cartesian coordingates: x-axis is 0 degrees, y-axis is 90 degrees, angle increase in
+        // counterclockwise direction
+        default_azimuths.push_back(std::atan2(point.y(), point.x()));
+      }
+    }
+
+    return std::make_tuple(default_points, default_azimuths);
+  }
+
   sensor_msgs::msg::PointCloud2 generatePointCloudMsg(
     bool generate_points, bool is_lidar_frame, std::string vendor, rclcpp::Time stamp,
     bool use_default_pointcloud, std::vector<Eigen::Vector3f> defined_points,
@@ -187,41 +232,7 @@ class DistortionCorrectorTest : public ::testing::Test
       std::vector<float> azimuths;
 
       if (use_default_pointcloud) {
-        std::vector<Eigen::Vector3f> default_points = {{
-          Eigen::Vector3f(10.0f, 0.0f, 1.0f),   // point 1
-          Eigen::Vector3f(5.0f, -5.0f, 2.0f),   // point 2
-          Eigen::Vector3f(0.0f, -10.0f, 3.0f),  // point 3
-          Eigen::Vector3f(-5.0f, -5.0f, 4.0f),  // point 4
-          Eigen::Vector3f(-10.0f, 0.0f, 5.0f),  // point 5
-          Eigen::Vector3f(-5.0f, 5.0f, -5.0f),  // point 6
-          Eigen::Vector3f(0.0f, 10.0f, -4.0f),  // point 7
-          Eigen::Vector3f(5.0f, 5.0f, -3.0f),   // point 8
-          Eigen::Vector3f(8.0f, 3.0f, -2.0f),   // point 9
-          Eigen::Vector3f(9.0f, 1.0f, -1.0f)    // point 10
-        }};
-
-        std::vector<float> default_azimuths;
-        for (const auto & point : default_points) {
-          if (vendor == "velodyne") {
-            float cartesian_deg =
-              std::atan2(point.y(), point.x()) * 180 / autoware::universe_utils::pi;
-            if (cartesian_deg < 0) cartesian_deg += 360;
-            float velodyne_deg = 360 - cartesian_deg;
-            if (velodyne_deg == 360) velodyne_deg = 0;
-            default_azimuths.push_back(velodyne_deg * autoware::universe_utils::pi / 180);
-          } else if (vendor == "hesai") {
-            float cartesian_deg =
-              std::atan2(point.y(), point.x()) * 180 / autoware::universe_utils::pi;
-            if (cartesian_deg < 0) cartesian_deg += 360;
-            float hesai_deg =
-              90 - cartesian_deg < 0 ? 90 - cartesian_deg + 360 : 90 - cartesian_deg;
-            if (hesai_deg == 360) hesai_deg = 0;
-            default_azimuths.push_back(hesai_deg * autoware::universe_utils::pi / 180);
-          } else {  // empty string
-            default_azimuths.push_back(std::atan2(point.y(), point.x()));
-          }
-        }
-
+        auto [default_points, default_azimuths] = generate_default_pointcloud(vendor);
         points = default_points;
         azimuths = default_azimuths;
       } else {

From 71b1917af62dd2463220502063c9a198cdf2c238 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Wed, 11 Sep 2024 08:48:32 +0900
Subject: [PATCH 31/72] chore: simplify code

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../test/test_distortion_corrector_node.cpp   | 145 ++++++------------
 1 file changed, 49 insertions(+), 96 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index a8c729f858163..bb880b5b2e863 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -299,6 +299,26 @@ class DistortionCorrectorTest : public ::testing::Test
     return timestamps;
   }
 
+  template <typename T>
+  void generateAndProcessTwistMsgs(
+    const std::shared_ptr<T> & distortion_corrector, rclcpp::Time timestamp)
+  {
+    auto twist_msgs = generateTwistMsgs(timestamp);
+    for (const auto & twist_msg : twist_msgs) {
+      distortion_corrector->processTwistMessage(twist_msg);
+    }
+  }
+
+  template <typename T>
+  void generateAndProcessIMUMsgs(
+    const std::shared_ptr<T> & distortion_corrector, rclcpp::Time timestamp)
+  {
+    auto imu_msgs = generateImuMsgs(timestamp);
+    for (const auto & imu_msg : imu_msgs) {
+      distortion_corrector->processIMUMessage("base_link", imu_msg);
+    }
+  }
+
   std::shared_ptr<rclcpp::Node> node_;
   std::shared_ptr<autoware::pointcloud_preprocessor::DistortionCorrector2D>
     distortion_corrector_2d_;
@@ -446,10 +466,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyPointCloud)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   // Generate and process multiple twist messages
-  auto twist_msgs = generateTwistMsgs(timestamp);
-  for (const auto & twist_msg : twist_msgs) {
-    distortion_corrector_2d_->processTwistMessage(twist_msg);
-  }
+  generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
   // Generate an empty point cloud message
   sensor_msgs::msg::PointCloud2 empty_pointcloud =
     generatePointCloudMsg(false, false, "", timestamp, true, {}, {});
@@ -471,10 +488,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink)
     generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
-  auto twist_msgs = generateTwistMsgs(timestamp);
-  for (const auto & twist_msg : twist_msgs) {
-    distortion_corrector_2d_->processTwistMessage(twist_msg);
-  }
+  generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
 
   // Test using only twist
   distortion_corrector_2d_->initialize();
@@ -518,16 +532,10 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink)
     generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
-  auto twist_msgs = generateTwistMsgs(timestamp);
-  for (const auto & twist_msg : twist_msgs) {
-    distortion_corrector_2d_->processTwistMessage(twist_msg);
-  }
+  generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
 
   // Generate and process multiple IMU messages
-  auto imu_msgs = generateImuMsgs(timestamp);
-  for (const auto & imu_msg : imu_msgs) {
-    distortion_corrector_2d_->processIMUMessage("base_link", imu_msg);
-  }
+  generateAndProcessIMUMsgs(distortion_corrector_2d_, timestamp);
 
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
@@ -570,16 +578,10 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame)
     generatePointCloudMsg(true, true, "", timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
-  auto twist_msgs = generateTwistMsgs(timestamp);
-  for (const auto & twist_msg : twist_msgs) {
-    distortion_corrector_2d_->processTwistMessage(twist_msg);
-  }
+  generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
 
   // Generate and process multiple IMU messages
-  auto imu_msgs = generateImuMsgs(timestamp);
-  for (const auto & imu_msg : imu_msgs) {
-    distortion_corrector_2d_->processIMUMessage("base_link", imu_msg);
-  }
+  generateAndProcessIMUMsgs(distortion_corrector_2d_, timestamp);
 
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
@@ -625,10 +627,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink)
     generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
-  auto twist_msgs = generateTwistMsgs(timestamp);
-  for (const auto & twist_msg : twist_msgs) {
-    distortion_corrector_3d_->processTwistMessage(twist_msg);
-  }
+  generateAndProcessTwistMsgs(distortion_corrector_3d_, timestamp);
 
   // Test using only twist
   distortion_corrector_3d_->initialize();
@@ -672,16 +671,10 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink)
     generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
-  auto twist_msgs = generateTwistMsgs(timestamp);
-  for (const auto & twist_msg : twist_msgs) {
-    distortion_corrector_3d_->processTwistMessage(twist_msg);
-  }
+  generateAndProcessTwistMsgs(distortion_corrector_3d_, timestamp);
 
   // Generate and process multiple IMU messages
-  auto imu_msgs = generateImuMsgs(timestamp);
-  for (const auto & imu_msg : imu_msgs) {
-    distortion_corrector_3d_->processIMUMessage("base_link", imu_msg);
-  }
+  generateAndProcessIMUMsgs(distortion_corrector_3d_, timestamp);
 
   distortion_corrector_3d_->initialize();
   distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link");
@@ -727,16 +720,10 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame)
     generatePointCloudMsg(true, true, "", timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
-  auto twist_msgs = generateTwistMsgs(timestamp);
-  for (const auto & twist_msg : twist_msgs) {
-    distortion_corrector_3d_->processTwistMessage(twist_msg);
-  }
+  generateAndProcessTwistMsgs(distortion_corrector_3d_, timestamp);
 
   // Generate and process multiple IMU messages
-  auto imu_msgs = generateImuMsgs(timestamp);
-  for (const auto & imu_msg : imu_msgs) {
-    distortion_corrector_3d_->processIMUMessage("base_link", imu_msg);
-  }
+  generateAndProcessIMUMsgs(distortion_corrector_3d_, timestamp);
 
   distortion_corrector_3d_->initialize();
   distortion_corrector_3d_->setPointCloudTransform("base_link", "lidar_top");
@@ -975,16 +962,10 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdatingAzimuthAndDist
     generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
-  auto twist_msgs = generateTwistMsgs(timestamp);
-  for (const auto & twist_msg : twist_msgs) {
-    distortion_corrector_2d_->processTwistMessage(twist_msg);
-  }
+  generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
 
   // Generate and process multiple IMU messages
-  auto imu_msgs = generateImuMsgs(timestamp);
-  for (const auto & imu_msg : imu_msgs) {
-    distortion_corrector_2d_->processIMUMessage("base_link", imu_msg);
-  }
+  generateAndProcessIMUMsgs(distortion_corrector_2d_, timestamp);
 
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
@@ -1035,16 +1016,10 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdatingAzimuthAndDist
     generatePointCloudMsg(true, true, "", timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
-  twist_msgs = generateTwistMsgs(timestamp);
-  for (const auto & twist_msg : twist_msgs) {
-    distortion_corrector_2d_->processTwistMessage(twist_msg);
-  }
+  generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
 
   // Generate and process multiple IMU messages
-  imu_msgs = generateImuMsgs(timestamp);
-  for (const auto & imu_msg : imu_msgs) {
-    distortion_corrector_2d_->processIMUMessage("base_link", imu_msg);
-  }
+  generateAndProcessIMUMsgs(distortion_corrector_2d_, timestamp);
 
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
@@ -1096,16 +1071,10 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
     generatePointCloudMsg(true, true, "velodyne", timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
-  auto twist_msgs = generateTwistMsgs(timestamp);
-  for (const auto & twist_msg : twist_msgs) {
-    distortion_corrector_2d_->processTwistMessage(twist_msg);
-  }
+  generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
 
   // Generate and process multiple IMU messages
-  auto imu_msgs = generateImuMsgs(timestamp);
-  for (const auto & imu_msg : imu_msgs) {
-    distortion_corrector_2d_->processIMUMessage("base_link", imu_msg);
-  }
+  generateAndProcessIMUMsgs(distortion_corrector_2d_, timestamp);
 
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
@@ -1161,16 +1130,10 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
     generatePointCloudMsg(true, true, "hesai", timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
-  auto twist_msgs = generateTwistMsgs(timestamp);
-  for (const auto & twist_msg : twist_msgs) {
-    distortion_corrector_2d_->processTwistMessage(twist_msg);
-  }
+  generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
 
   // Generate and process multiple IMU messages
-  auto imu_msgs = generateImuMsgs(timestamp);
-  for (const auto & imu_msg : imu_msgs) {
-    distortion_corrector_2d_->processIMUMessage("base_link", imu_msg);
-  }
+  generateAndProcessIMUMsgs(distortion_corrector_2d_, timestamp);
 
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
@@ -1222,10 +1185,8 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsEmptyPointcloud)
   // test empty pointcloud
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   // Generate and process multiple twist messages
-  auto twist_msgs = generateTwistMsgs(timestamp);
-  for (const auto & twist_msg : twist_msgs) {
-    distortion_corrector_2d_->processTwistMessage(twist_msg);
-  }
+  generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
+
   sensor_msgs::msg::PointCloud2 empty_pointcloud =
     generatePointCloudMsg(false, false, "", timestamp, true, {}, {});
   EXPECT_FALSE(distortion_corrector_2d_->azimuthConversionExists(empty_pointcloud));
@@ -1236,10 +1197,8 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsVelodynePointcloud)
   // test velodyne pointcloud (x-axis: 0 degree, y-axis: 270 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   // Generate and process multiple twist messages
-  auto twist_msgs = generateTwistMsgs(timestamp);
-  for (const auto & twist_msg : twist_msgs) {
-    distortion_corrector_2d_->processTwistMessage(twist_msg);
-  }
+  generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
+
   std::vector<Eigen::Vector3f> velodyne_points = {
     Eigen::Vector3f(0.0f, 0.0f, 0.0f),
     Eigen::Vector3f(1.0f, -1.0f, 1.0f),
@@ -1261,10 +1220,8 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsHesaiPointcloud)
   // test hesai pointcloud (x-axis: 90 degree, y-axis: 0 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   // Generate and process multiple twist messages
-  auto twist_msgs = generateTwistMsgs(timestamp);
-  for (const auto & twist_msg : twist_msgs) {
-    distortion_corrector_2d_->processTwistMessage(twist_msg);
-  }
+  generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
+
   std::vector<Eigen::Vector3f> hesai_points = {
     Eigen::Vector3f(0.0f, 0.0f, 0.0f),
     Eigen::Vector3f(1.0f, -1.0f, 1.0f),
@@ -1287,10 +1244,8 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsCartesianPointcloud)
   // test pointcloud that use cartesian coordinate for azimuth (x-axis: 0 degree, y-axis: 90 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   // Generate and process multiple twist messages
-  auto twist_msgs = generateTwistMsgs(timestamp);
-  for (const auto & twist_msg : twist_msgs) {
-    distortion_corrector_2d_->processTwistMessage(twist_msg);
-  }
+  generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
+
   std::vector<Eigen::Vector3f> cartesian_points = {
     Eigen::Vector3f(0.0f, 0.0f, 0.0f),
     Eigen::Vector3f(1.0f, 1.0f, 1.0f),
@@ -1312,10 +1267,8 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsRandomPointcloud1)
   // test pointcloud that use coordinate (x-axis: 270 degree, y-axis: 0 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   // Generate and process multiple twist messages
-  auto twist_msgs = generateTwistMsgs(timestamp);
-  for (const auto & twist_msg : twist_msgs) {
-    distortion_corrector_2d_->processTwistMessage(twist_msg);
-  }
+  generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
+
   std::vector<Eigen::Vector3f> points = {
     Eigen::Vector3f(0.0f, 1.0f, 0.0f),
     Eigen::Vector3f(2.0f, 0.0f, 1.0f),

From 80ad8e21af43bcfef446ae67cfb5f18c32f53595 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Wed, 11 Sep 2024 08:53:16 +0900
Subject: [PATCH 32/72] chore: fix sentence, angle conversion

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../src/distortion_corrector/distortion_corrector.cpp           | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index d1509765cd0da..f69df0bc7a654 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -208,7 +208,7 @@ bool DistortionCorrector<T>::azimuthConversionExists(sensor_msgs::msg::PointClou
   } else {
     RCLCPP_WARN(
       node_->get_logger(),
-      "Current point cloud only has a single point. Could not calculate the formula.");
+      "Current point cloud only has a single point. Could not calculate the angle conversion.");
     return false;
   }
 

From 11b88bd804a1b26a4f07c90f69c1c01a8dcf583d Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Wed, 11 Sep 2024 09:21:50 +0900
Subject: [PATCH 33/72] chore: add more invalid condition

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../src/distortion_corrector/distortion_corrector.cpp         | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index f69df0bc7a654..48dc92361a779 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -221,7 +221,9 @@ bool DistortionCorrector<T>::azimuthConversionExists(sensor_msgs::msg::PointClou
     // which could disrupt the calculation of the formula.
     if (
       abs(*next_it_azimuth - *it_azimuth) >= autoware::universe_utils::pi ||
-      abs(next_cartesian_rad - current_cartesian_rad) >= autoware::universe_utils::pi) {
+      abs(next_cartesian_rad - current_cartesian_rad) >= autoware::universe_utils::pi ||
+      abs(*next_it_azimuth - *it_azimuth) == 0 ||
+      abs(next_cartesian_rad - current_cartesian_rad) == 0) {
       RCLCPP_WARN(
         node_->get_logger(),
         "Angle between two points exceeds 180 degrees. Iterate to next point ...");

From 53f41627c1f8ad1f5d7ff650cef191ecb8f379f1 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Wed, 11 Sep 2024 12:00:59 +0900
Subject: [PATCH 34/72] chore: fix the string name to enum

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector.cpp                  | 10 ++-
 .../test/test_distortion_corrector_node.cpp   | 85 ++++++++++---------
 2 files changed, 52 insertions(+), 43 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index 48dc92361a779..16d7599ff8e56 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -224,7 +224,7 @@ bool DistortionCorrector<T>::azimuthConversionExists(sensor_msgs::msg::PointClou
       abs(next_cartesian_rad - current_cartesian_rad) >= autoware::universe_utils::pi ||
       abs(*next_it_azimuth - *it_azimuth) == 0 ||
       abs(next_cartesian_rad - current_cartesian_rad) == 0) {
-      RCLCPP_WARN(
+      RCLCPP_DEBUG(
         node_->get_logger(),
         "Angle between two points exceeds 180 degrees. Iterate to next point ...");
       continue;
@@ -239,9 +239,8 @@ bool DistortionCorrector<T>::azimuthConversionExists(sensor_msgs::msg::PointClou
     } else if (std::abs(sign + 1.0f) <= angle_conversion_.sign_threshold_) {
       angle_conversion_.sign_ = -1.0f;
     } else {
-      RCLCPP_WARN(
-        node_->get_logger(),
-        "Angle between two points exceeds 180 degrees. Iterate to next point ...");
+      RCLCPP_DEBUG(
+        node_->get_logger(), "Value of sign is not close to 1 or -1. Iterate to next point ...");
       continue;
     }
 
@@ -250,6 +249,9 @@ bool DistortionCorrector<T>::azimuthConversionExists(sensor_msgs::msg::PointClou
     if (
       std::abs(offset_rad - multiple_of_90_degrees * (autoware::universe_utils::pi / 2)) >
       angle_conversion_.offset_rad_threshold_) {
+      RCLCPP_DEBUG(
+        node_->get_logger(),
+        "Value of offset_rad is not close to 1 or -1. Iterate to next point ...");
       continue;
     }
 
diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index bb880b5b2e863..282a35549e47b 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -42,6 +42,7 @@
 
 #include <cassert>
 
+enum AngleCoordinateSystem { HESAI, VELODYNE, CARTESIAN };
 class DistortionCorrectorTest : public ::testing::Test
 {
 protected:
@@ -171,7 +172,7 @@ class DistortionCorrectorTest : public ::testing::Test
   }
 
   std::tuple<std::vector<Eigen::Vector3f>, std::vector<float>> generate_default_pointcloud(
-    std::string vendor)
+    AngleCoordinateSystem vendor)
   {
     // Generate all combinations of signs { -, 0, + } x { -, 0, + } for x and y.
     std::vector<Eigen::Vector3f> default_points = {{
@@ -189,7 +190,7 @@ class DistortionCorrectorTest : public ::testing::Test
 
     std::vector<float> default_azimuths;
     for (const auto & point : default_points) {
-      if (vendor == "velodyne") {
+      if (vendor == AngleCoordinateSystem::VELODYNE) {
         // velodyne coordinates: x-axis is 0 degrees, y-axis is 270 degrees, angle increase in
         // clockwise direction
         float cartesian_deg = std::atan2(point.y(), point.x()) * 180 / autoware::universe_utils::pi;
@@ -197,7 +198,7 @@ class DistortionCorrectorTest : public ::testing::Test
         float velodyne_deg = 360 - cartesian_deg;
         if (velodyne_deg == 360) velodyne_deg = 0;
         default_azimuths.push_back(velodyne_deg * autoware::universe_utils::pi / 180);
-      } else if (vendor == "hesai") {
+      } else if (vendor == AngleCoordinateSystem::HESAI) {
         // hesai coordinates: y-axis is 0 degrees, x-axis is 90 degrees, angle increase in clockwise
         // direction
         float cartesian_deg = std::atan2(point.y(), point.x()) * 180 / autoware::universe_utils::pi;
@@ -205,10 +206,12 @@ class DistortionCorrectorTest : public ::testing::Test
         float hesai_deg = 90 - cartesian_deg < 0 ? 90 - cartesian_deg + 360 : 90 - cartesian_deg;
         if (hesai_deg == 360) hesai_deg = 0;
         default_azimuths.push_back(hesai_deg * autoware::universe_utils::pi / 180);
-      } else {
+      } else if (vendor == AngleCoordinateSystem::CARTESIAN) {
         // Cartesian coordingates: x-axis is 0 degrees, y-axis is 90 degrees, angle increase in
         // counterclockwise direction
         default_azimuths.push_back(std::atan2(point.y(), point.x()));
+      } else {
+        throw std::runtime_error("Invalid angle coordinate system");
       }
     }
 
@@ -216,7 +219,7 @@ class DistortionCorrectorTest : public ::testing::Test
   }
 
   sensor_msgs::msg::PointCloud2 generatePointCloudMsg(
-    bool generate_points, bool is_lidar_frame, std::string vendor, rclcpp::Time stamp,
+    bool generate_points, bool is_lidar_frame, AngleCoordinateSystem vendor, rclcpp::Time stamp,
     bool use_default_pointcloud, std::vector<Eigen::Vector3f> defined_points,
     std::vector<float> defined_azimuths)
   {
@@ -385,7 +388,7 @@ TEST_F(DistortionCorrectorTest, TestIsInputValid)
 
   // input normal pointcloud without twist
   sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
+    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
   bool result = distortion_corrector_2d_->isInputValid(pointcloud);
   EXPECT_FALSE(result);
 
@@ -393,12 +396,14 @@ TEST_F(DistortionCorrectorTest, TestIsInputValid)
   auto twist_msg = generateTwistMsg(twist_linear_x_, twist_angular_z_, timestamp);
   distortion_corrector_2d_->processTwistMessage(twist_msg);
 
-  pointcloud = generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
+  pointcloud =
+    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
   result = distortion_corrector_2d_->isInputValid(pointcloud);
   EXPECT_TRUE(result);
 
   // input empty pointcloud
-  pointcloud = generatePointCloudMsg(false, false, "", timestamp, true, {}, {});
+  pointcloud =
+    generatePointCloudMsg(false, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
   result = distortion_corrector_2d_->isInputValid(pointcloud);
   EXPECT_FALSE(result);
 }
@@ -429,7 +434,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyTwist)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   // Generate the point cloud message
   sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
+    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
 
   // Process empty twist queue
   distortion_corrector_2d_->initialize();
@@ -469,7 +474,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyPointCloud)
   generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
   // Generate an empty point cloud message
   sensor_msgs::msg::PointCloud2 empty_pointcloud =
-    generatePointCloudMsg(false, false, "", timestamp, true, {}, {});
+    generatePointCloudMsg(false, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
 
   // Process empty point cloud
   distortion_corrector_2d_->initialize();
@@ -485,7 +490,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink)
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
+    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
@@ -529,7 +534,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink)
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
+    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
@@ -575,7 +580,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame)
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, true, "", timestamp, true, {}, {});
+    generatePointCloudMsg(true, true, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
@@ -624,7 +629,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink)
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
+    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_3d_, timestamp);
@@ -668,7 +673,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink)
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
+    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_3d_, timestamp);
@@ -717,7 +722,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame)
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, true, "", timestamp, true, {}, {});
+    generatePointCloudMsg(true, true, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_3d_, timestamp);
@@ -764,9 +769,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   sensor_msgs::msg::PointCloud2 test2d_pointcloud =
-    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
+    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
   sensor_msgs::msg::PointCloud2 test3d_pointcloud =
-    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
+    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
 
   // Generate and process a single twist message with constant linear velocity
   auto twist_msg = generateTwistMsg(1.0, 0.0, timestamp);
@@ -783,7 +788,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion)
 
   // Generate expected point cloud for testing
   sensor_msgs::msg::PointCloud2 expected_pointcloud_msg =
-    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
+    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
 
   // Calculate expected point cloud values based on constant linear motion
   double velocity = 1.0;  // 1 m/s linear velocity
@@ -853,9 +858,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   sensor_msgs::msg::PointCloud2 test2d_pointcloud =
-    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
+    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
   sensor_msgs::msg::PointCloud2 test3d_pointcloud =
-    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
+    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
 
   // Generate and process a single twist message with constant angular velocity
   auto twist_msg = generateTwistMsg(0.0, 0.1, timestamp);
@@ -872,7 +877,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion)
 
   // Generate expected point cloud for testing
   sensor_msgs::msg::PointCloud2 expected_pointcloud_msg =
-    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
+    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
 
   // Calculate expected point cloud values based on constant rotational motion
   double angular_velocity = 0.1;  // 0.1 rad/s rotational velocity
@@ -959,7 +964,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdatingAzimuthAndDist
   // Generate the point cloud message in base_link
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   sensor_msgs::msg::PointCloud2 pointcloud_base_link =
-    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
+    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
@@ -976,7 +981,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdatingAzimuthAndDist
     true, can_update_azimuth_and_distance, pointcloud_base_link);
 
   sensor_msgs::msg::PointCloud2 original_pointcloud_base_link =
-    generatePointCloudMsg(true, false, "", timestamp, true, {}, {});
+    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
 
   sensor_msgs::PointCloud2ConstIterator<float> test_iter_azimuth_base_link(
     pointcloud_base_link, "azimuth");
@@ -1013,7 +1018,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdatingAzimuthAndDist
 
   // Generate the point cloud message in sensor frame
   sensor_msgs::msg::PointCloud2 pointcloud_lidar_top =
-    generatePointCloudMsg(true, true, "", timestamp, true, {}, {});
+    generatePointCloudMsg(true, true, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
@@ -1029,7 +1034,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdatingAzimuthAndDist
     true, can_update_azimuth_and_distance, pointcloud_lidar_top);
 
   sensor_msgs::msg::PointCloud2 original_pointcloud_lidar_top =
-    generatePointCloudMsg(true, true, "", timestamp, true, {}, {});
+    generatePointCloudMsg(true, true, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
 
   sensor_msgs::PointCloud2ConstIterator<float> test_iter_azimuth_lidar_top(
     pointcloud_lidar_top, "azimuth");
@@ -1068,7 +1073,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   // Generate the point cloud message in sensor frame
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, true, "velodyne", timestamp, true, {}, {});
+    generatePointCloudMsg(true, true, AngleCoordinateSystem::VELODYNE, timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
@@ -1084,7 +1089,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   distortion_corrector_2d_->undistortPointCloud(true, can_update_azimuth_and_distance, pointcloud);
 
   sensor_msgs::msg::PointCloud2 original_pointcloud =
-    generatePointCloudMsg(true, true, "velodyne", timestamp, true, {}, {});
+    generatePointCloudMsg(true, true, AngleCoordinateSystem::VELODYNE, timestamp, true, {}, {});
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_azimuth(pointcloud, "azimuth");
   sensor_msgs::PointCloud2ConstIterator<float> iter_distance(pointcloud, "distance");
@@ -1127,7 +1132,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   // Generate the point cloud message in sensor frame
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, true, "hesai", timestamp, true, {}, {});
+    generatePointCloudMsg(true, true, AngleCoordinateSystem::HESAI, timestamp, true, {}, {});
 
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
@@ -1143,7 +1148,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   distortion_corrector_2d_->undistortPointCloud(true, can_update_azimuth_and_distance, pointcloud);
 
   sensor_msgs::msg::PointCloud2 original_pointcloud =
-    generatePointCloudMsg(true, true, "hesai", timestamp, true, {}, {});
+    generatePointCloudMsg(true, true, AngleCoordinateSystem::HESAI, timestamp, true, {}, {});
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_azimuth(pointcloud, "azimuth");
   sensor_msgs::PointCloud2ConstIterator<float> iter_distance(pointcloud, "distance");
@@ -1188,7 +1193,7 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsEmptyPointcloud)
   generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
 
   sensor_msgs::msg::PointCloud2 empty_pointcloud =
-    generatePointCloudMsg(false, false, "", timestamp, true, {}, {});
+    generatePointCloudMsg(false, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
   EXPECT_FALSE(distortion_corrector_2d_->azimuthConversionExists(empty_pointcloud));
 }
 
@@ -1206,8 +1211,9 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsVelodynePointcloud)
   };
   std::vector<float> velodyne_azimuths = {
     0.0f, autoware::universe_utils::pi / 4, autoware::universe_utils::pi / 2};
-  sensor_msgs::msg::PointCloud2 velodyne_pointcloud =
-    generatePointCloudMsg(true, true, "", timestamp, false, velodyne_points, velodyne_azimuths);
+  sensor_msgs::msg::PointCloud2 velodyne_pointcloud = generatePointCloudMsg(
+    true, true, AngleCoordinateSystem::CARTESIAN, timestamp, false, velodyne_points,
+    velodyne_azimuths);
   EXPECT_TRUE(distortion_corrector_2d_->azimuthConversionExists(velodyne_pointcloud));
 
   auto angle_conversion = distortion_corrector_2d_->getAngleConversion();
@@ -1230,8 +1236,8 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsHesaiPointcloud)
   std::vector<float> hesai_azimuths = {
     autoware::universe_utils::pi / 2, autoware::universe_utils::pi * 3 / 4,
     autoware::universe_utils::pi};
-  sensor_msgs::msg::PointCloud2 hesai_pointcloud =
-    generatePointCloudMsg(true, true, "", timestamp, false, hesai_points, hesai_azimuths);
+  sensor_msgs::msg::PointCloud2 hesai_pointcloud = generatePointCloudMsg(
+    true, true, AngleCoordinateSystem::CARTESIAN, timestamp, false, hesai_points, hesai_azimuths);
   EXPECT_TRUE(distortion_corrector_2d_->azimuthConversionExists(hesai_pointcloud));
 
   auto angle_conversion = distortion_corrector_2d_->getAngleConversion();
@@ -1253,8 +1259,9 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsCartesianPointcloud)
   };
   std::vector<float> cartesian_azimuths = {
     0, autoware::universe_utils::pi / 4, autoware::universe_utils::pi / 2};
-  sensor_msgs::msg::PointCloud2 cartesian_pointcloud =
-    generatePointCloudMsg(true, true, "", timestamp, false, cartesian_points, cartesian_azimuths);
+  sensor_msgs::msg::PointCloud2 cartesian_pointcloud = generatePointCloudMsg(
+    true, true, AngleCoordinateSystem::CARTESIAN, timestamp, false, cartesian_points,
+    cartesian_azimuths);
   EXPECT_TRUE(distortion_corrector_2d_->azimuthConversionExists(cartesian_pointcloud));
 
   auto angle_conversion = distortion_corrector_2d_->getAngleConversion();
@@ -1276,8 +1283,8 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsRandomPointcloud1)
   };
   std::vector<float> azimuths = {
     0, autoware::universe_utils::pi * 3 / 2, autoware::universe_utils::pi * 7 / 4};
-  sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, true, "", timestamp, false, points, azimuths);
+  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(
+    true, true, AngleCoordinateSystem::CARTESIAN, timestamp, false, points, azimuths);
   EXPECT_TRUE(distortion_corrector_2d_->azimuthConversionExists(pointcloud));
 
   auto angle_conversion = distortion_corrector_2d_->getAngleConversion();

From a65a2bf5bbbf8bb545be7eeb8dc721e03ae418c1 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Wed, 11 Sep 2024 13:09:09 +0900
Subject: [PATCH 35/72] chore: remove runtime error

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../src/distortion_corrector/distortion_corrector_node.cpp     | 3 ---
 1 file changed, 3 deletions(-)

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 7330438fe7afa..1160b1b4e3d6e 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
@@ -110,9 +110,6 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointclou
         "Failed to get the conversion formula between cartesian coordinates and LiDAR azimuth "
         "coordinates. Please check the LiDAR azimuth coordinate. This pointcloud will not update "
         "azimuth and distance");
-      throw std::runtime_error(
-        "Couldn't get the conversion formula between cartesian coordinates and LiDAR azimuth "
-        "coordinates");
     }
   }
 

From 8a24a79457c1204a21b0a398d90a4ad5185ea82e Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Wed, 11 Sep 2024 16:18:43 +0900
Subject: [PATCH 36/72] chore: use optional for AngleConversion structure

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector.hpp                  |  28 ++---
 .../distortion_corrector_node.hpp             |   4 +
 .../distortion_corrector.cpp                  |  57 +++++----
 .../distortion_corrector_node.cpp             |  42 +++++--
 .../test/test_distortion_corrector_node.cpp   | 113 +++++++++---------
 5 files changed, 133 insertions(+), 111 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
index 0caddff956717..3bb0637875b75 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -51,12 +51,12 @@ namespace autoware::pointcloud_preprocessor
 struct AngleConversion
 {
   // Equation for the conversion between sensor azimuth coordinates and Cartesian coordinates:
-  // sensor azimuth coordinates = offset_rad_ + sign_ * cartesian coordinates;
-  // offset_rad_ is restricted to be a multiple of 90, and sign_ is restricted to be 1 or -1.
-  float offset_rad_{0};
-  float sign_{1};
-  float offset_rad_threshold_{0.087f};  // (5 / 180) * M_PI
-  float sign_threshold_{0.1f};
+  // sensor azimuth coordinates = offset_rad + sign * cartesian coordinates;
+  // offset_rad is restricted to be a multiple of 90, and sign is restricted to be 1 or -1.
+  float offset_rad{0};
+  float sign{1};
+  float offset_rad_threshold{0.087f};  // (5 / 180) * M_PI
+  float sign_threshold{0.1f};
 };
 
 class DistortionCorrectorBase
@@ -74,10 +74,10 @@ class DistortionCorrectorBase
   virtual void setPointCloudTransform(
     const std::string & base_frame, const std::string & lidar_frame) = 0;
   virtual void initialize() = 0;
-  virtual bool azimuthConversionExists(sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
-  virtual AngleConversion getAngleConversion() = 0;
+  virtual std::optional<AngleConversion> tryComputeAngleConversion(
+    sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
   virtual void undistortPointCloud(
-    bool use_imu, bool can_update_azimuth_and_distance,
+    bool use_imu, std::optional<AngleConversion> angle_conversion_opt,
     sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
 };
 
@@ -95,8 +95,6 @@ class DistortionCorrector : public DistortionCorrectorBase
   std::deque<geometry_msgs::msg::TwistStamped> twist_queue_;
   std::deque<geometry_msgs::msg::Vector3Stamped> angular_velocity_queue_;
 
-  AngleConversion angle_conversion_;
-
   void getIMUTransformation(const std::string & base_frame, const std::string & imu_frame);
   void enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg);
   void getTwistAndIMUIterator(
@@ -132,12 +130,12 @@ class DistortionCorrector : public DistortionCorrectorBase
   void processIMUMessage(
     const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override;
   void undistortPointCloud(
-    bool use_imu, bool update_azimuth_and_distance,
+    bool use_imu, std::optional<AngleConversion> angle_conversion_opt,
+    sensor_msgs::msg::PointCloud2 & pointcloud) override;
+  std::optional<AngleConversion> tryComputeAngleConversion(
     sensor_msgs::msg::PointCloud2 & pointcloud) override;
-  bool azimuthConversionExists(sensor_msgs::msg::PointCloud2 & pointcloud) override;
-  AngleConversion getAngleConversion() override;
 
-  bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud);
+  bool isPointCloudValid(sensor_msgs::msg::PointCloud2 & pointcloud);
 };
 
 class DistortionCorrector2D : public DistortionCorrector<DistortionCorrector2D>
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 a7df01767d780..0dd781b4730ec 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
@@ -56,6 +56,10 @@ class DistortionCorrectorComponent : public rclcpp::Node
 
   bool can_update_azimuth_and_distance_{false};
 
+  std::optional<AngleConversion> angle_conversion_opt_;
+  int angle_conversion_failure_num_{0};
+  int failure_tolerance_{10};
+
   std::unique_ptr<DistortionCorrectorBase> distortion_corrector_;
 
   void onPointCloud(PointCloud2::UniquePtr points_msg);
diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index 16d7599ff8e56..2caa2b0cd12fd 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -151,12 +151,11 @@ void DistortionCorrector<T>::getTwistAndIMUIterator(
 }
 
 template <class T>
-bool DistortionCorrector<T>::isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud)
+bool DistortionCorrector<T>::isPointCloudValid(sensor_msgs::msg::PointCloud2 & pointcloud)
 {
-  if (pointcloud.data.empty() || twist_queue_.empty()) {
+  if (pointcloud.data.empty()) {
     RCLCPP_WARN_STREAM_THROTTLE(
-      node_->get_logger(), *node_->get_clock(), 10000 /* ms */,
-      "Input pointcloud or twist_queue_ is empty.");
+      node_->get_logger(), *node_->get_clock(), 10000 /* ms */, "Input pointcloud is empty.");
     return false;
   }
 
@@ -189,9 +188,15 @@ bool DistortionCorrector<T>::isInputValid(sensor_msgs::msg::PointCloud2 & pointc
 }
 
 template <class T>
-bool DistortionCorrector<T>::azimuthConversionExists(sensor_msgs::msg::PointCloud2 & pointcloud)
+std::optional<AngleConversion> DistortionCorrector<T>::tryComputeAngleConversion(
+  sensor_msgs::msg::PointCloud2 & pointcloud)
 {
-  if (!isInputValid(pointcloud)) return false;
+  // This function try to compute the angle conversion from Cartesian coordinates to LiDAR azimuth
+  // coordinates system
+
+  if (!isPointCloudValid(pointcloud)) return std::nullopt;
+
+  AngleConversion angle_conversion;
 
   sensor_msgs::PointCloud2Iterator<float> it_x(pointcloud, "x");
   sensor_msgs::PointCloud2Iterator<float> it_y(pointcloud, "y");
@@ -209,7 +214,7 @@ bool DistortionCorrector<T>::azimuthConversionExists(sensor_msgs::msg::PointClou
     RCLCPP_WARN(
       node_->get_logger(),
       "Current point cloud only has a single point. Could not calculate the angle conversion.");
-    return false;
+    return std::nullopt;
   }
 
   for (; next_it_x != it_x.end();
@@ -234,10 +239,10 @@ bool DistortionCorrector<T>::azimuthConversionExists(sensor_msgs::msg::PointClou
     float offset_rad = *it_azimuth - sign * current_cartesian_rad;
 
     // Check if 'sign' can be adjusted to 1 or -1
-    if (std::abs(sign - 1.0f) <= angle_conversion_.sign_threshold_) {
-      angle_conversion_.sign_ = 1.0f;
-    } else if (std::abs(sign + 1.0f) <= angle_conversion_.sign_threshold_) {
-      angle_conversion_.sign_ = -1.0f;
+    if (std::abs(sign - 1.0f) <= angle_conversion.sign_threshold) {
+      angle_conversion.sign = 1.0f;
+    } else if (std::abs(sign + 1.0f) <= angle_conversion.sign_threshold) {
+      angle_conversion.sign = -1.0f;
     } else {
       RCLCPP_DEBUG(
         node_->get_logger(), "Value of sign is not close to 1 or -1. Iterate to next point ...");
@@ -248,7 +253,7 @@ bool DistortionCorrector<T>::azimuthConversionExists(sensor_msgs::msg::PointClou
     int multiple_of_90_degrees = std::round(offset_rad / (autoware::universe_utils::pi / 2));
     if (
       std::abs(offset_rad - multiple_of_90_degrees * (autoware::universe_utils::pi / 2)) >
-      angle_conversion_.offset_rad_threshold_) {
+      angle_conversion.offset_rad_threshold) {
       RCLCPP_DEBUG(
         node_->get_logger(),
         "Value of offset_rad is not close to 1 or -1. Iterate to next point ...");
@@ -262,24 +267,24 @@ bool DistortionCorrector<T>::azimuthConversionExists(sensor_msgs::msg::PointClou
       multiple_of_90_degrees -= 4;
     }
 
-    angle_conversion_.offset_rad_ = multiple_of_90_degrees * (autoware::universe_utils::pi / 2);
+    angle_conversion.offset_rad = multiple_of_90_degrees * (autoware::universe_utils::pi / 2);
 
-    return true;
+    return angle_conversion;
   }
-  return false;
-}
-
-template <class T>
-AngleConversion DistortionCorrector<T>::getAngleConversion()
-{
-  return angle_conversion_;
+  return std::nullopt;
 }
 
 template <class T>
 void DistortionCorrector<T>::undistortPointCloud(
-  bool use_imu, bool can_update_azimuth_and_distance, sensor_msgs::msg::PointCloud2 & pointcloud)
+  bool use_imu, std::optional<AngleConversion> angle_conversion_opt,
+  sensor_msgs::msg::PointCloud2 & pointcloud)
 {
-  if (!isInputValid(pointcloud)) return;
+  if (!isPointCloudValid(pointcloud)) return;
+  if (twist_queue_.empty()) {
+    RCLCPP_WARN_STREAM_THROTTLE(
+      node_->get_logger(), *node_->get_clock(), 10000 /* ms */, "Twist queue is empty.");
+    return;
+  }
 
   sensor_msgs::PointCloud2Iterator<float> it_x(pointcloud, "x");
   sensor_msgs::PointCloud2Iterator<float> it_y(pointcloud, "y");
@@ -345,11 +350,11 @@ void DistortionCorrector<T>::undistortPointCloud(
     // Undistort a single point based on the strategy
     undistortPoint(it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid);
 
-    if (can_update_azimuth_and_distance && pointcloudTransformNeeded()) {
+    if (angle_conversion_opt.has_value() && pointcloudTransformNeeded()) {
       float cartesian_coordinate_azimuth =
         autoware::universe_utils::opencv_fast_atan2(*it_y, *it_x);
-      float updated_azimuth =
-        angle_conversion_.offset_rad_ + angle_conversion_.sign_ * cartesian_coordinate_azimuth;
+      float updated_azimuth = angle_conversion_opt->offset_rad +
+                              angle_conversion_opt->sign * cartesian_coordinate_azimuth;
       if (updated_azimuth < 0) {
         updated_azimuth += autoware::universe_utils::pi * 2;
       } else if (updated_azimuth > 2 * autoware::universe_utils::pi) {
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 1160b1b4e3d6e..f97622d2d6899 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
@@ -96,25 +96,45 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointclou
   distortion_corrector_->setPointCloudTransform(base_frame_, pointcloud_msg->header.frame_id);
   distortion_corrector_->initialize();
 
-  if (update_azimuth_and_distance_ && !can_update_azimuth_and_distance_) {
-    can_update_azimuth_and_distance_ =
-      distortion_corrector_->azimuthConversionExists(*pointcloud_msg);
-    if (can_update_azimuth_and_distance_) {
+  if (update_azimuth_and_distance_) {
+    angle_conversion_opt_ = distortion_corrector_->tryComputeAngleConversion(*pointcloud_msg);
+    if (angle_conversion_opt_.has_value()) {
       RCLCPP_INFO(
         this->get_logger(),
         "Success to get the conversion formula between cartesian coordinates and LiDAR azimuth "
         "coordinates");
     } else {
-      RCLCPP_ERROR(
+      RCLCPP_INFO(
         this->get_logger(),
-        "Failed to get the conversion formula between cartesian coordinates and LiDAR azimuth "
-        "coordinates. Please check the LiDAR azimuth coordinate. This pointcloud will not update "
-        "azimuth and distance");
+        "Failed to get the angle conversion between Cartesian coordinates and LiDAR azimuth "
+        "coordinates. This pointcloud will not update azimuth and distance");
+      angle_conversion_failure_num_++;
+
+      if (angle_conversion_failure_num_ > failure_tolerance_) {
+        throw std::runtime_error(
+          "Angle conversion failed more than " + std::to_string(failure_tolerance_) +
+          " times. The node has been interrupted. Please check the LiDAR azimuth coordinates.");
+      }
     }
   }
-
-  distortion_corrector_->undistortPointCloud(
-    use_imu_, can_update_azimuth_and_distance_, *pointcloud_msg);
+  // if (update_azimuth_and_distance_ && !can_update_azimuth_and_distance_) {
+  //   can_update_azimuth_and_distance_ =
+  //     distortion_corrector_->azimuthConversionExists(*pointcloud_msg);
+  //   if (can_update_azimuth_and_distance_) {
+  //     RCLCPP_INFO(
+  //       this->get_logger(),
+  //       "Success to get the conversion formula between cartesian coordinates and LiDAR azimuth "
+  //       "coordinates");
+  //   } else {
+  //     RCLCPP_ERROR(
+  //       this->get_logger(),
+  //       "Failed to get the conversion formula between cartesian coordinates and LiDAR azimuth "
+  //       "coordinates. Please check the LiDAR azimuth coordinate. This pointcloud will not update
+  //       " "azimuth and distance");
+  //   }
+  // }
+
+  distortion_corrector_->undistortPointCloud(use_imu_, angle_conversion_opt_, *pointcloud_msg);
 
   if (debug_publisher_) {
     auto pipeline_latency_ms =
diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index 282a35549e47b..77779168545e1 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -382,29 +382,19 @@ TEST_F(DistortionCorrectorTest, TestProcessIMUMessage)
     standard_tolerance_);
 }
 
-TEST_F(DistortionCorrectorTest, TestIsInputValid)
+TEST_F(DistortionCorrectorTest, TestIsPointCloudValid)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
 
-  // input normal pointcloud without twist
-  sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
-  bool result = distortion_corrector_2d_->isInputValid(pointcloud);
-  EXPECT_FALSE(result);
-
-  // input normal pointcloud with valid twist
-  auto twist_msg = generateTwistMsg(twist_linear_x_, twist_angular_z_, timestamp);
-  distortion_corrector_2d_->processTwistMessage(twist_msg);
-
-  pointcloud =
+  auto pointcloud =
     generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
-  result = distortion_corrector_2d_->isInputValid(pointcloud);
+  auto result = distortion_corrector_2d_->isPointCloudValid(pointcloud);
   EXPECT_TRUE(result);
 
   // input empty pointcloud
   pointcloud =
     generatePointCloudMsg(false, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
-  result = distortion_corrector_2d_->isInputValid(pointcloud);
+  result = distortion_corrector_2d_->isPointCloudValid(pointcloud);
   EXPECT_FALSE(result);
 }
 
@@ -438,7 +428,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyTwist)
 
   // Process empty twist queue
   distortion_corrector_2d_->initialize();
-  distortion_corrector_2d_->undistortPointCloud(false, false, pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(false, std::nullopt, pointcloud);
 
   // Verify the point cloud is not changed
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
@@ -478,7 +468,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyPointCloud)
 
   // Process empty point cloud
   distortion_corrector_2d_->initialize();
-  distortion_corrector_2d_->undistortPointCloud(true, false, empty_pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(true, std::nullopt, empty_pointcloud);
 
   // Verify the point cloud is still empty
   EXPECT_EQ(empty_pointcloud.width, 0);
@@ -498,7 +488,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink)
   // Test using only twist
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_2d_->undistortPointCloud(false, false, pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(false, std::nullopt, pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
@@ -544,7 +534,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink)
 
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_2d_->undistortPointCloud(true, false, pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(true, std::nullopt, pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
@@ -590,7 +580,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame)
 
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
-  distortion_corrector_2d_->undistortPointCloud(true, false, pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(true, std::nullopt, pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
@@ -637,7 +627,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink)
   // Test using only twist
   distortion_corrector_3d_->initialize();
   distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_3d_->undistortPointCloud(false, false, pointcloud);
+  distortion_corrector_3d_->undistortPointCloud(false, std::nullopt, pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
@@ -683,7 +673,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink)
 
   distortion_corrector_3d_->initialize();
   distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_3d_->undistortPointCloud(true, false, pointcloud);
+  distortion_corrector_3d_->undistortPointCloud(true, std::nullopt, pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
@@ -732,7 +722,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame)
 
   distortion_corrector_3d_->initialize();
   distortion_corrector_3d_->setPointCloudTransform("base_link", "lidar_top");
-  distortion_corrector_3d_->undistortPointCloud(true, false, pointcloud);
+  distortion_corrector_3d_->undistortPointCloud(true, std::nullopt, pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
@@ -779,12 +769,12 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion)
   distortion_corrector_2d_->processTwistMessage(twist_msg);
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_2d_->undistortPointCloud(false, false, test2d_pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(false, std::nullopt, test2d_pointcloud);
 
   distortion_corrector_3d_->processTwistMessage(twist_msg);
   distortion_corrector_3d_->initialize();
   distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_3d_->undistortPointCloud(false, false, test3d_pointcloud);
+  distortion_corrector_3d_->undistortPointCloud(false, std::nullopt, test3d_pointcloud);
 
   // Generate expected point cloud for testing
   sensor_msgs::msg::PointCloud2 expected_pointcloud_msg =
@@ -868,12 +858,12 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion)
   distortion_corrector_2d_->processTwistMessage(twist_msg);
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_2d_->undistortPointCloud(false, false, test2d_pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(false, std::nullopt, test2d_pointcloud);
 
   distortion_corrector_3d_->processTwistMessage(twist_msg);
   distortion_corrector_3d_->initialize();
   distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_3d_->undistortPointCloud(false, false, test3d_pointcloud);
+  distortion_corrector_3d_->undistortPointCloud(false, std::nullopt, test3d_pointcloud);
 
   // Generate expected point cloud for testing
   sensor_msgs::msg::PointCloud2 expected_pointcloud_msg =
@@ -959,7 +949,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion)
 TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdatingAzimuthAndDistance)
 {
   // Test the case when the cloud will not update the azimuth and distance values
-  // 1. when pointcloud is in base_link
+  // 1. when pointcloud is in base_link (pointcloudTransformNeeded() is false)
 
   // Generate the point cloud message in base_link
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -974,11 +964,10 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdatingAzimuthAndDist
 
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
-  auto can_update_azimuth_and_distance =
-    distortion_corrector_2d_->azimuthConversionExists(pointcloud_base_link);
+  auto angle_conversion_opt =
+    distortion_corrector_2d_->tryComputeAngleConversion(pointcloud_base_link);
 
-  distortion_corrector_2d_->undistortPointCloud(
-    true, can_update_azimuth_and_distance, pointcloud_base_link);
+  distortion_corrector_2d_->undistortPointCloud(true, angle_conversion_opt, pointcloud_base_link);
 
   sensor_msgs::msg::PointCloud2 original_pointcloud_base_link =
     generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
@@ -1029,9 +1018,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdatingAzimuthAndDist
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
   // set the empty coordinate system
-  can_update_azimuth_and_distance = false;
-  distortion_corrector_2d_->undistortPointCloud(
-    true, can_update_azimuth_and_distance, pointcloud_lidar_top);
+  angle_conversion_opt = distortion_corrector_2d_->tryComputeAngleConversion(pointcloud_base_link);
+  distortion_corrector_2d_->undistortPointCloud(true, angle_conversion_opt, pointcloud_lidar_top);
 
   sensor_msgs::msg::PointCloud2 original_pointcloud_lidar_top =
     generatePointCloudMsg(true, true, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
@@ -1083,10 +1071,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
 
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
-  // set the azimuth coordinate system
-  auto can_update_azimuth_and_distance =
-    distortion_corrector_2d_->azimuthConversionExists(pointcloud);
-  distortion_corrector_2d_->undistortPointCloud(true, can_update_azimuth_and_distance, pointcloud);
+
+  auto angle_conversion_opt = distortion_corrector_2d_->tryComputeAngleConversion(pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(true, angle_conversion_opt, pointcloud);
 
   sensor_msgs::msg::PointCloud2 original_pointcloud =
     generatePointCloudMsg(true, true, AngleCoordinateSystem::VELODYNE, timestamp, true, {}, {});
@@ -1142,10 +1129,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
 
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
-  // set the azimuth coordinate system
-  auto can_update_azimuth_and_distance =
-    distortion_corrector_2d_->azimuthConversionExists(pointcloud);
-  distortion_corrector_2d_->undistortPointCloud(true, can_update_azimuth_and_distance, pointcloud);
+
+  auto angle_conversion_opt = distortion_corrector_2d_->tryComputeAngleConversion(pointcloud);
+  distortion_corrector_2d_->undistortPointCloud(true, angle_conversion_opt, pointcloud);
 
   sensor_msgs::msg::PointCloud2 original_pointcloud =
     generatePointCloudMsg(true, true, AngleCoordinateSystem::HESAI, timestamp, true, {}, {});
@@ -1194,7 +1180,8 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsEmptyPointcloud)
 
   sensor_msgs::msg::PointCloud2 empty_pointcloud =
     generatePointCloudMsg(false, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
-  EXPECT_FALSE(distortion_corrector_2d_->azimuthConversionExists(empty_pointcloud));
+  auto angle_conversion_opt = distortion_corrector_2d_->tryComputeAngleConversion(empty_pointcloud);
+  EXPECT_FALSE(angle_conversion_opt.has_value());
 }
 
 TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsVelodynePointcloud)
@@ -1214,11 +1201,14 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsVelodynePointcloud)
   sensor_msgs::msg::PointCloud2 velodyne_pointcloud = generatePointCloudMsg(
     true, true, AngleCoordinateSystem::CARTESIAN, timestamp, false, velodyne_points,
     velodyne_azimuths);
-  EXPECT_TRUE(distortion_corrector_2d_->azimuthConversionExists(velodyne_pointcloud));
 
-  auto angle_conversion = distortion_corrector_2d_->getAngleConversion();
-  EXPECT_EQ(angle_conversion.sign_, -1);
-  EXPECT_NEAR(angle_conversion.offset_rad_, autoware::universe_utils::pi * 2, standard_tolerance_);
+  auto angle_conversion_opt =
+    distortion_corrector_2d_->tryComputeAngleConversion(velodyne_pointcloud);
+  EXPECT_TRUE(angle_conversion_opt.has_value());
+
+  EXPECT_EQ(angle_conversion_opt->sign, -1);
+  EXPECT_NEAR(
+    angle_conversion_opt->offset_rad, autoware::universe_utils::pi * 2, standard_tolerance_);
 }
 
 TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsHesaiPointcloud)
@@ -1238,11 +1228,13 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsHesaiPointcloud)
     autoware::universe_utils::pi};
   sensor_msgs::msg::PointCloud2 hesai_pointcloud = generatePointCloudMsg(
     true, true, AngleCoordinateSystem::CARTESIAN, timestamp, false, hesai_points, hesai_azimuths);
-  EXPECT_TRUE(distortion_corrector_2d_->azimuthConversionExists(hesai_pointcloud));
 
-  auto angle_conversion = distortion_corrector_2d_->getAngleConversion();
-  EXPECT_EQ(angle_conversion.sign_, -1);
-  EXPECT_NEAR(angle_conversion.offset_rad_, autoware::universe_utils::pi / 2, standard_tolerance_);
+  auto angle_conversion_opt = distortion_corrector_2d_->tryComputeAngleConversion(hesai_pointcloud);
+
+  EXPECT_TRUE(angle_conversion_opt.has_value());
+  EXPECT_EQ(angle_conversion_opt->sign, -1);
+  EXPECT_NEAR(
+    angle_conversion_opt->offset_rad, autoware::universe_utils::pi / 2, standard_tolerance_);
 }
 
 TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsCartesianPointcloud)
@@ -1262,11 +1254,13 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsCartesianPointcloud)
   sensor_msgs::msg::PointCloud2 cartesian_pointcloud = generatePointCloudMsg(
     true, true, AngleCoordinateSystem::CARTESIAN, timestamp, false, cartesian_points,
     cartesian_azimuths);
-  EXPECT_TRUE(distortion_corrector_2d_->azimuthConversionExists(cartesian_pointcloud));
 
-  auto angle_conversion = distortion_corrector_2d_->getAngleConversion();
-  EXPECT_EQ(angle_conversion.sign_, 1);
-  EXPECT_NEAR(angle_conversion.offset_rad_, 0, standard_tolerance_);
+  auto angle_conversion_opt =
+    distortion_corrector_2d_->tryComputeAngleConversion(cartesian_pointcloud);
+
+  EXPECT_TRUE(angle_conversion_opt.has_value());
+  EXPECT_EQ(angle_conversion_opt->sign, 1);
+  EXPECT_NEAR(angle_conversion_opt->offset_rad, 0, standard_tolerance_);
 }
 
 TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsRandomPointcloud1)
@@ -1285,12 +1279,13 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsRandomPointcloud1)
     0, autoware::universe_utils::pi * 3 / 2, autoware::universe_utils::pi * 7 / 4};
   sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(
     true, true, AngleCoordinateSystem::CARTESIAN, timestamp, false, points, azimuths);
-  EXPECT_TRUE(distortion_corrector_2d_->azimuthConversionExists(pointcloud));
 
-  auto angle_conversion = distortion_corrector_2d_->getAngleConversion();
-  EXPECT_EQ(angle_conversion.sign_, 1);
+  auto angle_conversion_opt = distortion_corrector_2d_->tryComputeAngleConversion(pointcloud);
+
+  EXPECT_TRUE(angle_conversion_opt.has_value());
+  EXPECT_EQ(angle_conversion_opt->sign, 1);
   EXPECT_NEAR(
-    angle_conversion.offset_rad_, autoware::universe_utils::pi * 3 / 2, standard_tolerance_);
+    angle_conversion_opt->offset_rad, autoware::universe_utils::pi * 3 / 2, standard_tolerance_);
 }
 
 int main(int argc, char ** argv)

From f6c5dab4d14de4a9e7a78f9d0f2aab59159431b0 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Wed, 11 Sep 2024 16:47:43 +0900
Subject: [PATCH 37/72] chore: fix bug and clean code

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector.hpp                   |  2 +-
 .../distortion_corrector_node.cpp              | 18 +-----------------
 2 files changed, 2 insertions(+), 18 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
index 3bb0637875b75..bc3c475a4aaad 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -52,7 +52,7 @@ struct AngleConversion
 {
   // Equation for the conversion between sensor azimuth coordinates and Cartesian coordinates:
   // sensor azimuth coordinates = offset_rad + sign * cartesian coordinates;
-  // offset_rad is restricted to be a multiple of 90, and sign is restricted to be 1 or -1.
+  // offset_rad i 0, and sign is restricted to be 1 or -1.
   float offset_rad{0};
   float sign{1};
   float offset_rad_threshold{0.087f};  // (5 / 180) * M_PI
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 f97622d2d6899..4ee1f630b2db6 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
@@ -96,7 +96,7 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointclou
   distortion_corrector_->setPointCloudTransform(base_frame_, pointcloud_msg->header.frame_id);
   distortion_corrector_->initialize();
 
-  if (update_azimuth_and_distance_) {
+  if (update_azimuth_and_distance_ && !angle_conversion_opt_.has_value()) {
     angle_conversion_opt_ = distortion_corrector_->tryComputeAngleConversion(*pointcloud_msg);
     if (angle_conversion_opt_.has_value()) {
       RCLCPP_INFO(
@@ -117,22 +117,6 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointclou
       }
     }
   }
-  // if (update_azimuth_and_distance_ && !can_update_azimuth_and_distance_) {
-  //   can_update_azimuth_and_distance_ =
-  //     distortion_corrector_->azimuthConversionExists(*pointcloud_msg);
-  //   if (can_update_azimuth_and_distance_) {
-  //     RCLCPP_INFO(
-  //       this->get_logger(),
-  //       "Success to get the conversion formula between cartesian coordinates and LiDAR azimuth "
-  //       "coordinates");
-  //   } else {
-  //     RCLCPP_ERROR(
-  //       this->get_logger(),
-  //       "Failed to get the conversion formula between cartesian coordinates and LiDAR azimuth "
-  //       "coordinates. Please check the LiDAR azimuth coordinate. This pointcloud will not update
-  //       " "azimuth and distance");
-  //   }
-  // }
 
   distortion_corrector_->undistortPointCloud(use_imu_, angle_conversion_opt_, *pointcloud_msg);
 

From 4342dca4d2e52c1147d3681cabd9ae4df6959c99 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Wed, 11 Sep 2024 18:24:12 +0900
Subject: [PATCH 38/72] chore: refactor the logic of calculating conversion

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector.cpp                  | 25 +++++++-----
 .../test/test_distortion_corrector_node.cpp   | 38 +++++++++++++++----
 2 files changed, 47 insertions(+), 16 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index 2caa2b0cd12fd..bf5afe09e849a 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -225,18 +225,23 @@ std::optional<AngleConversion> DistortionCorrector<T>::tryComputeAngleConversion
     // If the angle exceeds 180 degrees, it may cross the 0-degree axis,
     // which could disrupt the calculation of the formula.
     if (
-      abs(*next_it_azimuth - *it_azimuth) >= autoware::universe_utils::pi ||
-      abs(next_cartesian_rad - current_cartesian_rad) >= autoware::universe_utils::pi ||
       abs(*next_it_azimuth - *it_azimuth) == 0 ||
       abs(next_cartesian_rad - current_cartesian_rad) == 0) {
       RCLCPP_DEBUG(
-        node_->get_logger(),
-        "Angle between two points exceeds 180 degrees. Iterate to next point ...");
+        node_->get_logger(), "Angle between two points is 0 degrees. Iterate to next point ...");
       continue;
     }
 
-    float sign = (*next_it_azimuth - *it_azimuth) / (next_cartesian_rad - current_cartesian_rad);
-    float offset_rad = *it_azimuth - sign * current_cartesian_rad;
+    // restrict the angle difference between [-180, 180] (degrees)
+    float azimuth_diff = abs(*next_it_azimuth - *it_azimuth) > autoware::universe_utils::pi
+                           ? abs(*next_it_azimuth - *it_azimuth) - 2 * autoware::universe_utils::pi
+                           : *next_it_azimuth - *it_azimuth;
+    float cartesian_rad_diff =
+      abs(next_cartesian_rad - current_cartesian_rad) > autoware::universe_utils::pi
+        ? abs(next_cartesian_rad - current_cartesian_rad) - 2 * autoware::universe_utils::pi
+        : next_cartesian_rad - current_cartesian_rad;
+
+    float sign = azimuth_diff / cartesian_rad_diff;
 
     // Check if 'sign' can be adjusted to 1 or -1
     if (std::abs(sign - 1.0f) <= angle_conversion.sign_threshold) {
@@ -249,6 +254,7 @@ std::optional<AngleConversion> DistortionCorrector<T>::tryComputeAngleConversion
       continue;
     }
 
+    float offset_rad = *it_azimuth - sign * current_cartesian_rad;
     // Check if 'offset_rad' can be adjusted to offset_rad multiple of π/2
     int multiple_of_90_degrees = std::round(offset_rad / (autoware::universe_utils::pi / 2));
     if (
@@ -256,14 +262,15 @@ std::optional<AngleConversion> DistortionCorrector<T>::tryComputeAngleConversion
       angle_conversion.offset_rad_threshold) {
       RCLCPP_DEBUG(
         node_->get_logger(),
-        "Value of offset_rad is not close to 1 or -1. Iterate to next point ...");
+        "Value of offset_rad is not close to mutiplication of 90 degrees. Iterate to next point "
+        "...");
       continue;
     }
 
-    // Limit the range of offset_rad in [0, 360]
+    // Limit the range of offset_rad in [0, 360)
     if (multiple_of_90_degrees < 0) {
       multiple_of_90_degrees += 4;
-    } else if (multiple_of_90_degrees > 4) {
+    } else if (multiple_of_90_degrees >= 4) {
       multiple_of_90_degrees -= 4;
     }
 
diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index 77779168545e1..6b660d612063f 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -1171,7 +1171,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   }
 }
 
-TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsEmptyPointcloud)
+TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnEmptyPointcloud)
 {
   // test empty pointcloud
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1184,7 +1184,7 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsEmptyPointcloud)
   EXPECT_FALSE(angle_conversion_opt.has_value());
 }
 
-TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsVelodynePointcloud)
+TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnVelodynePointcloud)
 {
   // test velodyne pointcloud (x-axis: 0 degree, y-axis: 270 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1207,11 +1207,10 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsVelodynePointcloud)
   EXPECT_TRUE(angle_conversion_opt.has_value());
 
   EXPECT_EQ(angle_conversion_opt->sign, -1);
-  EXPECT_NEAR(
-    angle_conversion_opt->offset_rad, autoware::universe_utils::pi * 2, standard_tolerance_);
+  EXPECT_NEAR(angle_conversion_opt->offset_rad, 0, standard_tolerance_);
 }
 
-TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsHesaiPointcloud)
+TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnHesaiPointcloud)
 {
   // test hesai pointcloud (x-axis: 90 degree, y-axis: 0 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1237,7 +1236,7 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsHesaiPointcloud)
     angle_conversion_opt->offset_rad, autoware::universe_utils::pi / 2, standard_tolerance_);
 }
 
-TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsCartesianPointcloud)
+TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionCartesianPointcloud)
 {
   // test pointcloud that use cartesian coordinate for azimuth (x-axis: 0 degree, y-axis: 90 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1263,7 +1262,7 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsCartesianPointcloud)
   EXPECT_NEAR(angle_conversion_opt->offset_rad, 0, standard_tolerance_);
 }
 
-TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsRandomPointcloud1)
+TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnRandomPointcloud)
 {
   // test pointcloud that use coordinate (x-axis: 270 degree, y-axis: 0 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1288,6 +1287,31 @@ TEST_F(DistortionCorrectorTest, TestAzimuthConversionExistsRandomPointcloud1)
     angle_conversion_opt->offset_rad, autoware::universe_utils::pi * 3 / 2, standard_tolerance_);
 }
 
+TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnBadAzimuthPointcloud)
+{
+  // test pointcloud that has bad zimuth
+  // 1. angle difference is 0
+  // 2. azimuth value is wrong
+  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  // Generate and process multiple twist messages
+  generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
+
+  std::vector<Eigen::Vector3f> points = {
+    Eigen::Vector3f(0.0f, 1.0f, 0.0f),
+    Eigen::Vector3f(2.0f, 0.0f, 1.0f),
+    Eigen::Vector3f(1.0f, 1.0f, 1.0f),
+  };
+
+  // generate random bad azimuths
+  std::vector<float> azimuths = {0, 0, autoware::universe_utils::pi};
+  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(
+    true, true, AngleCoordinateSystem::CARTESIAN, timestamp, false, points, azimuths);
+
+  auto angle_conversion_opt = distortion_corrector_2d_->tryComputeAngleConversion(pointcloud);
+
+  EXPECT_FALSE(angle_conversion_opt.has_value());
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

From 242ab45094d82b2cd6003c8f4d63a2a044bb1250 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Thu, 12 Sep 2024 14:50:29 +0900
Subject: [PATCH 39/72] chore: refactor function in unit test

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../test/test_distortion_corrector_node.cpp   | 288 +++++++++---------
 1 file changed, 144 insertions(+), 144 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index 6b660d612063f..e9d76a78ea941 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -171,7 +171,7 @@ class DistortionCorrectorTest : public ::testing::Test
     return imu_msgs;
   }
 
-  std::tuple<std::vector<Eigen::Vector3f>, std::vector<float>> generate_default_pointcloud(
+  std::tuple<std::vector<Eigen::Vector3f>, std::vector<float>> generateDefaultPointcloud(
     AngleCoordinateSystem vendor)
   {
     // Generate all combinations of signs { -, 0, + } x { -, 0, + } for x and y.
@@ -218,10 +218,23 @@ class DistortionCorrectorTest : public ::testing::Test
     return std::make_tuple(default_points, default_azimuths);
   }
 
+  sensor_msgs::msg::PointCloud2 generateEmptyPointCloudMsg(rclcpp::Time stamp)
+  {
+    sensor_msgs::msg::PointCloud2 empty_pointcloud_msg;
+    empty_pointcloud_msg.header.stamp = stamp;
+    empty_pointcloud_msg.header.frame_id = "lidar_top";
+    empty_pointcloud_msg.height = 1;
+    empty_pointcloud_msg.is_dense = true;
+    empty_pointcloud_msg.is_bigendian = false;
+    empty_pointcloud_msg.width = 0;
+    empty_pointcloud_msg.row_step = 0;
+
+    return empty_pointcloud_msg;
+  }
+
   sensor_msgs::msg::PointCloud2 generatePointCloudMsg(
-    bool generate_points, bool is_lidar_frame, AngleCoordinateSystem vendor, rclcpp::Time stamp,
-    bool use_default_pointcloud, std::vector<Eigen::Vector3f> defined_points,
-    std::vector<float> defined_azimuths)
+    bool is_lidar_frame, rclcpp::Time stamp, std::vector<Eigen::Vector3f> points,
+    std::vector<float> azimuths)
   {
     sensor_msgs::msg::PointCloud2 pointcloud_msg;
     pointcloud_msg.header.stamp = stamp;
@@ -229,60 +242,44 @@ class DistortionCorrectorTest : public ::testing::Test
     pointcloud_msg.height = 1;
     pointcloud_msg.is_dense = true;
     pointcloud_msg.is_bigendian = false;
-
-    if (generate_points) {
-      std::vector<Eigen::Vector3f> points;
-      std::vector<float> azimuths;
-
-      if (use_default_pointcloud) {
-        auto [default_points, default_azimuths] = generate_default_pointcloud(vendor);
-        points = default_points;
-        azimuths = default_azimuths;
-      } else {
-        points = defined_points;
-        azimuths = defined_azimuths;
-      }
-
-      // Generate timestamps for the points
-      std::vector<std::uint32_t> timestamps = generatePointTimestamps(stamp, points.size());
-
-      sensor_msgs::PointCloud2Modifier modifier(pointcloud_msg);
-      modifier.setPointCloud2Fields(
-        10, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1,
-        sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32,
-        "intensity", 1, sensor_msgs::msg::PointField::UINT8, "return_type", 1,
-        sensor_msgs::msg::PointField::UINT8, "channel", 1, sensor_msgs::msg::PointField::UINT16,
-        "azimuth", 1, sensor_msgs::msg::PointField::FLOAT32, "elevation", 1,
-        sensor_msgs::msg::PointField::FLOAT32, "distance", 1, sensor_msgs::msg::PointField::FLOAT32,
-        "time_stamp", 1, sensor_msgs::msg::PointField::UINT32);
-
-      modifier.resize(points.size());
-
-      sensor_msgs::PointCloud2Iterator<float> iter_x(pointcloud_msg, "x");
-      sensor_msgs::PointCloud2Iterator<float> iter_y(pointcloud_msg, "y");
-      sensor_msgs::PointCloud2Iterator<float> iter_z(pointcloud_msg, "z");
-      sensor_msgs::PointCloud2Iterator<float> iter_azimuth(pointcloud_msg, "azimuth");
-      sensor_msgs::PointCloud2Iterator<float> iter_distance(pointcloud_msg, "distance");
-      sensor_msgs::PointCloud2Iterator<std::uint32_t> iter_t(pointcloud_msg, "time_stamp");
-
-      for (size_t i = 0; i < points.size(); ++i) {
-        *iter_x = points[i].x();
-        *iter_y = points[i].y();
-        *iter_z = points[i].z();
-
-        *iter_azimuth = azimuths[i];
-        *iter_distance = points[i].norm();
-        *iter_t = timestamps[i];
-        ++iter_x;
-        ++iter_y;
-        ++iter_z;
-        ++iter_azimuth;
-        ++iter_distance;
-        ++iter_t;
-      }
-    } else {
-      pointcloud_msg.width = 0;
-      pointcloud_msg.row_step = 0;
+    // auto [default_points, default_azimuths] = generateDefaultPointcloud(vendor);
+
+    // Generate timestamps for the points
+    std::vector<std::uint32_t> timestamps = generatePointTimestamps(stamp, points.size());
+
+    sensor_msgs::PointCloud2Modifier modifier(pointcloud_msg);
+    modifier.setPointCloud2Fields(
+      10, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1,
+      sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32,
+      "intensity", 1, sensor_msgs::msg::PointField::UINT8, "return_type", 1,
+      sensor_msgs::msg::PointField::UINT8, "channel", 1, sensor_msgs::msg::PointField::UINT16,
+      "azimuth", 1, sensor_msgs::msg::PointField::FLOAT32, "elevation", 1,
+      sensor_msgs::msg::PointField::FLOAT32, "distance", 1, sensor_msgs::msg::PointField::FLOAT32,
+      "time_stamp", 1, sensor_msgs::msg::PointField::UINT32);
+
+    modifier.resize(points.size());
+
+    sensor_msgs::PointCloud2Iterator<float> iter_x(pointcloud_msg, "x");
+    sensor_msgs::PointCloud2Iterator<float> iter_y(pointcloud_msg, "y");
+    sensor_msgs::PointCloud2Iterator<float> iter_z(pointcloud_msg, "z");
+    sensor_msgs::PointCloud2Iterator<float> iter_azimuth(pointcloud_msg, "azimuth");
+    sensor_msgs::PointCloud2Iterator<float> iter_distance(pointcloud_msg, "distance");
+    sensor_msgs::PointCloud2Iterator<std::uint32_t> iter_t(pointcloud_msg, "time_stamp");
+
+    for (size_t i = 0; i < points.size(); ++i) {
+      *iter_x = points[i].x();
+      *iter_y = points[i].y();
+      *iter_z = points[i].z();
+
+      *iter_azimuth = azimuths[i];
+      *iter_distance = points[i].norm();
+      *iter_t = timestamps[i];
+      ++iter_x;
+      ++iter_y;
+      ++iter_z;
+      ++iter_azimuth;
+      ++iter_distance;
+      ++iter_t;
     }
 
     return pointcloud_msg;
@@ -386,15 +383,15 @@ TEST_F(DistortionCorrectorTest, TestIsPointCloudValid)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
 
-  auto pointcloud =
-    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
+  auto [default_points, default_azimuths] =
+    generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
+  auto pointcloud = generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
   auto result = distortion_corrector_2d_->isPointCloudValid(pointcloud);
   EXPECT_TRUE(result);
 
   // input empty pointcloud
-  pointcloud =
-    generatePointCloudMsg(false, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
-  result = distortion_corrector_2d_->isPointCloudValid(pointcloud);
+  auto empty_pointcloud = generateEmptyPointCloudMsg(timestamp);
+  result = distortion_corrector_2d_->isPointCloudValid(empty_pointcloud);
   EXPECT_FALSE(result);
 }
 
@@ -423,8 +420,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyTwist)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   // Generate the point cloud message
-  sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
+  auto [default_points, default_azimuths] =
+    generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
+  auto pointcloud = generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
 
   // Process empty twist queue
   distortion_corrector_2d_->initialize();
@@ -463,9 +461,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyPointCloud)
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
   // Generate an empty point cloud message
-  sensor_msgs::msg::PointCloud2 empty_pointcloud =
-    generatePointCloudMsg(false, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
 
+  auto empty_pointcloud = generateEmptyPointCloudMsg(timestamp);
   // Process empty point cloud
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->undistortPointCloud(true, std::nullopt, empty_pointcloud);
@@ -479,8 +476,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
+  auto [default_points, default_azimuths] =
+    generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
+  auto pointcloud = generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
 
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
@@ -523,8 +521,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
+  auto [default_points, default_azimuths] =
+    generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
+  auto pointcloud = generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
 
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
@@ -569,8 +568,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, true, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
+  auto [default_points, default_azimuths] =
+    generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
+  auto pointcloud = generatePointCloudMsg(true, timestamp, default_points, default_azimuths);
 
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
@@ -618,8 +618,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
+  auto [default_points, default_azimuths] =
+    generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
+  auto pointcloud = generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
 
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_3d_, timestamp);
@@ -662,8 +663,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
+  auto [default_points, default_azimuths] =
+    generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
+  auto pointcloud = generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
 
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_3d_, timestamp);
@@ -711,8 +713,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, true, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
+  auto [default_points, default_azimuths] =
+    generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
+  auto pointcloud = generatePointCloudMsg(true, timestamp, default_points, default_azimuths);
 
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_3d_, timestamp);
@@ -758,10 +761,12 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame)
 TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 test2d_pointcloud =
-    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
-  sensor_msgs::msg::PointCloud2 test3d_pointcloud =
-    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
+  auto [default_points, default_azimuths] =
+    generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
+  auto test2d_pointcloud =
+    generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
+  auto test3d_pointcloud =
+    generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
 
   // Generate and process a single twist message with constant linear velocity
   auto twist_msg = generateTwistMsg(1.0, 0.0, timestamp);
@@ -777,15 +782,15 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion)
   distortion_corrector_3d_->undistortPointCloud(false, std::nullopt, test3d_pointcloud);
 
   // Generate expected point cloud for testing
-  sensor_msgs::msg::PointCloud2 expected_pointcloud_msg =
-    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
+  auto expected_pointcloud =
+    generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
 
   // Calculate expected point cloud values based on constant linear motion
   double velocity = 1.0;  // 1 m/s linear velocity
-  sensor_msgs::PointCloud2Iterator<float> iter_x(expected_pointcloud_msg, "x");
-  sensor_msgs::PointCloud2Iterator<float> iter_y(expected_pointcloud_msg, "y");
-  sensor_msgs::PointCloud2Iterator<float> iter_z(expected_pointcloud_msg, "z");
-  sensor_msgs::PointCloud2Iterator<std::uint32_t> iter_t(expected_pointcloud_msg, "time_stamp");
+  sensor_msgs::PointCloud2Iterator<float> iter_x(expected_pointcloud, "x");
+  sensor_msgs::PointCloud2Iterator<float> iter_y(expected_pointcloud, "y");
+  sensor_msgs::PointCloud2Iterator<float> iter_z(expected_pointcloud, "z");
+  sensor_msgs::PointCloud2Iterator<std::uint32_t> iter_t(expected_pointcloud, "time_stamp");
 
   std::vector<Eigen::Vector3f> expected_points;
   for (; iter_t != iter_t.end(); ++iter_t, ++iter_x, ++iter_y, ++iter_z) {
@@ -847,10 +852,12 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion)
 TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 test2d_pointcloud =
-    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
-  sensor_msgs::msg::PointCloud2 test3d_pointcloud =
-    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
+  auto [default_points, default_azimuths] =
+    generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
+  auto test2d_pointcloud =
+    generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
+  auto test3d_pointcloud =
+    generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
 
   // Generate and process a single twist message with constant angular velocity
   auto twist_msg = generateTwistMsg(0.0, 0.1, timestamp);
@@ -866,17 +873,17 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion)
   distortion_corrector_3d_->undistortPointCloud(false, std::nullopt, test3d_pointcloud);
 
   // Generate expected point cloud for testing
-  sensor_msgs::msg::PointCloud2 expected_pointcloud_msg =
-    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
+  auto expected_pointcloud =
+    generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
 
   // Calculate expected point cloud values based on constant rotational motion
   double angular_velocity = 0.1;  // 0.1 rad/s rotational velocity
-  sensor_msgs::PointCloud2Iterator<float> iter_x(expected_pointcloud_msg, "x");
-  sensor_msgs::PointCloud2Iterator<float> iter_y(expected_pointcloud_msg, "y");
-  sensor_msgs::PointCloud2Iterator<float> iter_z(expected_pointcloud_msg, "z");
-  sensor_msgs::PointCloud2Iterator<std::uint32_t> iter_t(expected_pointcloud_msg, "time_stamp");
+  sensor_msgs::PointCloud2Iterator<float> iter_x(expected_pointcloud, "x");
+  sensor_msgs::PointCloud2Iterator<float> iter_y(expected_pointcloud, "y");
+  sensor_msgs::PointCloud2Iterator<float> iter_z(expected_pointcloud, "z");
+  sensor_msgs::PointCloud2Iterator<std::uint32_t> iter_t(expected_pointcloud, "time_stamp");
 
-  std::vector<Eigen::Vector3f> expected_pointcloud;
+  std::vector<Eigen::Vector3f> expected_points;
   for (; iter_t != iter_t.end(); ++iter_t, ++iter_x, ++iter_y, ++iter_z) {
     double time_offset = static_cast<double>(*iter_t) / 1e9;
     float angle = angular_velocity * time_offset;
@@ -889,7 +896,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion)
 
     tf2::Vector3 point(*iter_x, *iter_y, *iter_z);
     tf2::Vector3 rotated_point = tf2::quatRotate(quaternion, point);
-    expected_pointcloud.emplace_back(
+    expected_points.emplace_back(
       static_cast<float>(rotated_point.x()), static_cast<float>(rotated_point.y()),
       static_cast<float>(rotated_point.z()));
   }
@@ -907,9 +914,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion)
        ++test2d_iter_x, ++test2d_iter_y, ++test2d_iter_z, ++i) {
     oss << "Point " << i << ": (" << *test2d_iter_x << ", " << *test2d_iter_y << ", "
         << *test2d_iter_z << ")\n";
-    EXPECT_FLOAT_EQ(*test2d_iter_x, expected_pointcloud[i].x());
-    EXPECT_FLOAT_EQ(*test2d_iter_y, expected_pointcloud[i].y());
-    EXPECT_FLOAT_EQ(*test2d_iter_z, expected_pointcloud[i].z());
+    EXPECT_FLOAT_EQ(*test2d_iter_x, expected_points[i].x());
+    EXPECT_FLOAT_EQ(*test2d_iter_y, expected_points[i].y());
+    EXPECT_FLOAT_EQ(*test2d_iter_z, expected_points[i].z());
   }
 
   if (debug_) {
@@ -953,8 +960,10 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdatingAzimuthAndDist
 
   // Generate the point cloud message in base_link
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud_base_link =
-    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
+  auto [default_points, default_azimuths] =
+    generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
+  auto pointcloud_base_link =
+    generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
 
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
@@ -969,8 +978,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdatingAzimuthAndDist
 
   distortion_corrector_2d_->undistortPointCloud(true, angle_conversion_opt, pointcloud_base_link);
 
-  sensor_msgs::msg::PointCloud2 original_pointcloud_base_link =
-    generatePointCloudMsg(true, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
+  auto original_pointcloud_base_link =
+    generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
 
   sensor_msgs::PointCloud2ConstIterator<float> test_iter_azimuth_base_link(
     pointcloud_base_link, "azimuth");
@@ -1003,11 +1012,12 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdatingAzimuthAndDist
   }
 
   // Test the case when the cloud will not update the azimuth and distance values
-  // 2. when "can_update_azimuth_and_distance" is false
+  // 2. when the return value of tryComputeAngleConversion is std::nullopt (couldn't find the angle
+  // conversion)
 
   // Generate the point cloud message in sensor frame
-  sensor_msgs::msg::PointCloud2 pointcloud_lidar_top =
-    generatePointCloudMsg(true, true, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
+  auto pointcloud_lidar_top =
+    generatePointCloudMsg(true, timestamp, default_points, default_azimuths);
 
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
@@ -1017,12 +1027,12 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdatingAzimuthAndDist
 
   distortion_corrector_2d_->initialize();
   distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
-  // set the empty coordinate system
-  angle_conversion_opt = distortion_corrector_2d_->tryComputeAngleConversion(pointcloud_base_link);
+
+  angle_conversion_opt = std::nullopt;
   distortion_corrector_2d_->undistortPointCloud(true, angle_conversion_opt, pointcloud_lidar_top);
 
-  sensor_msgs::msg::PointCloud2 original_pointcloud_lidar_top =
-    generatePointCloudMsg(true, true, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
+  auto original_pointcloud_lidar_top =
+    generatePointCloudMsg(true, timestamp, default_points, default_azimuths);
 
   sensor_msgs::PointCloud2ConstIterator<float> test_iter_azimuth_lidar_top(
     pointcloud_lidar_top, "azimuth");
@@ -1060,8 +1070,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
 {
   // Generate the point cloud message in sensor frame
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, true, AngleCoordinateSystem::VELODYNE, timestamp, true, {}, {});
+  auto [default_points, default_azimuths] =
+    generateDefaultPointcloud(AngleCoordinateSystem::VELODYNE);
+  auto pointcloud = generatePointCloudMsg(true, timestamp, default_points, default_azimuths);
 
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
@@ -1075,8 +1086,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   auto angle_conversion_opt = distortion_corrector_2d_->tryComputeAngleConversion(pointcloud);
   distortion_corrector_2d_->undistortPointCloud(true, angle_conversion_opt, pointcloud);
 
-  sensor_msgs::msg::PointCloud2 original_pointcloud =
-    generatePointCloudMsg(true, true, AngleCoordinateSystem::VELODYNE, timestamp, true, {}, {});
+  auto original_pointcloud =
+    generatePointCloudMsg(true, timestamp, default_points, default_azimuths);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_azimuth(pointcloud, "azimuth");
   sensor_msgs::PointCloud2ConstIterator<float> iter_distance(pointcloud, "distance");
@@ -1118,8 +1129,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
 {
   // Generate the point cloud message in sensor frame
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  sensor_msgs::msg::PointCloud2 pointcloud =
-    generatePointCloudMsg(true, true, AngleCoordinateSystem::HESAI, timestamp, true, {}, {});
+
+  auto [default_points, default_azimuths] = generateDefaultPointcloud(AngleCoordinateSystem::HESAI);
+  auto pointcloud = generatePointCloudMsg(true, timestamp, default_points, default_azimuths);
 
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
@@ -1133,8 +1145,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   auto angle_conversion_opt = distortion_corrector_2d_->tryComputeAngleConversion(pointcloud);
   distortion_corrector_2d_->undistortPointCloud(true, angle_conversion_opt, pointcloud);
 
-  sensor_msgs::msg::PointCloud2 original_pointcloud =
-    generatePointCloudMsg(true, true, AngleCoordinateSystem::HESAI, timestamp, true, {}, {});
+  auto original_pointcloud =
+    generatePointCloudMsg(true, timestamp, default_points, default_azimuths);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_azimuth(pointcloud, "azimuth");
   sensor_msgs::PointCloud2ConstIterator<float> iter_distance(pointcloud, "distance");
@@ -1175,12 +1187,10 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnEmptyPointcloud)
 {
   // test empty pointcloud
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  // Generate and process multiple twist messages
-  generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
 
-  sensor_msgs::msg::PointCloud2 empty_pointcloud =
-    generatePointCloudMsg(false, false, AngleCoordinateSystem::CARTESIAN, timestamp, true, {}, {});
+  auto empty_pointcloud = generateEmptyPointCloudMsg(timestamp);
   auto angle_conversion_opt = distortion_corrector_2d_->tryComputeAngleConversion(empty_pointcloud);
+
   EXPECT_FALSE(angle_conversion_opt.has_value());
 }
 
@@ -1188,8 +1198,6 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnVelodynePointclou
 {
   // test velodyne pointcloud (x-axis: 0 degree, y-axis: 270 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  // Generate and process multiple twist messages
-  generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
 
   std::vector<Eigen::Vector3f> velodyne_points = {
     Eigen::Vector3f(0.0f, 0.0f, 0.0f),
@@ -1198,10 +1206,9 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnVelodynePointclou
   };
   std::vector<float> velodyne_azimuths = {
     0.0f, autoware::universe_utils::pi / 4, autoware::universe_utils::pi / 2};
-  sensor_msgs::msg::PointCloud2 velodyne_pointcloud = generatePointCloudMsg(
-    true, true, AngleCoordinateSystem::CARTESIAN, timestamp, false, velodyne_points,
-    velodyne_azimuths);
 
+  auto velodyne_pointcloud =
+    generatePointCloudMsg(true, timestamp, velodyne_points, velodyne_azimuths);
   auto angle_conversion_opt =
     distortion_corrector_2d_->tryComputeAngleConversion(velodyne_pointcloud);
   EXPECT_TRUE(angle_conversion_opt.has_value());
@@ -1214,9 +1221,6 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnHesaiPointcloud)
 {
   // test hesai pointcloud (x-axis: 90 degree, y-axis: 0 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  // Generate and process multiple twist messages
-  generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
-
   std::vector<Eigen::Vector3f> hesai_points = {
     Eigen::Vector3f(0.0f, 0.0f, 0.0f),
     Eigen::Vector3f(1.0f, -1.0f, 1.0f),
@@ -1225,9 +1229,8 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnHesaiPointcloud)
   std::vector<float> hesai_azimuths = {
     autoware::universe_utils::pi / 2, autoware::universe_utils::pi * 3 / 4,
     autoware::universe_utils::pi};
-  sensor_msgs::msg::PointCloud2 hesai_pointcloud = generatePointCloudMsg(
-    true, true, AngleCoordinateSystem::CARTESIAN, timestamp, false, hesai_points, hesai_azimuths);
 
+  auto hesai_pointcloud = generatePointCloudMsg(true, timestamp, hesai_points, hesai_azimuths);
   auto angle_conversion_opt = distortion_corrector_2d_->tryComputeAngleConversion(hesai_pointcloud);
 
   EXPECT_TRUE(angle_conversion_opt.has_value());
@@ -1250,10 +1253,9 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionCartesianPointcloud
   };
   std::vector<float> cartesian_azimuths = {
     0, autoware::universe_utils::pi / 4, autoware::universe_utils::pi / 2};
-  sensor_msgs::msg::PointCloud2 cartesian_pointcloud = generatePointCloudMsg(
-    true, true, AngleCoordinateSystem::CARTESIAN, timestamp, false, cartesian_points,
-    cartesian_azimuths);
 
+  auto cartesian_pointcloud =
+    generatePointCloudMsg(true, timestamp, cartesian_points, cartesian_azimuths);
   auto angle_conversion_opt =
     distortion_corrector_2d_->tryComputeAngleConversion(cartesian_pointcloud);
 
@@ -1276,9 +1278,8 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnRandomPointcloud)
   };
   std::vector<float> azimuths = {
     0, autoware::universe_utils::pi * 3 / 2, autoware::universe_utils::pi * 7 / 4};
-  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(
-    true, true, AngleCoordinateSystem::CARTESIAN, timestamp, false, points, azimuths);
 
+  auto pointcloud = generatePointCloudMsg(true, timestamp, points, azimuths);
   auto angle_conversion_opt = distortion_corrector_2d_->tryComputeAngleConversion(pointcloud);
 
   EXPECT_TRUE(angle_conversion_opt.has_value());
@@ -1304,9 +1305,8 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnBadAzimuthPointcl
 
   // generate random bad azimuths
   std::vector<float> azimuths = {0, 0, autoware::universe_utils::pi};
-  sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(
-    true, true, AngleCoordinateSystem::CARTESIAN, timestamp, false, points, azimuths);
 
+  auto pointcloud = generatePointCloudMsg(true, timestamp, points, azimuths);
   auto angle_conversion_opt = distortion_corrector_2d_->tryComputeAngleConversion(pointcloud);
 
   EXPECT_FALSE(angle_conversion_opt.has_value());

From 1d5ccdb1c8b6717ca20bd527f298b218dcdb3be5 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Thu, 12 Sep 2024 15:05:09 +0900
Subject: [PATCH 40/72] chore: RCLCPP_WARN_STREAM logging when failed to get
 angle conversion

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector/distortion_corrector_node.hpp        | 2 +-
 .../src/distortion_corrector/distortion_corrector_node.cpp    | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

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 0dd781b4730ec..550122ee03a75 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
@@ -58,7 +58,7 @@ class DistortionCorrectorComponent : public rclcpp::Node
 
   std::optional<AngleConversion> angle_conversion_opt_;
   int angle_conversion_failure_num_{0};
-  int failure_tolerance_{10};
+  int failure_tolerance_{20};
 
   std::unique_ptr<DistortionCorrectorBase> distortion_corrector_;
 
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 4ee1f630b2db6..f2b789fa0d6b0 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
@@ -104,8 +104,8 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointclou
         "Success to get the conversion formula between cartesian coordinates and LiDAR azimuth "
         "coordinates");
     } else {
-      RCLCPP_INFO(
-        this->get_logger(),
+      RCLCPP_WARN_STREAM_THROTTLE(
+        this->get_logger(), *this->get_clock(), 10000 /* ms */,
         "Failed to get the angle conversion between Cartesian coordinates and LiDAR azimuth "
         "coordinates. This pointcloud will not update azimuth and distance");
       angle_conversion_failure_num_++;

From 5e55d016a266de4177830d4d2cdc794e5c864f5a Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Thu, 12 Sep 2024 15:22:10 +0900
Subject: [PATCH 41/72] chore: improve normalize angle algorithm

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../test/src/math/test_trigonometry.cpp                    | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp
index be33f340ba706..df05b698693d6 100644
--- a/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp
+++ b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp
@@ -53,10 +53,9 @@ TEST(trigonometry, sin_and_cos)
 
 float normalize_angle(double angle)
 {
-  if (angle < 0) {
-    return angle + 2 * autoware::universe_utils::pi;
-  }
-  return angle;
+  const double tau = 2 * autoware::universe_utils::pi;
+  double factor = std::floor(angle / tau);
+  return static_cast<float>(angle - (factor * tau));
 }
 
 TEST(trigonometry, opencv_fast_atan2)

From 02beb86b0ac1e45b0bc70b3ce76df8714604e69c Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Thu, 12 Sep 2024 15:33:20 +0900
Subject: [PATCH 42/72] chore: improve multiple_of_90_degrees logic

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../src/distortion_corrector/distortion_corrector.cpp       | 6 +-----
 1 file changed, 1 insertion(+), 5 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index bf5afe09e849a..cc76baafc9d04 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -268,11 +268,7 @@ std::optional<AngleConversion> DistortionCorrector<T>::tryComputeAngleConversion
     }
 
     // Limit the range of offset_rad in [0, 360)
-    if (multiple_of_90_degrees < 0) {
-      multiple_of_90_degrees += 4;
-    } else if (multiple_of_90_degrees >= 4) {
-      multiple_of_90_degrees -= 4;
-    }
+    multiple_of_90_degrees = (multiple_of_90_degrees % 4 + 4) % 4;
 
     angle_conversion.offset_rad = multiple_of_90_degrees * (autoware::universe_utils::pi / 2);
 

From 29f4b9602d33b7d9730ca7b6702a4de5cd1a8c54 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Thu, 12 Sep 2024 16:28:04 +0900
Subject: [PATCH 43/72] chore: add opencv license

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 common/autoware_universe_utils/NOTICE                          | 2 ++
 common/autoware_universe_utils/src/math/trigonometry.cpp       | 3 +++
 .../{LICENSE => third_party_licenses/opencv-license.md}        | 2 +-
 3 files changed, 6 insertions(+), 1 deletion(-)
 create mode 100644 common/autoware_universe_utils/NOTICE
 rename common/autoware_universe_utils/{LICENSE => third_party_licenses/opencv-license.md} (99%)

diff --git a/common/autoware_universe_utils/NOTICE b/common/autoware_universe_utils/NOTICE
new file mode 100644
index 0000000000000..b0b596e9487f2
--- /dev/null
+++ b/common/autoware_universe_utils/NOTICE
@@ -0,0 +1,2 @@
+The function 'opencv_fast_atan2' in /autoware/src/universe/autoware.universe/common/autoware_universe_utils/src/math/trigonometry.cpp
+was modified by function 'fastAtan2' in OpenCV project. (https://github.com/opencv/opencv/blob/4.x/modules/core/src/mathfuncs_core.simd.hpp)
diff --git a/common/autoware_universe_utils/src/math/trigonometry.cpp b/common/autoware_universe_utils/src/math/trigonometry.cpp
index 82785d1187220..b6b2c4f854c18 100644
--- a/common/autoware_universe_utils/src/math/trigonometry.cpp
+++ b/common/autoware_universe_utils/src/math/trigonometry.cpp
@@ -76,6 +76,9 @@ std::pair<float, float> sin_and_cos(float radian)
 // (https://github.com/opencv/opencv/blob/4.x/modules/core/src/mathfuncs_core.simd.hpp). It is
 // subject to the license terms in the LICENSE file found in the top-level directory of this
 // distribution and at http://opencv.org/license.html.
+// The license can be found in
+// /autoware/src/universe/autoware.universe/common/autoware_universe_utils/third_party_licenses/opencv-license.md
+// and https://github.com/opencv/opencv/blob/master/LICENSE
 
 // Modification:
 // 1. use autoware defined PI
diff --git a/common/autoware_universe_utils/LICENSE b/common/autoware_universe_utils/third_party_licenses/opencv-license.md
similarity index 99%
rename from common/autoware_universe_utils/LICENSE
rename to common/autoware_universe_utils/third_party_licenses/opencv-license.md
index d645695673349..7a4a3ea2424c0 100644
--- a/common/autoware_universe_utils/LICENSE
+++ b/common/autoware_universe_utils/third_party_licenses/opencv-license.md
@@ -199,4 +199,4 @@
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
-   limitations under the License.
+   limitations under the License.
\ No newline at end of file

From b9a5c56436665dfbc79a9e6fde6e7e93ee3e97f8 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Thu, 12 Sep 2024 07:30:22 +0000
Subject: [PATCH 44/72] style(pre-commit): autofix

---
 .../third_party_licenses/opencv-license.md    | 367 +++++++++---------
 1 file changed, 183 insertions(+), 184 deletions(-)

diff --git a/common/autoware_universe_utils/third_party_licenses/opencv-license.md b/common/autoware_universe_utils/third_party_licenses/opencv-license.md
index 7a4a3ea2424c0..c61b66391a3c1 100644
--- a/common/autoware_universe_utils/third_party_licenses/opencv-license.md
+++ b/common/autoware_universe_utils/third_party_licenses/opencv-license.md
@@ -1,182 +1,181 @@
-
                                  Apache License
                            Version 2.0, January 2004
                         http://www.apache.org/licenses/
 
-   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
-
-   1. Definitions.
-
-      "License" shall mean the terms and conditions for use, reproduction,
-      and distribution as defined by Sections 1 through 9 of this document.
-
-      "Licensor" shall mean the copyright owner or entity authorized by
-      the copyright owner that is granting the License.
-
-      "Legal Entity" shall mean the union of the acting entity and all
-      other entities that control, are controlled by, or are under common
-      control with that entity. For the purposes of this definition,
-      "control" means (i) the power, direct or indirect, to cause the
-      direction or management of such entity, whether by contract or
-      otherwise, or (ii) ownership of fifty percent (50%) or more of the
-      outstanding shares, or (iii) beneficial ownership of such entity.
-
-      "You" (or "Your") shall mean an individual or Legal Entity
-      exercising permissions granted by this License.
-
-      "Source" form shall mean the preferred form for making modifications,
-      including but not limited to software source code, documentation
-      source, and configuration files.
-
-      "Object" form shall mean any form resulting from mechanical
-      transformation or translation of a Source form, including but
-      not limited to compiled object code, generated documentation,
-      and conversions to other media types.
-
-      "Work" shall mean the work of authorship, whether in Source or
-      Object form, made available under the License, as indicated by a
-      copyright notice that is included in or attached to the work
-      (an example is provided in the Appendix below).
-
-      "Derivative Works" shall mean any work, whether in Source or Object
-      form, that is based on (or derived from) the Work and for which the
-      editorial revisions, annotations, elaborations, or other modifications
-      represent, as a whole, an original work of authorship. For the purposes
-      of this License, Derivative Works shall not include works that remain
-      separable from, or merely link (or bind by name) to the interfaces of,
-      the Work and Derivative Works thereof.
-
-      "Contribution" shall mean any work of authorship, including
-      the original version of the Work and any modifications or additions
-      to that Work or Derivative Works thereof, that is intentionally
-      submitted to Licensor for inclusion in the Work by the copyright owner
-      or by an individual or Legal Entity authorized to submit on behalf of
-      the copyright owner. For the purposes of this definition, "submitted"
-      means any form of electronic, verbal, or written communication sent
-      to the Licensor or its representatives, including but not limited to
-      communication on electronic mailing lists, source code control systems,
-      and issue tracking systems that are managed by, or on behalf of, the
-      Licensor for the purpose of discussing and improving the Work, but
-      excluding communication that is conspicuously marked or otherwise
-      designated in writing by the copyright owner as "Not a Contribution."
-
-      "Contributor" shall mean Licensor and any individual or Legal Entity
-      on behalf of whom a Contribution has been received by Licensor and
-      subsequently incorporated within the Work.
-
-   2. Grant of Copyright License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      copyright license to reproduce, prepare Derivative Works of,
-      publicly display, publicly perform, sublicense, and distribute the
-      Work and such Derivative Works in Source or Object form.
-
-   3. Grant of Patent License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      (except as stated in this section) patent license to make, have made,
-      use, offer to sell, sell, import, and otherwise transfer the Work,
-      where such license applies only to those patent claims licensable
-      by such Contributor that are necessarily infringed by their
-      Contribution(s) alone or by combination of their Contribution(s)
-      with the Work to which such Contribution(s) was submitted. If You
-      institute patent litigation against any entity (including a
-      cross-claim or counterclaim in a lawsuit) alleging that the Work
-      or a Contribution incorporated within the Work constitutes direct
-      or contributory patent infringement, then any patent licenses
-      granted to You under this License for that Work shall terminate
-      as of the date such litigation is filed.
-
-   4. Redistribution. You may reproduce and distribute copies of the
-      Work or Derivative Works thereof in any medium, with or without
-      modifications, and in Source or Object form, provided that You
-      meet the following conditions:
-
-      (a) You must give any other recipients of the Work or
-          Derivative Works a copy of this License; and
-
-      (b) You must cause any modified files to carry prominent notices
-          stating that You changed the files; and
-
-      (c) You must retain, in the Source form of any Derivative Works
-          that You distribute, all copyright, patent, trademark, and
-          attribution notices from the Source form of the Work,
-          excluding those notices that do not pertain to any part of
-          the Derivative Works; and
-
-      (d) If the Work includes a "NOTICE" text file as part of its
-          distribution, then any Derivative Works that You distribute must
-          include a readable copy of the attribution notices contained
-          within such NOTICE file, excluding those notices that do not
-          pertain to any part of the Derivative Works, in at least one
-          of the following places: within a NOTICE text file distributed
-          as part of the Derivative Works; within the Source form or
-          documentation, if provided along with the Derivative Works; or,
-          within a display generated by the Derivative Works, if and
-          wherever such third-party notices normally appear. The contents
-          of the NOTICE file are for informational purposes only and
-          do not modify the License. You may add Your own attribution
-          notices within Derivative Works that You distribute, alongside
-          or as an addendum to the NOTICE text from the Work, provided
-          that such additional attribution notices cannot be construed
-          as modifying the License.
-
-      You may add Your own copyright statement to Your modifications and
-      may provide additional or different license terms and conditions
-      for use, reproduction, or distribution of Your modifications, or
-      for any such Derivative Works as a whole, provided Your use,
-      reproduction, and distribution of the Work otherwise complies with
-      the conditions stated in this License.
-
-   5. Submission of Contributions. Unless You explicitly state otherwise,
-      any Contribution intentionally submitted for inclusion in the Work
-      by You to the Licensor shall be under the terms and conditions of
-      this License, without any additional terms or conditions.
-      Notwithstanding the above, nothing herein shall supersede or modify
-      the terms of any separate license agreement you may have executed
-      with Licensor regarding such Contributions.
-
-   6. Trademarks. This License does not grant permission to use the trade
-      names, trademarks, service marks, or product names of the Licensor,
-      except as required for reasonable and customary use in describing the
-      origin of the Work and reproducing the content of the NOTICE file.
-
-   7. Disclaimer of Warranty. Unless required by applicable law or
-      agreed to in writing, Licensor provides the Work (and each
-      Contributor provides its Contributions) on an "AS IS" BASIS,
-      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
-      implied, including, without limitation, any warranties or conditions
-      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
-      PARTICULAR PURPOSE. You are solely responsible for determining the
-      appropriateness of using or redistributing the Work and assume any
-      risks associated with Your exercise of permissions under this License.
-
-   8. Limitation of Liability. In no event and under no legal theory,
-      whether in tort (including negligence), contract, or otherwise,
-      unless required by applicable law (such as deliberate and grossly
-      negligent acts) or agreed to in writing, shall any Contributor be
-      liable to You for damages, including any direct, indirect, special,
-      incidental, or consequential damages of any character arising as a
-      result of this License or out of the use or inability to use the
-      Work (including but not limited to damages for loss of goodwill,
-      work stoppage, computer failure or malfunction, or any and all
-      other commercial damages or losses), even if such Contributor
-      has been advised of the possibility of such damages.
-
-   9. Accepting Warranty or Additional Liability. While redistributing
-      the Work or Derivative Works thereof, You may choose to offer,
-      and charge a fee for, acceptance of support, warranty, indemnity,
-      or other liability obligations and/or rights consistent with this
-      License. However, in accepting such obligations, You may act only
-      on Your own behalf and on Your sole responsibility, not on behalf
-      of any other Contributor, and only if You agree to indemnify,
-      defend, and hold each Contributor harmless for any liability
-      incurred by, or claims asserted against, such Contributor by reason
-      of your accepting any such warranty or additional liability.
-
-   END OF TERMS AND CONDITIONS
-
-   APPENDIX: How to apply the Apache License to your work.
+TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+1.  Definitions.
+
+    "License" shall mean the terms and conditions for use, reproduction,
+    and distribution as defined by Sections 1 through 9 of this document.
+
+    "Licensor" shall mean the copyright owner or entity authorized by
+    the copyright owner that is granting the License.
+
+    "Legal Entity" shall mean the union of the acting entity and all
+    other entities that control, are controlled by, or are under common
+    control with that entity. For the purposes of this definition,
+    "control" means (i) the power, direct or indirect, to cause the
+    direction or management of such entity, whether by contract or
+    otherwise, or (ii) ownership of fifty percent (50%) or more of the
+    outstanding shares, or (iii) beneficial ownership of such entity.
+
+    "You" (or "Your") shall mean an individual or Legal Entity
+    exercising permissions granted by this License.
+
+    "Source" form shall mean the preferred form for making modifications,
+    including but not limited to software source code, documentation
+    source, and configuration files.
+
+    "Object" form shall mean any form resulting from mechanical
+    transformation or translation of a Source form, including but
+    not limited to compiled object code, generated documentation,
+    and conversions to other media types.
+
+    "Work" shall mean the work of authorship, whether in Source or
+    Object form, made available under the License, as indicated by a
+    copyright notice that is included in or attached to the work
+    (an example is provided in the Appendix below).
+
+    "Derivative Works" shall mean any work, whether in Source or Object
+    form, that is based on (or derived from) the Work and for which the
+    editorial revisions, annotations, elaborations, or other modifications
+    represent, as a whole, an original work of authorship. For the purposes
+    of this License, Derivative Works shall not include works that remain
+    separable from, or merely link (or bind by name) to the interfaces of,
+    the Work and Derivative Works thereof.
+
+    "Contribution" shall mean any work of authorship, including
+    the original version of the Work and any modifications or additions
+    to that Work or Derivative Works thereof, that is intentionally
+    submitted to Licensor for inclusion in the Work by the copyright owner
+    or by an individual or Legal Entity authorized to submit on behalf of
+    the copyright owner. For the purposes of this definition, "submitted"
+    means any form of electronic, verbal, or written communication sent
+    to the Licensor or its representatives, including but not limited to
+    communication on electronic mailing lists, source code control systems,
+    and issue tracking systems that are managed by, or on behalf of, the
+    Licensor for the purpose of discussing and improving the Work, but
+    excluding communication that is conspicuously marked or otherwise
+    designated in writing by the copyright owner as "Not a Contribution."
+
+    "Contributor" shall mean Licensor and any individual or Legal Entity
+    on behalf of whom a Contribution has been received by Licensor and
+    subsequently incorporated within the Work.
+
+2.  Grant of Copyright License. Subject to the terms and conditions of
+    this License, each Contributor hereby grants to You a perpetual,
+    worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+    copyright license to reproduce, prepare Derivative Works of,
+    publicly display, publicly perform, sublicense, and distribute the
+    Work and such Derivative Works in Source or Object form.
+
+3.  Grant of Patent License. Subject to the terms and conditions of
+    this License, each Contributor hereby grants to You a perpetual,
+    worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+    (except as stated in this section) patent license to make, have made,
+    use, offer to sell, sell, import, and otherwise transfer the Work,
+    where such license applies only to those patent claims licensable
+    by such Contributor that are necessarily infringed by their
+    Contribution(s) alone or by combination of their Contribution(s)
+    with the Work to which such Contribution(s) was submitted. If You
+    institute patent litigation against any entity (including a
+    cross-claim or counterclaim in a lawsuit) alleging that the Work
+    or a Contribution incorporated within the Work constitutes direct
+    or contributory patent infringement, then any patent licenses
+    granted to You under this License for that Work shall terminate
+    as of the date such litigation is filed.
+
+4.  Redistribution. You may reproduce and distribute copies of the
+    Work or Derivative Works thereof in any medium, with or without
+    modifications, and in Source or Object form, provided that You
+    meet the following conditions:
+
+    (a) You must give any other recipients of the Work or
+    Derivative Works a copy of this License; and
+
+    (b) You must cause any modified files to carry prominent notices
+    stating that You changed the files; and
+
+    (c) You must retain, in the Source form of any Derivative Works
+    that You distribute, all copyright, patent, trademark, and
+    attribution notices from the Source form of the Work,
+    excluding those notices that do not pertain to any part of
+    the Derivative Works; and
+
+    (d) If the Work includes a "NOTICE" text file as part of its
+    distribution, then any Derivative Works that You distribute must
+    include a readable copy of the attribution notices contained
+    within such NOTICE file, excluding those notices that do not
+    pertain to any part of the Derivative Works, in at least one
+    of the following places: within a NOTICE text file distributed
+    as part of the Derivative Works; within the Source form or
+    documentation, if provided along with the Derivative Works; or,
+    within a display generated by the Derivative Works, if and
+    wherever such third-party notices normally appear. The contents
+    of the NOTICE file are for informational purposes only and
+    do not modify the License. You may add Your own attribution
+    notices within Derivative Works that You distribute, alongside
+    or as an addendum to the NOTICE text from the Work, provided
+    that such additional attribution notices cannot be construed
+    as modifying the License.
+
+    You may add Your own copyright statement to Your modifications and
+    may provide additional or different license terms and conditions
+    for use, reproduction, or distribution of Your modifications, or
+    for any such Derivative Works as a whole, provided Your use,
+    reproduction, and distribution of the Work otherwise complies with
+    the conditions stated in this License.
+
+5.  Submission of Contributions. Unless You explicitly state otherwise,
+    any Contribution intentionally submitted for inclusion in the Work
+    by You to the Licensor shall be under the terms and conditions of
+    this License, without any additional terms or conditions.
+    Notwithstanding the above, nothing herein shall supersede or modify
+    the terms of any separate license agreement you may have executed
+    with Licensor regarding such Contributions.
+
+6.  Trademarks. This License does not grant permission to use the trade
+    names, trademarks, service marks, or product names of the Licensor,
+    except as required for reasonable and customary use in describing the
+    origin of the Work and reproducing the content of the NOTICE file.
+
+7.  Disclaimer of Warranty. Unless required by applicable law or
+    agreed to in writing, Licensor provides the Work (and each
+    Contributor provides its Contributions) on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+    implied, including, without limitation, any warranties or conditions
+    of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+    PARTICULAR PURPOSE. You are solely responsible for determining the
+    appropriateness of using or redistributing the Work and assume any
+    risks associated with Your exercise of permissions under this License.
+
+8.  Limitation of Liability. In no event and under no legal theory,
+    whether in tort (including negligence), contract, or otherwise,
+    unless required by applicable law (such as deliberate and grossly
+    negligent acts) or agreed to in writing, shall any Contributor be
+    liable to You for damages, including any direct, indirect, special,
+    incidental, or consequential damages of any character arising as a
+    result of this License or out of the use or inability to use the
+    Work (including but not limited to damages for loss of goodwill,
+    work stoppage, computer failure or malfunction, or any and all
+    other commercial damages or losses), even if such Contributor
+    has been advised of the possibility of such damages.
+
+9.  Accepting Warranty or Additional Liability. While redistributing
+    the Work or Derivative Works thereof, You may choose to offer,
+    and charge a fee for, acceptance of support, warranty, indemnity,
+    or other liability obligations and/or rights consistent with this
+    License. However, in accepting such obligations, You may act only
+    on Your own behalf and on Your sole responsibility, not on behalf
+    of any other Contributor, and only if You agree to indemnify,
+    defend, and hold each Contributor harmless for any liability
+    incurred by, or claims asserted against, such Contributor by reason
+    of your accepting any such warranty or additional liability.
+
+END OF TERMS AND CONDITIONS
+
+APPENDIX: How to apply the Apache License to your work.
 
       To apply the Apache License to your work, attach the following
       boilerplate notice, with the fields enclosed by brackets "[]"
@@ -187,16 +186,16 @@
       same "printed page" as the copyright notice for easier
       identification within third-party archives.
 
-   Copyright [yyyy] [name of copyright owner]
+Copyright [yyyy] [name of copyright owner]
 
-   Licensed under the Apache License, Version 2.0 (the "License");
-   you may not use this file except in compliance with the License.
-   You may obtain a copy of the License at
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
 
        http://www.apache.org/licenses/LICENSE-2.0
 
-   Unless required by applicable law or agreed to in writing, software
-   distributed under the License is distributed on an "AS IS" BASIS,
-   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-   See the License for the specific language governing permissions and
-   limitations under the License.
\ No newline at end of file
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.

From 13d981f70a5fa22fd07a8f7dfa302420a1700a6e Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Thu, 12 Sep 2024 16:37:02 +0900
Subject: [PATCH 45/72] chore: clean code

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector/distortion_corrector_node.hpp          | 2 --
 1 file changed, 2 deletions(-)

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 550122ee03a75..4e317ee6039cf 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
@@ -54,8 +54,6 @@ class DistortionCorrectorComponent : public rclcpp::Node
   bool use_3d_distortion_correction_;
   bool update_azimuth_and_distance_;
 
-  bool can_update_azimuth_and_distance_{false};
-
   std::optional<AngleConversion> angle_conversion_opt_;
   int angle_conversion_failure_num_{0};
   int failure_tolerance_{20};

From 02b12177da2092ae663a3c27b0bccca07a3876c2 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Thu, 12 Sep 2024 16:39:55 +0900
Subject: [PATCH 46/72] chore: fix sentence

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector/distortion_corrector.hpp               | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
index bc3c475a4aaad..3bb0637875b75 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -52,7 +52,7 @@ struct AngleConversion
 {
   // Equation for the conversion between sensor azimuth coordinates and Cartesian coordinates:
   // sensor azimuth coordinates = offset_rad + sign * cartesian coordinates;
-  // offset_rad i 0, and sign is restricted to be 1 or -1.
+  // offset_rad is restricted to be a multiple of 90, and sign is restricted to be 1 or -1.
   float offset_rad{0};
   float sign{1};
   float offset_rad_threshold{0.087f};  // (5 / 180) * M_PI

From cdb88ae97913ef959599ab10ea7089d83909fdf5 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Thu, 12 Sep 2024 07:39:50 +0000
Subject: [PATCH 47/72] style(pre-commit): autofix

---
 .../third_party_licenses/opencv-license.md    | 336 +++++++++---------
 1 file changed, 168 insertions(+), 168 deletions(-)

diff --git a/common/autoware_universe_utils/third_party_licenses/opencv-license.md b/common/autoware_universe_utils/third_party_licenses/opencv-license.md
index c61b66391a3c1..c319da33b7428 100644
--- a/common/autoware_universe_utils/third_party_licenses/opencv-license.md
+++ b/common/autoware_universe_utils/third_party_licenses/opencv-license.md
@@ -4,174 +4,174 @@
 
 TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
 
-1.  Definitions.
-
-    "License" shall mean the terms and conditions for use, reproduction,
-    and distribution as defined by Sections 1 through 9 of this document.
-
-    "Licensor" shall mean the copyright owner or entity authorized by
-    the copyright owner that is granting the License.
-
-    "Legal Entity" shall mean the union of the acting entity and all
-    other entities that control, are controlled by, or are under common
-    control with that entity. For the purposes of this definition,
-    "control" means (i) the power, direct or indirect, to cause the
-    direction or management of such entity, whether by contract or
-    otherwise, or (ii) ownership of fifty percent (50%) or more of the
-    outstanding shares, or (iii) beneficial ownership of such entity.
-
-    "You" (or "Your") shall mean an individual or Legal Entity
-    exercising permissions granted by this License.
-
-    "Source" form shall mean the preferred form for making modifications,
-    including but not limited to software source code, documentation
-    source, and configuration files.
-
-    "Object" form shall mean any form resulting from mechanical
-    transformation or translation of a Source form, including but
-    not limited to compiled object code, generated documentation,
-    and conversions to other media types.
-
-    "Work" shall mean the work of authorship, whether in Source or
-    Object form, made available under the License, as indicated by a
-    copyright notice that is included in or attached to the work
-    (an example is provided in the Appendix below).
-
-    "Derivative Works" shall mean any work, whether in Source or Object
-    form, that is based on (or derived from) the Work and for which the
-    editorial revisions, annotations, elaborations, or other modifications
-    represent, as a whole, an original work of authorship. For the purposes
-    of this License, Derivative Works shall not include works that remain
-    separable from, or merely link (or bind by name) to the interfaces of,
-    the Work and Derivative Works thereof.
-
-    "Contribution" shall mean any work of authorship, including
-    the original version of the Work and any modifications or additions
-    to that Work or Derivative Works thereof, that is intentionally
-    submitted to Licensor for inclusion in the Work by the copyright owner
-    or by an individual or Legal Entity authorized to submit on behalf of
-    the copyright owner. For the purposes of this definition, "submitted"
-    means any form of electronic, verbal, or written communication sent
-    to the Licensor or its representatives, including but not limited to
-    communication on electronic mailing lists, source code control systems,
-    and issue tracking systems that are managed by, or on behalf of, the
-    Licensor for the purpose of discussing and improving the Work, but
-    excluding communication that is conspicuously marked or otherwise
-    designated in writing by the copyright owner as "Not a Contribution."
-
-    "Contributor" shall mean Licensor and any individual or Legal Entity
-    on behalf of whom a Contribution has been received by Licensor and
-    subsequently incorporated within the Work.
-
-2.  Grant of Copyright License. Subject to the terms and conditions of
-    this License, each Contributor hereby grants to You a perpetual,
-    worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-    copyright license to reproduce, prepare Derivative Works of,
-    publicly display, publicly perform, sublicense, and distribute the
-    Work and such Derivative Works in Source or Object form.
-
-3.  Grant of Patent License. Subject to the terms and conditions of
-    this License, each Contributor hereby grants to You a perpetual,
-    worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-    (except as stated in this section) patent license to make, have made,
-    use, offer to sell, sell, import, and otherwise transfer the Work,
-    where such license applies only to those patent claims licensable
-    by such Contributor that are necessarily infringed by their
-    Contribution(s) alone or by combination of their Contribution(s)
-    with the Work to which such Contribution(s) was submitted. If You
-    institute patent litigation against any entity (including a
-    cross-claim or counterclaim in a lawsuit) alleging that the Work
-    or a Contribution incorporated within the Work constitutes direct
-    or contributory patent infringement, then any patent licenses
-    granted to You under this License for that Work shall terminate
-    as of the date such litigation is filed.
-
-4.  Redistribution. You may reproduce and distribute copies of the
-    Work or Derivative Works thereof in any medium, with or without
-    modifications, and in Source or Object form, provided that You
-    meet the following conditions:
-
-    (a) You must give any other recipients of the Work or
-    Derivative Works a copy of this License; and
-
-    (b) You must cause any modified files to carry prominent notices
-    stating that You changed the files; and
-
-    (c) You must retain, in the Source form of any Derivative Works
-    that You distribute, all copyright, patent, trademark, and
-    attribution notices from the Source form of the Work,
-    excluding those notices that do not pertain to any part of
-    the Derivative Works; and
-
-    (d) If the Work includes a "NOTICE" text file as part of its
-    distribution, then any Derivative Works that You distribute must
-    include a readable copy of the attribution notices contained
-    within such NOTICE file, excluding those notices that do not
-    pertain to any part of the Derivative Works, in at least one
-    of the following places: within a NOTICE text file distributed
-    as part of the Derivative Works; within the Source form or
-    documentation, if provided along with the Derivative Works; or,
-    within a display generated by the Derivative Works, if and
-    wherever such third-party notices normally appear. The contents
-    of the NOTICE file are for informational purposes only and
-    do not modify the License. You may add Your own attribution
-    notices within Derivative Works that You distribute, alongside
-    or as an addendum to the NOTICE text from the Work, provided
-    that such additional attribution notices cannot be construed
-    as modifying the License.
-
-    You may add Your own copyright statement to Your modifications and
-    may provide additional or different license terms and conditions
-    for use, reproduction, or distribution of Your modifications, or
-    for any such Derivative Works as a whole, provided Your use,
-    reproduction, and distribution of the Work otherwise complies with
-    the conditions stated in this License.
-
-5.  Submission of Contributions. Unless You explicitly state otherwise,
-    any Contribution intentionally submitted for inclusion in the Work
-    by You to the Licensor shall be under the terms and conditions of
-    this License, without any additional terms or conditions.
-    Notwithstanding the above, nothing herein shall supersede or modify
-    the terms of any separate license agreement you may have executed
-    with Licensor regarding such Contributions.
-
-6.  Trademarks. This License does not grant permission to use the trade
-    names, trademarks, service marks, or product names of the Licensor,
-    except as required for reasonable and customary use in describing the
-    origin of the Work and reproducing the content of the NOTICE file.
-
-7.  Disclaimer of Warranty. Unless required by applicable law or
-    agreed to in writing, Licensor provides the Work (and each
-    Contributor provides its Contributions) on an "AS IS" BASIS,
-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
-    implied, including, without limitation, any warranties or conditions
-    of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
-    PARTICULAR PURPOSE. You are solely responsible for determining the
-    appropriateness of using or redistributing the Work and assume any
-    risks associated with Your exercise of permissions under this License.
-
-8.  Limitation of Liability. In no event and under no legal theory,
-    whether in tort (including negligence), contract, or otherwise,
-    unless required by applicable law (such as deliberate and grossly
-    negligent acts) or agreed to in writing, shall any Contributor be
-    liable to You for damages, including any direct, indirect, special,
-    incidental, or consequential damages of any character arising as a
-    result of this License or out of the use or inability to use the
-    Work (including but not limited to damages for loss of goodwill,
-    work stoppage, computer failure or malfunction, or any and all
-    other commercial damages or losses), even if such Contributor
-    has been advised of the possibility of such damages.
-
-9.  Accepting Warranty or Additional Liability. While redistributing
-    the Work or Derivative Works thereof, You may choose to offer,
-    and charge a fee for, acceptance of support, warranty, indemnity,
-    or other liability obligations and/or rights consistent with this
-    License. However, in accepting such obligations, You may act only
-    on Your own behalf and on Your sole responsibility, not on behalf
-    of any other Contributor, and only if You agree to indemnify,
-    defend, and hold each Contributor harmless for any liability
-    incurred by, or claims asserted against, such Contributor by reason
-    of your accepting any such warranty or additional liability.
+1. Definitions.
+
+   "License" shall mean the terms and conditions for use, reproduction,
+   and distribution as defined by Sections 1 through 9 of this document.
+
+   "Licensor" shall mean the copyright owner or entity authorized by
+   the copyright owner that is granting the License.
+
+   "Legal Entity" shall mean the union of the acting entity and all
+   other entities that control, are controlled by, or are under common
+   control with that entity. For the purposes of this definition,
+   "control" means (i) the power, direct or indirect, to cause the
+   direction or management of such entity, whether by contract or
+   otherwise, or (ii) ownership of fifty percent (50%) or more of the
+   outstanding shares, or (iii) beneficial ownership of such entity.
+
+   "You" (or "Your") shall mean an individual or Legal Entity
+   exercising permissions granted by this License.
+
+   "Source" form shall mean the preferred form for making modifications,
+   including but not limited to software source code, documentation
+   source, and configuration files.
+
+   "Object" form shall mean any form resulting from mechanical
+   transformation or translation of a Source form, including but
+   not limited to compiled object code, generated documentation,
+   and conversions to other media types.
+
+   "Work" shall mean the work of authorship, whether in Source or
+   Object form, made available under the License, as indicated by a
+   copyright notice that is included in or attached to the work
+   (an example is provided in the Appendix below).
+
+   "Derivative Works" shall mean any work, whether in Source or Object
+   form, that is based on (or derived from) the Work and for which the
+   editorial revisions, annotations, elaborations, or other modifications
+   represent, as a whole, an original work of authorship. For the purposes
+   of this License, Derivative Works shall not include works that remain
+   separable from, or merely link (or bind by name) to the interfaces of,
+   the Work and Derivative Works thereof.
+
+   "Contribution" shall mean any work of authorship, including
+   the original version of the Work and any modifications or additions
+   to that Work or Derivative Works thereof, that is intentionally
+   submitted to Licensor for inclusion in the Work by the copyright owner
+   or by an individual or Legal Entity authorized to submit on behalf of
+   the copyright owner. For the purposes of this definition, "submitted"
+   means any form of electronic, verbal, or written communication sent
+   to the Licensor or its representatives, including but not limited to
+   communication on electronic mailing lists, source code control systems,
+   and issue tracking systems that are managed by, or on behalf of, the
+   Licensor for the purpose of discussing and improving the Work, but
+   excluding communication that is conspicuously marked or otherwise
+   designated in writing by the copyright owner as "Not a Contribution."
+
+   "Contributor" shall mean Licensor and any individual or Legal Entity
+   on behalf of whom a Contribution has been received by Licensor and
+   subsequently incorporated within the Work.
+
+2. Grant of Copyright License. Subject to the terms and conditions of
+   this License, each Contributor hereby grants to You a perpetual,
+   worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+   copyright license to reproduce, prepare Derivative Works of,
+   publicly display, publicly perform, sublicense, and distribute the
+   Work and such Derivative Works in Source or Object form.
+
+3. Grant of Patent License. Subject to the terms and conditions of
+   this License, each Contributor hereby grants to You a perpetual,
+   worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+   (except as stated in this section) patent license to make, have made,
+   use, offer to sell, sell, import, and otherwise transfer the Work,
+   where such license applies only to those patent claims licensable
+   by such Contributor that are necessarily infringed by their
+   Contribution(s) alone or by combination of their Contribution(s)
+   with the Work to which such Contribution(s) was submitted. If You
+   institute patent litigation against any entity (including a
+   cross-claim or counterclaim in a lawsuit) alleging that the Work
+   or a Contribution incorporated within the Work constitutes direct
+   or contributory patent infringement, then any patent licenses
+   granted to You under this License for that Work shall terminate
+   as of the date such litigation is filed.
+
+4. Redistribution. You may reproduce and distribute copies of the
+   Work or Derivative Works thereof in any medium, with or without
+   modifications, and in Source or Object form, provided that You
+   meet the following conditions:
+
+   (a) You must give any other recipients of the Work or
+   Derivative Works a copy of this License; and
+
+   (b) You must cause any modified files to carry prominent notices
+   stating that You changed the files; and
+
+   (c) You must retain, in the Source form of any Derivative Works
+   that You distribute, all copyright, patent, trademark, and
+   attribution notices from the Source form of the Work,
+   excluding those notices that do not pertain to any part of
+   the Derivative Works; and
+
+   (d) If the Work includes a "NOTICE" text file as part of its
+   distribution, then any Derivative Works that You distribute must
+   include a readable copy of the attribution notices contained
+   within such NOTICE file, excluding those notices that do not
+   pertain to any part of the Derivative Works, in at least one
+   of the following places: within a NOTICE text file distributed
+   as part of the Derivative Works; within the Source form or
+   documentation, if provided along with the Derivative Works; or,
+   within a display generated by the Derivative Works, if and
+   wherever such third-party notices normally appear. The contents
+   of the NOTICE file are for informational purposes only and
+   do not modify the License. You may add Your own attribution
+   notices within Derivative Works that You distribute, alongside
+   or as an addendum to the NOTICE text from the Work, provided
+   that such additional attribution notices cannot be construed
+   as modifying the License.
+
+   You may add Your own copyright statement to Your modifications and
+   may provide additional or different license terms and conditions
+   for use, reproduction, or distribution of Your modifications, or
+   for any such Derivative Works as a whole, provided Your use,
+   reproduction, and distribution of the Work otherwise complies with
+   the conditions stated in this License.
+
+5. Submission of Contributions. Unless You explicitly state otherwise,
+   any Contribution intentionally submitted for inclusion in the Work
+   by You to the Licensor shall be under the terms and conditions of
+   this License, without any additional terms or conditions.
+   Notwithstanding the above, nothing herein shall supersede or modify
+   the terms of any separate license agreement you may have executed
+   with Licensor regarding such Contributions.
+
+6. Trademarks. This License does not grant permission to use the trade
+   names, trademarks, service marks, or product names of the Licensor,
+   except as required for reasonable and customary use in describing the
+   origin of the Work and reproducing the content of the NOTICE file.
+
+7. Disclaimer of Warranty. Unless required by applicable law or
+   agreed to in writing, Licensor provides the Work (and each
+   Contributor provides its Contributions) on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+   implied, including, without limitation, any warranties or conditions
+   of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+   PARTICULAR PURPOSE. You are solely responsible for determining the
+   appropriateness of using or redistributing the Work and assume any
+   risks associated with Your exercise of permissions under this License.
+
+8. Limitation of Liability. In no event and under no legal theory,
+   whether in tort (including negligence), contract, or otherwise,
+   unless required by applicable law (such as deliberate and grossly
+   negligent acts) or agreed to in writing, shall any Contributor be
+   liable to You for damages, including any direct, indirect, special,
+   incidental, or consequential damages of any character arising as a
+   result of this License or out of the use or inability to use the
+   Work (including but not limited to damages for loss of goodwill,
+   work stoppage, computer failure or malfunction, or any and all
+   other commercial damages or losses), even if such Contributor
+   has been advised of the possibility of such damages.
+
+9. Accepting Warranty or Additional Liability. While redistributing
+   the Work or Derivative Works thereof, You may choose to offer,
+   and charge a fee for, acceptance of support, warranty, indemnity,
+   or other liability obligations and/or rights consistent with this
+   License. However, in accepting such obligations, You may act only
+   on Your own behalf and on Your sole responsibility, not on behalf
+   of any other Contributor, and only if You agree to indemnify,
+   defend, and hold each Contributor harmless for any liability
+   incurred by, or claims asserted against, such Contributor by reason
+   of your accepting any such warranty or additional liability.
 
 END OF TERMS AND CONDITIONS
 

From a8532934917f9c6b77882c2831cd8efe60aac560 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Thu, 12 Sep 2024 19:17:40 +0900
Subject: [PATCH 48/72] chore: add 0 0 0 points in test case

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../test/test_distortion_corrector_node.cpp   | 178 +++++++++---------
 1 file changed, 90 insertions(+), 88 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index e9d76a78ea941..184cc622b4124 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -175,17 +175,18 @@ class DistortionCorrectorTest : public ::testing::Test
     AngleCoordinateSystem vendor)
   {
     // Generate all combinations of signs { -, 0, + } x { -, 0, + } for x and y.
+    // Also include the case of (0, 0 ,0)
     std::vector<Eigen::Vector3f> default_points = {{
-      Eigen::Vector3f(10.0f, 0.0f, 1.0f),   // point 1
-      Eigen::Vector3f(5.0f, -5.0f, 2.0f),   // point 2
-      Eigen::Vector3f(0.0f, -10.0f, 3.0f),  // point 3
-      Eigen::Vector3f(-5.0f, -5.0f, 4.0f),  // point 4
-      Eigen::Vector3f(-10.0f, 0.0f, 5.0f),  // point 5
-      Eigen::Vector3f(-5.0f, 5.0f, -5.0f),  // point 6
-      Eigen::Vector3f(0.0f, 10.0f, -4.0f),  // point 7
-      Eigen::Vector3f(5.0f, 5.0f, -3.0f),   // point 8
-      Eigen::Vector3f(8.0f, 3.0f, -2.0f),   // point 9
-      Eigen::Vector3f(9.0f, 1.0f, -1.0f)    // point 10
+      Eigen::Vector3f(0.0f, 0.0f, 0.0f),    // point 1
+      Eigen::Vector3f(0.0f, 0.0f, 0.0f),    // point 2
+      Eigen::Vector3f(10.0f, 0.0f, 1.0f),   // point 3
+      Eigen::Vector3f(5.0f, -5.0f, 2.0f),   // point 4
+      Eigen::Vector3f(0.0f, -10.0f, 3.0f),  // point 5
+      Eigen::Vector3f(-5.0f, -5.0f, 4.0f),  // point 6
+      Eigen::Vector3f(-10.0f, 0.0f, 5.0f),  // point 7
+      Eigen::Vector3f(-5.0f, 5.0f, -5.0f),  // point 8
+      Eigen::Vector3f(0.0f, 10.0f, -4.0f),  // point 9
+      Eigen::Vector3f(5.0f, 5.0f, -3.0f),   // point 10
     }};
 
     std::vector<float> default_azimuths;
@@ -242,7 +243,6 @@ class DistortionCorrectorTest : public ::testing::Test
     pointcloud_msg.height = 1;
     pointcloud_msg.is_dense = true;
     pointcloud_msg.is_bigendian = false;
-    // auto [default_points, default_azimuths] = generateDefaultPointcloud(vendor);
 
     // Generate timestamps for the points
     std::vector<std::uint32_t> timestamps = generatePointTimestamps(stamp, points.size());
@@ -433,12 +433,13 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyTwist)
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
   sensor_msgs::PointCloud2ConstIterator<float> iter_z(pointcloud, "z");
 
-  std::array<Eigen::Vector3f, number_of_points_> expected_pointcloud = {
-    {Eigen::Vector3f(10.0f, 0.0f, 1.0f), Eigen::Vector3f(5.0f, -5.0f, 2.0f),
+  std::array<Eigen::Vector3f, 10> expected_pointcloud = {
+    {Eigen::Vector3f(0.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 0.0f, 0.0f),
+     Eigen::Vector3f(10.0f, 0.0f, 1.0f), Eigen::Vector3f(5.0f, -5.0f, 2.0f),
      Eigen::Vector3f(0.0f, -10.0f, 3.0f), Eigen::Vector3f(-5.0f, -5.0f, 4.0f),
      Eigen::Vector3f(-10.0f, 0.0f, 5.0f), Eigen::Vector3f(-5.0f, 5.0f, -5.0f),
-     Eigen::Vector3f(0.0f, 10.0f, -4.0f), Eigen::Vector3f(5.0f, 5.0f, -3.0f),
-     Eigen::Vector3f(8.0f, 3.0f, -2.0f), Eigen::Vector3f(9.0f, 1.0f, -1.0f)}};
+     Eigen::Vector3f(0.0f, 10.0f, -4.0f), Eigen::Vector3f(5.0f, 5.0f, -3.0f)}};
+
   size_t i = 0;
   std::ostringstream oss;
   oss << "Expected pointcloud:\n";
@@ -493,12 +494,12 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink)
   sensor_msgs::PointCloud2ConstIterator<float> iter_z(pointcloud, "z");
 
   // Expected undistorted point cloud values
-  std::array<Eigen::Vector3f, number_of_points_> expected_pointcloud = {
-    {Eigen::Vector3f(10.0f, 0.0f, 1.0f), Eigen::Vector3f(5.12144f, -4.99853f, 2.0f),
-     Eigen::Vector3f(0.266711f, -9.99986f, 3.0f), Eigen::Vector3f(-4.59472f, -5.00498f, 4.0f),
-     Eigen::Vector3f(-9.45999f, -0.0148422f, 5.0f), Eigen::Vector3f(-4.31006f, 4.99074f, -5.0f),
-     Eigen::Vector3f(0.835072f, 10.0012f, -4.0f), Eigen::Vector3f(6.02463f, 5.0171f, -3.0f),
-     Eigen::Vector3f(9.20872f, 3.03234f, -2.0f), Eigen::Vector3f(10.3956f, 1.04204f, -1.0f)}};
+  std::array<Eigen::Vector3f, 10> expected_pointcloud = {
+    {Eigen::Vector3f(0.0f, 0.0f, 0.0f), Eigen::Vector3f(0.12f, 3.45146e-05f, 0.0f),
+     Eigen::Vector3f(10.26f, 0.00684635f, 1.0f), Eigen::Vector3f(5.40527f, -4.99443f, 2.0f),
+     Eigen::Vector3f(0.55534f, -9.99949f, 3.0f), Eigen::Vector3f(-4.28992f, -5.00924f, 4.0f),
+     Eigen::Vector3f(-9.13997f, -0.0237086f, 5.0f), Eigen::Vector3f(-3.97532f, 4.98642f, -5.0f),
+     Eigen::Vector3f(1.18261f, 10.0024f, -4.0f), Eigen::Vector3f(6.37838f, 5.02475f, -3.0f)}};
 
   // Verify each point in the undistorted point cloud
   size_t i = 0;
@@ -540,12 +541,12 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink)
   sensor_msgs::PointCloud2ConstIterator<float> iter_z(pointcloud, "z");
 
   // Expected undistorted point cloud values
-  std::array<Eigen::Vector3f, number_of_points_> expected_pointcloud = {
-    {Eigen::Vector3f(10.0f, 0.0f, 1.0f), Eigen::Vector3f(5.11856f, -5.00147f, 2.0f),
-     Eigen::Vector3f(0.254248f, -10.0001f, 3.0f), Eigen::Vector3f(-4.60431f, -4.99592f, 4.0f),
-     Eigen::Vector3f(-9.45999f, 0.0111079f, 5.0f), Eigen::Vector3f(-4.2928f, 5.00656f, -5.0f),
-     Eigen::Vector3f(0.877257f, 9.99908f, -4.0f), Eigen::Vector3f(6.05006f, 4.98867f, -3.0f),
-     Eigen::Vector3f(9.22659f, 2.98069f, -2.0f), Eigen::Vector3f(10.4025f, 0.975451f, -1.0f)}};
+  std::array<Eigen::Vector3f, 10> expected_pointcloud = {
+    {Eigen::Vector3f(0.0f, 0.0f, 0.0f), Eigen::Vector3f(0.12f, -3.45146e-05f, 0.0f),
+     Eigen::Vector3f(10.26f, -0.00586748f, 1.0f), Eigen::Vector3f(5.39568f, -5.00455f, 2.0f),
+     Eigen::Vector3f(0.528495f, -10.0004f, 3.0f), Eigen::Vector3f(-4.30719f, -4.99343f, 4.0f),
+     Eigen::Vector3f(-9.13999f, 0.0163541f, 5.0f), Eigen::Vector3f(-3.94992f, 5.0088f, -5.0f),
+     Eigen::Vector3f(1.24205f, 9.99831f, -4.0f), Eigen::Vector3f(6.41245f, 4.98541f, -3.0f)}};
 
   // Verify each point in the undistorted point cloud
   size_t i = 0;
@@ -587,15 +588,15 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame)
   sensor_msgs::PointCloud2ConstIterator<float> iter_z(pointcloud, "z");
 
   // Expected undistorted point cloud values
-  std::array<Eigen::Vector3f, number_of_points_> expected_pointcloud = {
-    {Eigen::Vector3f(10.0f, -2.63462e-08f, 1.0f), Eigen::Vector3f(5.05137f, -4.93869f, 2.09267f),
-     Eigen::Vector3f(0.112092f, -9.86876f, 3.19715f),
-     Eigen::Vector3f(-4.83021f, -4.80022f, 4.30059f),
-     Eigen::Vector3f(-9.7743f, 0.266927f, 5.40228f),
-     Eigen::Vector3f(-4.69786f, 5.35289f, -4.47032f),
-     Eigen::Vector3f(0.365836f, 10.4368f, -3.33969f),
-     Eigen::Vector3f(5.44511f, 5.53184f, -2.19585f), Eigen::Vector3f(8.52226f, 3.62536f, -1.0538f),
-     Eigen::Vector3f(9.59921f, 1.71784f, 0.0862978f)}};
+  std::array<Eigen::Vector3f, 10> expected_pointcloud = {
+    {Eigen::Vector3f(0.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0512387f, 0.0608269f, 0.0917824f),
+     Eigen::Vector3f(10.1106f, 0.134026f, 1.20356f), Eigen::Vector3f(5.17128f, -4.79604f, 2.30806f),
+     Eigen::Vector3f(0.232686f, -9.7275f, 3.40938f),
+     Eigen::Vector3f(-4.70281f, -4.65034f, 4.52609f),
+     Eigen::Vector3f(-9.64009f, 0.425434f, 5.64106f),
+     Eigen::Vector3f(-4.55139f, 5.5241f, -4.21327f),
+     Eigen::Vector3f(0.519385f, 10.6188f, -3.06522f),
+     Eigen::Vector3f(5.5992f, 5.71475f, -1.91985f)}};
 
   // Verify each point in the undistorted point cloud
   size_t i = 0;
@@ -635,12 +636,12 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink)
   sensor_msgs::PointCloud2ConstIterator<float> iter_z(pointcloud, "z");
 
   // Expected undistorted point cloud values
-  std::array<Eigen::Vector3f, number_of_points_> expected_pointcloud = {
-    {Eigen::Vector3f(10.0f, 0.0f, 1.0f), Eigen::Vector3f(5.1215f, -4.99848f, 2.0f),
-     Eigen::Vector3f(0.267f, -9.99991f, 3.0f), Eigen::Vector3f(-4.5945f, -5.00528f, 4.0f),
-     Eigen::Vector3f(-9.45999f, -0.0146016f, 5.0f), Eigen::Vector3f(-4.30999f, 4.9907f, -5.0f),
-     Eigen::Vector3f(0.834999f, 10.0011f, -4.0f), Eigen::Vector3f(6.02447f, 5.01714f, -3.0f),
-     Eigen::Vector3f(9.20884f, 3.03192f, -2.0f), Eigen::Vector3f(10.3956f, 1.04182f, -1.0f)}};
+  std::array<Eigen::Vector3f, 10> expected_pointcloud = {
+    {Eigen::Vector3f(0.0f, 0.0f, 0.0f), Eigen::Vector3f(0.12f, 2.38419e-05f, 0.0f),
+     Eigen::Vector3f(10.26f, 0.0070927f, 1.0f), Eigen::Vector3f(5.4055f, -4.99428f, 2.0f),
+     Eigen::Vector3f(0.555f, -9.99959f, 3.0f), Eigen::Vector3f(-4.28999f, -5.00928f, 4.0f),
+     Eigen::Vector3f(-9.13997f, -0.0239053f, 5.0f), Eigen::Vector3f(-3.97548f, 4.98614f, -5.0f),
+     Eigen::Vector3f(1.183f, 10.0023f, -4.0f), Eigen::Vector3f(6.37845f, 5.02458f, -3.0f)}};
 
   // Verify each point in the undistorted point cloud
   size_t i = 0;
@@ -682,15 +683,15 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink)
   sensor_msgs::PointCloud2ConstIterator<float> iter_z(pointcloud, "z");
 
   // Expected undistorted point cloud values
-  std::array<Eigen::Vector3f, number_of_points_> expected_pointcloud = {
-    {Eigen::Vector3f(10.0f, 0.0f, 1.0f), Eigen::Vector3f(5.11902f, -5.00239f, 1.9965f),
-     Eigen::Vector3f(0.255945f, -10.0027f, 2.99104f),
-     Eigen::Vector3f(-4.60055f, -5.00116f, 3.99787f),
-     Eigen::Vector3f(-9.45347f, 0.00231253f, 5.01268f),
-     Eigen::Vector3f(-4.30145f, 5.01808f, -4.98054f),
-     Eigen::Vector3f(0.868468f, 10.0103f, -3.97336f),
-     Eigen::Vector3f(6.04213f, 4.99888f, -2.99811f), Eigen::Vector3f(9.22048f, 2.98838f, -2.01552f),
-     Eigen::Vector3f(10.3988f, 0.980287f, -1.03088f)}};
+  std::array<Eigen::Vector3f, 10> expected_pointcloud = {
+    {Eigen::Vector3f(0.0f, 0.0f, 0.0f), Eigen::Vector3f(0.12f, -1.86084e-05f, -1.63216e-05f),
+     Eigen::Vector3f(10.2606f, -0.00683919f, 0.993812f),
+     Eigen::Vector3f(5.39753f, -5.00722f, 1.9883f), Eigen::Vector3f(0.532273f, -10.0057f, 2.98165f),
+     Eigen::Vector3f(-4.30025f, -5.0024f, 3.99665f),
+     Eigen::Vector3f(-9.12918f, 0.00256404f, 5.02064f),
+     Eigen::Vector3f(-3.96298f, 5.02511f, -4.97218f),
+     Eigen::Vector3f(1.23005f, 10.0137f, -3.96452f),
+     Eigen::Vector3f(6.40165f, 4.99868f, -2.99944f)}};
 
   // Verify each point in the undistorted point cloud
   size_t i = 0;
@@ -733,13 +734,13 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame)
 
   // Expected undistorted point cloud values
   std::array<Eigen::Vector3f, 10> expected_pointcloud = {
-    {Eigen::Vector3f(10.0f, 4.76837e-07f, 1.0f), Eigen::Vector3f(5.05124f, -4.93713f, 2.09134f),
-     Eigen::Vector3f(0.114215f, -9.86881f, 3.19198f),
-     Eigen::Vector3f(-4.82904f, -4.80461f, 4.30305f),
-     Eigen::Vector3f(-9.77529f, 0.255465f, 5.41964f), Eigen::Vector3f(-4.71431f, 5.36722f, -4.441f),
-     Eigen::Vector3f(0.342873f, 10.4561f, -3.29519f), Eigen::Vector3f(5.426f, 5.55628f, -2.16926f),
-     Eigen::Vector3f(8.50523f, 3.65298f, -1.03512f),
-     Eigen::Vector3f(9.58544f, 1.74465f, 0.0959219f)}};
+    {Eigen::Vector3f(0.0f, 4.76837e-07f, 0.0f), Eigen::Vector3f(0.049716f, 0.0622373f, 0.0935726f),
+     Eigen::Vector3f(10.1082f, 0.139472f, 1.20323f), Eigen::Vector3f(5.17113f, -4.79225f, 2.30392f),
+     Eigen::Vector3f(0.23695f, -9.72807f, 3.39875f),
+     Eigen::Vector3f(-4.70053f, -4.65832f, 4.53053f),
+     Eigen::Vector3f(-9.64065f, 0.407413f, 5.66885f), Eigen::Vector3f(-4.5738f, 5.5446f, -4.17022f),
+     Eigen::Vector3f(0.489763f, 10.6448f, -3.00165f),
+     Eigen::Vector3f(5.57566f, 5.74589f, -1.88189f)}};
 
   // Verify each point in the undistorted point cloud
   size_t i = 0;
@@ -1093,17 +1094,17 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   sensor_msgs::PointCloud2ConstIterator<float> iter_distance(pointcloud, "distance");
 
   // Expected undistorted azimuth and distance values
-  std::array<std::array<float, 2>, 10> expected_distance_azimuth = {{
-    {0.0f, 10.0499f},      // points: (10.0f, -2.63462e-08f, 1.0f)
-    {0.77413f, 7.36792f},  // points: (5.05137f, -4.93869f, 2.09267f)
-    {1.55944f, 10.3743f},  // points: (0.112092f, -9.86876f, 3.19715f)
-    {2.35942f, 8.05408f},  // points: (-4.83021f, -4.80022f, 4.30059f)
-    {3.16889f, 11.1711f},  // points: (-9.7743f, 0.266927f, 5.40228f)
-    {3.99196f, 8.40875f},  // points: (-4.69786f, 5.35289f, -4.47032f)
-    {4.74742f, 10.9642f},  // points: (0.365836f, 10.4368f, -3.33969f)
-    {5.48985f, 8.06673f},  // points: (5.44511f, 5.53184f, -2.19585f)
-    {5.8809f, 9.32108f},   // points: (8.52226f, 3.62536f, -1.0538f)
-    {6.10611f, 9.75209f}   // points: (9.59921f, 1.71784f, 0.0862978f)
+  std::array<std::array<float, 2>, 10> expected_azimuth_distance = {{
+    {0.0f, 0.0f},           // points: (0.0f, 0.0f, 0.0f)
+    {5.41248f, 0.121447f},  // points: (0.0512387f, 0.0608269f, 0.0917824f)
+    {6.26993f, 10.1829f},   // points: (10.1106f, 0.134026f, 1.20356f)
+    {0.747926f, 7.421f},    // points: (5.17128f, -4.79604f, 2.30806f)
+    {1.54689f, 10.3103f},   // points: (0.232686f, -9.7275f, 3.40938f)
+    {2.36187f, 8.01421f},   // points: (-4.70281f, -4.65034f, 4.52609f)
+    {3.18569f, 11.1774f},   // points: (-9.64009f, 0.425434f, 5.64106f)
+    {4.02323f, 8.30557f},   // points: (-4.55139f, 5.5241f, -4.21327f)
+    {4.76125f, 11.0645f},   // points: (0.519385f, 10.6188f, -3.06522f)
+    {5.48757f, 8.22771f}    // points: (5.5992f, 5.71475f, -1.91985f)
   }};
 
   size_t i = 0;
@@ -1113,11 +1114,11 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   for (; iter_azimuth != iter_azimuth.end(); ++iter_azimuth, ++iter_distance, ++i) {
     oss << "Point " << i << " - Output azimuth and distance: (" << *iter_azimuth << ", "
         << *iter_distance << ")"
-        << " vs Expected azimuth and distance: (" << expected_distance_azimuth[i][0] << ", "
-        << expected_distance_azimuth[i][1] << ")\n";
+        << " vs Expected azimuth and distance: (" << expected_azimuth_distance[i][0] << ", "
+        << expected_azimuth_distance[i][1] << ")\n";
 
-    EXPECT_NEAR(*iter_azimuth, expected_distance_azimuth[i][0], standard_tolerance_);
-    EXPECT_NEAR(*iter_distance, expected_distance_azimuth[i][1], standard_tolerance_);
+    EXPECT_NEAR(*iter_azimuth, expected_azimuth_distance[i][0], standard_tolerance_);
+    EXPECT_NEAR(*iter_distance, expected_azimuth_distance[i][1], standard_tolerance_);
   }
 
   if (debug_) {
@@ -1152,17 +1153,18 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   sensor_msgs::PointCloud2ConstIterator<float> iter_distance(pointcloud, "distance");
 
   // Expected undistorted azimuth and distance values
-  std::array<std::array<float, 2>, 10> expected_distance_azimuth = {
-    {{1.5708f, 10.0499f},
-     {2.34493f, 7.36792f},
-     {3.13024f, 10.3743f},
-     {3.93021f, 8.05408f},
-     {4.73969f, 11.1711f},
-     {5.56275f, 8.40875f},
-     {0.0350311f, 10.9642f},
-     {0.777465f, 8.06673f},
-     {1.16851f, 9.32108f},
-     {1.39372f, 9.75209f}}};
+  std::array<std::array<float, 2>, 10> expected_azimuth_distance = {{
+    {1.5708f, 0.0f},         // points: (0.0f, 0.0f, 0.0f)
+    {0.70009f, 0.121447f},   // points: (0.0512387f, 0.0608269f, 0.0917824f)
+    {1.55754f, 10.1829f},    // points: (10.1106f, 0.134026f, 1.20356f)
+    {2.31872f, 7.421f},      // points: (5.17128f, -4.79604f, 2.30806f)
+    {3.11768f, 10.3103f},    // points: (0.232686f, -9.7275f, 3.40938f)
+    {3.93267f, 8.01421f},    // points: (-4.70281f, -4.65034f, 4.52609f)
+    {4.75648f, 11.1774f},    // points: (-9.64009f, 0.425434f, 5.64106f)
+    {5.59403f, 8.30557f},    // points: (-4.55139f, 5.5241f, -4.21327f)
+    {0.0488634f, 11.0645f},  // points: (0.519385f, 10.6188f, -3.06522f)
+    {0.775183f, 8.22771f}    // points: (5.5992f, 5.71475f, -1.91985f)
+  }};
 
   size_t i = 0;
   std::ostringstream oss;
@@ -1171,11 +1173,11 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   for (; iter_azimuth != iter_azimuth.end(); ++iter_azimuth, ++iter_distance, ++i) {
     oss << "Point " << i << " - Output azimuth and distance: (" << *iter_azimuth << ", "
         << *iter_distance << ")"
-        << " vs Expected azimuth and distance: (" << expected_distance_azimuth[i][0] << ", "
-        << expected_distance_azimuth[i][1] << ")\n";
+        << " vs Expected azimuth and distance: (" << expected_azimuth_distance[i][0] << ", "
+        << expected_azimuth_distance[i][1] << ")\n";
 
-    EXPECT_NEAR(*iter_azimuth, expected_distance_azimuth[i][0], standard_tolerance_);
-    EXPECT_NEAR(*iter_distance, expected_distance_azimuth[i][1], standard_tolerance_);
+    EXPECT_NEAR(*iter_azimuth, expected_azimuth_distance[i][0], standard_tolerance_);
+    EXPECT_NEAR(*iter_distance, expected_azimuth_distance[i][1], standard_tolerance_);
   }
 
   if (debug_) {

From cd066c405a48a40f413c9369f406a99132eb54a9 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Thu, 12 Sep 2024 19:22:35 +0900
Subject: [PATCH 49/72] chore: fix spell error

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../src/distortion_corrector/distortion_corrector.cpp         | 2 +-
 .../test/test_distortion_corrector_node.cpp                   | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index cc76baafc9d04..6f10c7adbe099 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -262,7 +262,7 @@ std::optional<AngleConversion> DistortionCorrector<T>::tryComputeAngleConversion
       angle_conversion.offset_rad_threshold) {
       RCLCPP_DEBUG(
         node_->get_logger(),
-        "Value of offset_rad is not close to mutiplication of 90 degrees. Iterate to next point "
+        "Value of offset_rad is not close to multiplication of 90 degrees. Iterate to next point "
         "...");
       continue;
     }
diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index 184cc622b4124..710f5e5517f05 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -208,7 +208,7 @@ class DistortionCorrectorTest : public ::testing::Test
         if (hesai_deg == 360) hesai_deg = 0;
         default_azimuths.push_back(hesai_deg * autoware::universe_utils::pi / 180);
       } else if (vendor == AngleCoordinateSystem::CARTESIAN) {
-        // Cartesian coordingates: x-axis is 0 degrees, y-axis is 90 degrees, angle increase in
+        // Cartesian coordinates: x-axis is 0 degrees, y-axis is 90 degrees, angle increase in
         // counterclockwise direction
         default_azimuths.push_back(std::atan2(point.y(), point.x()));
       } else {
@@ -1292,7 +1292,7 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnRandomPointcloud)
 
 TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnBadAzimuthPointcloud)
 {
-  // test pointcloud that has bad zimuth
+  // test pointcloud that can cause the angle conversion to fail.
   // 1. angle difference is 0
   // 2. azimuth value is wrong
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);

From 5560c1d9e1e2d0840eb8e7dc2cbf76d0795dff8a Mon Sep 17 00:00:00 2001
From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com>
Date: Fri, 13 Sep 2024 10:32:01 +0900
Subject: [PATCH 50/72] Update common/autoware_universe_utils/NOTICE

Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com>
---
 common/autoware_universe_utils/NOTICE | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/common/autoware_universe_utils/NOTICE b/common/autoware_universe_utils/NOTICE
index b0b596e9487f2..845c72f5edb36 100644
--- a/common/autoware_universe_utils/NOTICE
+++ b/common/autoware_universe_utils/NOTICE
@@ -1,2 +1,2 @@
 The function 'opencv_fast_atan2' in /autoware/src/universe/autoware.universe/common/autoware_universe_utils/src/math/trigonometry.cpp
-was modified by function 'fastAtan2' in OpenCV project. (https://github.com/opencv/opencv/blob/4.x/modules/core/src/mathfuncs_core.simd.hpp)
+is a modified version of 'fastAtan2' in the OpenCV project. (https://github.com/opencv/opencv/blob/4.x/modules/core/src/mathfuncs_core.simd.hpp)

From 4c521c0d96230a50490281420fce2a0179e34346 Mon Sep 17 00:00:00 2001
From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com>
Date: Fri, 13 Sep 2024 10:38:47 +0900
Subject: [PATCH 51/72] Update
 sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp

Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com>
---
 .../src/distortion_corrector/distortion_corrector_node.cpp      | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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 f2b789fa0d6b0..1d63918db65f3 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
@@ -113,7 +113,7 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointclou
       if (angle_conversion_failure_num_ > failure_tolerance_) {
         throw std::runtime_error(
           "Angle conversion failed more than " + std::to_string(failure_tolerance_) +
-          " times. The node has been interrupted. Please check the LiDAR azimuth coordinates.");
+          " times. Please check the LiDAR azimuth coordinates.");
       }
     }
   }

From 8e26988f249790af9d72f9075b6f6061d7a16729 Mon Sep 17 00:00:00 2001
From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com>
Date: Fri, 13 Sep 2024 10:39:05 +0900
Subject: [PATCH 52/72] Update
 sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp

Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com>
---
 .../src/distortion_corrector/distortion_corrector.cpp           | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index 6f10c7adbe099..6fdc9da30a1a7 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -191,7 +191,7 @@ template <class T>
 std::optional<AngleConversion> DistortionCorrector<T>::tryComputeAngleConversion(
   sensor_msgs::msg::PointCloud2 & pointcloud)
 {
-  // This function try to compute the angle conversion from Cartesian coordinates to LiDAR azimuth
+  // This function tries to compute the angle conversion from Cartesian coordinates to LiDAR azimuth
   // coordinates system
 
   if (!isPointCloudValid(pointcloud)) return std::nullopt;

From 41f086ac34d763220b78ad657c628019c0b9ebc2 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Fri, 13 Sep 2024 16:27:43 +0900
Subject: [PATCH 53/72] chore: use constexpr for threshold

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector/distortion_corrector.hpp             | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
index 3bb0637875b75..4aa9c96f9c350 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -55,8 +55,8 @@ struct AngleConversion
   // offset_rad is restricted to be a multiple of 90, and sign is restricted to be 1 or -1.
   float offset_rad{0};
   float sign{1};
-  float offset_rad_threshold{0.087f};  // (5 / 180) * M_PI
-  float sign_threshold{0.1f};
+  static constexpr float offset_rad_threshold{0.087f};  // (5 / 180) * M_PI
+  static constexpr float sign_threshold{0.1f};
 };
 
 class DistortionCorrectorBase

From a3e5cd81df086d4e1d51d74fdb3aa9d50ae66db1 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Fri, 13 Sep 2024 16:30:53 +0900
Subject: [PATCH 54/72] chore: fix the path of license

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 common/autoware_universe_utils/src/math/trigonometry.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/common/autoware_universe_utils/src/math/trigonometry.cpp b/common/autoware_universe_utils/src/math/trigonometry.cpp
index b6b2c4f854c18..d9345c7a154bf 100644
--- a/common/autoware_universe_utils/src/math/trigonometry.cpp
+++ b/common/autoware_universe_utils/src/math/trigonometry.cpp
@@ -77,7 +77,7 @@ std::pair<float, float> sin_and_cos(float radian)
 // subject to the license terms in the LICENSE file found in the top-level directory of this
 // distribution and at http://opencv.org/license.html.
 // The license can be found in
-// /autoware/src/universe/autoware.universe/common/autoware_universe_utils/third_party_licenses/opencv-license.md
+// common/autoware_universe_utils/third_party_licenses/opencv-license.md
 // and https://github.com/opencv/opencv/blob/master/LICENSE
 
 // Modification:

From 550a6466f4fe034c7f45ecbaa3f14f20951c3a82 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Fri, 13 Sep 2024 16:59:35 +0900
Subject: [PATCH 55/72] chore: explanation for failures

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector/distortion_corrector_node.hpp     | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

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 4e317ee6039cf..a958a36ea3abd 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
@@ -55,8 +55,13 @@ class DistortionCorrectorComponent : public rclcpp::Node
   bool update_azimuth_and_distance_;
 
   std::optional<AngleConversion> angle_conversion_opt_;
+
+  /// @brief 'angle_conversion_failure_num_' counts the number of failures during angle conversion.
   int angle_conversion_failure_num_{0};
-  int failure_tolerance_{20};
+
+  /// @brief 'failure_tolerance_' allows for failures when the input point clouds do not contain
+  /// enough points to calculate angle conversion.
+  static constexpr int failure_tolerance_{20};
 
   std::unique_ptr<DistortionCorrectorBase> distortion_corrector_;
 

From 03354c85ca8ec274c2360466078429d7d782d2e9 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Fri, 13 Sep 2024 17:04:07 +0900
Subject: [PATCH 56/72] chore: use throttle

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector.cpp                   | 18 ++++++++++--------
 1 file changed, 10 insertions(+), 8 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index 6fdc9da30a1a7..8d976586689ce 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -211,8 +211,8 @@ std::optional<AngleConversion> DistortionCorrector<T>::tryComputeAngleConversion
     next_it_y = it_y + 1;
     next_it_azimuth = it_azimuth + 1;
   } else {
-    RCLCPP_WARN(
-      node_->get_logger(),
+    RCLCPP_WARN_STREAM_THROTTLE(
+      node_->get_logger(), *node_->get_clock(), 10000 /* ms */,
       "Current point cloud only has a single point. Could not calculate the angle conversion.");
     return std::nullopt;
   }
@@ -227,8 +227,9 @@ std::optional<AngleConversion> DistortionCorrector<T>::tryComputeAngleConversion
     if (
       abs(*next_it_azimuth - *it_azimuth) == 0 ||
       abs(next_cartesian_rad - current_cartesian_rad) == 0) {
-      RCLCPP_DEBUG(
-        node_->get_logger(), "Angle between two points is 0 degrees. Iterate to next point ...");
+      RCLCPP_DEBUG_STREAM_THROTTLE(
+        node_->get_logger(), *node_->get_clock(), 10000 /* ms */,
+        "Angle between two points is 0 degrees. Iterate to next point ...");
       continue;
     }
 
@@ -249,8 +250,9 @@ std::optional<AngleConversion> DistortionCorrector<T>::tryComputeAngleConversion
     } else if (std::abs(sign + 1.0f) <= angle_conversion.sign_threshold) {
       angle_conversion.sign = -1.0f;
     } else {
-      RCLCPP_DEBUG(
-        node_->get_logger(), "Value of sign is not close to 1 or -1. Iterate to next point ...");
+      RCLCPP_DEBUG_STREAM_THROTTLE(
+        node_->get_logger(), *node_->get_clock(), 10000 /* ms */,
+        "Value of sign is not close to 1 or -1. Iterate to next point ...");
       continue;
     }
 
@@ -260,8 +262,8 @@ std::optional<AngleConversion> DistortionCorrector<T>::tryComputeAngleConversion
     if (
       std::abs(offset_rad - multiple_of_90_degrees * (autoware::universe_utils::pi / 2)) >
       angle_conversion.offset_rad_threshold) {
-      RCLCPP_DEBUG(
-        node_->get_logger(),
+      RCLCPP_DEBUG_STREAM_THROTTLE(
+        node_->get_logger(), *node_->get_clock(), 10000 /* ms */,
         "Value of offset_rad is not close to multiplication of 90 degrees. Iterate to next point "
         "...");
       continue;

From 320e9610d1dd43eeffc04bdfb3accd17009bd97a Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Fri, 13 Sep 2024 17:11:37 +0900
Subject: [PATCH 57/72] chore: fix empty pointcloud function

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../test/test_distortion_corrector_node.cpp            | 10 +---------
 1 file changed, 1 insertion(+), 9 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index 710f5e5517f05..1d47fc65c7b3a 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -221,15 +221,7 @@ class DistortionCorrectorTest : public ::testing::Test
 
   sensor_msgs::msg::PointCloud2 generateEmptyPointCloudMsg(rclcpp::Time stamp)
   {
-    sensor_msgs::msg::PointCloud2 empty_pointcloud_msg;
-    empty_pointcloud_msg.header.stamp = stamp;
-    empty_pointcloud_msg.header.frame_id = "lidar_top";
-    empty_pointcloud_msg.height = 1;
-    empty_pointcloud_msg.is_dense = true;
-    empty_pointcloud_msg.is_bigendian = false;
-    empty_pointcloud_msg.width = 0;
-    empty_pointcloud_msg.row_step = 0;
-
+    auto empty_pointcloud_msg = generatePointCloudMsg(true, stamp, {}, {});
     return empty_pointcloud_msg;
   }
 

From f6be6374320c33f9a3bea6e6f0f58fd9cc010d44 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Tue, 17 Sep 2024 23:02:32 +0900
Subject: [PATCH 58/72] refactor: change camel to snake case

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector.hpp                  |  59 +++---
 .../distortion_corrector_node.hpp             |   7 +-
 .../distortion_corrector.cpp                  |  56 +++---
 .../distortion_corrector_node.cpp             |  22 +--
 .../test/test_distortion_corrector_node.cpp   | 184 +++++++++---------
 5 files changed, 166 insertions(+), 162 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
index 4aa9c96f9c350..65e34787bad71 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -62,21 +62,21 @@ struct AngleConversion
 class DistortionCorrectorBase
 {
 public:
-  virtual bool pointcloudTransformExists() = 0;
-  virtual bool pointcloudTransformNeeded() = 0;
-  virtual std::deque<geometry_msgs::msg::TwistStamped> getTwistQueue() = 0;
-  virtual std::deque<geometry_msgs::msg::Vector3Stamped> getAngularVelocityQueue() = 0;
+  virtual bool pointcloud_transform_exists() = 0;
+  virtual bool pointcloud_transform_needed() = 0;
+  virtual std::deque<geometry_msgs::msg::TwistStamped> get_twist_queue() = 0;
+  virtual std::deque<geometry_msgs::msg::Vector3Stamped> get_angular_velocity_queue() = 0;
 
-  virtual void processTwistMessage(
+  virtual void process_twist_message(
     const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) = 0;
-  virtual void processIMUMessage(
+  virtual void process_imu_message(
     const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) = 0;
-  virtual void setPointCloudTransform(
+  virtual void set_pointcloud_transform(
     const std::string & base_frame, const std::string & lidar_frame) = 0;
   virtual void initialize() = 0;
-  virtual std::optional<AngleConversion> tryComputeAngleConversion(
+  virtual std::optional<AngleConversion> try_compute_angle_conversion(
     sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
-  virtual void undistortPointCloud(
+  virtual void undistort_pointcloud(
     bool use_imu, std::optional<AngleConversion> angle_conversion_opt,
     sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
 };
@@ -95,24 +95,25 @@ class DistortionCorrector : public DistortionCorrectorBase
   std::deque<geometry_msgs::msg::TwistStamped> twist_queue_;
   std::deque<geometry_msgs::msg::Vector3Stamped> angular_velocity_queue_;
 
-  void getIMUTransformation(const std::string & base_frame, const std::string & imu_frame);
-  void enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg);
-  void getTwistAndIMUIterator(
+  void get_imu_transformation(const std::string & base_frame, const std::string & imu_frame);
+  void enqueue_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg);
+  void get_twist_and_imu_iterator(
     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 warnIfTimestampIsTooLate(bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late);
-  void undistortPoint(
+  void warn_if_timestamp_is_too_late(
+    bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late);
+  void undistort_point(
     sensor_msgs::PointCloud2Iterator<float> & it_x, sensor_msgs::PointCloud2Iterator<float> & it_y,
     sensor_msgs::PointCloud2Iterator<float> & it_z,
     std::deque<geometry_msgs::msg::TwistStamped>::iterator & it_twist,
     std::deque<geometry_msgs::msg::Vector3Stamped>::iterator & it_imu, float const & time_offset,
     const bool & is_twist_valid, const bool & is_imu_valid)
   {
-    static_cast<T *>(this)->undistortPointImplementation(
+    static_cast<T *>(this)->undistort_point_implementation(
       it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid);
   };
-  void convertMatrixToTransform(const Eigen::Matrix4f & matrix, tf2::Transform & transform);
+  void convert_matrix_to_transform(const Eigen::Matrix4f & matrix, tf2::Transform & transform);
 
 public:
   explicit DistortionCorrector(rclcpp::Node * node, const bool & has_static_tf_only) : node_(node)
@@ -120,22 +121,22 @@ class DistortionCorrector : public DistortionCorrectorBase
     managed_tf_buffer_ =
       std::make_unique<autoware::universe_utils::ManagedTransformBuffer>(node, has_static_tf_only);
   }
-  bool pointcloudTransformExists() override;
-  bool pointcloudTransformNeeded() override;
-  std::deque<geometry_msgs::msg::TwistStamped> getTwistQueue();
-  std::deque<geometry_msgs::msg::Vector3Stamped> getAngularVelocityQueue();
-  void processTwistMessage(
+  bool pointcloud_transform_exists() override;
+  bool pointcloud_transform_needed() override;
+  std::deque<geometry_msgs::msg::TwistStamped> get_twist_queue();
+  std::deque<geometry_msgs::msg::Vector3Stamped> get_angular_velocity_queue();
+  void process_twist_message(
     const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override;
 
-  void processIMUMessage(
+  void process_imu_message(
     const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override;
-  void undistortPointCloud(
+  void undistort_pointcloud(
     bool use_imu, std::optional<AngleConversion> angle_conversion_opt,
     sensor_msgs::msg::PointCloud2 & pointcloud) override;
-  std::optional<AngleConversion> tryComputeAngleConversion(
+  std::optional<AngleConversion> try_compute_angle_conversion(
     sensor_msgs::msg::PointCloud2 & pointcloud) override;
 
-  bool isPointCloudValid(sensor_msgs::msg::PointCloud2 & pointcloud);
+  bool is_pointcloud_valid(sensor_msgs::msg::PointCloud2 & pointcloud);
 };
 
 class DistortionCorrector2D : public DistortionCorrector<DistortionCorrector2D>
@@ -160,14 +161,14 @@ class DistortionCorrector2D : public DistortionCorrector<DistortionCorrector2D>
   {
   }
   void initialize() override;
-  void undistortPointImplementation(
+  void undistort_point_implementation(
     sensor_msgs::PointCloud2Iterator<float> & it_x, sensor_msgs::PointCloud2Iterator<float> & it_y,
     sensor_msgs::PointCloud2Iterator<float> & it_z,
     std::deque<geometry_msgs::msg::TwistStamped>::iterator & it_twist,
     std::deque<geometry_msgs::msg::Vector3Stamped>::iterator & it_imu, const float & time_offset,
     const bool & is_twist_valid, const bool & is_imu_valid);
 
-  void setPointCloudTransform(
+  void set_pointcloud_transform(
     const std::string & base_frame, const std::string & lidar_frame) override;
 };
 
@@ -190,13 +191,13 @@ class DistortionCorrector3D : public DistortionCorrector<DistortionCorrector3D>
   {
   }
   void initialize() override;
-  void undistortPointImplementation(
+  void undistort_point_implementation(
     sensor_msgs::PointCloud2Iterator<float> & it_x, sensor_msgs::PointCloud2Iterator<float> & it_y,
     sensor_msgs::PointCloud2Iterator<float> & it_z,
     std::deque<geometry_msgs::msg::TwistStamped>::iterator & it_twist,
     std::deque<geometry_msgs::msg::Vector3Stamped>::iterator & it_imu, const float & time_offset,
     const bool & is_twist_valid, const bool & is_imu_valid);
-  void setPointCloudTransform(
+  void set_pointcloud_transform(
     const std::string & base_frame, const std::string & lidar_frame) override;
 };
 
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 a958a36ea3abd..4c86b153b6f2c 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
@@ -65,9 +65,10 @@ class DistortionCorrectorComponent : public rclcpp::Node
 
   std::unique_ptr<DistortionCorrectorBase> distortion_corrector_;
 
-  void onPointCloud(PointCloud2::UniquePtr points_msg);
-  void onTwist(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg);
-  void onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg);
+  void pointcloud_callback(PointCloud2::UniquePtr points_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.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index 8d976586689ce..b4597d2fbe95e 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -26,31 +26,31 @@ namespace autoware::pointcloud_preprocessor
 {
 
 template <class T>
-bool DistortionCorrector<T>::pointcloudTransformExists()
+bool DistortionCorrector<T>::pointcloud_transform_exists()
 {
   return pointcloud_transform_exists_;
 }
 
 template <class T>
-bool DistortionCorrector<T>::pointcloudTransformNeeded()
+bool DistortionCorrector<T>::pointcloud_transform_needed()
 {
   return pointcloud_transform_needed_;
 }
 
 template <class T>
-std::deque<geometry_msgs::msg::TwistStamped> DistortionCorrector<T>::getTwistQueue()
+std::deque<geometry_msgs::msg::TwistStamped> DistortionCorrector<T>::get_twist_queue()
 {
   return twist_queue_;
 }
 
 template <class T>
-std::deque<geometry_msgs::msg::Vector3Stamped> DistortionCorrector<T>::getAngularVelocityQueue()
+std::deque<geometry_msgs::msg::Vector3Stamped> DistortionCorrector<T>::get_angular_velocity_queue()
 {
   return angular_velocity_queue_;
 }
 
 template <class T>
-void DistortionCorrector<T>::processTwistMessage(
+void DistortionCorrector<T>::process_twist_message(
   const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg)
 {
   geometry_msgs::msg::TwistStamped msg;
@@ -73,15 +73,15 @@ void DistortionCorrector<T>::processTwistMessage(
 }
 
 template <class T>
-void DistortionCorrector<T>::processIMUMessage(
+void DistortionCorrector<T>::process_imu_message(
   const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg)
 {
-  getIMUTransformation(base_frame, imu_msg->header.frame_id);
-  enqueueIMU(imu_msg);
+  get_imu_transformation(base_frame, imu_msg->header.frame_id);
+  enqueue_imu(imu_msg);
 }
 
 template <class T>
-void DistortionCorrector<T>::getIMUTransformation(
+void DistortionCorrector<T>::get_imu_transformation(
   const std::string & base_frame, const std::string & imu_frame)
 {
   if (imu_transform_exists_) {
@@ -92,7 +92,7 @@ void DistortionCorrector<T>::getIMUTransformation(
   Eigen::Matrix4f eigen_imu_to_base_link;
   imu_transform_exists_ =
     managed_tf_buffer_->getTransform(base_frame, imu_frame, eigen_imu_to_base_link);
-  convertMatrixToTransform(eigen_imu_to_base_link, tf2_imu_to_base_link);
+  convert_matrix_to_transform(eigen_imu_to_base_link, tf2_imu_to_base_link);
 
   geometry_imu_to_base_link_ptr_ = std::make_shared<geometry_msgs::msg::TransformStamped>();
   geometry_imu_to_base_link_ptr_->transform.rotation =
@@ -100,7 +100,7 @@ void DistortionCorrector<T>::getIMUTransformation(
 }
 
 template <class T>
-void DistortionCorrector<T>::enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg)
+void DistortionCorrector<T>::enqueue_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg)
 {
   geometry_msgs::msg::Vector3Stamped angular_velocity;
   angular_velocity.vector = imu_msg->angular_velocity;
@@ -127,7 +127,7 @@ void DistortionCorrector<T>::enqueueIMU(const sensor_msgs::msg::Imu::ConstShared
 }
 
 template <class T>
-void DistortionCorrector<T>::getTwistAndIMUIterator(
+void DistortionCorrector<T>::get_twist_and_imu_iterator(
   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)
@@ -151,7 +151,7 @@ void DistortionCorrector<T>::getTwistAndIMUIterator(
 }
 
 template <class T>
-bool DistortionCorrector<T>::isPointCloudValid(sensor_msgs::msg::PointCloud2 & pointcloud)
+bool DistortionCorrector<T>::is_pointcloud_valid(sensor_msgs::msg::PointCloud2 & pointcloud)
 {
   if (pointcloud.data.empty()) {
     RCLCPP_WARN_STREAM_THROTTLE(
@@ -188,13 +188,13 @@ bool DistortionCorrector<T>::isPointCloudValid(sensor_msgs::msg::PointCloud2 & p
 }
 
 template <class T>
-std::optional<AngleConversion> DistortionCorrector<T>::tryComputeAngleConversion(
+std::optional<AngleConversion> DistortionCorrector<T>::try_compute_angle_conversion(
   sensor_msgs::msg::PointCloud2 & pointcloud)
 {
   // This function tries to compute the angle conversion from Cartesian coordinates to LiDAR azimuth
   // coordinates system
 
-  if (!isPointCloudValid(pointcloud)) return std::nullopt;
+  if (!is_pointcloud_valid(pointcloud)) return std::nullopt;
 
   AngleConversion angle_conversion;
 
@@ -280,11 +280,11 @@ std::optional<AngleConversion> DistortionCorrector<T>::tryComputeAngleConversion
 }
 
 template <class T>
-void DistortionCorrector<T>::undistortPointCloud(
+void DistortionCorrector<T>::undistort_pointcloud(
   bool use_imu, std::optional<AngleConversion> angle_conversion_opt,
   sensor_msgs::msg::PointCloud2 & pointcloud)
 {
-  if (!isPointCloudValid(pointcloud)) return;
+  if (!is_pointcloud_valid(pointcloud)) return;
   if (twist_queue_.empty()) {
     RCLCPP_WARN_STREAM_THROTTLE(
       node_->get_logger(), *node_->get_clock(), 10000 /* ms */, "Twist queue is empty.");
@@ -305,7 +305,7 @@ void DistortionCorrector<T>::undistortPointCloud(
 
   std::deque<geometry_msgs::msg::TwistStamped>::iterator it_twist;
   std::deque<geometry_msgs::msg::Vector3Stamped>::iterator it_imu;
-  getTwistAndIMUIterator(use_imu, first_point_time_stamp_sec, it_twist, it_imu);
+  get_twist_and_imu_iterator(use_imu, first_point_time_stamp_sec, it_twist, it_imu);
 
   // For performance, do not instantiate `rclcpp::Time` inside of the for-loop
   double twist_stamp = rclcpp::Time(it_twist->header.stamp).seconds();
@@ -353,9 +353,9 @@ void DistortionCorrector<T>::undistortPointCloud(
     float time_offset = static_cast<float>(global_point_stamp - prev_time_stamp_sec);
 
     // Undistort a single point based on the strategy
-    undistortPoint(it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid);
+    undistort_point(it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid);
 
-    if (angle_conversion_opt.has_value() && pointcloudTransformNeeded()) {
+    if (angle_conversion_opt.has_value() && pointcloud_transform_needed()) {
       float cartesian_coordinate_azimuth =
         autoware::universe_utils::opencv_fast_atan2(*it_y, *it_x);
       float updated_azimuth = angle_conversion_opt->offset_rad +
@@ -376,11 +376,11 @@ void DistortionCorrector<T>::undistortPointCloud(
     prev_time_stamp_sec = global_point_stamp;
   }
 
-  warnIfTimestampIsTooLate(is_twist_time_stamp_too_late, is_imu_time_stamp_too_late);
+  warn_if_timestamp_is_too_late(is_twist_time_stamp_too_late, is_imu_time_stamp_too_late);
 }
 
 template <class T>
-void DistortionCorrector<T>::warnIfTimestampIsTooLate(
+void DistortionCorrector<T>::warn_if_timestamp_is_too_late(
   bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late)
 {
   if (is_twist_time_stamp_too_late) {
@@ -397,7 +397,7 @@ void DistortionCorrector<T>::warnIfTimestampIsTooLate(
 }
 
 template <class T>
-void DistortionCorrector<T>::convertMatrixToTransform(
+void DistortionCorrector<T>::convert_matrix_to_transform(
   const Eigen::Matrix4f & matrix, tf2::Transform & transform)
 {
   transform.setOrigin(tf2::Vector3(matrix(0, 3), matrix(1, 3), matrix(2, 3)));
@@ -420,7 +420,7 @@ void DistortionCorrector3D::initialize()
   prev_transformation_matrix_ = Eigen::Matrix4f::Identity();
 }
 
-void DistortionCorrector2D::setPointCloudTransform(
+void DistortionCorrector2D::set_pointcloud_transform(
   const std::string & base_frame, const std::string & lidar_frame)
 {
   if (pointcloud_transform_exists_) {
@@ -430,12 +430,12 @@ void DistortionCorrector2D::setPointCloudTransform(
   Eigen::Matrix4f eigen_lidar_to_base_link;
   pointcloud_transform_exists_ =
     managed_tf_buffer_->getTransform(base_frame, lidar_frame, eigen_lidar_to_base_link);
-  convertMatrixToTransform(eigen_lidar_to_base_link, tf2_lidar_to_base_link_);
+  convert_matrix_to_transform(eigen_lidar_to_base_link, tf2_lidar_to_base_link_);
   tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_.inverse();
   pointcloud_transform_needed_ = base_frame != lidar_frame && pointcloud_transform_exists_;
 }
 
-void DistortionCorrector3D::setPointCloudTransform(
+void DistortionCorrector3D::set_pointcloud_transform(
   const std::string & base_frame, const std::string & lidar_frame)
 {
   if (pointcloud_transform_exists_) {
@@ -448,7 +448,7 @@ void DistortionCorrector3D::setPointCloudTransform(
   pointcloud_transform_needed_ = base_frame != lidar_frame && pointcloud_transform_exists_;
 }
 
-inline void DistortionCorrector2D::undistortPointImplementation(
+inline void DistortionCorrector2D::undistort_point_implementation(
   sensor_msgs::PointCloud2Iterator<float> & it_x, sensor_msgs::PointCloud2Iterator<float> & it_y,
   sensor_msgs::PointCloud2Iterator<float> & it_z,
   std::deque<geometry_msgs::msg::TwistStamped>::iterator & it_twist,
@@ -496,7 +496,7 @@ inline void DistortionCorrector2D::undistortPointImplementation(
   *it_z = static_cast<float>(undistorted_point_tf_.getZ());
 }
 
-inline void DistortionCorrector3D::undistortPointImplementation(
+inline void DistortionCorrector3D::undistort_point_implementation(
   sensor_msgs::PointCloud2Iterator<float> & it_x, sensor_msgs::PointCloud2Iterator<float> & it_y,
   sensor_msgs::PointCloud2Iterator<float> & it_z,
   std::deque<geometry_msgs::msg::TwistStamped>::iterator & it_twist,
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 1d63918db65f3..5516e033cde96 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
@@ -51,13 +51,13 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt
   // Subscriber
   twist_sub_ = this->create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
     "~/input/twist", 10,
-    std::bind(&DistortionCorrectorComponent::onTwist, this, std::placeholders::_1));
+    std::bind(&DistortionCorrectorComponent::twist_callback, this, std::placeholders::_1));
   imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
     "~/input/imu", 10,
-    std::bind(&DistortionCorrectorComponent::onImu, this, std::placeholders::_1));
+    std::bind(&DistortionCorrectorComponent::imu_callback, this, std::placeholders::_1));
   pointcloud_sub_ = this->create_subscription<PointCloud2>(
     "~/input/pointcloud", rclcpp::SensorDataQoS(),
-    std::bind(&DistortionCorrectorComponent::onPointCloud, this, std::placeholders::_1));
+    std::bind(&DistortionCorrectorComponent::pointcloud_callback, this, std::placeholders::_1));
 
   // Setup the distortion corrector
 
@@ -68,22 +68,22 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt
   }
 }
 
-void DistortionCorrectorComponent::onTwist(
+void DistortionCorrectorComponent::twist_callback(
   const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg)
 {
-  distortion_corrector_->processTwistMessage(twist_msg);
+  distortion_corrector_->process_twist_message(twist_msg);
 }
 
-void DistortionCorrectorComponent::onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg)
+void DistortionCorrectorComponent::imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg)
 {
   if (!use_imu_) {
     return;
   }
 
-  distortion_corrector_->processIMUMessage(base_frame_, imu_msg);
+  distortion_corrector_->process_imu_message(base_frame_, imu_msg);
 }
 
-void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointcloud_msg)
+void DistortionCorrectorComponent::pointcloud_callback(PointCloud2::UniquePtr pointcloud_msg)
 {
   stop_watch_ptr_->toc("processing_time", true);
   const auto points_sub_count = undistorted_pointcloud_pub_->get_subscription_count() +
@@ -93,11 +93,11 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointclou
     return;
   }
 
-  distortion_corrector_->setPointCloudTransform(base_frame_, pointcloud_msg->header.frame_id);
+  distortion_corrector_->set_pointcloud_transform(base_frame_, pointcloud_msg->header.frame_id);
   distortion_corrector_->initialize();
 
   if (update_azimuth_and_distance_ && !angle_conversion_opt_.has_value()) {
-    angle_conversion_opt_ = distortion_corrector_->tryComputeAngleConversion(*pointcloud_msg);
+    angle_conversion_opt_ = distortion_corrector_->try_compute_angle_conversion(*pointcloud_msg);
     if (angle_conversion_opt_.has_value()) {
       RCLCPP_INFO(
         this->get_logger(),
@@ -118,7 +118,7 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointclou
     }
   }
 
-  distortion_corrector_->undistortPointCloud(use_imu_, angle_conversion_opt_, *pointcloud_msg);
+  distortion_corrector_->undistort_pointcloud(use_imu_, angle_conversion_opt_, *pointcloud_msg);
 
   if (debug_publisher_) {
     auto pipeline_latency_ms =
diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index 1d47fc65c7b3a..0e4d7333266e3 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -297,7 +297,7 @@ class DistortionCorrectorTest : public ::testing::Test
   {
     auto twist_msgs = generateTwistMsgs(timestamp);
     for (const auto & twist_msg : twist_msgs) {
-      distortion_corrector->processTwistMessage(twist_msg);
+      distortion_corrector->process_twist_message(twist_msg);
     }
   }
 
@@ -307,7 +307,7 @@ class DistortionCorrectorTest : public ::testing::Test
   {
     auto imu_msgs = generateImuMsgs(timestamp);
     for (const auto & imu_msg : imu_msgs) {
-      distortion_corrector->processIMUMessage("base_link", imu_msg);
+      distortion_corrector->process_imu_message("base_link", imu_msg);
     }
   }
 
@@ -348,67 +348,67 @@ class DistortionCorrectorTest : public ::testing::Test
   bool debug_{false};
 };
 
-TEST_F(DistortionCorrectorTest, TestProcessTwistMessage)
+TEST_F(DistortionCorrectorTest, Testprocess_twist_message)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   auto twist_msg = generateTwistMsg(twist_linear_x_, twist_angular_z_, timestamp);
-  distortion_corrector_2d_->processTwistMessage(twist_msg);
+  distortion_corrector_2d_->process_twist_message(twist_msg);
 
-  ASSERT_FALSE(distortion_corrector_2d_->getTwistQueue().empty());
-  EXPECT_EQ(distortion_corrector_2d_->getTwistQueue().front().twist.linear.x, twist_linear_x_);
-  EXPECT_EQ(distortion_corrector_2d_->getTwistQueue().front().twist.angular.z, twist_angular_z_);
+  ASSERT_FALSE(distortion_corrector_2d_->get_twist_queue().empty());
+  EXPECT_EQ(distortion_corrector_2d_->get_twist_queue().front().twist.linear.x, twist_linear_x_);
+  EXPECT_EQ(distortion_corrector_2d_->get_twist_queue().front().twist.angular.z, twist_angular_z_);
 }
 
-TEST_F(DistortionCorrectorTest, TestProcessIMUMessage)
+TEST_F(DistortionCorrectorTest, Testprocess_imu_message)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   auto imu_msg = generateImuMsg(imu_angular_x_, imu_angular_y_, imu_angular_z_, timestamp);
-  distortion_corrector_2d_->processIMUMessage("base_link", imu_msg);
+  distortion_corrector_2d_->process_imu_message("base_link", imu_msg);
 
-  ASSERT_FALSE(distortion_corrector_2d_->getAngularVelocityQueue().empty());
+  ASSERT_FALSE(distortion_corrector_2d_->get_angular_velocity_queue().empty());
   EXPECT_NEAR(
-    distortion_corrector_2d_->getAngularVelocityQueue().front().vector.z, -0.03159,
+    distortion_corrector_2d_->get_angular_velocity_queue().front().vector.z, -0.03159,
     standard_tolerance_);
 }
 
-TEST_F(DistortionCorrectorTest, TestIsPointCloudValid)
+TEST_F(DistortionCorrectorTest, Testis_pointcloud_valid)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
 
   auto [default_points, default_azimuths] =
     generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
   auto pointcloud = generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
-  auto result = distortion_corrector_2d_->isPointCloudValid(pointcloud);
+  auto result = distortion_corrector_2d_->is_pointcloud_valid(pointcloud);
   EXPECT_TRUE(result);
 
   // input empty pointcloud
   auto empty_pointcloud = generateEmptyPointCloudMsg(timestamp);
-  result = distortion_corrector_2d_->isPointCloudValid(empty_pointcloud);
+  result = distortion_corrector_2d_->is_pointcloud_valid(empty_pointcloud);
   EXPECT_FALSE(result);
 }
 
-TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithBaseLink)
+TEST_F(DistortionCorrectorTest, Testset_pointcloud_transformWithBaseLink)
 {
-  distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
-  EXPECT_TRUE(distortion_corrector_2d_->pointcloudTransformExists());
-  EXPECT_FALSE(distortion_corrector_2d_->pointcloudTransformNeeded());
+  distortion_corrector_2d_->set_pointcloud_transform("base_link", "base_link");
+  EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_exists());
+  EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_needed());
 }
 
-TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithLidarFrame)
+TEST_F(DistortionCorrectorTest, Testset_pointcloud_transformWithLidarFrame)
 {
-  distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
-  EXPECT_TRUE(distortion_corrector_2d_->pointcloudTransformExists());
-  EXPECT_TRUE(distortion_corrector_2d_->pointcloudTransformNeeded());
+  distortion_corrector_2d_->set_pointcloud_transform("base_link", "lidar_top");
+  EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_exists());
+  EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_needed());
 }
 
-TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithMissingFrame)
+TEST_F(DistortionCorrectorTest, Testset_pointcloud_transformWithMissingFrame)
 {
-  distortion_corrector_2d_->setPointCloudTransform("base_link", "missing_lidar_frame");
-  EXPECT_FALSE(distortion_corrector_2d_->pointcloudTransformExists());
-  EXPECT_FALSE(distortion_corrector_2d_->pointcloudTransformNeeded());
+  distortion_corrector_2d_->set_pointcloud_transform("base_link", "missing_lidar_frame");
+  EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_exists());
+  EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_needed());
 }
 
-TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyTwist)
+TEST_F(DistortionCorrectorTest, Testundistort_pointcloudWithEmptyTwist)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   // Generate the point cloud message
@@ -418,7 +418,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyTwist)
 
   // Process empty twist queue
   distortion_corrector_2d_->initialize();
-  distortion_corrector_2d_->undistortPointCloud(false, std::nullopt, pointcloud);
+  distortion_corrector_2d_->undistort_pointcloud(false, std::nullopt, pointcloud);
 
   // Verify the point cloud is not changed
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
@@ -448,7 +448,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyTwist)
   }
 }
 
-TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyPointCloud)
+TEST_F(DistortionCorrectorTest, Testundistort_pointcloudWithEmptyPointCloud)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   // Generate and process multiple twist messages
@@ -458,14 +458,14 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyPointCloud)
   auto empty_pointcloud = generateEmptyPointCloudMsg(timestamp);
   // Process empty point cloud
   distortion_corrector_2d_->initialize();
-  distortion_corrector_2d_->undistortPointCloud(true, std::nullopt, empty_pointcloud);
+  distortion_corrector_2d_->undistort_pointcloud(true, std::nullopt, empty_pointcloud);
 
   // Verify the point cloud is still empty
   EXPECT_EQ(empty_pointcloud.width, 0);
   EXPECT_EQ(empty_pointcloud.row_step, 0);
 }
 
-TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink)
+TEST_F(DistortionCorrectorTest, Testundistort_pointcloud2dWithoutImuInBaseLink)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -478,8 +478,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink)
 
   // Test using only twist
   distortion_corrector_2d_->initialize();
-  distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_2d_->undistortPointCloud(false, std::nullopt, pointcloud);
+  distortion_corrector_2d_->set_pointcloud_transform("base_link", "base_link");
+  distortion_corrector_2d_->undistort_pointcloud(false, std::nullopt, pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
@@ -510,7 +510,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink)
   }
 }
 
-TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink)
+TEST_F(DistortionCorrectorTest, Testundistort_pointcloud2dWithImuInBaseLink)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -525,8 +525,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink)
   generateAndProcessIMUMsgs(distortion_corrector_2d_, timestamp);
 
   distortion_corrector_2d_->initialize();
-  distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_2d_->undistortPointCloud(true, std::nullopt, pointcloud);
+  distortion_corrector_2d_->set_pointcloud_transform("base_link", "base_link");
+  distortion_corrector_2d_->undistort_pointcloud(true, std::nullopt, pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
@@ -557,7 +557,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink)
   }
 }
 
-TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame)
+TEST_F(DistortionCorrectorTest, Testundistort_pointcloud2dWithImuInLidarFrame)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -572,8 +572,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame)
   generateAndProcessIMUMsgs(distortion_corrector_2d_, timestamp);
 
   distortion_corrector_2d_->initialize();
-  distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
-  distortion_corrector_2d_->undistortPointCloud(true, std::nullopt, pointcloud);
+  distortion_corrector_2d_->set_pointcloud_transform("base_link", "lidar_top");
+  distortion_corrector_2d_->undistort_pointcloud(true, std::nullopt, pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
@@ -607,7 +607,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame)
   }
 }
 
-TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink)
+TEST_F(DistortionCorrectorTest, Testundistort_pointcloud3dWithoutImuInBaseLink)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -620,8 +620,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink)
 
   // Test using only twist
   distortion_corrector_3d_->initialize();
-  distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_3d_->undistortPointCloud(false, std::nullopt, pointcloud);
+  distortion_corrector_3d_->set_pointcloud_transform("base_link", "base_link");
+  distortion_corrector_3d_->undistort_pointcloud(false, std::nullopt, pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
@@ -652,7 +652,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink)
   }
 }
 
-TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink)
+TEST_F(DistortionCorrectorTest, Testundistort_pointcloud3dWithImuInBaseLink)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -667,8 +667,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink)
   generateAndProcessIMUMsgs(distortion_corrector_3d_, timestamp);
 
   distortion_corrector_3d_->initialize();
-  distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_3d_->undistortPointCloud(true, std::nullopt, pointcloud);
+  distortion_corrector_3d_->set_pointcloud_transform("base_link", "base_link");
+  distortion_corrector_3d_->undistort_pointcloud(true, std::nullopt, pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
@@ -702,7 +702,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink)
   }
 }
 
-TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame)
+TEST_F(DistortionCorrectorTest, Testundistort_pointcloud3dWithImuInLidarFrame)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -717,8 +717,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame)
   generateAndProcessIMUMsgs(distortion_corrector_3d_, timestamp);
 
   distortion_corrector_3d_->initialize();
-  distortion_corrector_3d_->setPointCloudTransform("base_link", "lidar_top");
-  distortion_corrector_3d_->undistortPointCloud(true, std::nullopt, pointcloud);
+  distortion_corrector_3d_->set_pointcloud_transform("base_link", "lidar_top");
+  distortion_corrector_3d_->undistort_pointcloud(true, std::nullopt, pointcloud);
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(pointcloud, "y");
@@ -751,7 +751,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame)
   }
 }
 
-TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion)
+TEST_F(DistortionCorrectorTest, Testundistort_pointcloudWithPureLinearMotion)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   auto [default_points, default_azimuths] =
@@ -764,15 +764,15 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion)
   // Generate and process a single twist message with constant linear velocity
   auto twist_msg = generateTwistMsg(1.0, 0.0, timestamp);
 
-  distortion_corrector_2d_->processTwistMessage(twist_msg);
+  distortion_corrector_2d_->process_twist_message(twist_msg);
   distortion_corrector_2d_->initialize();
-  distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_2d_->undistortPointCloud(false, std::nullopt, test2d_pointcloud);
+  distortion_corrector_2d_->set_pointcloud_transform("base_link", "base_link");
+  distortion_corrector_2d_->undistort_pointcloud(false, std::nullopt, test2d_pointcloud);
 
-  distortion_corrector_3d_->processTwistMessage(twist_msg);
+  distortion_corrector_3d_->process_twist_message(twist_msg);
   distortion_corrector_3d_->initialize();
-  distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_3d_->undistortPointCloud(false, std::nullopt, test3d_pointcloud);
+  distortion_corrector_3d_->set_pointcloud_transform("base_link", "base_link");
+  distortion_corrector_3d_->undistort_pointcloud(false, std::nullopt, test3d_pointcloud);
 
   // Generate expected point cloud for testing
   auto expected_pointcloud =
@@ -842,7 +842,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion)
   }
 }
 
-TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion)
+TEST_F(DistortionCorrectorTest, Testundistort_pointcloudWithPureRotationalMotion)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   auto [default_points, default_azimuths] =
@@ -855,15 +855,15 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion)
   // Generate and process a single twist message with constant angular velocity
   auto twist_msg = generateTwistMsg(0.0, 0.1, timestamp);
 
-  distortion_corrector_2d_->processTwistMessage(twist_msg);
+  distortion_corrector_2d_->process_twist_message(twist_msg);
   distortion_corrector_2d_->initialize();
-  distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_2d_->undistortPointCloud(false, std::nullopt, test2d_pointcloud);
+  distortion_corrector_2d_->set_pointcloud_transform("base_link", "base_link");
+  distortion_corrector_2d_->undistort_pointcloud(false, std::nullopt, test2d_pointcloud);
 
-  distortion_corrector_3d_->processTwistMessage(twist_msg);
+  distortion_corrector_3d_->process_twist_message(twist_msg);
   distortion_corrector_3d_->initialize();
-  distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link");
-  distortion_corrector_3d_->undistortPointCloud(false, std::nullopt, test3d_pointcloud);
+  distortion_corrector_3d_->set_pointcloud_transform("base_link", "base_link");
+  distortion_corrector_3d_->undistort_pointcloud(false, std::nullopt, test3d_pointcloud);
 
   // Generate expected point cloud for testing
   auto expected_pointcloud =
@@ -946,10 +946,10 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion)
   }
 }
 
-TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdatingAzimuthAndDistance)
+TEST_F(DistortionCorrectorTest, Testundistort_pointcloudNotUpdatingAzimuthAndDistance)
 {
   // Test the case when the cloud will not update the azimuth and distance values
-  // 1. when pointcloud is in base_link (pointcloudTransformNeeded() is false)
+  // 1. when pointcloud is in base_link (pointcloud_transform_needed() is false)
 
   // Generate the point cloud message in base_link
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -965,11 +965,11 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdatingAzimuthAndDist
   generateAndProcessIMUMsgs(distortion_corrector_2d_, timestamp);
 
   distortion_corrector_2d_->initialize();
-  distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
+  distortion_corrector_2d_->set_pointcloud_transform("base_link", "base_link");
   auto angle_conversion_opt =
-    distortion_corrector_2d_->tryComputeAngleConversion(pointcloud_base_link);
+    distortion_corrector_2d_->try_compute_angle_conversion(pointcloud_base_link);
 
-  distortion_corrector_2d_->undistortPointCloud(true, angle_conversion_opt, pointcloud_base_link);
+  distortion_corrector_2d_->undistort_pointcloud(true, angle_conversion_opt, pointcloud_base_link);
 
   auto original_pointcloud_base_link =
     generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
@@ -1005,8 +1005,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdatingAzimuthAndDist
   }
 
   // Test the case when the cloud will not update the azimuth and distance values
-  // 2. when the return value of tryComputeAngleConversion is std::nullopt (couldn't find the angle
-  // conversion)
+  // 2. when the return value of try_compute_angle_conversion is std::nullopt (couldn't find the
+  // angle conversion)
 
   // Generate the point cloud message in sensor frame
   auto pointcloud_lidar_top =
@@ -1019,10 +1019,10 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdatingAzimuthAndDist
   generateAndProcessIMUMsgs(distortion_corrector_2d_, timestamp);
 
   distortion_corrector_2d_->initialize();
-  distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
+  distortion_corrector_2d_->set_pointcloud_transform("base_link", "lidar_top");
 
   angle_conversion_opt = std::nullopt;
-  distortion_corrector_2d_->undistortPointCloud(true, angle_conversion_opt, pointcloud_lidar_top);
+  distortion_corrector_2d_->undistort_pointcloud(true, angle_conversion_opt, pointcloud_lidar_top);
 
   auto original_pointcloud_lidar_top =
     generatePointCloudMsg(true, timestamp, default_points, default_azimuths);
@@ -1059,7 +1059,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdatingAzimuthAndDist
   }
 }
 
-TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceInVelodyne)
+TEST_F(DistortionCorrectorTest, Testundistort_pointcloudUpdateAzimuthAndDistanceInVelodyne)
 {
   // Generate the point cloud message in sensor frame
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1074,10 +1074,10 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   generateAndProcessIMUMsgs(distortion_corrector_2d_, timestamp);
 
   distortion_corrector_2d_->initialize();
-  distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
+  distortion_corrector_2d_->set_pointcloud_transform("base_link", "lidar_top");
 
-  auto angle_conversion_opt = distortion_corrector_2d_->tryComputeAngleConversion(pointcloud);
-  distortion_corrector_2d_->undistortPointCloud(true, angle_conversion_opt, pointcloud);
+  auto angle_conversion_opt = distortion_corrector_2d_->try_compute_angle_conversion(pointcloud);
+  distortion_corrector_2d_->undistort_pointcloud(true, angle_conversion_opt, pointcloud);
 
   auto original_pointcloud =
     generatePointCloudMsg(true, timestamp, default_points, default_azimuths);
@@ -1118,7 +1118,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   }
 }
 
-TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceInHesai)
+TEST_F(DistortionCorrectorTest, Testundistort_pointcloudUpdateAzimuthAndDistanceInHesai)
 {
   // Generate the point cloud message in sensor frame
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1133,10 +1133,10 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   generateAndProcessIMUMsgs(distortion_corrector_2d_, timestamp);
 
   distortion_corrector_2d_->initialize();
-  distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");
+  distortion_corrector_2d_->set_pointcloud_transform("base_link", "lidar_top");
 
-  auto angle_conversion_opt = distortion_corrector_2d_->tryComputeAngleConversion(pointcloud);
-  distortion_corrector_2d_->undistortPointCloud(true, angle_conversion_opt, pointcloud);
+  auto angle_conversion_opt = distortion_corrector_2d_->try_compute_angle_conversion(pointcloud);
+  distortion_corrector_2d_->undistort_pointcloud(true, angle_conversion_opt, pointcloud);
 
   auto original_pointcloud =
     generatePointCloudMsg(true, timestamp, default_points, default_azimuths);
@@ -1177,18 +1177,19 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceI
   }
 }
 
-TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnEmptyPointcloud)
+TEST_F(DistortionCorrectorTest, Testtry_compute_angle_conversionOnEmptyPointcloud)
 {
   // test empty pointcloud
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
 
   auto empty_pointcloud = generateEmptyPointCloudMsg(timestamp);
-  auto angle_conversion_opt = distortion_corrector_2d_->tryComputeAngleConversion(empty_pointcloud);
+  auto angle_conversion_opt =
+    distortion_corrector_2d_->try_compute_angle_conversion(empty_pointcloud);
 
   EXPECT_FALSE(angle_conversion_opt.has_value());
 }
 
-TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnVelodynePointcloud)
+TEST_F(DistortionCorrectorTest, Testtry_compute_angle_conversionOnVelodynePointcloud)
 {
   // test velodyne pointcloud (x-axis: 0 degree, y-axis: 270 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1204,14 +1205,14 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnVelodynePointclou
   auto velodyne_pointcloud =
     generatePointCloudMsg(true, timestamp, velodyne_points, velodyne_azimuths);
   auto angle_conversion_opt =
-    distortion_corrector_2d_->tryComputeAngleConversion(velodyne_pointcloud);
+    distortion_corrector_2d_->try_compute_angle_conversion(velodyne_pointcloud);
   EXPECT_TRUE(angle_conversion_opt.has_value());
 
   EXPECT_EQ(angle_conversion_opt->sign, -1);
   EXPECT_NEAR(angle_conversion_opt->offset_rad, 0, standard_tolerance_);
 }
 
-TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnHesaiPointcloud)
+TEST_F(DistortionCorrectorTest, Testtry_compute_angle_conversionOnHesaiPointcloud)
 {
   // test hesai pointcloud (x-axis: 90 degree, y-axis: 0 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1225,7 +1226,8 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnHesaiPointcloud)
     autoware::universe_utils::pi};
 
   auto hesai_pointcloud = generatePointCloudMsg(true, timestamp, hesai_points, hesai_azimuths);
-  auto angle_conversion_opt = distortion_corrector_2d_->tryComputeAngleConversion(hesai_pointcloud);
+  auto angle_conversion_opt =
+    distortion_corrector_2d_->try_compute_angle_conversion(hesai_pointcloud);
 
   EXPECT_TRUE(angle_conversion_opt.has_value());
   EXPECT_EQ(angle_conversion_opt->sign, -1);
@@ -1233,7 +1235,7 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnHesaiPointcloud)
     angle_conversion_opt->offset_rad, autoware::universe_utils::pi / 2, standard_tolerance_);
 }
 
-TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionCartesianPointcloud)
+TEST_F(DistortionCorrectorTest, Testtry_compute_angle_conversionCartesianPointcloud)
 {
   // test pointcloud that use cartesian coordinate for azimuth (x-axis: 0 degree, y-axis: 90 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1251,14 +1253,14 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionCartesianPointcloud
   auto cartesian_pointcloud =
     generatePointCloudMsg(true, timestamp, cartesian_points, cartesian_azimuths);
   auto angle_conversion_opt =
-    distortion_corrector_2d_->tryComputeAngleConversion(cartesian_pointcloud);
+    distortion_corrector_2d_->try_compute_angle_conversion(cartesian_pointcloud);
 
   EXPECT_TRUE(angle_conversion_opt.has_value());
   EXPECT_EQ(angle_conversion_opt->sign, 1);
   EXPECT_NEAR(angle_conversion_opt->offset_rad, 0, standard_tolerance_);
 }
 
-TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnRandomPointcloud)
+TEST_F(DistortionCorrectorTest, Testtry_compute_angle_conversionOnRandomPointcloud)
 {
   // test pointcloud that use coordinate (x-axis: 270 degree, y-axis: 0 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1274,7 +1276,7 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnRandomPointcloud)
     0, autoware::universe_utils::pi * 3 / 2, autoware::universe_utils::pi * 7 / 4};
 
   auto pointcloud = generatePointCloudMsg(true, timestamp, points, azimuths);
-  auto angle_conversion_opt = distortion_corrector_2d_->tryComputeAngleConversion(pointcloud);
+  auto angle_conversion_opt = distortion_corrector_2d_->try_compute_angle_conversion(pointcloud);
 
   EXPECT_TRUE(angle_conversion_opt.has_value());
   EXPECT_EQ(angle_conversion_opt->sign, 1);
@@ -1282,7 +1284,7 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnRandomPointcloud)
     angle_conversion_opt->offset_rad, autoware::universe_utils::pi * 3 / 2, standard_tolerance_);
 }
 
-TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnBadAzimuthPointcloud)
+TEST_F(DistortionCorrectorTest, Testtry_compute_angle_conversionOnBadAzimuthPointcloud)
 {
   // test pointcloud that can cause the angle conversion to fail.
   // 1. angle difference is 0
@@ -1301,7 +1303,7 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnBadAzimuthPointcl
   std::vector<float> azimuths = {0, 0, autoware::universe_utils::pi};
 
   auto pointcloud = generatePointCloudMsg(true, timestamp, points, azimuths);
-  auto angle_conversion_opt = distortion_corrector_2d_->tryComputeAngleConversion(pointcloud);
+  auto angle_conversion_opt = distortion_corrector_2d_->try_compute_angle_conversion(pointcloud);
 
   EXPECT_FALSE(angle_conversion_opt.has_value());
 }

From 8d4bcd7c080686392a5fe579e048e26c144b5b11 Mon Sep 17 00:00:00 2001
From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com>
Date: Thu, 19 Sep 2024 12:44:40 +0900
Subject: [PATCH 59/72] Update
 sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp

Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com>
---
 .../distortion_corrector/distortion_corrector_node.hpp          | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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 4c86b153b6f2c..860b52bada783 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
@@ -56,7 +56,7 @@ class DistortionCorrectorComponent : public rclcpp::Node
 
   std::optional<AngleConversion> angle_conversion_opt_;
 
-  /// @brief 'angle_conversion_failure_num_' counts the number of failures during angle conversion.
+  /// @brief Counts the number of pointclouds for which angle conversion failed.
   int angle_conversion_failure_num_{0};
 
   /// @brief 'failure_tolerance_' allows for failures when the input point clouds do not contain

From 4d5883962ec8d734b7ab0423dcab9570c295105c Mon Sep 17 00:00:00 2001
From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com>
Date: Thu, 19 Sep 2024 12:45:15 +0900
Subject: [PATCH 60/72] Update
 sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp

Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com>
---
 .../distortion_corrector/distortion_corrector_node.hpp         | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

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 860b52bada783..304f3a0ef53ed 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
@@ -59,8 +59,7 @@ class DistortionCorrectorComponent : public rclcpp::Node
   /// @brief Counts the number of pointclouds for which angle conversion failed.
   int angle_conversion_failure_num_{0};
 
-  /// @brief 'failure_tolerance_' allows for failures when the input point clouds do not contain
-  /// enough points to calculate angle conversion.
+  /// @brief The maximum number of failed angle conversions due to pointclouds not containing enough points before throwing an error.
   static constexpr int failure_tolerance_{20};
 
   std::unique_ptr<DistortionCorrectorBase> distortion_corrector_;

From 1f144d32989c33dedc8aee98afb655eb15147236 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Thu, 19 Sep 2024 03:48:43 +0000
Subject: [PATCH 61/72] style(pre-commit): autofix

---
 .../distortion_corrector/distortion_corrector_node.hpp         | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

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 304f3a0ef53ed..11c6e3849c807 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
@@ -59,7 +59,8 @@ class DistortionCorrectorComponent : public rclcpp::Node
   /// @brief Counts the number of pointclouds for which angle conversion failed.
   int angle_conversion_failure_num_{0};
 
-  /// @brief The maximum number of failed angle conversions due to pointclouds not containing enough points before throwing an error.
+  /// @brief The maximum number of failed angle conversions due to pointclouds not containing enough
+  /// points before throwing an error.
   static constexpr int failure_tolerance_{20};
 
   std::unique_ptr<DistortionCorrectorBase> distortion_corrector_;

From f58307d0baab4e7a835718b4258f428654d7272a Mon Sep 17 00:00:00 2001
From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com>
Date: Thu, 19 Sep 2024 15:03:02 +0900
Subject: [PATCH 62/72] Update
 sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp

Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com>
---
 .../test/test_distortion_corrector_node.cpp               | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index 0e4d7333266e3..1c22c557b4c6c 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -172,7 +172,7 @@ class DistortionCorrectorTest : public ::testing::Test
   }
 
   std::tuple<std::vector<Eigen::Vector3f>, std::vector<float>> generateDefaultPointcloud(
-    AngleCoordinateSystem vendor)
+    AngleCoordinateSystem coordinate_system)
   {
     // Generate all combinations of signs { -, 0, + } x { -, 0, + } for x and y.
     // Also include the case of (0, 0 ,0)
@@ -191,7 +191,7 @@ class DistortionCorrectorTest : public ::testing::Test
 
     std::vector<float> default_azimuths;
     for (const auto & point : default_points) {
-      if (vendor == AngleCoordinateSystem::VELODYNE) {
+      if (coordinate_system == AngleCoordinateSystem::VELODYNE) {
         // velodyne coordinates: x-axis is 0 degrees, y-axis is 270 degrees, angle increase in
         // clockwise direction
         float cartesian_deg = std::atan2(point.y(), point.x()) * 180 / autoware::universe_utils::pi;
@@ -199,7 +199,7 @@ class DistortionCorrectorTest : public ::testing::Test
         float velodyne_deg = 360 - cartesian_deg;
         if (velodyne_deg == 360) velodyne_deg = 0;
         default_azimuths.push_back(velodyne_deg * autoware::universe_utils::pi / 180);
-      } else if (vendor == AngleCoordinateSystem::HESAI) {
+      } else if (coordinate_system == AngleCoordinateSystem::HESAI) {
         // hesai coordinates: y-axis is 0 degrees, x-axis is 90 degrees, angle increase in clockwise
         // direction
         float cartesian_deg = std::atan2(point.y(), point.x()) * 180 / autoware::universe_utils::pi;
@@ -207,7 +207,7 @@ class DistortionCorrectorTest : public ::testing::Test
         float hesai_deg = 90 - cartesian_deg < 0 ? 90 - cartesian_deg + 360 : 90 - cartesian_deg;
         if (hesai_deg == 360) hesai_deg = 0;
         default_azimuths.push_back(hesai_deg * autoware::universe_utils::pi / 180);
-      } else if (vendor == AngleCoordinateSystem::CARTESIAN) {
+      } else if (coordinate_system == AngleCoordinateSystem::CARTESIAN) {
         // Cartesian coordinates: x-axis is 0 degrees, y-axis is 90 degrees, angle increase in
         // counterclockwise direction
         default_azimuths.push_back(std::atan2(point.y(), point.x()));

From 896d843b0f527a5b203f4baacd1e3cc214e57638 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Thu, 19 Sep 2024 17:29:41 +0900
Subject: [PATCH 63/72] refactor: refactor virtual function in base class

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector.hpp                  | 89 +++++++++----------
 .../distortion_corrector.cpp                  | 85 ++++++++----------
 2 files changed, 79 insertions(+), 95 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
index 65e34787bad71..047e93e9208f6 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -61,29 +61,6 @@ struct AngleConversion
 
 class DistortionCorrectorBase
 {
-public:
-  virtual bool pointcloud_transform_exists() = 0;
-  virtual bool pointcloud_transform_needed() = 0;
-  virtual std::deque<geometry_msgs::msg::TwistStamped> get_twist_queue() = 0;
-  virtual std::deque<geometry_msgs::msg::Vector3Stamped> get_angular_velocity_queue() = 0;
-
-  virtual void process_twist_message(
-    const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) = 0;
-  virtual void process_imu_message(
-    const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) = 0;
-  virtual void set_pointcloud_transform(
-    const std::string & base_frame, const std::string & lidar_frame) = 0;
-  virtual void initialize() = 0;
-  virtual std::optional<AngleConversion> try_compute_angle_conversion(
-    sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
-  virtual void undistort_pointcloud(
-    bool use_imu, std::optional<AngleConversion> angle_conversion_opt,
-    sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
-};
-
-template <class T>
-class DistortionCorrector : public DistortionCorrectorBase
-{
 protected:
   geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr_;
   bool pointcloud_transform_needed_{false};
@@ -103,40 +80,61 @@ class DistortionCorrector : public DistortionCorrectorBase
     std::deque<geometry_msgs::msg::Vector3Stamped>::iterator & it_imu);
   void warn_if_timestamp_is_too_late(
     bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late);
-  void undistort_point(
-    sensor_msgs::PointCloud2Iterator<float> & it_x, sensor_msgs::PointCloud2Iterator<float> & it_y,
-    sensor_msgs::PointCloud2Iterator<float> & it_z,
-    std::deque<geometry_msgs::msg::TwistStamped>::iterator & it_twist,
-    std::deque<geometry_msgs::msg::Vector3Stamped>::iterator & it_imu, float const & time_offset,
-    const bool & is_twist_valid, const bool & is_imu_valid)
-  {
-    static_cast<T *>(this)->undistort_point_implementation(
-      it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid);
-  };
   void convert_matrix_to_transform(const Eigen::Matrix4f & matrix, tf2::Transform & transform);
 
 public:
-  explicit DistortionCorrector(rclcpp::Node * node, const bool & has_static_tf_only) : node_(node)
+  explicit DistortionCorrectorBase(rclcpp::Node * node, const bool & has_static_tf_only)
+  : node_(node)
   {
     managed_tf_buffer_ =
       std::make_unique<autoware::universe_utils::ManagedTransformBuffer>(node, has_static_tf_only);
   }
-  bool pointcloud_transform_exists() override;
-  bool pointcloud_transform_needed() override;
+  bool pointcloud_transform_exists();
+  bool pointcloud_transform_needed();
   std::deque<geometry_msgs::msg::TwistStamped> get_twist_queue();
   std::deque<geometry_msgs::msg::Vector3Stamped> get_angular_velocity_queue();
   void process_twist_message(
-    const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override;
+    const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg);
 
   void process_imu_message(
-    const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override;
+    const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg);
+
+  std::optional<AngleConversion> try_compute_angle_conversion(
+    sensor_msgs::msg::PointCloud2 & pointcloud);
+
+  bool is_pointcloud_valid(sensor_msgs::msg::PointCloud2 & pointcloud);
+
+  virtual void set_pointcloud_transform(
+    const std::string & base_frame, const std::string & lidar_frame) = 0;
+  virtual void initialize() = 0;
+  virtual void undistort_pointcloud(
+    bool use_imu, std::optional<AngleConversion> angle_conversion_opt,
+    sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
+};
+
+template <class T>
+class DistortionCorrector : public DistortionCorrectorBase
+{
+public:
+  explicit DistortionCorrector(rclcpp::Node * node, const bool & has_static_tf_only)
+  : DistortionCorrectorBase(node, has_static_tf_only)
+  {
+  }
+
   void undistort_pointcloud(
     bool use_imu, std::optional<AngleConversion> angle_conversion_opt,
     sensor_msgs::msg::PointCloud2 & pointcloud) override;
-  std::optional<AngleConversion> try_compute_angle_conversion(
-    sensor_msgs::msg::PointCloud2 & pointcloud) override;
 
-  bool is_pointcloud_valid(sensor_msgs::msg::PointCloud2 & pointcloud);
+  void undistort_point(
+    sensor_msgs::PointCloud2Iterator<float> & it_x, sensor_msgs::PointCloud2Iterator<float> & it_y,
+    sensor_msgs::PointCloud2Iterator<float> & it_z,
+    std::deque<geometry_msgs::msg::TwistStamped>::iterator & it_twist,
+    std::deque<geometry_msgs::msg::Vector3Stamped>::iterator & it_imu, float const & time_offset,
+    const bool & is_twist_valid, const bool & is_imu_valid)
+  {
+    static_cast<T *>(this)->undistort_point_implementation(
+      it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid);
+  };
 };
 
 class DistortionCorrector2D : public DistortionCorrector<DistortionCorrector2D>
@@ -161,15 +159,14 @@ class DistortionCorrector2D : public DistortionCorrector<DistortionCorrector2D>
   {
   }
   void initialize() override;
+  void set_pointcloud_transform(
+    const std::string & base_frame, const std::string & lidar_frame) override;
   void undistort_point_implementation(
     sensor_msgs::PointCloud2Iterator<float> & it_x, sensor_msgs::PointCloud2Iterator<float> & it_y,
     sensor_msgs::PointCloud2Iterator<float> & it_z,
     std::deque<geometry_msgs::msg::TwistStamped>::iterator & it_twist,
     std::deque<geometry_msgs::msg::Vector3Stamped>::iterator & it_imu, const float & time_offset,
     const bool & is_twist_valid, const bool & is_imu_valid);
-
-  void set_pointcloud_transform(
-    const std::string & base_frame, const std::string & lidar_frame) override;
 };
 
 class DistortionCorrector3D : public DistortionCorrector<DistortionCorrector3D>
@@ -191,14 +188,14 @@ class DistortionCorrector3D : public DistortionCorrector<DistortionCorrector3D>
   {
   }
   void initialize() override;
+  void set_pointcloud_transform(
+    const std::string & base_frame, const std::string & lidar_frame) override;
   void undistort_point_implementation(
     sensor_msgs::PointCloud2Iterator<float> & it_x, sensor_msgs::PointCloud2Iterator<float> & it_y,
     sensor_msgs::PointCloud2Iterator<float> & it_z,
     std::deque<geometry_msgs::msg::TwistStamped>::iterator & it_twist,
     std::deque<geometry_msgs::msg::Vector3Stamped>::iterator & it_imu, const float & time_offset,
     const bool & is_twist_valid, const bool & is_imu_valid);
-  void set_pointcloud_transform(
-    const std::string & base_frame, const std::string & lidar_frame) override;
 };
 
 }  // namespace autoware::pointcloud_preprocessor
diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index b4597d2fbe95e..16211f005421c 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -25,32 +25,27 @@
 namespace autoware::pointcloud_preprocessor
 {
 
-template <class T>
-bool DistortionCorrector<T>::pointcloud_transform_exists()
+bool DistortionCorrectorBase::pointcloud_transform_exists()
 {
   return pointcloud_transform_exists_;
 }
 
-template <class T>
-bool DistortionCorrector<T>::pointcloud_transform_needed()
+bool DistortionCorrectorBase::pointcloud_transform_needed()
 {
   return pointcloud_transform_needed_;
 }
 
-template <class T>
-std::deque<geometry_msgs::msg::TwistStamped> DistortionCorrector<T>::get_twist_queue()
+std::deque<geometry_msgs::msg::TwistStamped> DistortionCorrectorBase::get_twist_queue()
 {
   return twist_queue_;
 }
 
-template <class T>
-std::deque<geometry_msgs::msg::Vector3Stamped> DistortionCorrector<T>::get_angular_velocity_queue()
+std::deque<geometry_msgs::msg::Vector3Stamped> DistortionCorrectorBase::get_angular_velocity_queue()
 {
   return angular_velocity_queue_;
 }
 
-template <class T>
-void DistortionCorrector<T>::process_twist_message(
+void DistortionCorrectorBase::process_twist_message(
   const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg)
 {
   geometry_msgs::msg::TwistStamped msg;
@@ -72,16 +67,14 @@ void DistortionCorrector<T>::process_twist_message(
   }
 }
 
-template <class T>
-void DistortionCorrector<T>::process_imu_message(
+void DistortionCorrectorBase::process_imu_message(
   const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg)
 {
   get_imu_transformation(base_frame, imu_msg->header.frame_id);
   enqueue_imu(imu_msg);
 }
 
-template <class T>
-void DistortionCorrector<T>::get_imu_transformation(
+void DistortionCorrectorBase::get_imu_transformation(
   const std::string & base_frame, const std::string & imu_frame)
 {
   if (imu_transform_exists_) {
@@ -99,8 +92,7 @@ void DistortionCorrector<T>::get_imu_transformation(
     tf2::toMsg(tf2_imu_to_base_link.getRotation());
 }
 
-template <class T>
-void DistortionCorrector<T>::enqueue_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg)
+void DistortionCorrectorBase::enqueue_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg)
 {
   geometry_msgs::msg::Vector3Stamped angular_velocity;
   angular_velocity.vector = imu_msg->angular_velocity;
@@ -126,8 +118,7 @@ void DistortionCorrector<T>::enqueue_imu(const sensor_msgs::msg::Imu::ConstShare
   }
 }
 
-template <class T>
-void DistortionCorrector<T>::get_twist_and_imu_iterator(
+void DistortionCorrectorBase::get_twist_and_imu_iterator(
   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)
@@ -150,8 +141,7 @@ void DistortionCorrector<T>::get_twist_and_imu_iterator(
   }
 }
 
-template <class T>
-bool DistortionCorrector<T>::is_pointcloud_valid(sensor_msgs::msg::PointCloud2 & pointcloud)
+bool DistortionCorrectorBase::is_pointcloud_valid(sensor_msgs::msg::PointCloud2 & pointcloud)
 {
   if (pointcloud.data.empty()) {
     RCLCPP_WARN_STREAM_THROTTLE(
@@ -187,8 +177,7 @@ bool DistortionCorrector<T>::is_pointcloud_valid(sensor_msgs::msg::PointCloud2 &
   return true;
 }
 
-template <class T>
-std::optional<AngleConversion> DistortionCorrector<T>::try_compute_angle_conversion(
+std::optional<AngleConversion> DistortionCorrectorBase::try_compute_angle_conversion(
   sensor_msgs::msg::PointCloud2 & pointcloud)
 {
   // This function tries to compute the angle conversion from Cartesian coordinates to LiDAR azimuth
@@ -279,6 +268,31 @@ std::optional<AngleConversion> DistortionCorrector<T>::try_compute_angle_convers
   return std::nullopt;
 }
 
+void DistortionCorrectorBase::warn_if_timestamp_is_too_late(
+  bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late)
+{
+  if (is_twist_time_stamp_too_late) {
+    RCLCPP_WARN_STREAM_THROTTLE(
+      node_->get_logger(), *node_->get_clock(), 10000 /* ms */,
+      "Twist time_stamp is too late. Could not interpolate.");
+  }
+
+  if (is_imu_time_stamp_too_late) {
+    RCLCPP_WARN_STREAM_THROTTLE(
+      node_->get_logger(), *node_->get_clock(), 10000 /* ms */,
+      "IMU time_stamp is too late. Could not interpolate.");
+  }
+}
+
+void DistortionCorrectorBase::convert_matrix_to_transform(
+  const Eigen::Matrix4f & matrix, tf2::Transform & transform)
+{
+  transform.setOrigin(tf2::Vector3(matrix(0, 3), matrix(1, 3), matrix(2, 3)));
+  transform.setBasis(tf2::Matrix3x3(
+    matrix(0, 0), matrix(0, 1), matrix(0, 2), matrix(1, 0), matrix(1, 1), matrix(1, 2),
+    matrix(2, 0), matrix(2, 1), matrix(2, 2)));
+}
+
 template <class T>
 void DistortionCorrector<T>::undistort_pointcloud(
   bool use_imu, std::optional<AngleConversion> angle_conversion_opt,
@@ -379,33 +393,6 @@ void DistortionCorrector<T>::undistort_pointcloud(
   warn_if_timestamp_is_too_late(is_twist_time_stamp_too_late, is_imu_time_stamp_too_late);
 }
 
-template <class T>
-void DistortionCorrector<T>::warn_if_timestamp_is_too_late(
-  bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late)
-{
-  if (is_twist_time_stamp_too_late) {
-    RCLCPP_WARN_STREAM_THROTTLE(
-      node_->get_logger(), *node_->get_clock(), 10000 /* ms */,
-      "Twist time_stamp is too late. Could not interpolate.");
-  }
-
-  if (is_imu_time_stamp_too_late) {
-    RCLCPP_WARN_STREAM_THROTTLE(
-      node_->get_logger(), *node_->get_clock(), 10000 /* ms */,
-      "IMU time_stamp is too late. Could not interpolate.");
-  }
-}
-
-template <class T>
-void DistortionCorrector<T>::convert_matrix_to_transform(
-  const Eigen::Matrix4f & matrix, tf2::Transform & transform)
-{
-  transform.setOrigin(tf2::Vector3(matrix(0, 3), matrix(1, 3), matrix(2, 3)));
-  transform.setBasis(tf2::Matrix3x3(
-    matrix(0, 0), matrix(0, 1), matrix(0, 2), matrix(1, 0), matrix(1, 1), matrix(1, 2),
-    matrix(2, 0), matrix(2, 1), matrix(2, 2)));
-}
-
 ///////////////////////// Functions for different undistortion strategies /////////////////////////
 
 void DistortionCorrector2D::initialize()

From 3027ec1f9f255eeb0f9f8e3b968e0cbcd1cafcc2 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Thu, 19 Sep 2024 17:38:57 +0900
Subject: [PATCH 64/72] chore: fix test naming error

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector.hpp                  |  3 +-
 .../test/test_distortion_corrector_node.cpp   | 50 +++++++++----------
 2 files changed, 27 insertions(+), 26 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
index 047e93e9208f6..9d2c156124f59 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -66,12 +66,13 @@ class DistortionCorrectorBase
   bool pointcloud_transform_needed_{false};
   bool pointcloud_transform_exists_{false};
   bool imu_transform_exists_{false};
-  rclcpp::Node * node_;
   std::unique_ptr<autoware::universe_utils::ManagedTransformBuffer> managed_tf_buffer_{nullptr};
 
   std::deque<geometry_msgs::msg::TwistStamped> twist_queue_;
   std::deque<geometry_msgs::msg::Vector3Stamped> angular_velocity_queue_;
 
+  rclcpp::Node * node_;
+
   void get_imu_transformation(const std::string & base_frame, const std::string & imu_frame);
   void enqueue_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg);
   void get_twist_and_imu_iterator(
diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index 1c22c557b4c6c..7834b57075c34 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -348,7 +348,7 @@ class DistortionCorrectorTest : public ::testing::Test
   bool debug_{false};
 };
 
-TEST_F(DistortionCorrectorTest, Testprocess_twist_message)
+TEST_F(DistortionCorrectorTest, TestProcessTwistMessage)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   auto twist_msg = generateTwistMsg(twist_linear_x_, twist_angular_z_, timestamp);
@@ -359,7 +359,7 @@ TEST_F(DistortionCorrectorTest, Testprocess_twist_message)
   EXPECT_EQ(distortion_corrector_2d_->get_twist_queue().front().twist.angular.z, twist_angular_z_);
 }
 
-TEST_F(DistortionCorrectorTest, Testprocess_imu_message)
+TEST_F(DistortionCorrectorTest, TestProcessImuMessage)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   auto imu_msg = generateImuMsg(imu_angular_x_, imu_angular_y_, imu_angular_z_, timestamp);
@@ -371,7 +371,7 @@ TEST_F(DistortionCorrectorTest, Testprocess_imu_message)
     standard_tolerance_);
 }
 
-TEST_F(DistortionCorrectorTest, Testis_pointcloud_valid)
+TEST_F(DistortionCorrectorTest, TestIsPointcloudValid)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
 
@@ -387,28 +387,28 @@ TEST_F(DistortionCorrectorTest, Testis_pointcloud_valid)
   EXPECT_FALSE(result);
 }
 
-TEST_F(DistortionCorrectorTest, Testset_pointcloud_transformWithBaseLink)
+TEST_F(DistortionCorrectorTest, TestSetPointcloudTransformWithBaseLink)
 {
   distortion_corrector_2d_->set_pointcloud_transform("base_link", "base_link");
   EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_exists());
   EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_needed());
 }
 
-TEST_F(DistortionCorrectorTest, Testset_pointcloud_transformWithLidarFrame)
+TEST_F(DistortionCorrectorTest, TestSetPointcloudTransformWithLidarFrame)
 {
   distortion_corrector_2d_->set_pointcloud_transform("base_link", "lidar_top");
   EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_exists());
   EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_needed());
 }
 
-TEST_F(DistortionCorrectorTest, Testset_pointcloud_transformWithMissingFrame)
+TEST_F(DistortionCorrectorTest, TestSetPointcloudTransformWithMissingFrame)
 {
   distortion_corrector_2d_->set_pointcloud_transform("base_link", "missing_lidar_frame");
   EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_exists());
   EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_needed());
 }
 
-TEST_F(DistortionCorrectorTest, Testundistort_pointcloudWithEmptyTwist)
+TEST_F(DistortionCorrectorTest, TestUndistortPointcloudWithEmptyTwist)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   // Generate the point cloud message
@@ -448,7 +448,7 @@ TEST_F(DistortionCorrectorTest, Testundistort_pointcloudWithEmptyTwist)
   }
 }
 
-TEST_F(DistortionCorrectorTest, Testundistort_pointcloudWithEmptyPointCloud)
+TEST_F(DistortionCorrectorTest, TestUndistortPointcloudWithEmptyPointCloud)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   // Generate and process multiple twist messages
@@ -465,7 +465,7 @@ TEST_F(DistortionCorrectorTest, Testundistort_pointcloudWithEmptyPointCloud)
   EXPECT_EQ(empty_pointcloud.row_step, 0);
 }
 
-TEST_F(DistortionCorrectorTest, Testundistort_pointcloud2dWithoutImuInBaseLink)
+TEST_F(DistortionCorrectorTest, TestUndistortPointcloud2dWithoutImuInBaseLink)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -510,7 +510,7 @@ TEST_F(DistortionCorrectorTest, Testundistort_pointcloud2dWithoutImuInBaseLink)
   }
 }
 
-TEST_F(DistortionCorrectorTest, Testundistort_pointcloud2dWithImuInBaseLink)
+TEST_F(DistortionCorrectorTest, TestUndistortPointcloud2dWithImuInBaseLink)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -557,7 +557,7 @@ TEST_F(DistortionCorrectorTest, Testundistort_pointcloud2dWithImuInBaseLink)
   }
 }
 
-TEST_F(DistortionCorrectorTest, Testundistort_pointcloud2dWithImuInLidarFrame)
+TEST_F(DistortionCorrectorTest, TestUndistortPointcloud2dWithImuInLidarFrame)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -607,7 +607,7 @@ TEST_F(DistortionCorrectorTest, Testundistort_pointcloud2dWithImuInLidarFrame)
   }
 }
 
-TEST_F(DistortionCorrectorTest, Testundistort_pointcloud3dWithoutImuInBaseLink)
+TEST_F(DistortionCorrectorTest, TestUndistortPointcloud3dWithoutImuInBaseLink)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -652,7 +652,7 @@ TEST_F(DistortionCorrectorTest, Testundistort_pointcloud3dWithoutImuInBaseLink)
   }
 }
 
-TEST_F(DistortionCorrectorTest, Testundistort_pointcloud3dWithImuInBaseLink)
+TEST_F(DistortionCorrectorTest, TestUndistortPointcloud3dWithImuInBaseLink)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -702,7 +702,7 @@ TEST_F(DistortionCorrectorTest, Testundistort_pointcloud3dWithImuInBaseLink)
   }
 }
 
-TEST_F(DistortionCorrectorTest, Testundistort_pointcloud3dWithImuInLidarFrame)
+TEST_F(DistortionCorrectorTest, TestUndistortPointcloud3dWithImuInLidarFrame)
 {
   // Generate the point cloud message
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -751,7 +751,7 @@ TEST_F(DistortionCorrectorTest, Testundistort_pointcloud3dWithImuInLidarFrame)
   }
 }
 
-TEST_F(DistortionCorrectorTest, Testundistort_pointcloudWithPureLinearMotion)
+TEST_F(DistortionCorrectorTest, TestUndistortPointcloudWithPureLinearMotion)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   auto [default_points, default_azimuths] =
@@ -842,7 +842,7 @@ TEST_F(DistortionCorrectorTest, Testundistort_pointcloudWithPureLinearMotion)
   }
 }
 
-TEST_F(DistortionCorrectorTest, Testundistort_pointcloudWithPureRotationalMotion)
+TEST_F(DistortionCorrectorTest, TestUndistortPointcloudWithPureRotationalMotion)
 {
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
   auto [default_points, default_azimuths] =
@@ -946,7 +946,7 @@ TEST_F(DistortionCorrectorTest, Testundistort_pointcloudWithPureRotationalMotion
   }
 }
 
-TEST_F(DistortionCorrectorTest, Testundistort_pointcloudNotUpdatingAzimuthAndDistance)
+TEST_F(DistortionCorrectorTest, TestUndistortPointcloudNotUpdatingAzimuthAndDistance)
 {
   // Test the case when the cloud will not update the azimuth and distance values
   // 1. when pointcloud is in base_link (pointcloud_transform_needed() is false)
@@ -1059,7 +1059,7 @@ TEST_F(DistortionCorrectorTest, Testundistort_pointcloudNotUpdatingAzimuthAndDis
   }
 }
 
-TEST_F(DistortionCorrectorTest, Testundistort_pointcloudUpdateAzimuthAndDistanceInVelodyne)
+TEST_F(DistortionCorrectorTest, TestUndistortPointcloudUpdateAzimuthAndDistanceInVelodyne)
 {
   // Generate the point cloud message in sensor frame
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1118,7 +1118,7 @@ TEST_F(DistortionCorrectorTest, Testundistort_pointcloudUpdateAzimuthAndDistance
   }
 }
 
-TEST_F(DistortionCorrectorTest, Testundistort_pointcloudUpdateAzimuthAndDistanceInHesai)
+TEST_F(DistortionCorrectorTest, TestUndistortPointcloudUpdateAzimuthAndDistanceInHesai)
 {
   // Generate the point cloud message in sensor frame
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1177,7 +1177,7 @@ TEST_F(DistortionCorrectorTest, Testundistort_pointcloudUpdateAzimuthAndDistance
   }
 }
 
-TEST_F(DistortionCorrectorTest, Testtry_compute_angle_conversionOnEmptyPointcloud)
+TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnEmptyPointcloud)
 {
   // test empty pointcloud
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1189,7 +1189,7 @@ TEST_F(DistortionCorrectorTest, Testtry_compute_angle_conversionOnEmptyPointclou
   EXPECT_FALSE(angle_conversion_opt.has_value());
 }
 
-TEST_F(DistortionCorrectorTest, Testtry_compute_angle_conversionOnVelodynePointcloud)
+TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnVelodynePointcloud)
 {
   // test velodyne pointcloud (x-axis: 0 degree, y-axis: 270 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1212,7 +1212,7 @@ TEST_F(DistortionCorrectorTest, Testtry_compute_angle_conversionOnVelodynePointc
   EXPECT_NEAR(angle_conversion_opt->offset_rad, 0, standard_tolerance_);
 }
 
-TEST_F(DistortionCorrectorTest, Testtry_compute_angle_conversionOnHesaiPointcloud)
+TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnHesaiPointcloud)
 {
   // test hesai pointcloud (x-axis: 90 degree, y-axis: 0 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1235,7 +1235,7 @@ TEST_F(DistortionCorrectorTest, Testtry_compute_angle_conversionOnHesaiPointclou
     angle_conversion_opt->offset_rad, autoware::universe_utils::pi / 2, standard_tolerance_);
 }
 
-TEST_F(DistortionCorrectorTest, Testtry_compute_angle_conversionCartesianPointcloud)
+TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionCartesianPointcloud)
 {
   // test pointcloud that use cartesian coordinate for azimuth (x-axis: 0 degree, y-axis: 90 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1260,7 +1260,7 @@ TEST_F(DistortionCorrectorTest, Testtry_compute_angle_conversionCartesianPointcl
   EXPECT_NEAR(angle_conversion_opt->offset_rad, 0, standard_tolerance_);
 }
 
-TEST_F(DistortionCorrectorTest, Testtry_compute_angle_conversionOnRandomPointcloud)
+TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnRandomPointcloud)
 {
   // test pointcloud that use coordinate (x-axis: 270 degree, y-axis: 0 degree)
   rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
@@ -1284,7 +1284,7 @@ TEST_F(DistortionCorrectorTest, Testtry_compute_angle_conversionOnRandomPointclo
     angle_conversion_opt->offset_rad, autoware::universe_utils::pi * 3 / 2, standard_tolerance_);
 }
 
-TEST_F(DistortionCorrectorTest, Testtry_compute_angle_conversionOnBadAzimuthPointcloud)
+TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnBadAzimuthPointcloud)
 {
   // test pointcloud that can cause the angle conversion to fail.
   // 1. angle difference is 0

From 3c743036bc154918cd01537d1460155515b31b46 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Thu, 19 Sep 2024 18:29:57 +0900
Subject: [PATCH 65/72] chore: fix clang error

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector/distortion_corrector_node.hpp          | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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 11c6e3849c807..ef2ce7b2ab6c8 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
@@ -61,7 +61,7 @@ class DistortionCorrectorComponent : public rclcpp::Node
 
   /// @brief The maximum number of failed angle conversions due to pointclouds not containing enough
   /// points before throwing an error.
-  static constexpr int failure_tolerance_{20};
+  static constexpr int failure_tolerance{20};
 
   std::unique_ptr<DistortionCorrectorBase> distortion_corrector_;
 

From 2439956dbbc08a3631bddfed5bb55fb9f6985b43 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Fri, 20 Sep 2024 15:19:05 +0900
Subject: [PATCH 66/72] chore: fix error

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../src/distortion_corrector/distortion_corrector_node.cpp    | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

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 5516e033cde96..ee5151d520a68 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
@@ -110,9 +110,9 @@ void DistortionCorrectorComponent::pointcloud_callback(PointCloud2::UniquePtr po
         "coordinates. This pointcloud will not update azimuth and distance");
       angle_conversion_failure_num_++;
 
-      if (angle_conversion_failure_num_ > failure_tolerance_) {
+      if (angle_conversion_failure_num_ > failure_tolerance) {
         throw std::runtime_error(
-          "Angle conversion failed more than " + std::to_string(failure_tolerance_) +
+          "Angle conversion failed more than " + std::to_string(failure_tolerance) +
           " times. Please check the LiDAR azimuth coordinates.");
       }
     }

From 783219fae48471f7639278d4a7bf1fccf4f6705d Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Mon, 30 Sep 2024 20:51:49 +0900
Subject: [PATCH 67/72] chore: fix clangd

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector.hpp                  |   8 +-
 .../distortion_corrector_node.hpp             |   2 +-
 .../distortion_corrector.cpp                  |  38 ++--
 .../test/test_distortion_corrector_node.cpp   | 187 +++++++++---------
 4 files changed, 118 insertions(+), 117 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
index 9d2c156124f59..3c45913b9928d 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -43,7 +43,6 @@
 #include <deque>
 #include <memory>
 #include <string>
-#include <tuple>
 
 namespace autoware::pointcloud_preprocessor
 {
@@ -81,7 +80,8 @@ class DistortionCorrectorBase
     std::deque<geometry_msgs::msg::Vector3Stamped>::iterator & it_imu);
   void warn_if_timestamp_is_too_late(
     bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late);
-  void convert_matrix_to_transform(const Eigen::Matrix4f & matrix, tf2::Transform & transform);
+  static void convert_matrix_to_transform(
+    const Eigen::Matrix4f & matrix, tf2::Transform & transform);
 
 public:
   explicit DistortionCorrectorBase(rclcpp::Node * node, const bool & has_static_tf_only)
@@ -90,8 +90,8 @@ class DistortionCorrectorBase
     managed_tf_buffer_ =
       std::make_unique<autoware::universe_utils::ManagedTransformBuffer>(node, has_static_tf_only);
   }
-  bool pointcloud_transform_exists();
-  bool pointcloud_transform_needed();
+  [[nodiscard]] bool pointcloud_transform_exists() const;
+  [[nodiscard]] bool pointcloud_transform_needed() const;
   std::deque<geometry_msgs::msg::TwistStamped> get_twist_queue();
   std::deque<geometry_msgs::msg::Vector3Stamped> get_angular_velocity_queue();
   void process_twist_message(
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 ef2ce7b2ab6c8..fff10977b918d 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
@@ -65,7 +65,7 @@ class DistortionCorrectorComponent : public rclcpp::Node
 
   std::unique_ptr<DistortionCorrectorBase> distortion_corrector_;
 
-  void pointcloud_callback(PointCloud2::UniquePtr points_msg);
+  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);
diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index 16211f005421c..2ba896749cb5d 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -20,17 +20,15 @@
 #include <autoware/universe_utils/math/trigonometry.hpp>
 #include <tf2_eigen/tf2_eigen.hpp>
 
-#include <cstddef>
-
 namespace autoware::pointcloud_preprocessor
 {
 
-bool DistortionCorrectorBase::pointcloud_transform_exists()
+bool DistortionCorrectorBase::pointcloud_transform_exists() const
 {
   return pointcloud_transform_exists_;
 }
 
-bool DistortionCorrectorBase::pointcloud_transform_needed()
+bool DistortionCorrectorBase::pointcloud_transform_needed() const
 {
   return pointcloud_transform_needed_;
 }
@@ -364,7 +362,7 @@ void DistortionCorrector<T>::undistort_pointcloud(
       is_imu_valid = false;
     }
 
-    float time_offset = static_cast<float>(global_point_stamp - prev_time_stamp_sec);
+    auto time_offset = static_cast<float>(global_point_stamp - prev_time_stamp_sec);
 
     // Undistort a single point based on the strategy
     undistort_point(it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid);
@@ -443,7 +441,8 @@ inline void DistortionCorrector2D::undistort_point_implementation(
   const bool & is_twist_valid, const bool & is_imu_valid)
 {
   // Initialize linear velocity and angular velocity
-  float v{0.0f}, w{0.0f};
+  float v{0.0f};
+  float w{0.0f};
   if (is_twist_valid) {
     v = static_cast<float>(it_twist->twist.linear.x);
     w = static_cast<float>(it_twist->twist.angular.z);
@@ -491,19 +490,24 @@ inline void DistortionCorrector3D::undistort_point_implementation(
   const bool & is_twist_valid, const bool & is_imu_valid)
 {
   // Initialize linear velocity and angular velocity
-  float v_x_{0.0f}, v_y_{0.0f}, v_z_{0.0f}, w_x_{0.0f}, w_y_{0.0f}, w_z_{0.0f};
+  float v_x{0.0f};
+  float v_y{0.0f};
+  float v_z{0.0f};
+  float w_x{0.0f};
+  float w_y{0.0f};
+  float w_z{0.0f};
   if (is_twist_valid) {
-    v_x_ = static_cast<float>(it_twist->twist.linear.x);
-    v_y_ = static_cast<float>(it_twist->twist.linear.y);
-    v_z_ = static_cast<float>(it_twist->twist.linear.z);
-    w_x_ = static_cast<float>(it_twist->twist.angular.x);
-    w_y_ = static_cast<float>(it_twist->twist.angular.y);
-    w_z_ = static_cast<float>(it_twist->twist.angular.z);
+    v_x = static_cast<float>(it_twist->twist.linear.x);
+    v_y = static_cast<float>(it_twist->twist.linear.y);
+    v_z = static_cast<float>(it_twist->twist.linear.z);
+    w_x = static_cast<float>(it_twist->twist.angular.x);
+    w_y = static_cast<float>(it_twist->twist.angular.y);
+    w_z = static_cast<float>(it_twist->twist.angular.z);
   }
   if (is_imu_valid) {
-    w_x_ = static_cast<float>(it_imu->vector.x);
-    w_y_ = static_cast<float>(it_imu->vector.y);
-    w_z_ = static_cast<float>(it_imu->vector.z);
+    w_x = static_cast<float>(it_imu->vector.x);
+    w_y = static_cast<float>(it_imu->vector.y);
+    w_z = static_cast<float>(it_imu->vector.z);
   }
 
   // Undistort point
@@ -512,7 +516,7 @@ inline void DistortionCorrector3D::undistort_point_implementation(
     point_eigen_ = eigen_lidar_to_base_link_ * point_eigen_;
   }
 
-  Sophus::SE3f::Tangent twist(v_x_, v_y_, v_z_, w_x_, w_y_, w_z_);
+  Sophus::SE3f::Tangent twist(v_x, v_y, v_z, w_x, w_y, w_z);
   twist = twist * time_offset;
   transformation_matrix_ = Sophus::SE3f::exp(twist).matrix();
   transformation_matrix_ = transformation_matrix_ * prev_transformation_matrix_;
diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index 7834b57075c34..165d0685b5ef1 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -89,7 +89,7 @@ class DistortionCorrectorTest : public ::testing::Test
     const std::string & parent_frame, const std::string & child_frame, double x, double y, double z,
     double qx, double qy, double qz, double qw)
   {
-    rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+    rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
     geometry_msgs::msg::TransformStamped tf_msg;
     tf_msg.header.stamp = timestamp;
     tf_msg.header.frame_id = parent_frame;
@@ -141,13 +141,13 @@ class DistortionCorrectorTest : public ::testing::Test
     std::vector<std::shared_ptr<geometry_msgs::msg::TwistWithCovarianceStamped>> twist_msgs;
     rclcpp::Time twist_stamp = subtractMilliseconds(pointcloud_timestamp, 5);
 
-    for (int i = 0; i < number_of_twist_msgs_; ++i) {
+    for (int i = 0; i < number_of_twist_msgs; ++i) {
       auto twist_msg = generateTwistMsg(
-        twist_linear_x_ + i * twist_linear_x_increment_,
-        twist_angular_z_ + i * twist_angular_z_increment_, twist_stamp);
+        twist_linear_x + i * twist_linear_x_increment,
+        twist_angular_z + i * twist_angular_z_increment, twist_stamp);
       twist_msgs.push_back(twist_msg);
 
-      twist_stamp = addMilliseconds(twist_stamp, twist_msgs_interval_ms_);
+      twist_stamp = addMilliseconds(twist_stamp, twist_msgs_interval_ms);
     }
 
     return twist_msgs;
@@ -159,13 +159,12 @@ class DistortionCorrectorTest : public ::testing::Test
     std::vector<std::shared_ptr<sensor_msgs::msg::Imu>> imu_msgs;
     rclcpp::Time imu_stamp = subtractMilliseconds(pointcloud_timestamp, 10);
 
-    for (int i = 0; i < number_of_imu_msgs_; ++i) {
+    for (int i = 0; i < number_of_imu_msgs; ++i) {
       auto imu_msg = generateImuMsg(
-        imu_angular_x_ + i * imu_angular_x_increment_,
-        imu_angular_y_ + i * imu_angular_y_increment_,
-        imu_angular_z_ + i * imu_angular_z_increment_, imu_stamp);
+        imu_angular_x + i * imu_angular_x_increment, imu_angular_y + i * imu_angular_y_increment,
+        imu_angular_z + i * imu_angular_z_increment, imu_stamp);
       imu_msgs.push_back(imu_msg);
-      imu_stamp = addMilliseconds(imu_stamp, imu_msgs_interval_ms_);
+      imu_stamp = addMilliseconds(imu_stamp, imu_msgs_interval_ms);
     }
 
     return imu_msgs;
@@ -285,7 +284,7 @@ class DistortionCorrectorTest : public ::testing::Test
     for (size_t i = 0; i < number_of_points; ++i) {
       std::uint32_t relative_timestamp = (global_point_stamp - pointcloud_timestamp).nanoseconds();
       timestamps.push_back(relative_timestamp);
-      global_point_stamp = addMilliseconds(global_point_stamp, points_interval_ms_);
+      global_point_stamp = addMilliseconds(global_point_stamp, points_interval_ms);
     }
 
     return timestamps;
@@ -318,31 +317,29 @@ class DistortionCorrectorTest : public ::testing::Test
     distortion_corrector_3d_;
   std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_broadcaster_;
 
-  static constexpr float standard_tolerance_{1e-4};
-  static constexpr float coarse_tolerance_{5e-3};
-  static constexpr int number_of_twist_msgs_{6};
-  static constexpr int number_of_imu_msgs_{6};
-  static constexpr size_t number_of_points_{10};
-  static constexpr int32_t timestamp_seconds_{10};
-  static constexpr uint32_t timestamp_nanoseconds_{100000000};
-
-  static constexpr double twist_linear_x_{10.0};
-  static constexpr double twist_angular_z_{0.02};
-  static constexpr double twist_linear_x_increment_{2.0};
-  static constexpr double twist_angular_z_increment_{0.01};
-
-  static constexpr double imu_angular_x_{0.01};
-  static constexpr double imu_angular_y_{-0.02};
-  static constexpr double imu_angular_z_{0.05};
-  static constexpr double imu_angular_x_increment_{0.005};
-  static constexpr double imu_angular_y_increment_{0.005};
-  static constexpr double imu_angular_z_increment_{0.005};
-
-  static constexpr int points_interval_ms_{10};
-  static constexpr int twist_msgs_interval_ms_{24};
-  static constexpr int imu_msgs_interval_ms_{27};
-
-  static constexpr double epsilon = 1e-5;
+  static constexpr float standard_tolerance{1e-4};
+  static constexpr float coarse_tolerance{5e-3};
+  static constexpr int number_of_twist_msgs{6};
+  static constexpr int number_of_imu_msgs{6};
+  static constexpr size_t number_of_points{10};
+  static constexpr int32_t timestamp_seconds{10};
+  static constexpr uint32_t timestamp_nanoseconds{100000000};
+
+  static constexpr double twist_linear_x{10.0};
+  static constexpr double twist_angular_z{0.02};
+  static constexpr double twist_linear_x_increment{2.0};
+  static constexpr double twist_angular_z_increment{0.01};
+
+  static constexpr double imu_angular_x{0.01};
+  static constexpr double imu_angular_y{-0.02};
+  static constexpr double imu_angular_z{0.05};
+  static constexpr double imu_angular_x_increment{0.005};
+  static constexpr double imu_angular_y_increment{0.005};
+  static constexpr double imu_angular_z_increment{0.005};
+
+  static constexpr int points_interval_ms{10};
+  static constexpr int twist_msgs_interval_ms{24};
+  static constexpr int imu_msgs_interval_ms{27};
 
   // for debugging or regenerating the ground truth point cloud
   bool debug_{false};
@@ -350,30 +347,30 @@ class DistortionCorrectorTest : public ::testing::Test
 
 TEST_F(DistortionCorrectorTest, TestProcessTwistMessage)
 {
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  auto twist_msg = generateTwistMsg(twist_linear_x_, twist_angular_z_, timestamp);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
+  auto twist_msg = generateTwistMsg(twist_linear_x, twist_angular_z, timestamp);
   distortion_corrector_2d_->process_twist_message(twist_msg);
 
   ASSERT_FALSE(distortion_corrector_2d_->get_twist_queue().empty());
-  EXPECT_EQ(distortion_corrector_2d_->get_twist_queue().front().twist.linear.x, twist_linear_x_);
-  EXPECT_EQ(distortion_corrector_2d_->get_twist_queue().front().twist.angular.z, twist_angular_z_);
+  EXPECT_EQ(distortion_corrector_2d_->get_twist_queue().front().twist.linear.x, twist_linear_x);
+  EXPECT_EQ(distortion_corrector_2d_->get_twist_queue().front().twist.angular.z, twist_angular_z);
 }
 
 TEST_F(DistortionCorrectorTest, TestProcessImuMessage)
 {
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
-  auto imu_msg = generateImuMsg(imu_angular_x_, imu_angular_y_, imu_angular_z_, timestamp);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
+  auto imu_msg = generateImuMsg(imu_angular_x, imu_angular_y, imu_angular_z, timestamp);
   distortion_corrector_2d_->process_imu_message("base_link", imu_msg);
 
   ASSERT_FALSE(distortion_corrector_2d_->get_angular_velocity_queue().empty());
   EXPECT_NEAR(
     distortion_corrector_2d_->get_angular_velocity_queue().front().vector.z, -0.03159,
-    standard_tolerance_);
+    standard_tolerance);
 }
 
 TEST_F(DistortionCorrectorTest, TestIsPointcloudValid)
 {
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
 
   auto [default_points, default_azimuths] =
     generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
@@ -410,7 +407,7 @@ TEST_F(DistortionCorrectorTest, TestSetPointcloudTransformWithMissingFrame)
 
 TEST_F(DistortionCorrectorTest, TestUndistortPointcloudWithEmptyTwist)
 {
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
   // Generate the point cloud message
   auto [default_points, default_azimuths] =
     generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
@@ -438,9 +435,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloudWithEmptyTwist)
 
   for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) {
     oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n";
-    EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_);
-    EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_);
-    EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_);
+    EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance);
+    EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance);
+    EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance);
   }
 
   if (debug_) {
@@ -450,7 +447,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloudWithEmptyTwist)
 
 TEST_F(DistortionCorrectorTest, TestUndistortPointcloudWithEmptyPointCloud)
 {
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
   // Generate an empty point cloud message
@@ -468,7 +465,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloudWithEmptyPointCloud)
 TEST_F(DistortionCorrectorTest, TestUndistortPointcloud2dWithoutImuInBaseLink)
 {
   // Generate the point cloud message
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
   auto [default_points, default_azimuths] =
     generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
   auto pointcloud = generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
@@ -500,9 +497,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloud2dWithoutImuInBaseLink)
 
   for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) {
     oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n";
-    EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_);
-    EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_);
-    EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_);
+    EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance);
+    EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance);
+    EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance);
   }
 
   if (debug_) {
@@ -513,7 +510,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloud2dWithoutImuInBaseLink)
 TEST_F(DistortionCorrectorTest, TestUndistortPointcloud2dWithImuInBaseLink)
 {
   // Generate the point cloud message
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
   auto [default_points, default_azimuths] =
     generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
   auto pointcloud = generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
@@ -547,9 +544,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloud2dWithImuInBaseLink)
 
   for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) {
     oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n";
-    EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_);
-    EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_);
-    EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_);
+    EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance);
+    EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance);
+    EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance);
   }
 
   if (debug_) {
@@ -560,7 +557,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloud2dWithImuInBaseLink)
 TEST_F(DistortionCorrectorTest, TestUndistortPointcloud2dWithImuInLidarFrame)
 {
   // Generate the point cloud message
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
   auto [default_points, default_azimuths] =
     generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
   auto pointcloud = generatePointCloudMsg(true, timestamp, default_points, default_azimuths);
@@ -597,9 +594,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloud2dWithImuInLidarFrame)
 
   for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) {
     oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n";
-    EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_);
-    EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_);
-    EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_);
+    EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance);
+    EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance);
+    EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance);
   }
 
   if (debug_) {
@@ -610,7 +607,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloud2dWithImuInLidarFrame)
 TEST_F(DistortionCorrectorTest, TestUndistortPointcloud3dWithoutImuInBaseLink)
 {
   // Generate the point cloud message
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
   auto [default_points, default_azimuths] =
     generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
   auto pointcloud = generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
@@ -642,9 +639,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloud3dWithoutImuInBaseLink)
 
   for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) {
     oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n";
-    EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_);
-    EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_);
-    EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_);
+    EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance);
+    EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance);
+    EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance);
   }
 
   if (debug_) {
@@ -655,7 +652,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloud3dWithoutImuInBaseLink)
 TEST_F(DistortionCorrectorTest, TestUndistortPointcloud3dWithImuInBaseLink)
 {
   // Generate the point cloud message
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
   auto [default_points, default_azimuths] =
     generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
   auto pointcloud = generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
@@ -692,9 +689,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloud3dWithImuInBaseLink)
 
   for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) {
     oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n";
-    EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_);
-    EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_);
-    EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_);
+    EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance);
+    EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance);
+    EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance);
   }
 
   if (debug_) {
@@ -705,7 +702,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloud3dWithImuInBaseLink)
 TEST_F(DistortionCorrectorTest, TestUndistortPointcloud3dWithImuInLidarFrame)
 {
   // Generate the point cloud message
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
   auto [default_points, default_azimuths] =
     generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
   auto pointcloud = generatePointCloudMsg(true, timestamp, default_points, default_azimuths);
@@ -741,9 +738,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloud3dWithImuInLidarFrame)
 
   for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) {
     oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n";
-    EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_);
-    EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_);
-    EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_);
+    EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance);
+    EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance);
+    EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance);
   }
 
   if (debug_) {
@@ -753,7 +750,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloud3dWithImuInLidarFrame)
 
 TEST_F(DistortionCorrectorTest, TestUndistortPointcloudWithPureLinearMotion)
 {
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
   auto [default_points, default_azimuths] =
     generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
   auto test2d_pointcloud =
@@ -844,7 +841,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloudWithPureLinearMotion)
 
 TEST_F(DistortionCorrectorTest, TestUndistortPointcloudWithPureRotationalMotion)
 {
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
   auto [default_points, default_azimuths] =
     generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
   auto test2d_pointcloud =
@@ -936,9 +933,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloudWithPureRotationalMotion)
         << *test2d_iter_z << ")"
         << " vs 3D: (" << *test3d_iter_x << ", " << *test3d_iter_y << ", " << *test3d_iter_z
         << ")\n";
-    EXPECT_NEAR(*test2d_iter_x, *test3d_iter_x, coarse_tolerance_);
-    EXPECT_NEAR(*test2d_iter_y, *test3d_iter_y, coarse_tolerance_);
-    EXPECT_NEAR(*test2d_iter_z, *test3d_iter_z, coarse_tolerance_);
+    EXPECT_NEAR(*test2d_iter_x, *test3d_iter_x, coarse_tolerance);
+    EXPECT_NEAR(*test2d_iter_y, *test3d_iter_y, coarse_tolerance);
+    EXPECT_NEAR(*test2d_iter_z, *test3d_iter_z, coarse_tolerance);
   }
 
   if (debug_) {
@@ -952,7 +949,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloudNotUpdatingAzimuthAndDist
   // 1. when pointcloud is in base_link (pointcloud_transform_needed() is false)
 
   // Generate the point cloud message in base_link
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
   auto [default_points, default_azimuths] =
     generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
   auto pointcloud_base_link =
@@ -1062,7 +1059,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloudNotUpdatingAzimuthAndDist
 TEST_F(DistortionCorrectorTest, TestUndistortPointcloudUpdateAzimuthAndDistanceInVelodyne)
 {
   // Generate the point cloud message in sensor frame
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
   auto [default_points, default_azimuths] =
     generateDefaultPointcloud(AngleCoordinateSystem::VELODYNE);
   auto pointcloud = generatePointCloudMsg(true, timestamp, default_points, default_azimuths);
@@ -1109,8 +1106,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloudUpdateAzimuthAndDistanceI
         << " vs Expected azimuth and distance: (" << expected_azimuth_distance[i][0] << ", "
         << expected_azimuth_distance[i][1] << ")\n";
 
-    EXPECT_NEAR(*iter_azimuth, expected_azimuth_distance[i][0], standard_tolerance_);
-    EXPECT_NEAR(*iter_distance, expected_azimuth_distance[i][1], standard_tolerance_);
+    EXPECT_NEAR(*iter_azimuth, expected_azimuth_distance[i][0], standard_tolerance);
+    EXPECT_NEAR(*iter_distance, expected_azimuth_distance[i][1], standard_tolerance);
   }
 
   if (debug_) {
@@ -1121,7 +1118,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloudUpdateAzimuthAndDistanceI
 TEST_F(DistortionCorrectorTest, TestUndistortPointcloudUpdateAzimuthAndDistanceInHesai)
 {
   // Generate the point cloud message in sensor frame
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
 
   auto [default_points, default_azimuths] = generateDefaultPointcloud(AngleCoordinateSystem::HESAI);
   auto pointcloud = generatePointCloudMsg(true, timestamp, default_points, default_azimuths);
@@ -1168,8 +1165,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloudUpdateAzimuthAndDistanceI
         << " vs Expected azimuth and distance: (" << expected_azimuth_distance[i][0] << ", "
         << expected_azimuth_distance[i][1] << ")\n";
 
-    EXPECT_NEAR(*iter_azimuth, expected_azimuth_distance[i][0], standard_tolerance_);
-    EXPECT_NEAR(*iter_distance, expected_azimuth_distance[i][1], standard_tolerance_);
+    EXPECT_NEAR(*iter_azimuth, expected_azimuth_distance[i][0], standard_tolerance);
+    EXPECT_NEAR(*iter_distance, expected_azimuth_distance[i][1], standard_tolerance);
   }
 
   if (debug_) {
@@ -1180,7 +1177,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloudUpdateAzimuthAndDistanceI
 TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnEmptyPointcloud)
 {
   // test empty pointcloud
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
 
   auto empty_pointcloud = generateEmptyPointCloudMsg(timestamp);
   auto angle_conversion_opt =
@@ -1192,7 +1189,7 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnEmptyPointcloud)
 TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnVelodynePointcloud)
 {
   // test velodyne pointcloud (x-axis: 0 degree, y-axis: 270 degree)
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
 
   std::vector<Eigen::Vector3f> velodyne_points = {
     Eigen::Vector3f(0.0f, 0.0f, 0.0f),
@@ -1209,13 +1206,13 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnVelodynePointclou
   EXPECT_TRUE(angle_conversion_opt.has_value());
 
   EXPECT_EQ(angle_conversion_opt->sign, -1);
-  EXPECT_NEAR(angle_conversion_opt->offset_rad, 0, standard_tolerance_);
+  EXPECT_NEAR(angle_conversion_opt->offset_rad, 0, standard_tolerance);
 }
 
 TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnHesaiPointcloud)
 {
   // test hesai pointcloud (x-axis: 90 degree, y-axis: 0 degree)
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
   std::vector<Eigen::Vector3f> hesai_points = {
     Eigen::Vector3f(0.0f, 0.0f, 0.0f),
     Eigen::Vector3f(1.0f, -1.0f, 1.0f),
@@ -1232,13 +1229,13 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnHesaiPointcloud)
   EXPECT_TRUE(angle_conversion_opt.has_value());
   EXPECT_EQ(angle_conversion_opt->sign, -1);
   EXPECT_NEAR(
-    angle_conversion_opt->offset_rad, autoware::universe_utils::pi / 2, standard_tolerance_);
+    angle_conversion_opt->offset_rad, autoware::universe_utils::pi / 2, standard_tolerance);
 }
 
 TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionCartesianPointcloud)
 {
   // test pointcloud that use cartesian coordinate for azimuth (x-axis: 0 degree, y-axis: 90 degree)
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
 
@@ -1257,13 +1254,13 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionCartesianPointcloud
 
   EXPECT_TRUE(angle_conversion_opt.has_value());
   EXPECT_EQ(angle_conversion_opt->sign, 1);
-  EXPECT_NEAR(angle_conversion_opt->offset_rad, 0, standard_tolerance_);
+  EXPECT_NEAR(angle_conversion_opt->offset_rad, 0, standard_tolerance);
 }
 
 TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnRandomPointcloud)
 {
   // test pointcloud that use coordinate (x-axis: 270 degree, y-axis: 0 degree)
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
 
@@ -1281,7 +1278,7 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnRandomPointcloud)
   EXPECT_TRUE(angle_conversion_opt.has_value());
   EXPECT_EQ(angle_conversion_opt->sign, 1);
   EXPECT_NEAR(
-    angle_conversion_opt->offset_rad, autoware::universe_utils::pi * 3 / 2, standard_tolerance_);
+    angle_conversion_opt->offset_rad, autoware::universe_utils::pi * 3 / 2, standard_tolerance);
 }
 
 TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnBadAzimuthPointcloud)
@@ -1289,7 +1286,7 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnBadAzimuthPointcl
   // test pointcloud that can cause the angle conversion to fail.
   // 1. angle difference is 0
   // 2. azimuth value is wrong
-  rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
+  rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
   // Generate and process multiple twist messages
   generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);
 

From 72f5bb264d7ad0f66607f2a75fd4bab42e0009ab Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Thu, 3 Oct 2024 15:08:56 +0900
Subject: [PATCH 68/72] chore: add runtime error if the setting is wrong

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector.cpp                  | 37 +++++++++++--------
 1 file changed, 22 insertions(+), 15 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index 2ba896749cb5d..67e30f2001de6 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -367,22 +367,29 @@ void DistortionCorrector<T>::undistort_pointcloud(
     // Undistort a single point based on the strategy
     undistort_point(it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid);
 
-    if (angle_conversion_opt.has_value() && pointcloud_transform_needed()) {
-      float cartesian_coordinate_azimuth =
-        autoware::universe_utils::opencv_fast_atan2(*it_y, *it_x);
-      float updated_azimuth = angle_conversion_opt->offset_rad +
-                              angle_conversion_opt->sign * cartesian_coordinate_azimuth;
-      if (updated_azimuth < 0) {
-        updated_azimuth += autoware::universe_utils::pi * 2;
-      } else if (updated_azimuth > 2 * autoware::universe_utils::pi) {
-        updated_azimuth -= autoware::universe_utils::pi * 2;
+    if (angle_conversion_opt.has_value()) {
+      if (pointcloud_transform_needed_) {
+        float cartesian_coordinate_azimuth =
+          autoware::universe_utils::opencv_fast_atan2(*it_y, *it_x);
+        float updated_azimuth = angle_conversion_opt->offset_rad +
+                                angle_conversion_opt->sign * cartesian_coordinate_azimuth;
+        if (updated_azimuth < 0) {
+          updated_azimuth += autoware::universe_utils::pi * 2;
+        } else if (updated_azimuth > 2 * autoware::universe_utils::pi) {
+          updated_azimuth -= autoware::universe_utils::pi * 2;
+        }
+
+        *it_azimuth = updated_azimuth;
+        *it_distance = sqrt(*it_x * *it_x + *it_y * *it_y + *it_z * *it_z);
+
+        ++it_azimuth;
+        ++it_distance;
+      } else {
+        throw std::runtime_error(
+          "The pointcloud is not in the sensor frame, thus it will not update azimuth and distance "
+          "values. Please change the input pointcloud or set update_azimuth_and_distance_ to "
+          "false.");
       }
-
-      *it_azimuth = updated_azimuth;
-      *it_distance = sqrt(*it_x * *it_x + *it_y * *it_y + *it_z * *it_z);
-
-      ++it_azimuth;
-      ++it_distance;
     }
 
     prev_time_stamp_sec = global_point_stamp;

From 9663eaa7e89105e95da88737c45c1cc604da21b3 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Thu, 3 Oct 2024 16:20:01 +0900
Subject: [PATCH 69/72] chore: clean code

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../distortion_corrector.cpp                  | 33 +++++++++----------
 1 file changed, 16 insertions(+), 17 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index 67e30f2001de6..7c11fba787a68 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -368,28 +368,27 @@ void DistortionCorrector<T>::undistort_pointcloud(
     undistort_point(it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid);
 
     if (angle_conversion_opt.has_value()) {
-      if (pointcloud_transform_needed_) {
-        float cartesian_coordinate_azimuth =
-          autoware::universe_utils::opencv_fast_atan2(*it_y, *it_x);
-        float updated_azimuth = angle_conversion_opt->offset_rad +
-                                angle_conversion_opt->sign * cartesian_coordinate_azimuth;
-        if (updated_azimuth < 0) {
-          updated_azimuth += autoware::universe_utils::pi * 2;
-        } else if (updated_azimuth > 2 * autoware::universe_utils::pi) {
-          updated_azimuth -= autoware::universe_utils::pi * 2;
-        }
-
-        *it_azimuth = updated_azimuth;
-        *it_distance = sqrt(*it_x * *it_x + *it_y * *it_y + *it_z * *it_z);
-
-        ++it_azimuth;
-        ++it_distance;
-      } else {
+      if (!pointcloud_transform_needed_) {
         throw std::runtime_error(
           "The pointcloud is not in the sensor frame, thus it will not update azimuth and distance "
           "values. Please change the input pointcloud or set update_azimuth_and_distance_ to "
           "false.");
       }
+      float cartesian_coordinate_azimuth =
+        autoware::universe_utils::opencv_fast_atan2(*it_y, *it_x);
+      float updated_azimuth = angle_conversion_opt->offset_rad +
+                              angle_conversion_opt->sign * cartesian_coordinate_azimuth;
+      if (updated_azimuth < 0) {
+        updated_azimuth += autoware::universe_utils::pi * 2;
+      } else if (updated_azimuth > 2 * autoware::universe_utils::pi) {
+        updated_azimuth -= autoware::universe_utils::pi * 2;
+      }
+
+      *it_azimuth = updated_azimuth;
+      *it_distance = sqrt(*it_x * *it_x + *it_y * *it_y + *it_z * *it_z);
+
+      ++it_azimuth;
+      ++it_distance;
     }
 
     prev_time_stamp_sec = global_point_stamp;

From a06ad319f3d2a27c0faad42a88799cac3c455a79 Mon Sep 17 00:00:00 2001
From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com>
Date: Thu, 3 Oct 2024 16:30:38 +0900
Subject: [PATCH 70/72] Update
 sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp

Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com>
---
 .../src/distortion_corrector/distortion_corrector.cpp        | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index 7c11fba787a68..20ea0805be647 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -370,9 +370,8 @@ void DistortionCorrector<T>::undistort_pointcloud(
     if (angle_conversion_opt.has_value()) {
       if (!pointcloud_transform_needed_) {
         throw std::runtime_error(
-          "The pointcloud is not in the sensor frame, thus it will not update azimuth and distance "
-          "values. Please change the input pointcloud or set update_azimuth_and_distance_ to "
-          "false.");
+          "The pointcloud is not in the sensor's frame and thus azimuth and distance cannot be updated. " 
+          "Please change the input pointcloud or set update_azimuth_and_distance to false.");
       }
       float cartesian_coordinate_azimuth =
         autoware::universe_utils::opencv_fast_atan2(*it_y, *it_x);

From 20ffc99c3f37fd47aa40465ea767ff28e1dd0530 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Thu, 3 Oct 2024 07:33:00 +0000
Subject: [PATCH 71/72] style(pre-commit): autofix

---
 .../src/distortion_corrector/distortion_corrector.cpp          | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index 20ea0805be647..f59031c80c75a 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -370,7 +370,8 @@ void DistortionCorrector<T>::undistort_pointcloud(
     if (angle_conversion_opt.has_value()) {
       if (!pointcloud_transform_needed_) {
         throw std::runtime_error(
-          "The pointcloud is not in the sensor's frame and thus azimuth and distance cannot be updated. " 
+          "The pointcloud is not in the sensor's frame and thus azimuth and distance cannot be "
+          "updated. "
           "Please change the input pointcloud or set update_azimuth_and_distance to false.");
       }
       float cartesian_coordinate_azimuth =

From 7f31139d6842c14f4cfdf79899401db67d91fc17 Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Thu, 3 Oct 2024 17:02:45 +0900
Subject: [PATCH 72/72] chore: fix unit test for runtime error

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../test/test_distortion_corrector_node.cpp   | 46 ++++---------------
 1 file changed, 9 insertions(+), 37 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
index 165d0685b5ef1..ed46ffe6f224f 100644
--- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp
@@ -966,40 +966,13 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloudNotUpdatingAzimuthAndDist
   auto angle_conversion_opt =
     distortion_corrector_2d_->try_compute_angle_conversion(pointcloud_base_link);
 
-  distortion_corrector_2d_->undistort_pointcloud(true, angle_conversion_opt, pointcloud_base_link);
-
-  auto original_pointcloud_base_link =
-    generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
-
-  sensor_msgs::PointCloud2ConstIterator<float> test_iter_azimuth_base_link(
-    pointcloud_base_link, "azimuth");
-  sensor_msgs::PointCloud2ConstIterator<float> test_iter_distance_base_link(
-    pointcloud_base_link, "distance");
-
-  sensor_msgs::PointCloud2ConstIterator<float> original_iter_azimuth_base_link(
-    original_pointcloud_base_link, "azimuth");
-  sensor_msgs::PointCloud2ConstIterator<float> original_iter_distance_base_link(
-    original_pointcloud_base_link, "distance");
-
-  size_t i = 0;
-  std::ostringstream oss;
-
-  oss << "Expected pointcloud:\n";
-  for (; test_iter_azimuth_base_link != test_iter_azimuth_base_link.end();
-       ++test_iter_azimuth_base_link, ++test_iter_distance_base_link,
-       ++original_iter_azimuth_base_link, ++original_iter_distance_base_link, ++i) {
-    oss << "Point " << i << " - Output azimuth and distance: (" << *test_iter_azimuth_base_link
-        << ", " << *test_iter_distance_base_link << ")"
-        << " vs Original azimuth and distance: (" << *original_iter_azimuth_base_link << ", "
-        << *original_iter_distance_base_link << ")\n";
-
-    EXPECT_FLOAT_EQ(*test_iter_azimuth_base_link, *original_iter_azimuth_base_link);
-    EXPECT_FLOAT_EQ(*test_iter_distance_base_link, *original_iter_distance_base_link);
-  }
-
-  if (debug_) {
-    RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str());
-  }
+  // Test for expected runtime error
+  EXPECT_THROW(
+    {
+      distortion_corrector_2d_->undistort_pointcloud(
+        true, angle_conversion_opt, pointcloud_base_link);
+    },
+    std::runtime_error);
 
   // Test the case when the cloud will not update the azimuth and distance values
   // 2. when the return value of try_compute_angle_conversion is std::nullopt (couldn't find the
@@ -1034,9 +1007,8 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloudNotUpdatingAzimuthAndDist
   sensor_msgs::PointCloud2ConstIterator<float> original_iter_distance_lidar_top(
     original_pointcloud_lidar_top, "distance");
 
-  i = 0;
-  oss.str("");
-  oss.clear();
+  size_t i = 0;
+  std::ostringstream oss;
 
   oss << "Expected pointcloud:\n";
   for (; test_iter_azimuth_lidar_top != test_iter_azimuth_lidar_top.end();