feat(distortion_corrector): use gyroscope for correcting LiDAR distor…

…tion (tier4#1120) * first commit Signed-off-by: kminoda <koji.m.minoda@gmail.com> * ci(pre-commit): autofix * check if angular_velocity_queue_ is empty or not Signed-off-by: kminoda <koji.m.minoda@gmail.com> * move vehicle velocity converter to sensing Signed-off-by: kminoda <koji.m.minoda@gmail.com> * ci(pre-commit): autofix * fix Signed-off-by: kminoda <koji.m.minoda@gmail.com> * ci(pre-commit): autofix * reflected reviews Signed-off-by: kminoda <koji.m.minoda@gmail.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
boyali · Oct 3, 2022 · 762a116 · 762a116
1 parent 56ddd41
commit 762a116
Show file tree

Hide file tree

Showing 12 changed files with 118 additions and 53 deletions.
diff --git a/launch/tier4_localization_launch/launch/twist_estimator/twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/twist_estimator/twist_estimator.launch.xml
@@ -1,11 +1,7 @@
 <?xml version="1.0"?>
 <launch>
-  <include file="$(find-pkg-share vehicle_velocity_converter)/launch/vehicle_velocity_converter.launch.xml">
-    <arg name="input_vehicle_velocity_topic" value="/vehicle/status/velocity_status"/>
-    <arg name="output_twist_with_covariance" value="/localization/twist_estimator/vehicle_velocity_converter/twist_with_covariance"/>
-  </include>
   <include file="$(find-pkg-share gyro_odometer)/launch/gyro_odometer.launch.xml">
-    <arg name="input_vehicle_twist_with_covariance_topic" value="/localization/twist_estimator/vehicle_velocity_converter/twist_with_covariance"/>
+    <arg name="input_vehicle_twist_with_covariance_topic" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
     <arg name="output_twist_with_covariance_topic" value="/localization/twist_estimator/twist_with_covariance"/>
     <arg name="output_twist_with_covariance_raw_topic" value="/localization/twist_estimator/twist_with_covariance_raw"/>
   </include>

diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md
@@ -23,10 +23,11 @@ $ ExactPointTime = TimeStamp + TimeOffset $
 
 ### Input
 
-| Name                      | Type                                              | Description      |
-| ------------------------- | ------------------------------------------------- | ---------------- |
-| `~/input/points`          | `sensor_msgs::msg::PointCloud2`                   | reference points |
-| `~/input/velocity_report` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | vehicle velocity |
+| Name             | Type                                             | Description      |
+| ---------------- | ------------------------------------------------ | ---------------- |
+| `~/input/points` | `sensor_msgs::msg::PointCloud2`                  | reference points |
+| `~/input/twist`  | `geometry_msgs::msg::TwistWithCovarianceStamped` | twist            |
+| `~/input/imu`    | `sensor_msgs::msg::Imu`                          | imu data         |
 
 ### Output
 
@@ -38,8 +39,9 @@ $ ExactPointTime = TimeStamp + TimeOffset $
 
 ### Core Parameters
 
-| Name                   | Type   | Default Value | Description           |
-| ---------------------- | ------ | ------------- | --------------------- |
-| `timestamp_field_name` | string | "time_stamp"  | time stamp field name |
+| 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 |
 
 ## Assumptions / Known limits
diff --git a/...reprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/...reprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -17,7 +17,9 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include <autoware_auto_vehicle_msgs/msg/velocity_report.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include <sensor_msgs/msg/imu.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 
@@ -43,7 +45,6 @@
 
 namespace pointcloud_preprocessor
 {
-using autoware_auto_vehicle_msgs::msg::VelocityReport;
 using rcl_interfaces::msg::SetParametersResult;
 using sensor_msgs::msg::PointCloud2;
 
@@ -54,17 +55,18 @@ class DistortionCorrectorComponent : public rclcpp::Node
 
 private:
   void onPointCloud(PointCloud2::UniquePtr points_msg);
-  void onVelocityReport(const VelocityReport::ConstSharedPtr velocity_report_msg);
+  void onTwistWithCovarianceStamped(
+    const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg);
+  void onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg);
   bool getTransform(
     const std::string & target_frame, const std::string & source_frame,
     tf2::Transform * tf2_transform_ptr);
 
-  bool undistortPointCloud(
-    const std::deque<VelocityReport> & velocity_report_queue,
-    const tf2::Transform & tf2_base_link_to_sensor, PointCloud2 & points);
+  bool undistortPointCloud(const tf2::Transform & tf2_base_link_to_sensor, PointCloud2 & points);
 
   rclcpp::Subscription<PointCloud2>::SharedPtr input_points_sub_;
-  rclcpp::Subscription<VelocityReport>::SharedPtr velocity_report_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
+  rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_;
   rclcpp::Publisher<PointCloud2>::SharedPtr undistorted_points_pub_;
 
   std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
@@ -73,10 +75,12 @@ class DistortionCorrectorComponent : public rclcpp::Node
   tf2_ros::Buffer tf2_buffer_{get_clock()};
   tf2_ros::TransformListener tf2_listener_{tf2_buffer_};
 
-  std::deque<autoware_auto_vehicle_msgs::msg::VelocityReport> velocity_report_queue_;
+  std::deque<geometry_msgs::msg::TwistStamped> twist_queue_;
+  std::deque<geometry_msgs::msg::Vector3Stamped> angular_velocity_queue_;
 
   std::string base_link_frame_ = "base_link";
   std::string time_stamp_field_name_;
+  bool use_imu_;
 };
 
 }  // namespace pointcloud_preprocessor

diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -36,35 +36,76 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt
 
   // Parameter
   time_stamp_field_name_ = declare_parameter("time_stamp_field_name", "time_stamp");
+  use_imu_ = declare_parameter("use_imu", true);
 
   // Publisher
   undistorted_points_pub_ =
     this->create_publisher<PointCloud2>("~/output/pointcloud", rclcpp::SensorDataQoS());
 
   // Subscriber
-  velocity_report_sub_ = this->create_subscription<VelocityReport>(
-    "~/input/velocity_report", 10,
-    std::bind(&DistortionCorrectorComponent::onVelocityReport, this, std::placeholders::_1));
+  twist_sub_ = this->create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
+    "~/input/twist", 10,
+    std::bind(
+      &DistortionCorrectorComponent::onTwistWithCovarianceStamped, this, std::placeholders::_1));
+  imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
+    "~/input/imu", 10,
+    std::bind(&DistortionCorrectorComponent::onImu, this, std::placeholders::_1));
   input_points_sub_ = this->create_subscription<PointCloud2>(
     "~/input/pointcloud", rclcpp::SensorDataQoS(),
     std::bind(&DistortionCorrectorComponent::onPointCloud, this, std::placeholders::_1));
 }
 
-void DistortionCorrectorComponent::onVelocityReport(
-  const VelocityReport::ConstSharedPtr velocity_report_msg)
+void DistortionCorrectorComponent::onTwistWithCovarianceStamped(
+  const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg)
 {
-  velocity_report_queue_.push_back(*velocity_report_msg);
+  geometry_msgs::msg::TwistStamped msg;
+  msg.header = twist_msg->header;
+  msg.twist = twist_msg->twist.twist;
+  twist_queue_.push_back(msg);
 
-  while (!velocity_report_queue_.empty()) {
+  while (!twist_queue_.empty()) {
+    // for replay rosbag
+    if (rclcpp::Time(twist_queue_.front().header.stamp) > rclcpp::Time(twist_msg->header.stamp)) {
+      twist_queue_.pop_front();
+    } else if (  // NOLINT
+      rclcpp::Time(twist_queue_.front().header.stamp) <
+      rclcpp::Time(twist_msg->header.stamp) - rclcpp::Duration::from_seconds(1.0)) {
+      twist_queue_.pop_front();
+    }
+    break;
+  }
+}
+
+void DistortionCorrectorComponent::onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg)
+{
+  if (!use_imu_) {
+    return;
+  }
+
+  tf2::Transform tf2_imu_link_to_base_link{};
+  getTransform(base_link_frame_, imu_msg->header.frame_id, &tf2_imu_link_to_base_link);
+  geometry_msgs::msg::TransformStamped::SharedPtr tf_base2imu_ptr =
+    std::make_shared<geometry_msgs::msg::TransformStamped>();
+  tf_base2imu_ptr->transform.rotation = tf2::toMsg(tf2_imu_link_to_base_link.getRotation());
+
+  geometry_msgs::msg::Vector3Stamped angular_velocity;
+  angular_velocity.vector = imu_msg->angular_velocity;
+
+  geometry_msgs::msg::Vector3Stamped transformed_angular_velocity;
+  tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_base2imu_ptr);
+  transformed_angular_velocity.header = imu_msg->header;
+  angular_velocity_queue_.push_back(transformed_angular_velocity);
+
+  while (!angular_velocity_queue_.empty()) {
     // for replay rosbag
     if (
-      rclcpp::Time(velocity_report_queue_.front().header.stamp) >
-      rclcpp::Time(velocity_report_msg->header.stamp)) {
-      velocity_report_queue_.pop_front();
+      rclcpp::Time(angular_velocity_queue_.front().header.stamp) >
+      rclcpp::Time(imu_msg->header.stamp)) {
+      angular_velocity_queue_.pop_front();
     } else if (  // NOLINT
-      rclcpp::Time(velocity_report_queue_.front().header.stamp) <
-      rclcpp::Time(velocity_report_msg->header.stamp) - rclcpp::Duration::from_seconds(1.0)) {
-      velocity_report_queue_.pop_front();
+      rclcpp::Time(angular_velocity_queue_.front().header.stamp) <
+      rclcpp::Time(imu_msg->header.stamp) - rclcpp::Duration::from_seconds(1.0)) {
+      angular_velocity_queue_.pop_front();
     }
     break;
   }
