From 69ad812ff98e504ac5cdb5f123736bb964487b1e Mon Sep 17 00:00:00 2001
From: Ryohei Sasaki <ryohei.sasaki@map4.jp>
Date: Tue, 28 Nov 2023 16:51:24 +0900
Subject: [PATCH] Add cov in reliable_rtkfix_msg

---
 eagleye_rt/src/rtkfix_plane_validation_node.cpp | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/eagleye_rt/src/rtkfix_plane_validation_node.cpp b/eagleye_rt/src/rtkfix_plane_validation_node.cpp
index b03aed0d..6aa896f9 100644
--- a/eagleye_rt/src/rtkfix_plane_validation_node.cpp
+++ b/eagleye_rt/src/rtkfix_plane_validation_node.cpp
@@ -70,6 +70,7 @@ class RtkfixPlaneValidationNode
   rclcpp::Subscription<geometry_msgs::msg::Vector3Stamped>::SharedPtr enu_vel_sub_;
 
   nmea_msgs::msg::Gpgga gga_;
+  sensor_msgs::msg::NavSatFix::ConstSharedPtr fix_ptr_;
   eagleye_msgs::msg::Distance distance_;
   geometry_msgs::msg::Vector3Stamped enu_vel_;
 
@@ -110,6 +111,7 @@ class RtkfixPlaneValidationNode
     }
 
     gga_ = gga;
+    fix_ptr_ = msg;
     is_gga_ready_ = true;
   }
 
@@ -157,6 +159,10 @@ class RtkfixPlaneValidationNode
       sensor_msgs::msg::NavSatFix reliable_rtkfix_msg;
       reliable_rtkfix_msg = reliable_rtkfix_status.fix_msg;
 
+      if(fix_ptr_ != nullptr)
+      {
+        reliable_rtkfix_msg.position_covariance = fix_ptr_->position_covariance;
+      }
       // Publish
       reliable_rtkfix_pub_->publish(reliable_rtkfix_msg);
     }