feat(ekf_lolcalizer): output variance for z, roll and pitch (autoware…

…foundation#8554) publish covariance of z,roll.pitch Signed-off-by: Yamato Ando <[email protected]>
kyoichi-sugahara · Aug 27, 2024 · f1518d7 · f1518d7
1 parent 8cd9705
commit f1518d7
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 0 deletions.
diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
@@ -87,6 +87,7 @@ class Simple1DFilter
   };
   void set_proc_dev(const double proc_dev) { proc_dev_x_c_ = proc_dev; }
   [[nodiscard]] double get_x() const { return x_; }
+  [[nodiscard]] double get_dev() const { return dev_; }
 
 private:
   bool initialized_;

diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp
@@ -365,6 +365,12 @@ void EKFLocalizer::publish_estimate_result(
   pose_cov.header.frame_id = current_ekf_pose.header.frame_id;
   pose_cov.pose.pose = current_ekf_pose.pose;
   pose_cov.pose.covariance = ekf_module_->get_current_pose_covariance();
+
+  using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
+  pose_cov.pose.covariance[COV_IDX::Z_Z] = z_filter_.get_dev();
+  pose_cov.pose.covariance[COV_IDX::ROLL_ROLL] = roll_filter_.get_dev();
+  pose_cov.pose.covariance[COV_IDX::PITCH_PITCH] = pitch_filter_.get_dev();
+
   pub_pose_cov_->publish(pose_cov);
 
   geometry_msgs::msg::PoseWithCovarianceStamped biased_pose_cov = pose_cov;