covariance source

include/subscriber_gnss_fix.h

@@ -38,6 +38,10 @@ namespace wolf
 
 class SubscriberGnssFix : public Subscriber
 {
+        std::string cov_mode_;
+        double cov_factor_;
+        Eigen::Matrix3d cov_;
+
     public:
         // Constructor
         SubscriberGnssFix(const std::string& _unique_name,

src/subscriber_gnss_fix.cpp

@@ -8,7 +8,16 @@ SubscriberGnssFix::SubscriberGnssFix(const std::string& _unique_name,
                                      const ParamsServer& _server,
                                      const SensorBasePtr _sensor_ptr)
     : Subscriber(_unique_name, _server, _sensor_ptr)
+    , cov_factor_(1)
 {
+
+    cov_mode_  = _server.getParam<std::string>(prefix_ + "/cov_mode");
+
+    //if (cov_mode_ == "msg") //nothing to be done
+    if (cov_mode_ == "manual")
+        cov_ = _server.getParam<Eigen::Matrix3d>(prefix_ + "/cov");
+    if (cov_mode_ == "factor")
+        cov_factor_ = _server.getParam<double>(prefix_ + "/cov_factor");
 }
 
 
@@ -19,14 +28,15 @@ void SubscriberGnssFix::initialize(ros::NodeHandle& nh, const std::string& topic
 
 void SubscriberGnssFix::callback(const sensor_msgs::NavSatFix::ConstPtr& msg)
 {
-    Eigen::Matrix3d cov = Eigen::Map<const Eigen::Matrix3d>(msg->position_covariance.data());
+    if (cov_mode_ == "msg" or cov_mode_ == "factor")
+        cov_ = cov_factor_ * Eigen::Map<const Eigen::Matrix3d>(msg->position_covariance.data());
 
     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,
                                                                                       msg->longitude * M_PI / 180.0,
                                                                                       msg->altitude),
-                                                                      cov,
+                                                                      cov_,
                                                                       false); // false = {LatLonAlt fix and ENU cov}
     cap_gnss_ptr->process();
 }