Add cov in reliable_rtkfix_msg

MapIV · Nov 28, 2023 · 69ad812 · 69ad812
1 parent a7479c0
commit 69ad812
Showing 1 changed file with 6 additions and 0 deletions.
diff --git 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);
     }