From a948a7221c520f22cb5db723e6a2636cbef5ea25 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Thu, 1 Dec 2022 11:11:27 +0900 Subject: [PATCH] fix(ekf_localizer): lowpass filter initialization (#2402) * fix(ekf_localizer): lowpass filter initialization Signed-off-by: kminoda * ci(pre-commit): autofix * debug Signed-off-by: kminoda * ci(pre-commit): autofix Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Signed-off-by: Kotaro Yoshimoto --- .../include/ekf_localizer/ekf_localizer.hpp | 5 +++++ .../ekf_localizer/src/ekf_localizer.cpp | 21 ++++++++++++++++++- 2 files changed, 25 insertions(+), 1 deletion(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 8f53e5a63af58..dc5e5659001dd 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -270,6 +270,11 @@ class EKFLocalizer : public rclcpp::Node */ void updateSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); + /** + * @brief initialize simple1DFilter + */ + void initSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); + /** * @brief trigger node */ diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 3d1f2e8bab006..d505f5c528a35 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -351,7 +351,7 @@ void EKFLocalizer::callbackInitialPose( ekf_.init(X, P, params_.extend_state_step); - updateSimple1DFilters(*initialpose); + initSimple1DFilters(*initialpose); } /* @@ -652,6 +652,25 @@ void EKFLocalizer::updateSimple1DFilters(const geometry_msgs::msg::PoseWithCovar pitch_filter_.update(pitch, pitch_dev, pose.header.stamp); } +void EKFLocalizer::initSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +{ + double z = pose.pose.pose.position.z; + double roll = 0.0, pitch = 0.0, yaw_tmp = 0.0; + + tf2::Quaternion q_tf; + tf2::fromMsg(pose.pose.pose.orientation, q_tf); + tf2::Matrix3x3(q_tf).getRPY(roll, pitch, yaw_tmp); + + using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + double z_dev = pose.pose.covariance[COV_IDX::Z_Z]; + double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL]; + double pitch_dev = pose.pose.covariance[COV_IDX::PITCH_PITCH]; + + z_filter_.init(z, z_dev, pose.header.stamp); + roll_filter_.init(roll, roll_dev, pose.header.stamp); + pitch_filter_.init(pitch, pitch_dev, pose.header.stamp); +} + /** * @brief trigger node */