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