From baa63c20343c309dcf4d51b81e357f3663014a24 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Thu, 10 Nov 2022 13:31:51 +0100
Subject: [PATCH] checking natsatfix status and cov_min diag values
---
include/subscriber_gnss_fix.h | 1 +
src/subscriber_gnss_fix.cpp | 10 +++++++++-
2 files changed, 10 insertions(+), 1 deletion(-)
diff --git a/include/subscriber_gnss_fix.h b/include/subscriber_gnss_fix.h
index b005546..e59de44 100644
--- a/include/subscriber_gnss_fix.h
+++ b/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:
diff --git a/src/subscriber_gnss_fix.cpp b/src/subscriber_gnss_fix.cpp
index f987893..51d2400 100644
--- a/src/subscriber_gnss_fix.cpp
+++ b/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_;
--
GitLab