fix: solve conflicts

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
autowarefoundation · vividf · Jul 3, 2024 · May 16, 2024 · May 16, 2024 · May 20, 2024
commit 7f76efba0698c73643b836a7d56e0e71d43651e9
diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp
@@ -37,8 +37,13 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt
   use_3d_distortion_correction_ = declare_parameter<bool>("use_3d_distortion_correction");
 
   // Publisher
-  undistorted_pointcloud_pub_ =
-    this->create_publisher<PointCloud2>("~/output/pointcloud", rclcpp::SensorDataQoS());
+  {
+    rclcpp::PublisherOptions pub_options;
+    pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
+    // Publisher
+    undistorted_pointcloud_pub_ = this->create_publisher<PointCloud2>(
+      "~/output/pointcloud", rclcpp::SensorDataQoS(), pub_options);
+  }
 
   // Subscriber
   twist_sub_ = this->create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(

diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
 // 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.
  }
 }

 template <class T>
 bool DistortionCorrector<T>::isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud)
 {
  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.");
    return false;
  }

  auto time_stamp_field_it = std::find_if(
    std::cbegin(pointcloud.fields), std::cend(pointcloud.fields),
    [](const sensor_msgs::msg::PointField & field) { return field.name == "time_stamp"; });
  if (time_stamp_field_it == pointcloud.fields.cend()) {
    RCLCPP_WARN_STREAM_THROTTLE(
      node_->get_logger(), *node_->get_clock(), 10000 /* ms */,
      "Required field time stamp doesn't exist in the point cloud.");
    return false;
  }
  return true;
 }

 template <class T>
 void DistortionCorrector<T>::undistortPointCloud(
  bool use_imu, 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::PointCloud2ConstIterator<double> it_time_stamp(pointcloud, "time_stamp");

  double prev_time_stamp_sec{*it_time_stamp};
  const double first_point_time_stamp_sec{*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);

  // 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;
  bool is_twist_valid = true;
  bool is_imu_valid = true;

  for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) {
    is_twist_valid = true;
    is_imu_valid = true;

    // Get closest twist information
    while (it_twist != std::end(twist_queue_) - 1 && *it_time_stamp > twist_stamp) {
      ++it_twist;
      twist_stamp = rclcpp::Time(it_twist->header.stamp).seconds();
    }
    if (std::abs(*it_time_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 && *it_time_stamp > imu_stamp) {
        ++it_imu;
        imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds();
      }

      if (std::abs(*it_time_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>(*it_time_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);

    prev_time_stamp_sec = *it_time_stamp;
  }

  warnIfTimestampIsTooLate(is_twist_time_stamp_too_late, is_imu_time_stamp_too_late);
 }

 template <class T>
 void DistortionCorrector<T>::warnIfTimestampIsTooLate(
  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.");
  }
 }

 ///////////////////////// Functions for different undistortion strategies /////////////////////////

 void DistortionCorrector2D::initialize()
 {
  x_ = 0.0f;
  y_ = 0.0f;
  theta_ = 0.0f;
 }

 void DistortionCorrector3D::initialize()
 {
  prev_transformation_matrix_ = Eigen::Matrix4f::Identity();
 }

 void DistortionCorrector2D::setPointCloudTransform(
  const std::string & base_frame, const std::string & lidar_frame)
 {
  if (pointcloud_transform_exists_) {
    return;
  }

  if (base_frame == lidar_frame) {
    tf2_lidar_to_base_link_.setOrigin(tf2::Vector3(0.0, 0.0, 0.0));
    tf2_lidar_to_base_link_.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0));
    tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_;
    pointcloud_transform_exists_ = true;
  } else {
    try {
      const auto transform_msg =
        tf_buffer_.lookupTransform(base_frame, lidar_frame, tf2::TimePointZero);
      tf2::convert(transform_msg.transform, tf2_lidar_to_base_link_);
      tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_.inverse();
      pointcloud_transform_exists_ = true;
      pointcloud_transform_needed_ = true;
    } catch (const tf2::TransformException & ex) {
      RCLCPP_WARN(node_->get_logger(), "%s", ex.what());
      RCLCPP_ERROR(
        node_->get_logger(), "Please publish TF %s to %s", base_frame.c_str(), lidar_frame.c_str());

      tf2_lidar_to_base_link_.setOrigin(tf2::Vector3(0.0, 0.0, 0.0));
      tf2_lidar_to_base_link_.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0));
      tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_;
    }
  }
 }

 void DistortionCorrector3D::setPointCloudTransform(
  const std::string & base_frame, const std::string & lidar_frame)
 {
  if (pointcloud_transform_exists_) {
    return;
  }

  if (base_frame == lidar_frame) {
    eigen_lidar_to_base_link_ = Eigen::Matrix4f::Identity();
    eigen_base_link_to_lidar_ = Eigen::Matrix4f::Identity();
    pointcloud_transform_exists_ = true;
  }

  try {
    const auto transform_msg =
      tf_buffer_.lookupTransform(base_frame, lidar_frame, tf2::TimePointZero);
    eigen_lidar_to_base_link_ =
      tf2::transformToEigen(transform_msg.transform).matrix().cast<float>();
    eigen_base_link_to_lidar_ = eigen_lidar_to_base_link_.inverse();
    pointcloud_transform_exists_ = true;
    pointcloud_transform_needed_ = true;
  } catch (const tf2::TransformException & ex) {
    RCLCPP_WARN(node_->get_logger(), "%s", ex.what());
    RCLCPP_ERROR(
      node_->get_logger(), "Please publish TF %s to %s", base_frame.c_str(), lidar_frame.c_str());
    eigen_lidar_to_base_link_ = Eigen::Matrix4f::Identity();
    eigen_base_link_to_lidar_ = Eigen::Matrix4f::Identity();
  }
 }

 inline void DistortionCorrector2D::undistortPointImplementation(
  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;
  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);
  const float dis = v * time_offset;
  x_ += dis * autoware::universe_utils::cos(theta_);
  y_ += dis * autoware::universe_utils::sin(theta_);

  baselink_tf_odom_.setOrigin(tf2::Vector3(x_, y_, 0.0));
  baselink_tf_odom_.setRotation(baselink_quat_);

  undistorted_point_tf_ = baselink_tf_odom_ * point_tf_;

  if (pointcloud_transform_needed_) {
    undistorted_point_tf_ = tf2_base_link_to_lidar_ * undistorted_point_tf_;
  }

  *it_x = static_cast<float>(undistorted_point_tf_.getX());
  *it_y = static_cast<float>(undistorted_point_tf_.getY());
  *it_z = static_cast<float>(undistorted_point_tf_.getZ());
 }

 inline void DistortionCorrector3D::undistortPointImplementation(
  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_x_{0.0f}, v_y_{0.0f}, v_z_{0.0f}, w_x_{0.0f}, w_y_{0.0f}, 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);
  }
  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);
  }

  // Undistort point
  point_eigen_ << *it_x, *it_y, *it_z, 1.0;
  if (pointcloud_transform_needed_) {
    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_);
  twist = twist * time_offset;
  transformation_matrix_ = Sophus::SE3f::exp(twist).matrix();
  transformation_matrix_ = transformation_matrix_ * prev_transformation_matrix_;
  undistorted_point_eigen_ = transformation_matrix_ * point_eigen_;

  if (pointcloud_transform_needed_) {
    undistorted_point_eigen_ = eigen_base_link_to_lidar_ * undistorted_point_eigen_;
  }
  *it_x = undistorted_point_eigen_[0];
  *it_y = undistorted_point_eigen_[1];
  *it_z = undistorted_point_eigen_[2];

  prev_transformation_matrix_ = transformation_matrix_;
 }

 }  // namespace pointcloud_preprocessor
	// Copyright 2024 TIER IV, Inc.
