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 */