fix(ekf_localizer): add_delay_compensation_for_roll_and_pitch (#8095)

* fix(ekf_localizer)add_delay_compensation_for_roll_and_pitch

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

* rename compensate_pose_with_delay function

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

* rename variables

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

* update comment

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

* rename compensated pose name

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

* rename function name

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

* style(pre-commit): autofix

* calculate delta_orientation with orientation

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

* rename - angular velocity vector

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

* add z element delay compensation

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

* fix typo

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

* check twist msg for last_angular_velocity_

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

* use corrected pitch for delta_z calculation

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

* update pose_with_delay header stamp

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

* style(pre-commit): autofix

* fix local variable naming

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

---------

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Yamato Ando <yamato.ando@gmail.com>

Author: 3 people

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

@@ -239,6 +239,11 @@ class EKFLocalizer : public rclcpp::Node
 
   autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch_;
 
+  /**
+   * @brief last angular velocity for compensating rph with delay
+   */
+  tf2::Vector3 last_angular_velocity_;
+
   friend class EKFLocalizerTestSuite;  // for test code
 };
 #endif  // EKF_LOCALIZER__EKF_LOCALIZER_HPP_

localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp

@@ -29,6 +29,8 @@
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
 
+#include <tf2/utils.h>
+
 #include <memory>
 #include <vector>
 
@@ -77,8 +79,8 @@ class EKFModule
   bool measurement_update_twist(
     const TwistWithCovariance & twist, const rclcpp::Time & t_curr,
     EKFDiagnosticInfo & twist_diag_info);
-  geometry_msgs::msg::PoseWithCovarianceStamped compensate_pose_with_z_delay(
-    const PoseWithCovariance & pose, const double delay_time);
+  geometry_msgs::msg::PoseWithCovarianceStamped compensate_rph_with_delay(
+    const PoseWithCovariance & pose, tf2::Vector3 last_angular_velocity, const double delay_time);
 
 private:
   TimeDelayKalmanFilter kalman_filter_;

localization/ekf_localizer/src/ekf_localizer.cpp

@@ -47,7 +47,8 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options)
   params_(this),
   ekf_dt_(params_.ekf_dt),
   pose_queue_(params_.pose_smoothing_steps),
-  twist_queue_(params_.twist_smoothing_steps)
+  twist_queue_(params_.twist_smoothing_steps),
+  last_angular_velocity_(0.0, 0.0, 0.0)
 {
   /* convert to continuous to discrete */
   proc_cov_vx_d_ = std::pow(params_.proc_stddev_vx_c * ekf_dt_, 2.0);
@@ -187,11 +188,13 @@ void EKFLocalizer::timer_callback()
       if (is_updated) {
         pose_is_updated = true;
 
-        // Update Simple 1D filter with considering change of z value due to measurement pose delay
+        // Update Simple 1D filter with considering change of roll, pitch and height (position z)
+        // values due to measurement pose delay
         const double delay_time =
           (current_time - pose->header.stamp).seconds() + params_.pose_additional_delay;
-        const auto pose_with_z_delay = ekf_module_->compensate_pose_with_z_delay(*pose, delay_time);
-        update_simple_1d_filters(pose_with_z_delay, params_.pose_smoothing_steps);
+        auto pose_with_rph_delay_compensation =
+          ekf_module_->compensate_rph_with_delay(*pose, last_angular_velocity_, delay_time);
+        update_simple_1d_filters(pose_with_rph_delay_compensation, params_.pose_smoothing_steps);
       }
     }
     DEBUG_INFO(
@@ -222,6 +225,10 @@ void EKFLocalizer::timer_callback()
         ekf_module_->measurement_update_twist(*twist, current_time, twist_diag_info_);
       if (is_updated) {
         twist_is_updated = true;
+        last_angular_velocity_ = tf2::Vector3(
+          twist->twist.twist.angular.x, twist->twist.twist.angular.y, twist->twist.twist.angular.z);
+      } else {
+        last_angular_velocity_ = tf2::Vector3(0.0, 0.0, 0.0);
       }
     }
     DEBUG_INFO(

localization/ekf_localizer/src/ekf_module.cpp

@@ -282,15 +282,39 @@ bool EKFModule::measurement_update_pose(
   return true;
 }
 
-geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_pose_with_z_delay(
-  const PoseWithCovariance & pose, const double delay_time)
+geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_rph_with_delay(
+  const PoseWithCovariance & pose, tf2::Vector3 last_angular_velocity, const double delay_time)
 {
-  const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation);
-  const double dz_delay = kalman_filter_.getXelement(IDX::VX) * delay_time * std::sin(-rpy.y);
-  PoseWithCovariance pose_with_z_delay;
-  pose_with_z_delay = pose;
-  pose_with_z_delay.pose.pose.position.z += dz_delay;
-  return pose_with_z_delay;
+  tf2::Quaternion delta_orientation;
+  if (last_angular_velocity.length() > 0.0) {
+    delta_orientation.setRotation(
+      last_angular_velocity.normalized(), last_angular_velocity.length() * delay_time);
+  } else {
+    delta_orientation.setValue(0.0, 0.0, 0.0, 1.0);
+  }
+
+  tf2::Quaternion prev_orientation = tf2::Quaternion(
+    pose.pose.pose.orientation.x, pose.pose.pose.orientation.y, pose.pose.pose.orientation.z,
+    pose.pose.pose.orientation.w);
+
+  tf2::Quaternion curr_orientation;
+  curr_orientation = prev_orientation * delta_orientation;
+  curr_orientation.normalize();
+
+  PoseWithCovariance pose_with_delay;
+  pose_with_delay = pose;
+  pose_with_delay.header.stamp =
+    rclcpp::Time(pose.header.stamp) + rclcpp::Duration::from_seconds(delay_time);
+  pose_with_delay.pose.pose.orientation.x = curr_orientation.x();
+  pose_with_delay.pose.pose.orientation.y = curr_orientation.y();
+  pose_with_delay.pose.pose.orientation.z = curr_orientation.z();
+  pose_with_delay.pose.pose.orientation.w = curr_orientation.w();
+
+  const auto rpy = autoware::universe_utils::getRPY(pose_with_delay.pose.pose.orientation);
+  const double delta_z = kalman_filter_.getXelement(IDX::VX) * delay_time * std::sin(-rpy.y);
+  pose_with_delay.pose.pose.position.z += delta_z;
+
+  return pose_with_delay;
 }
 
 bool EKFModule::measurement_update_twist(