Check warning on line 1 in sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main) ❌ New issue: Overall Code Complexity `This module has a mean cyclomatic complexity of 4.14 across 14 functions. The mean complexity threshold is 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.` Check warning on line 1 in sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main) ❌ New issue: Primitive Obsession `In this module, 48.6% of all function arguments are primitive types, threshold = 30.0%. The functions in this file have too many primitive types (e.g. int, double, float) in their function argument lists. Using many primitive types lead to the code smell Primitive Obsession. Avoid adding more primitive arguments.`
	//
	// Licensed under the Apache License, Version 2.0 (the "License");
	// you may not use this file except in compliance with the License.
	}
	}

	template <class T>
	bool DistortionCorrector<T>::isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud)
	{
	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.");
	return false;
	}

	auto time_stamp_field_it = std::find_if(
	std::cbegin(pointcloud.fields), std::cend(pointcloud.fields),
	[](const sensor_msgs::msg::PointField & field) { return field.name == "time_stamp"; });
	if (time_stamp_field_it == pointcloud.fields.cend()) {
	RCLCPP_WARN_STREAM_THROTTLE(
	node_->get_logger(), node_->get_clock(), 10000 / ms */,
	"Required field time stamp doesn't exist in the point cloud.");
	return false;
	}
	return true;
	}

	template <class T>
	void DistortionCorrector<T>::undistortPointCloud(
	bool use_imu, 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::PointCloud2ConstIterator<double> it_time_stamp(pointcloud, "time_stamp");

	double prev_time_stamp_sec{*it_time_stamp};
	const double first_point_time_stamp_sec{*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);

	// 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;
	bool is_twist_valid = true;
	bool is_imu_valid = true;

	for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) {
	is_twist_valid = true;
	is_imu_valid = true;

	// Get closest twist information
	while (it_twist != std::end(twist_queue_) - 1 && *it_time_stamp > twist_stamp) {
	++it_twist;
	twist_stamp = rclcpp::Time(it_twist->header.stamp).seconds();
	}
	if (std::abs(*it_time_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 && *it_time_stamp > imu_stamp) {
	++it_imu;
	imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds();
	}

	if (std::abs(*it_time_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>(*it_time_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);

	prev_time_stamp_sec = *it_time_stamp;
	}

	warnIfTimestampIsTooLate(is_twist_time_stamp_too_late, is_imu_time_stamp_too_late);
	}
Check notice on line 234 in sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main) ℹ New issue: Complex Method `undistortPointCloud has a cyclomatic complexity of 13, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.` Check notice on line 234 in sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main) ℹ New issue: Bumpy Road Ahead `undistortPointCloud has 2 blocks with nested conditional logic. Any nesting of 2 or deeper is considered. Threshold is one single, nested block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.`

	template <class T>
	void DistortionCorrector<T>::warnIfTimestampIsTooLate(
	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.");
	}
	}

	///////////////////////// Functions for different undistortion strategies /////////////////////////

	void DistortionCorrector2D::initialize()
	{
	x_ = 0.0f;
	y_ = 0.0f;
	theta_ = 0.0f;
	}

	void DistortionCorrector3D::initialize()
	{
	prev_transformation_matrix_ = Eigen::Matrix4f::Identity();
	}

	void DistortionCorrector2D::setPointCloudTransform(
	const std::string & base_frame, const std::string & lidar_frame)
	{
	if (pointcloud_transform_exists_) {
	return;
	}

	if (base_frame == lidar_frame) {
	tf2_lidar_to_base_link_.setOrigin(tf2::Vector3(0.0, 0.0, 0.0));
	tf2_lidar_to_base_link_.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0));
	tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_;
	pointcloud_transform_exists_ = true;
	} else {
	try {
	const auto transform_msg =
	tf_buffer_.lookupTransform(base_frame, lidar_frame, tf2::TimePointZero);
	tf2::convert(transform_msg.transform, tf2_lidar_to_base_link_);
	tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_.inverse();
	pointcloud_transform_exists_ = true;
	pointcloud_transform_needed_ = true;
	} catch (const tf2::TransformException & ex) {
	RCLCPP_WARN(node_->get_logger(), "%s", ex.what());
	RCLCPP_ERROR(
	node_->get_logger(), "Please publish TF %s to %s", base_frame.c_str(), lidar_frame.c_str());

	tf2_lidar_to_base_link_.setOrigin(tf2::Vector3(0.0, 0.0, 0.0));
	tf2_lidar_to_base_link_.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0));
	tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_;
	}
	}
	}

	void DistortionCorrector3D::setPointCloudTransform(
	const std::string & base_frame, const std::string & lidar_frame)
	{
	if (pointcloud_transform_exists_) {
	return;
	}

	if (base_frame == lidar_frame) {
	eigen_lidar_to_base_link_ = Eigen::Matrix4f::Identity();
	eigen_base_link_to_lidar_ = Eigen::Matrix4f::Identity();
	pointcloud_transform_exists_ = true;
	}

	try {
	const auto transform_msg =
	tf_buffer_.lookupTransform(base_frame, lidar_frame, tf2::TimePointZero);
	eigen_lidar_to_base_link_ =
	tf2::transformToEigen(transform_msg.transform).matrix().cast<float>();
	eigen_base_link_to_lidar_ = eigen_lidar_to_base_link_.inverse();
	pointcloud_transform_exists_ = true;
	pointcloud_transform_needed_ = true;
	} catch (const tf2::TransformException & ex) {
	RCLCPP_WARN(node_->get_logger(), "%s", ex.what());
	RCLCPP_ERROR(
	node_->get_logger(), "Please publish TF %s to %s", base_frame.c_str(), lidar_frame.c_str());
	eigen_lidar_to_base_link_ = Eigen::Matrix4f::Identity();
	eigen_base_link_to_lidar_ = Eigen::Matrix4f::Identity();
	}
	}

	inline void DistortionCorrector2D::undistortPointImplementation(
	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;
	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);
	const float dis = v * time_offset;
	x_ += dis * autoware::universe_utils::cos(theta_);
	y_ += dis * autoware::universe_utils::sin(theta_);

	baselink_tf_odom_.setOrigin(tf2::Vector3(x_, y_, 0.0));
	baselink_tf_odom_.setRotation(baselink_quat_);

	undistorted_point_tf_ = baselink_tf_odom_ * point_tf_;

	if (pointcloud_transform_needed_) {
	undistorted_point_tf_ = tf2_base_link_to_lidar_ * undistorted_point_tf_;
	}

	*it_x = static_cast<float>(undistorted_point_tf_.getX());
	*it_y = static_cast<float>(undistorted_point_tf_.getY());
	*it_z = static_cast<float>(undistorted_point_tf_.getZ());
