checking natsatfix status and cov_min diag values

include/subscriber_gnss_fix.h

@@ -61,6 +61,7 @@ class SubscriberGnssFix : public Subscriber
 {
         std::string cov_mode_;
         double cov_factor_;
+        double cov_min_;
         Eigen::Matrix3d cov_;
 
     public:

src/subscriber_gnss_fix.cpp

@@ -20,6 +20,7 @@
 //
 //--------LICENSE_END--------
 #include "../include/subscriber_gnss_fix.h"
+// #include <sensor_msgs/NavSatStatus.h>
 
 namespace wolf
 {
@@ -32,7 +33,8 @@ SubscriberGnssFix::SubscriberGnssFix(const std::string& _unique_name,
     , cov_factor_(1)
 {
 
-    cov_mode_  = _server.getParam<std::string>(prefix_ + "/cov_mode");
+    cov_mode_ = _server.getParam<std::string>(prefix_ + "/cov_mode");
+    cov_min_  = getParamWithDefault<double>   (_server, prefix_ + "/cov_min", 1e-6);
 
     //if (cov_mode_ == "msg") //nothing to be done
     if (cov_mode_ == "manual")
@@ -51,9 +53,15 @@ void SubscriberGnssFix::callback(const sensor_msgs::NavSatFix::ConstPtr& msg)
 {
     updateLastHeader(msg->header);
 
+    if (msg->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX)
+        return;
+
     if (cov_mode_ == "msg" or cov_mode_ == "factor")
         cov_ = cov_factor_ * Eigen::Map<const Eigen::Matrix3d>(msg->position_covariance.data());
 
+    // min cov diagonal values
+    cov_.diagonal() = cov_.diagonal().cwiseMax(cov_min_);
+    
     // Cov fix has the 4th element being clock bias (no information about this)
     Matrix4d cov_fix = Matrix4d::Identity() * 0.1;
     cov_fix.topLeftCorner<3,3>() = cov_;