feat(pointcloud_preprocessor): update azimuth and distance in distortion_correction_node

sensing/pointcloud_preprocessor/docs/distortion-corrector.md

@@ -39,9 +39,12 @@
 
 ### Core Parameters
 
-| Name                   | Type   | Default Value | Description                                                 |
-| ---------------------- | ------ | ------------- | ----------------------------------------------------------- |
-| `timestamp_field_name` | string | "time_stamp"  | time stamp field name                                       |
-| `use_imu`              | bool   | true          | use gyroscope for yaw rate if true, else use vehicle status |
+| Name                          | Type   | Default Value | Description                                                 |
+| ----------------------------- | ------ | ------------- | ----------------------------------------------------------- |
+| `timestamp_field_name`        | string | "time_stamp"  | time stamp field name                                       |
+| `use_imu`                     | bool   | true          | use gyroscope for yaw rate if true, else use vehicle status |
+| `update_azimuth_and_distance` | bool   | false         | update the azimuth and distance based on undistorted xyz    |
 
 ## Assumptions / Known limits
+
+When setting the parameter `update_azimuth_and_distance` to True, the node will calculate the azimuth value based on the undistorted XYZ by using atan2, which will return the azimuth value with the coordinate system that the x-axis is 0 degrees and the y-axis is 90 degrees. Please make sure the frame coordinates are same as the coordinate system mentioned in the above. For autoware user, the frame_id of input pointcloud is base_link, which the coordinate system is same as the coordinate system mentioned above, therefore it won't casue any issues. Also note that by setting the parameter `update_azimuth_and_distance` to True, the time will incrase around 13%.

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp

@@ -36,6 +36,7 @@
 #include <tf2_ros/transform_listener.h>
 
 // Include tier4 autoware utils
+#include <opencv2/opencv.hpp>
 #include <tier4_autoware_utils/ros/debug_publisher.hpp>
 #include <tier4_autoware_utils/system/stop_watch.hpp>
 
@@ -81,6 +82,7 @@ class DistortionCorrectorComponent : public rclcpp::Node
   std::string base_link_frame_ = "base_link";
   std::string time_stamp_field_name_;
   bool use_imu_;
+  bool update_azimuth_and_distance_;
 };
 
 }  // namespace pointcloud_preprocessor

sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp

@@ -39,6 +39,7 @@
   // Parameter
   time_stamp_field_name_ = declare_parameter("time_stamp_field_name", "time_stamp");
   use_imu_ = declare_parameter("use_imu", true);
+  update_azimuth_and_distance_ = declare_parameter("update_azimuth_and_distance", false);
 
   // Publisher
   undistorted_points_pub_ =
@@ -123,6 +124,8 @@
     return;
   }
 
+  update_azimuth_and_distance_ = get_parameter("update_azimuth_and_distance").as_bool();
+
   tf2::Transform tf2_base_link_to_sensor{};
   getTransform(points_msg->header.frame_id, base_link_frame_, &tf2_base_link_to_sensor);
 
@@ -192,116 +195,122 @@
   sensor_msgs::PointCloud2Iterator<float> it_x(points, "x");
   sensor_msgs::PointCloud2Iterator<float> it_y(points, "y");
   sensor_msgs::PointCloud2Iterator<float> it_z(points, "z");
+  sensor_msgs::PointCloud2Iterator<float> it_azimuth(points, "azimuth");
+  sensor_msgs::PointCloud2Iterator<float> it_distance(points, "distance");
   sensor_msgs::PointCloud2ConstIterator<double> it_time_stamp(points, time_stamp_field_name_);
 
   float theta{0.0f};
   float x{0.0f};
   float y{0.0f};
   double prev_time_stamp_sec{*it_time_stamp};
   const double first_point_time_stamp_sec{*it_time_stamp};
 
   auto twist_it = std::lower_bound(
     std::begin(twist_queue_), std::end(twist_queue_), first_point_time_stamp_sec,
     [](const geometry_msgs::msg::TwistStamped & x, const double t) {
       return rclcpp::Time(x.header.stamp).seconds() < t;
     });
   twist_it = twist_it == std::end(twist_queue_) ? std::end(twist_queue_) - 1 : twist_it;
 
   decltype(angular_velocity_queue_)::iterator imu_it;
   if (use_imu_ && !angular_velocity_queue_.empty()) {
     imu_it = std::lower_bound(
       std::begin(angular_velocity_queue_), std::end(angular_velocity_queue_),
       first_point_time_stamp_sec, [](const geometry_msgs::msg::Vector3Stamped & x, const double t) {
         return rclcpp::Time(x.header.stamp).seconds() < t;
       });
     imu_it =
       imu_it == std::end(angular_velocity_queue_) ? std::end(angular_velocity_queue_) - 1 : imu_it;
   }
 
   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();
 
   // 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) {
+  for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_azimuth, ++it_distance, ++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();
     }
 
     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) {
       RCLCPP_WARN_STREAM_THROTTLE(
         get_logger(), *get_clock(), 10000 /* ms */,
         "twist time_stamp is too late. Could not interpolate.");
       v = 0.0f;
       w = 0.0f;
     }
 
     if (use_imu_ && !angular_velocity_queue_.empty()) {
       // For performance, do not instantiate `rclcpp::Time` inside of the for-loop
       double imu_stamp = rclcpp::Time(imu_it->header.stamp).seconds();
 
       for (;
            (imu_it != std::end(angular_velocity_queue_) - 1 &&
             *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) {
         RCLCPP_WARN_STREAM_THROTTLE(
           get_logger(), *get_clock(), 10000 /* ms */,
           "imu time_stamp is too late. Could not interpolate.");
       } else {
         w = static_cast<float>(imu_it->vector.z);
       }
     }
 
     const auto time_offset = static_cast<float>(*it_time_stamp - prev_time_stamp_sec);
 
     point.setValue(*it_x, *it_y, *it_z);
 
     if (need_transform) {
       point = tf2_base_link_to_sensor_inv * 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);
     const float dis = v * time_offset;
     x += dis * tier4_autoware_utils::cos(theta);
     y += dis * tier4_autoware_utils::sin(theta);
 
     baselink_tf_odom.setOrigin(tf2::Vector3(x, y, 0.0));
     baselink_tf_odom.setRotation(baselink_quat);
 
     undistorted_point = baselink_tf_odom * point;
 
     if (need_transform) {
       undistorted_point = tf2_base_link_to_sensor * undistorted_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());
 
+    if (update_azimuth_and_distance_) {
+      *it_distance = sqrt(*it_x * *it_x + *it_y * *it_y + *it_z * *it_z);
+      *it_azimuth = cv::fastAtan2(*it_y, *it_x) * 100;
+    }
     prev_time_stamp_sec = *it_time_stamp;
   }
   return true;