From dd5ec99d08ab6d6c7ff9402509bc23c5f91b64ca Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Thu, 27 Oct 2022 09:07:26 +0200
Subject: [PATCH] New CaptureGnssFix api
---
src/subscriber_gnss_fix.cpp | 11 ++++++++---
1 file changed, 8 insertions(+), 3 deletions(-)
diff --git a/src/subscriber_gnss_fix.cpp b/src/subscriber_gnss_fix.cpp
index 5ce3ca8..f987893 100644
--- a/src/subscriber_gnss_fix.cpp
+++ b/src/subscriber_gnss_fix.cpp
@@ -54,12 +54,17 @@ void SubscriberGnssFix::callback(const sensor_msgs::NavSatFix::ConstPtr& msg)
if (cov_mode_ == "msg" or cov_mode_ == "factor")
cov_ = cov_factor_ * Eigen::Map<const Eigen::Matrix3d>(msg->position_covariance.data());
+ // 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_;
+
CaptureGnssFixPtr cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec),
sensor_ptr_,
- Eigen::Vector3d(msg->latitude * M_PI / 180.0,
+ Eigen::Vector4d(msg->latitude * M_PI / 180.0,
msg->longitude * M_PI / 180.0,
- msg->altitude),
- cov_,
+ msg->altitude,
+ 0),
+ cov_fix,
false); // false = {LatLonAlt fix and ENU cov}
cap_gnss_ptr->process();
}
--
GitLab