@@ -83,11 +124,10 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr points_ms
   tf2::Transform tf2_base_link_to_sensor{};
   getTransform(points_msg->header.frame_id, base_link_frame_, &tf2_base_link_to_sensor);
 
-  undistortPointCloud(velocity_report_queue_, tf2_base_link_to_sensor, *points_msg);
+  undistortPointCloud(tf2_base_link_to_sensor, *points_msg);
+
+  undistorted_points_pub_->publish(std::move(points_msg));
 
-  if (points_sub_count > 0) {
-    undistorted_points_pub_->publish(std::move(points_msg));
-  }
   // add processing time for debug
   if (debug_publisher_) {
     const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
@@ -126,13 +166,12 @@ bool DistortionCorrectorComponent::getTransform(
 }
 
 bool DistortionCorrectorComponent::undistortPointCloud(
-  const std::deque<VelocityReport> & velocity_report_queue_,
   const tf2::Transform & tf2_base_link_to_sensor, PointCloud2 & points)
 {
-  if (points.data.empty() || velocity_report_queue_.empty()) {
+  if (points.data.empty() || twist_queue_.empty()) {
     RCLCPP_WARN_STREAM_THROTTLE(
       get_logger(), *get_clock(), 10000 /* ms */,
-      "input_pointcloud->points or velocity_report_queue_ is empty.");
+      "input_pointcloud->points or twist_queue_ is empty.");
     return false;
   }
 
@@ -159,34 +198,58 @@ bool DistortionCorrectorComponent::undistortPointCloud(
   double prev_time_stamp_sec{*it_time_stamp};
   const double first_point_time_stamp_sec{*it_time_stamp};
 
-  auto velocity_report_it = std::lower_bound(
-    std::begin(velocity_report_queue_), std::end(velocity_report_queue_),
-    first_point_time_stamp_sec, [](const VelocityReport & x, const double t) {
+  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;
     });
-  velocity_report_it = velocity_report_it == std::end(velocity_report_queue_)
-                         ? std::end(velocity_report_queue_) - 1
-                         : velocity_report_it;
+  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 (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) {
     for (;
-         (velocity_report_it != std::end(velocity_report_queue_) - 1 &&
-          *it_time_stamp > rclcpp::Time(velocity_report_it->header.stamp).seconds());
-         ++velocity_report_it) {
+         (twist_it != std::end(twist_queue_) - 1 &&
+          *it_time_stamp > rclcpp::Time(twist_it->header.stamp).seconds());
+         ++twist_it) {
     }
 
-    float v{static_cast<float>(velocity_report_it->longitudinal_velocity)};
-    float w{static_cast<float>(velocity_report_it->heading_rate)};
+    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 - rclcpp::Time(velocity_report_it->header.stamp).seconds()) > 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 */,
-        "velocity_report time_stamp is too late. Cloud not interpolate.");
+        "twist time_stamp is too late. Could not interpolate.");
       v = 0.0f;
       w = 0.0f;
     }
 
