working

src/wolf_ros_subscriber_gnss_fix.cpp

@@ -18,14 +18,11 @@ void SubscriberWrapperGnssFix::callback(const sensor_msgs::NavSatFix::ConstPtr&
 {
     Eigen::Matrix3d cov = Eigen::Map<const Eigen::Matrix3d>(msg->position_covariance.data());
 
-    std::cout << "New fix: " << Eigen::Vector3d(msg->latitude,msg->longitude,msg->altitude).transpose() << std::endl;
-
-    Eigen::Vector3d latlonalt_rads(msg->latitude,msg->longitude,msg->altitude);
-    latlonalt_rads.head<2>() *= M_PI/180.0;
-
     CaptureGnssFixPtr cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec),
                                                                       sensor_ptr_,
-                                                                      latlonalt_rads,
+                                                                      Eigen::Vector3d(msg->latitude * M_PI / 180.0,
+                                                                                      msg->longitude * M_PI / 180.0,
+                                                                                      msg->altitude),
                                                                       cov,
                                                                       false); // false = {LatLonAlt fix and ENU cov}
     cap_gnss_ptr->process();