refactor: change camel to snake case

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

sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp

@@ -62,21 +62,21 @@
 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,47 +95,48 @@
   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)
   {
     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 @@
   {
   }
   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 @@
   {
   }
   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;
 };
 

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

sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp

@@ -1,4 +1,4 @@
 // Copyright 2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -26,31 +26,31 @@
 {
 
 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 @@
 }
 
 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,15 +92,15 @@
   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 =
     tf2::toMsg(tf2_imu_to_base_link.getRotation());
 }
 
 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 @@
 }
 
 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 @@
 }
 
 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,33 +188,33 @@
 }
 
 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;
 
   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 {
     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;
   }
 
   for (; next_it_x != it_x.end();
@@ -262,11 +262,11 @@
     if (
       std::abs(offset_rad - multiple_of_90_degrees * (autoware::universe_utils::pi / 2)) >
       angle_conversion.offset_rad_threshold) {
       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;
     }
 
     // Limit the range of offset_rad in [0, 360)
@@ -279,91 +279,91 @@
   return std::nullopt;
 }
 
 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.");
     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{
     pointcloud.header.stamp.sec + 1e-9 * (pointcloud.header.stamp.nanosec + *it_time_stamp)};
   const double first_point_time_stamp_sec{
     pointcloud.header.stamp.sec + 1e-9 * (pointcloud.header.stamp.nanosec + *it_time_stamp)};
 
   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();
   double imu_stamp{0.0};
   if (use_imu && !angular_velocity_queue_.empty()) {
     imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds();
   }
 
   // If there is a point in a pointcloud that cannot be associated, record it to issue a warning
   bool is_twist_time_stamp_too_late = false;
   bool is_imu_time_stamp_too_late = false;
 
   for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) {
     bool is_twist_valid = true;
     bool is_imu_valid = true;
 
     const double global_point_stamp =
       pointcloud.header.stamp.sec + 1e-9 * (pointcloud.header.stamp.nanosec + *it_time_stamp);
 
     // Get closest twist information
     while (it_twist != std::end(twist_queue_) - 1 && global_point_stamp > twist_stamp) {
       ++it_twist;
       twist_stamp = rclcpp::Time(it_twist->header.stamp).seconds();
     }
     if (std::abs(global_point_stamp - twist_stamp) > 0.1) {
       is_twist_time_stamp_too_late = true;
       is_twist_valid = false;
     }
 
     // Get closest IMU information
     if (use_imu && !angular_velocity_queue_.empty()) {
       while (it_imu != std::end(angular_velocity_queue_) - 1 && global_point_stamp > imu_stamp) {
         ++it_imu;
         imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds();
       }
 
       if (std::abs(global_point_stamp - imu_stamp) > 0.1) {
         is_imu_time_stamp_too_late = true;
         is_imu_valid = false;
       }
     } else {
       is_imu_valid = false;
     }
 
     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 +
                               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;
@@ -376,11 +376,11 @@
     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 @@
 }
 
 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 @@
   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 @@
   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,39 +448,39 @@
   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,
   std::deque<geometry_msgs::msg::Vector3Stamped>::iterator & it_imu, const float & time_offset,
   const bool & is_twist_valid, const bool & is_imu_valid)
 {
   // Initialize linear velocity and angular velocity
   float v{0.0f}, 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);
   }
   if (is_imu_valid) {
     w = static_cast<float>(it_imu->vector.z);
   }
 
   // Undistort point
   point_tf_.setValue(*it_x, *it_y, *it_z);
 
   if (pointcloud_transform_needed_) {
     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, 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 * cos_theta;
   y_ += dis * sin_theta;
 
   baselink_tf_odom_.setOrigin(tf2::Vector3(x_, y_, 0.0));
   baselink_tf_odom_.setRotation(baselink_quat_);
@@ -496,7 +496,7 @@
   *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,