diff --git a/include/subscriber_gnss_fix.h b/include/subscriber_gnss_fix.h index eb43a5790e6964717d860ce7765b6a3508e7bb16..ab5dcf2d78c6cb202f96a08aba4b7f2b323552c9 100644 --- a/include/subscriber_gnss_fix.h +++ b/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, diff --git a/src/subscriber_gnss_fix.cpp b/src/subscriber_gnss_fix.cpp index 0e7243292956ca6d86f3694101f3b861fdd7a154..59f0877808e0882e7d03256ab18a63438e4cc81f 100644 --- a/src/subscriber_gnss_fix.cpp +++ b/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(); }