fix(ekf_localizer)add_delay_compensation_for_roll_and_pitch

Signed-off-by: Melike Tanrıkulu <[email protected]>

localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp

@@ -77,7 +77,7 @@ 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(
+  geometry_msgs::msg::PoseWithCovarianceStamped compensate_pose_with_delay(
     const PoseWithCovariance & pose, const double delay_time);
 
 private:
@@ -86,6 +86,8 @@ class EKFModule
   std::shared_ptr<Warning> warning_;
   const int dim_x_;
   std::vector<double> accumulated_delay_times_;
+  double roll_rate_ = 0.0;
+  double pitch_rate_ = 0.0;
   const HyperParameters params_;
 };
 

localization/ekf_localizer/src/ekf_localizer.cpp

@@ -190,8 +190,8 @@ void EKFLocalizer::timer_callback()
         // Update Simple 1D filter with considering change of z value 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_delay = ekf_module_->compensate_pose_with_delay(*pose, delay_time);
+        update_simple_1d_filters(pose_with_delay, params_.pose_smoothing_steps);
       }
     }
     DEBUG_INFO(

localization/ekf_localizer/src/ekf_module.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include "ekf_localizer/ekf_module.hpp"
+#include "ekf_localizer/ekf_localizer.hpp"
 
 #include "ekf_localizer/covariance.hpp"
 #include "ekf_localizer/mahalanobis.hpp"
@@ -282,15 +283,24 @@ bool EKFModule::measurement_update_pose(
   return true;
 }
 
-geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_pose_with_z_delay(
+geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_pose_with_delay(
   const PoseWithCovariance & pose, 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;
+  PoseWithCovariance pose_with_delay;
+  pose_with_delay = pose;
+  pose_with_delay.pose.pose.position.z += dz_delay;
+
+  const double roll_delay = roll_rate_ * delay_time;
+  const double pitch_delay = pitch_rate_ * delay_time;
+  const double new_roll = rpy.x + roll_delay;
+  const double new_pitch = rpy.y + pitch_delay;
+  tf2::Quaternion quat;
+  quat.setRPY(new_roll, new_pitch, rpy.z);
+  pose_with_delay.pose.pose.orientation = tf2::toMsg(quat);
+
+  return pose_with_delay;
 }
 
 bool EKFModule::measurement_update_twist(
@@ -329,6 +339,8 @@ bool EKFModule::measurement_update_twist(
   /* Set measurement matrix */
   Eigen::MatrixXd y(dim_y, 1);
   y << twist.twist.twist.linear.x, twist.twist.twist.angular.z;
+  roll_rate_ = twist.twist.twist.angular.x;
+  pitch_rate_ = twist.twist.twist.angular.y;
 
   if (has_nan(y) || has_inf(y)) {
     warning_->warn(