Check notice on line 371 in sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main) ✅ No longer an issue: Complex Method `DistortionCorrectorComponent::undistortPointCloud is no longer above the threshold for cyclomatic complexity. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.` Check notice on line 371 in sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main) ✅ No longer an issue: Bumpy Road Ahead `DistortionCorrectorComponent::undistortPointCloud is no longer above the threshold for logical blocks with deeply nested code. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.` Check warning on line 371 in sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main) ❌ New issue: Excess Number of Function Arguments `DistortionCorrector2D::undistortPointImplementation has 8 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.`
	}

	inline void DistortionCorrector3D::undistortPointImplementation(
	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_x_{0.0f}, v_y_{0.0f}, v_z_{0.0f}, w_x_{0.0f}, w_y_{0.0f}, 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);
	}
	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);
	}

	// Undistort point
	point_eigen_ << it_x, it_y, *it_z, 1.0;
	if (pointcloud_transform_needed_) {
	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_);
	twist = twist * time_offset;
	transformation_matrix_ = Sophus::SE3f::exp(twist).matrix();
	transformation_matrix_ = transformation_matrix_ * prev_transformation_matrix_;
	undistorted_point_eigen_ = transformation_matrix_ * point_eigen_;

	if (pointcloud_transform_needed_) {
	undistorted_point_eigen_ = eigen_base_link_to_lidar_ * undistorted_point_eigen_;
	}
	*it_x = undistorted_point_eigen_[0];
	*it_y = undistorted_point_eigen_[1];
	*it_z = undistorted_point_eigen_[2];

	prev_transformation_matrix_ = transformation_matrix_;
	}
Check warning on line 417 in sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main) ❌ New issue: Excess Number of Function Arguments `DistortionCorrector3D::undistortPointImplementation has 8 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.`

	} // namespace pointcloud_preprocessor