From f3f773c34868c6c9bdefda5554141dc3c9566701 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Melike=20Tanr=C4=B1kulu?= Date: Thu, 18 Jul 2024 13:53:55 +0300 Subject: [PATCH 01/16] fix(ekf_localizer)add_delay_compensation_for_roll_and_pitch MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Melike Tanrıkulu --- .../include/ekf_localizer/ekf_module.hpp | 4 +++- .../ekf_localizer/src/ekf_localizer.cpp | 4 ++-- localization/ekf_localizer/src/ekf_module.cpp | 24 +++++++++++++++---- 3 files changed, 24 insertions(+), 8 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp index ee360e798f492..0b65ecfeb1ad4 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp +++ b/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_; const int dim_x_; std::vector accumulated_delay_times_; + double roll_rate_; + double pitch_rate_; const HyperParameters params_; }; diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index e541939026d95..88e26fab9cefb 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/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( diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index ba6b7dedca82c..238d8dfa1721c 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -37,6 +37,8 @@ EKFModule::EKFModule(std::shared_ptr warning, const HyperParameters & p : warning_(std::move(warning)), dim_x_(6), // x, y, yaw, yaw_bias, vx, wz accumulated_delay_times_(params.extend_state_step, 1.0E15), + roll_rate_(0.0), + pitch_rate_(0.0), params_(params) { Eigen::MatrixXd x = Eigen::MatrixXd::Zero(dim_x_, 1); @@ -282,15 +284,25 @@ 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); + quat.normalize(); + pose_with_delay.pose.pose.orientation = tf2::toMsg(quat); + + return pose_with_delay; } bool EKFModule::measurement_update_twist( @@ -329,6 +341,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( From 91f61469170b07afe599cc6b89c5e774c7788096 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Melike=20Tanr=C4=B1kulu?= Date: Tue, 23 Jul 2024 14:05:16 +0300 Subject: [PATCH 02/16] rename compensate_pose_with_delay function MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Melike Tanrıkulu --- localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp | 2 +- localization/ekf_localizer/src/ekf_localizer.cpp | 2 +- localization/ekf_localizer/src/ekf_module.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp index 0b65ecfeb1ad4..f22919c55414b 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp +++ b/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_delay( + geometry_msgs::msg::PoseWithCovarianceStamped compensate_roll_pitch_height_with_delay( const PoseWithCovariance & pose, const double delay_time); private: diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 88e26fab9cefb..f60ce4fb6813d 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -190,7 +190,7 @@ 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; - auto pose_with_delay = ekf_module_->compensate_pose_with_delay(*pose, delay_time); + auto pose_with_delay = ekf_module_->compensate_roll_pitch_height_with_delay(*pose, delay_time); update_simple_1d_filters(pose_with_delay, params_.pose_smoothing_steps); } } diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 238d8dfa1721c..4ceae04f39410 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -284,7 +284,7 @@ bool EKFModule::measurement_update_pose( return true; } -geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_pose_with_delay( +geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_roll_pitch_height_with_delay( const PoseWithCovariance & pose, const double delay_time) { const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); From bbb95611a208516c751d462dbf0faed8872fb20c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Melike=20Tanr=C4=B1kulu?= Date: Tue, 23 Jul 2024 15:45:37 +0300 Subject: [PATCH 03/16] rename variables MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Melike Tanrıkulu --- localization/ekf_localizer/src/ekf_localizer.cpp | 3 ++- localization/ekf_localizer/src/ekf_module.cpp | 16 ++++++++-------- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index f60ce4fb6813d..b413ff86bccce 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -190,7 +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; - auto pose_with_delay = ekf_module_->compensate_roll_pitch_height_with_delay(*pose, delay_time); + auto pose_with_delay = + ekf_module_->compensate_roll_pitch_height_with_delay(*pose, delay_time); update_simple_1d_filters(pose_with_delay, params_.pose_smoothing_steps); } } diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 4ceae04f39410..06fb5ae443293 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -288,17 +288,17 @@ geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_roll_pitch_h 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); + const double delta_z = kalman_filter_.getXelement(IDX::VX) * delay_time * std::sin(-rpy.y); + const double delta_roll = roll_rate_ * delay_time; + const double delta_pitch = pitch_rate_ * delay_time; + 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; + pose_with_delay.pose.pose.position.z += delta_z; + const double corrected_roll = rpy.x + delta_roll; + const double corrected_pitch = rpy.y + delta_pitch; tf2::Quaternion quat; - quat.setRPY(new_roll, new_pitch, rpy.z); + quat.setRPY(corrected_roll, corrected_pitch, rpy.z); quat.normalize(); pose_with_delay.pose.pose.orientation = tf2::toMsg(quat); From 8a16c03750fde3cb445436c962a1941533773b26 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Melike=20Tanr=C4=B1kulu?= Date: Tue, 23 Jul 2024 17:37:52 +0300 Subject: [PATCH 04/16] update comment MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Melike Tanrıkulu --- localization/ekf_localizer/src/ekf_localizer.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index b413ff86bccce..b91fd6aadc813 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -187,7 +187,8 @@ 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 z values due to + // measurement pose delay const double delay_time = (current_time - pose->header.stamp).seconds() + params_.pose_additional_delay; auto pose_with_delay = From b82629851aa816c52c53a153b54adbbac762b5c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Melike=20Tanr=C4=B1kulu?= Date: Tue, 23 Jul 2024 17:54:33 +0300 Subject: [PATCH 05/16] rename compensated pose name MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Melike Tanrıkulu --- localization/ekf_localizer/src/ekf_localizer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index b91fd6aadc813..4f8f033f9a242 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -191,9 +191,9 @@ void EKFLocalizer::timer_callback() // measurement pose delay const double delay_time = (current_time - pose->header.stamp).seconds() + params_.pose_additional_delay; - auto pose_with_delay = + auto pose_with_rph_delay_compensation = ekf_module_->compensate_roll_pitch_height_with_delay(*pose, delay_time); - update_simple_1d_filters(pose_with_delay, params_.pose_smoothing_steps); + update_simple_1d_filters(pose_with_rph_delay_compensation, params_.pose_smoothing_steps); } } DEBUG_INFO( From b7327c70df9ce0525b8a684910f8770de10d4eb8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Melike=20Tanr=C4=B1kulu?= Date: Tue, 23 Jul 2024 17:58:24 +0300 Subject: [PATCH 06/16] rename function name MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Melike Tanrıkulu --- .../ekf_localizer/include/ekf_localizer/ekf_module.hpp | 2 +- localization/ekf_localizer/src/ekf_localizer.cpp | 4 ++-- localization/ekf_localizer/src/ekf_module.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp index f22919c55414b..8fcdf5bd8b1dc 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp +++ b/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_roll_pitch_height_with_delay( + geometry_msgs::msg::PoseWithCovarianceStamped compensate_rph_with_delay( const PoseWithCovariance & pose, const double delay_time); private: diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 4f8f033f9a242..1bcfb0585345f 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -187,12 +187,12 @@ void EKFLocalizer::timer_callback() if (is_updated) { pose_is_updated = true; - // Update Simple 1D filter with considering change of roll, pitch and z values due to + // 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; auto pose_with_rph_delay_compensation = - ekf_module_->compensate_roll_pitch_height_with_delay(*pose, delay_time); + ekf_module_->compensate_rph_with_delay(*pose, delay_time); update_simple_1d_filters(pose_with_rph_delay_compensation, params_.pose_smoothing_steps); } } diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 06fb5ae443293..2c8efa181326e 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -284,7 +284,7 @@ bool EKFModule::measurement_update_pose( return true; } -geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_roll_pitch_height_with_delay( +geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_rph_with_delay( const PoseWithCovariance & pose, const double delay_time) { const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); From 83161553b805a9a228c313447047569b0c250744 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 23 Jul 2024 15:02:53 +0000 Subject: [PATCH 07/16] style(pre-commit): autofix --- localization/ekf_localizer/src/ekf_localizer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 1bcfb0585345f..fb8b0d670b532 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -187,8 +187,8 @@ void EKFLocalizer::timer_callback() if (is_updated) { pose_is_updated = true; - // Update Simple 1D filter with considering change of roll, pitch and height (position z) values 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; auto pose_with_rph_delay_compensation = From 49935649b5b91a5ccd1161ef5ab85742a1d2a9e4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Melike=20Tanr=C4=B1kulu?= Date: Mon, 29 Jul 2024 13:24:28 +0300 Subject: [PATCH 08/16] calculate delta_orientation with orientation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Melike Tanrıkulu --- .../include/ekf_localizer/ekf_module.hpp | 5 ++- localization/ekf_localizer/src/ekf_module.cpp | 38 +++++++++++-------- 2 files changed, 26 insertions(+), 17 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp index 8fcdf5bd8b1dc..ae2ed969a456f 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp @@ -29,6 +29,8 @@ #include #include +#include + #include #include @@ -86,8 +88,7 @@ class EKFModule std::shared_ptr warning_; const int dim_x_; std::vector accumulated_delay_times_; - double roll_rate_; - double pitch_rate_; + tf2::Vector3 last_rotation_axis_; const HyperParameters params_; }; diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 2c8efa181326e..3652f3bcd657a 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -37,8 +37,7 @@ EKFModule::EKFModule(std::shared_ptr warning, const HyperParameters & p : warning_(std::move(warning)), dim_x_(6), // x, y, yaw, yaw_bias, vx, wz accumulated_delay_times_(params.extend_state_step, 1.0E15), - roll_rate_(0.0), - pitch_rate_(0.0), + last_rotation_axis_(0, 0, 0), params_(params) { Eigen::MatrixXd x = Eigen::MatrixXd::Zero(dim_x_, 1); @@ -287,20 +286,28 @@ bool EKFModule::measurement_update_pose( geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_rph_with_delay( const PoseWithCovariance & pose, const double delay_time) { - const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); - const double delta_z = kalman_filter_.getXelement(IDX::VX) * delay_time * std::sin(-rpy.y); - const double delta_roll = roll_rate_ * delay_time; - const double delta_pitch = pitch_rate_ * delay_time; + tf2::Quaternion delta_orientation; + if (last_rotation_axis_.length() > 0.0) { + delta_orientation.setRotation( + last_rotation_axis_.normalized(), last_rotation_axis_.length() * delay_time); + } else { + delta_orientation.setValue(0.0, 0.0, 0.0, 1.0); + } + + tf2::Quaternion prew_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 = prew_orientation * delta_orientation; + curr_orientation.normalize(); PoseWithCovariance pose_with_delay; pose_with_delay = pose; - pose_with_delay.pose.pose.position.z += delta_z; - const double corrected_roll = rpy.x + delta_roll; - const double corrected_pitch = rpy.y + delta_pitch; - tf2::Quaternion quat; - quat.setRPY(corrected_roll, corrected_pitch, rpy.z); - quat.normalize(); - pose_with_delay.pose.pose.orientation = tf2::toMsg(quat); + 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(); return pose_with_delay; } @@ -341,8 +348,9 @@ 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; + + last_rotation_axis_ = tf2::Vector3( + twist.twist.twist.angular.x, twist.twist.twist.angular.y, twist.twist.twist.angular.z); if (has_nan(y) || has_inf(y)) { warning_->warn( From 03309e236ca9acbdaecdbb626375262e5728b3f1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Melike=20Tanr=C4=B1kulu?= Date: Mon, 29 Jul 2024 13:35:51 +0300 Subject: [PATCH 09/16] rename - angular velocity vector MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Melike Tanrıkulu --- .../ekf_localizer/include/ekf_localizer/ekf_module.hpp | 2 +- localization/ekf_localizer/src/ekf_module.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp index ae2ed969a456f..cf9623c29e879 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp @@ -88,7 +88,7 @@ class EKFModule std::shared_ptr warning_; const int dim_x_; std::vector accumulated_delay_times_; - tf2::Vector3 last_rotation_axis_; + tf2::Vector3 last_angular_velocity_; const HyperParameters params_; }; diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 3652f3bcd657a..3d9320a51b057 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -37,7 +37,7 @@ EKFModule::EKFModule(std::shared_ptr warning, const HyperParameters & p : warning_(std::move(warning)), dim_x_(6), // x, y, yaw, yaw_bias, vx, wz accumulated_delay_times_(params.extend_state_step, 1.0E15), - last_rotation_axis_(0, 0, 0), + last_angular_velocity_(0, 0, 0), params_(params) { Eigen::MatrixXd x = Eigen::MatrixXd::Zero(dim_x_, 1); @@ -287,9 +287,9 @@ geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_rph_with_del const PoseWithCovariance & pose, const double delay_time) { tf2::Quaternion delta_orientation; - if (last_rotation_axis_.length() > 0.0) { + if (last_angular_velocity_.length() > 0.0) { delta_orientation.setRotation( - last_rotation_axis_.normalized(), last_rotation_axis_.length() * delay_time); + last_angular_velocity_.normalized(), last_angular_velocity_.length() * delay_time); } else { delta_orientation.setValue(0.0, 0.0, 0.0, 1.0); } @@ -349,7 +349,7 @@ bool EKFModule::measurement_update_twist( Eigen::MatrixXd y(dim_y, 1); y << twist.twist.twist.linear.x, twist.twist.twist.angular.z; - last_rotation_axis_ = tf2::Vector3( + last_angular_velocity_ = tf2::Vector3( twist.twist.twist.angular.x, twist.twist.twist.angular.y, twist.twist.twist.angular.z); if (has_nan(y) || has_inf(y)) { From 1ddbee81490f8b35ed5e159f011c7dc79fad6091 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Melike=20Tanr=C4=B1kulu?= Date: Tue, 30 Jul 2024 12:06:06 +0300 Subject: [PATCH 10/16] add z element delay compensation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Melike Tanrıkulu --- localization/ekf_localizer/src/ekf_module.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 3d9320a51b057..2efcaf2ce4e9e 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -302,8 +302,12 @@ geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_rph_with_del curr_orientation = prew_orientation * delta_orientation; curr_orientation.normalize(); + const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); + const double delta_z = kalman_filter_.getXelement(IDX::VX) * delay_time * std::sin(-rpy.y); + PoseWithCovariance pose_with_delay; pose_with_delay = pose; + pose_with_delay.pose.pose.position.z += delta_z; 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(); From 33989271a435a9856c46c36fbb6918433a9384cc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Melike=20Tanr=C4=B1kulu?= Date: Tue, 30 Jul 2024 13:32:16 +0300 Subject: [PATCH 11/16] fix typo MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Melike Tanrıkulu --- localization/ekf_localizer/src/ekf_module.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 2efcaf2ce4e9e..db3e330bfce07 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -294,12 +294,12 @@ geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_rph_with_del delta_orientation.setValue(0.0, 0.0, 0.0, 1.0); } - tf2::Quaternion prew_orientation = tf2::Quaternion( + 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 = prew_orientation * delta_orientation; + curr_orientation = prev_orientation * delta_orientation; curr_orientation.normalize(); const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); From a061f420eb6f960737c1e18793f67912e6591712 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Melike=20Tanr=C4=B1kulu?= Date: Tue, 30 Jul 2024 16:11:51 +0300 Subject: [PATCH 12/16] check twist msg for last_angular_velocity_ MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Melike Tanrıkulu --- .../ekf_localizer/include/ekf_localizer/ekf_localizer.hpp | 5 +++++ .../ekf_localizer/include/ekf_localizer/ekf_module.hpp | 3 +-- localization/ekf_localizer/src/ekf_localizer.cpp | 8 ++++++-- localization/ekf_localizer/src/ekf_module.cpp | 6 +----- 4 files changed, 13 insertions(+), 9 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index ada9024faceee..0b3f64caabe41 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -239,6 +239,11 @@ class EKFLocalizer : public rclcpp::Node autoware::universe_utils::StopWatch 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_ diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp index cf9623c29e879..7b22ba9bdb73f 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp @@ -80,7 +80,7 @@ class EKFModule const TwistWithCovariance & twist, const rclcpp::Time & t_curr, EKFDiagnosticInfo & twist_diag_info); geometry_msgs::msg::PoseWithCovarianceStamped compensate_rph_with_delay( - const PoseWithCovariance & pose, const double delay_time); + const PoseWithCovariance & pose,tf2::Vector3 last_angular_velocity, const double delay_time); private: TimeDelayKalmanFilter kalman_filter_; @@ -88,7 +88,6 @@ class EKFModule std::shared_ptr warning_; const int dim_x_; std::vector accumulated_delay_times_; - tf2::Vector3 last_angular_velocity_; const HyperParameters params_; }; diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index fb8b0d670b532..09c42163c5f49 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/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); @@ -192,7 +193,7 @@ void EKFLocalizer::timer_callback() const double delay_time = (current_time - pose->header.stamp).seconds() + params_.pose_additional_delay; auto pose_with_rph_delay_compensation = - ekf_module_->compensate_rph_with_delay(*pose, delay_time); + 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); } } @@ -224,6 +225,9 @@ 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( diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index db3e330bfce07..dd2012e4c9492 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -37,7 +37,6 @@ EKFModule::EKFModule(std::shared_ptr warning, const HyperParameters & p : warning_(std::move(warning)), dim_x_(6), // x, y, yaw, yaw_bias, vx, wz accumulated_delay_times_(params.extend_state_step, 1.0E15), - last_angular_velocity_(0, 0, 0), params_(params) { Eigen::MatrixXd x = Eigen::MatrixXd::Zero(dim_x_, 1); @@ -284,7 +283,7 @@ bool EKFModule::measurement_update_pose( } geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_rph_with_delay( - const PoseWithCovariance & pose, const double delay_time) + const PoseWithCovariance & pose, tf2::Vector3 last_angular_velocity_, const double delay_time) { tf2::Quaternion delta_orientation; if (last_angular_velocity_.length() > 0.0) { @@ -353,9 +352,6 @@ bool EKFModule::measurement_update_twist( Eigen::MatrixXd y(dim_y, 1); y << twist.twist.twist.linear.x, twist.twist.twist.angular.z; - last_angular_velocity_ = tf2::Vector3( - twist.twist.twist.angular.x, twist.twist.twist.angular.y, twist.twist.twist.angular.z); - if (has_nan(y) || has_inf(y)) { warning_->warn( "[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message."); From 5af1316998995ce8770d54a7f5d2cb87772927f3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Melike=20Tanr=C4=B1kulu?= Date: Tue, 30 Jul 2024 16:26:14 +0300 Subject: [PATCH 13/16] use corrected pitch for delta_z calculation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Melike Tanrıkulu --- .../ekf_localizer/include/ekf_localizer/ekf_module.hpp | 2 +- localization/ekf_localizer/src/ekf_localizer.cpp | 5 +++-- localization/ekf_localizer/src/ekf_module.cpp | 8 ++++---- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp index 7b22ba9bdb73f..5eda1156327b5 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp @@ -80,7 +80,7 @@ class EKFModule const TwistWithCovariance & twist, const rclcpp::Time & t_curr, EKFDiagnosticInfo & twist_diag_info); geometry_msgs::msg::PoseWithCovarianceStamped compensate_rph_with_delay( - const PoseWithCovariance & pose,tf2::Vector3 last_angular_velocity, const double delay_time); + const PoseWithCovariance & pose, tf2::Vector3 last_angular_velocity, const double delay_time); private: TimeDelayKalmanFilter kalman_filter_; diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 09c42163c5f49..37880f4e4139a 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -225,8 +225,9 @@ 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( + 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); } } diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index dd2012e4c9492..b41eb80db70dd 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -301,17 +301,17 @@ geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_rph_with_del curr_orientation = prev_orientation * delta_orientation; curr_orientation.normalize(); - const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); - const double delta_z = kalman_filter_.getXelement(IDX::VX) * delay_time * std::sin(-rpy.y); - PoseWithCovariance pose_with_delay; pose_with_delay = pose; - pose_with_delay.pose.pose.position.z += delta_z; 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; } From 622c0769a2e29fd61b69c1a08d8d5251807fdd1c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Melike=20Tanr=C4=B1kulu?= Date: Tue, 30 Jul 2024 16:40:43 +0300 Subject: [PATCH 14/16] update pose_with_delay header stamp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Melike Tanrıkulu --- localization/ekf_localizer/src/ekf_module.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index b41eb80db70dd..90c9b0ae4729a 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -303,6 +303,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_rph_with_del 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(); From 1acb118a6c9a8885a26056a943c1812113f2bdd6 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 30 Jul 2024 13:43:31 +0000 Subject: [PATCH 15/16] style(pre-commit): autofix --- localization/ekf_localizer/src/ekf_module.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 90c9b0ae4729a..72e4df5f71817 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -303,7 +303,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_rph_with_del 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.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(); From e33ab613afb6332d5fc633dcb35c4d75f1c34fbc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Melike=20Tanr=C4=B1kulu?= Date: Wed, 31 Jul 2024 12:02:28 +0300 Subject: [PATCH 16/16] fix local variable naming MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Melike Tanrıkulu --- localization/ekf_localizer/src/ekf_module.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 72e4df5f71817..5c804afdc8cda 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -283,12 +283,12 @@ bool EKFModule::measurement_update_pose( } geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_rph_with_delay( - const PoseWithCovariance & pose, tf2::Vector3 last_angular_velocity_, const double delay_time) + const PoseWithCovariance & pose, tf2::Vector3 last_angular_velocity, const double delay_time) { tf2::Quaternion delta_orientation; - if (last_angular_velocity_.length() > 0.0) { + if (last_angular_velocity.length() > 0.0) { delta_orientation.setRotation( - last_angular_velocity_.normalized(), last_angular_velocity_.length() * delay_time); + last_angular_velocity.normalized(), last_angular_velocity.length() * delay_time); } else { delta_orientation.setValue(0.0, 0.0, 0.0, 1.0); }