+    if (use_imu_ && !angular_velocity_queue_.empty()) {
+      for (;
+           (imu_it != std::end(angular_velocity_queue_) - 1 &&
+            *it_time_stamp > rclcpp::Time(imu_it->header.stamp).seconds());
+           ++imu_it) {
+      }
+      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.");
+      } else {
+        w = static_cast<float>(imu_it->vector.z);
+      }
+    }
+
     const float time_offset = static_cast<float>(*it_time_stamp - prev_time_stamp_sec);
 
     const tf2::Vector3 sensorTF_point{*it_x, *it_y, *it_z};

diff --git a/...vehicle_velocity_converter/CMakeLists.txt → ...vehicle_velocity_converter/CMakeLists.txt b/...vehicle_velocity_converter/CMakeLists.txt → ...vehicle_velocity_converter/CMakeLists.txt
diff --git a/...tion/vehicle_velocity_converter/README.md → sensing/vehicle_velocity_converter/README.md b/...tion/vehicle_velocity_converter/README.md → sensing/vehicle_velocity_converter/README.md
diff --git a/...fig/vehicle_velocity_converter.param.yaml → ...fig/vehicle_velocity_converter.param.yaml b/...fig/vehicle_velocity_converter.param.yaml → ...fig/vehicle_velocity_converter.param.yaml
diff --git a/..._converter/vehicle_velocity_converter.hpp → ..._converter/vehicle_velocity_converter.hpp b/..._converter/vehicle_velocity_converter.hpp → ..._converter/vehicle_velocity_converter.hpp
diff --git a/...nch/vehicle_velocity_converter.launch.xml → ...nch/vehicle_velocity_converter.launch.xml b/...nch/vehicle_velocity_converter.launch.xml → ...nch/vehicle_velocity_converter.launch.xml
diff --git a/...on/vehicle_velocity_converter/package.xml → ...ng/vehicle_velocity_converter/package.xml b/...on/vehicle_velocity_converter/package.xml → ...ng/vehicle_velocity_converter/package.xml
diff --git a/...verter/src/vehicle_velocity_converter.cpp → ...verter/src/vehicle_velocity_converter.cpp b/...verter/src/vehicle_velocity_converter.cpp → ...verter/src/vehicle_velocity_converter.cpp
diff --git a/...r/src/vehicle_velocity_converter_node.cpp → ...r/src/vehicle_velocity_converter_node.cpp b/...r/src/vehicle_velocity_converter_node.cpp → ...r/src/vehicle_velocity_converter_node.cpp