revert(distortion corrector_node): performance tuning (autowarefounda…

…tion#2913) (autowarefoundation#512)

Revert "perf(distortion_corrector_node): performance tuning (autowarefoundation#2913)"

This reverts commit b72ca85.

common/tier4_autoware_utils/CMakeLists.txt

@@ -9,8 +9,6 @@ find_package(Boost REQUIRED)
 ament_auto_add_library(tier4_autoware_utils SHARED
   src/tier4_autoware_utils.cpp
   src/geometry/boost_polygon_utils.cpp
-  src/math/sin_table.cpp
-  src/math/trigonometry.cpp
   src/ros/msg_operation.cpp
 )
 

common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp

@@ -23,8 +23,6 @@
 #include "tier4_autoware_utils/math/constants.hpp"
 #include "tier4_autoware_utils/math/normalization.hpp"
 #include "tier4_autoware_utils/math/range.hpp"
-#include "tier4_autoware_utils/math/sin_table.hpp"
-#include "tier4_autoware_utils/math/trigonometry.hpp"
 #include "tier4_autoware_utils/math/unit_conversion.hpp"
 #include "tier4_autoware_utils/ros/debug_publisher.hpp"
 #include "tier4_autoware_utils/ros/debug_traits.hpp"

sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp

@@ -14,8 +14,6 @@
 
 #include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp"
 
-#include "tier4_autoware_utils/math/trigonometry.hpp"
-
 #include <deque>
 #include <string>
 #include <utility>
@@ -219,30 +217,17 @@ bool DistortionCorrectorComponent::undistortPointCloud(
   }
 
   const tf2::Transform tf2_base_link_to_sensor_inv{tf2_base_link_to_sensor.inverse()};
-
-  // For performance, do not instantiate `rclcpp::Time` inside of the for-loop
-  double twist_stamp = rclcpp::Time(twist_it->header.stamp).seconds();
-  double imu_stamp = rclcpp::Time(imu_it->header.stamp).seconds();
-
-  // For performance, instantiate outside of the for-loop
-  tf2::Quaternion baselink_quat{};
-  tf2::Transform baselink_tf_odom{};
-  tf2::Vector3 point{};
-  tf2::Vector3 undistorted_point{};
-
-  // For performance, avoid transform computation if unnecessary
-  bool need_transform = points.header.frame_id != base_link_frame_;
-
   for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) {
-    while (twist_it != std::end(twist_queue_) - 1 && *it_time_stamp > twist_stamp) {
-      ++twist_it;
-      twist_stamp = rclcpp::Time(twist_it->header.stamp).seconds();
+    for (;
+         (twist_it != std::end(twist_queue_) - 1 &&
+          *it_time_stamp > rclcpp::Time(twist_it->header.stamp).seconds());
+         ++twist_it) {
     }
 
     float v{static_cast<float>(twist_it->twist.linear.x)};
     float w{static_cast<float>(twist_it->twist.angular.z)};
 
-    if (std::abs(*it_time_stamp - twist_stamp) > 0.1) {
+    if (std::abs(*it_time_stamp - rclcpp::Time(twist_it->header.stamp).seconds()) > 0.1) {
       RCLCPP_WARN_STREAM_THROTTLE(
         get_logger(), *get_clock(), 10000 /* ms */,
         "twist time_stamp is too late. Could not interpolate.");
@@ -256,13 +241,7 @@ bool DistortionCorrectorComponent::undistortPointCloud(
             *it_time_stamp > rclcpp::Time(imu_it->header.stamp).seconds());
            ++imu_it) {
       }
-
-      while (imu_it != std::end(angular_velocity_queue_) - 1 && *it_time_stamp > imu_stamp) {
-        ++imu_it;
-        imu_stamp = rclcpp::Time(imu_it->header.stamp).seconds();
-      }
-
-      if (std::abs(*it_time_stamp - imu_stamp) > 0.1) {
+      if (std::abs(*it_time_stamp - rclcpp::Time(imu_it->header.stamp).seconds()) > 0.1) {
         RCLCPP_WARN_STREAM_THROTTLE(
           get_logger(), *get_clock(), 10000 /* ms */,
           "imu time_stamp is too late. Could not interpolate.");
@@ -271,34 +250,30 @@ bool DistortionCorrectorComponent::undistortPointCloud(
       }
     }
 
-    const auto time_offset = static_cast<float>(*it_time_stamp - prev_time_stamp_sec);
+    const float time_offset = static_cast<float>(*it_time_stamp - prev_time_stamp_sec);
 
-    point.setValue(*it_x, *it_y, *it_z);
+    const tf2::Vector3 sensorTF_point{*it_x, *it_y, *it_z};
 
-    if (need_transform) {
-      point = tf2_base_link_to_sensor_inv * point;
-    }
+    const tf2::Vector3 base_linkTF_point{tf2_base_link_to_sensor_inv * sensorTF_point};
 
     theta += w * time_offset;
-    baselink_quat.setValue(
-      0, 0, tier4_autoware_utils::sin(theta * 0.5f),
-      tier4_autoware_utils::cos(theta * 0.5f));  // baselink_quat.setRPY(0.0, 0.0, theta);
+    tf2::Quaternion baselink_quat{};
+    baselink_quat.setRPY(0.0, 0.0, theta);
     const float dis = v * time_offset;
-    x += dis * tier4_autoware_utils::cos(theta);
-    y += dis * tier4_autoware_utils::sin(theta);
+    x += dis * std::cos(theta);
+    y += dis * std::sin(theta);
 
-    baselink_tf_odom.setOrigin(tf2::Vector3(x, y, 0.0));
-    baselink_tf_odom.setRotation(baselink_quat);
+    tf2::Transform baselinkTF_odom{};
+    baselinkTF_odom.setOrigin(tf2::Vector3(x, y, 0.0));
+    baselinkTF_odom.setRotation(baselink_quat);
 
-    undistorted_point = baselink_tf_odom * point;
+    const tf2::Vector3 base_linkTF_trans_point{baselinkTF_odom * base_linkTF_point};
 
-    if (need_transform) {
-      undistorted_point = tf2_base_link_to_sensor * undistorted_point;
-    }
+    const tf2::Vector3 sensorTF_trans_point{tf2_base_link_to_sensor * base_linkTF_trans_point};
 
-    *it_x = static_cast<float>(undistorted_point.getX());
-    *it_y = static_cast<float>(undistorted_point.getY());
-    *it_z = static_cast<float>(undistorted_point.getZ());
+    *it_x = sensorTF_trans_point.getX();
+    *it_y = sensorTF_trans_point.getY();
+    *it_z = sensorTF_trans_point.getZ();
 
     prev_time_stamp_sec = *it_time_stamp;
   }