diff --git a/src/subscriber_gnss.cpp b/src/subscriber_gnss.cpp
index 5ac0abb70ee6495f2da8a8f12bddf8f7008188f1..af3d6f2378a8094ac8ab5c248fcecaceb6f626fe 100644
--- a/src/subscriber_gnss.cpp
+++ b/src/subscriber_gnss.cpp
@@ -42,7 +42,7 @@ void SubscriberGnss::initialize(ros::NodeHandle& nh, const std::string& topic)
 
 void SubscriberGnss::callbackObservation(const iri_gnss_msgs::Observation::ConstPtr& msg)
 {
-    setLastStamp(msg->header.stamp);
+    updateLastHeader(msg->header);
 
     ROS_DEBUG("callbackObs!");
     if (last_nav_ptr_==nullptr)
diff --git a/src/subscriber_gnss_fix.cpp b/src/subscriber_gnss_fix.cpp
index 7b4dc930625ed47d017db25637e35d11d165990d..ba3d7ab3286610344b7f229066dd01cff78cb190 100644
--- a/src/subscriber_gnss_fix.cpp
+++ b/src/subscriber_gnss_fix.cpp
@@ -49,7 +49,7 @@ void SubscriberGnssFix::initialize(ros::NodeHandle& nh, const std::string& topic
 
 void SubscriberGnssFix::callback(const sensor_msgs::NavSatFix::ConstPtr& msg)
 {
-    setLastStamp(msg->header.stamp);
+    updateLastHeader(msg->header);
 
     if (cov_mode_ == "msg" or cov_mode_ == "factor")
         cov_ = cov_factor_ * Eigen::Map<const Eigen::Matrix3d>(msg->position_covariance.data());
diff --git a/src/subscriber_gnss_fix_novatel_publisher_ecef.cpp b/src/subscriber_gnss_fix_novatel_publisher_ecef.cpp
index a0e90d02396f178defed9155c43ec376a87ec7b0..77a4e3caaae58701919fd8d9b33837d840727ad7 100644
--- a/src/subscriber_gnss_fix_novatel_publisher_ecef.cpp
+++ b/src/subscriber_gnss_fix_novatel_publisher_ecef.cpp
@@ -44,7 +44,7 @@ void SubscriberGnssFixNovatelPublisherEcef::initialize(ros::NodeHandle& nh, cons
 
 void SubscriberGnssFixNovatelPublisherEcef::callback(const novatel_oem7_msgs::BESTPOS::ConstPtr& _msg)
 {
-    setLastStamp(_msg->header.stamp);
+    updateLastHeader(_msg->header);
 
     computeAndPublish(Eigen::Vector3d(_msg->lat * M_PI / 180.0,
                                       _msg->lon * M_PI / 180.0,
diff --git a/src/subscriber_gnss_fix_publisher_ecef.cpp b/src/subscriber_gnss_fix_publisher_ecef.cpp
index 8da4e48297e0d2a352fd007679f4fa4c3c3ce8c6..eebf9b6ccc6765e7277f02f2d4f55f7b4a92baae 100644
--- a/src/subscriber_gnss_fix_publisher_ecef.cpp
+++ b/src/subscriber_gnss_fix_publisher_ecef.cpp
@@ -77,7 +77,7 @@ void SubscriberGnssFixPublisherEcef::initialize(ros::NodeHandle& nh, const std::
 
 void SubscriberGnssFixPublisherEcef::callback(const sensor_msgs::NavSatFix::ConstPtr& _msg)
 {
-    setLastStamp(_msg->header.stamp);
+    updateLastHeader(_msg->header);
 
     computeAndPublish(Eigen::Vector3d(_msg->latitude * M_PI / 180.0,
                                       _msg->longitude * M_PI / 180.0,
diff --git a/src/subscriber_gnss_novatel.cpp b/src/subscriber_gnss_novatel.cpp
index 7bca69db9b9f18d4fb8fc35c933ac144086ec332..4081c70fe114e7b31b9503f3a5be342b188e55be 100644
--- a/src/subscriber_gnss_novatel.cpp
+++ b/src/subscriber_gnss_novatel.cpp
@@ -44,7 +44,7 @@ void SubscriberGnssNovatel::initialize(ros::NodeHandle& nh, const std::string& t
 
 void SubscriberGnssNovatel::callback(const novatel_oem7_msgs::Oem7RawMsg& msg)
 {
-    setLastStamp(msg.header.stamp);
+    updateLastHeader(msg.header);
 
     GnssUtils::RawDataType res = receiver_->addDataStream(msg.message_data);
 
diff --git a/src/subscriber_gnss_ublox.cpp b/src/subscriber_gnss_ublox.cpp
index 80feae9c9cd720e0005ccb4d53f5a3cfbb457177..97a15c5e7579ea20647f05cdb529b6abf3d0b91c 100644
--- a/src/subscriber_gnss_ublox.cpp
+++ b/src/subscriber_gnss_ublox.cpp
@@ -58,9 +58,11 @@ void SubscriberGnssUblox::callback(const std_msgs::UInt8MultiArray& msg)
 {
     ROS_DEBUG("SubscriberGnssUblox::callback message received!");
 
-    auto stamp = ros::Time::now();
-    setLastStamp(stamp);
+    std_msgs::Header header;
+    header.stamp = ros::Time::now();
     n_msgs_++;
+    header.seq = n_msgs_;
+    updateLastHeader(header);
 
     GnssUtils::RawDataType res = receiver_->addDataStream(msg.data);
 
@@ -70,7 +72,7 @@ void SubscriberGnssUblox::callback(const std_msgs::UInt8MultiArray& msg)
     if (res == GnssUtils::OBS)
     {
         //ROS_INFO("Observation received!");
-        createCaptureAndProcess(stamp);
+        createCaptureAndProcess(header.stamp);
     }
 }