diff --git a/src/subscriber_gnss.cpp b/src/subscriber_gnss.cpp
index 7099f02d60e538bd62ee98b4ee50c04a48cfb08f..3d4337f43f2db5e3446511c3c9b50c1d06f4913a 100644
--- a/src/subscriber_gnss.cpp
+++ b/src/subscriber_gnss.cpp
@@ -21,6 +21,8 @@ void SubscriberGnss::initialize(ros::NodeHandle& nh, const std::string& topic)
 
 void SubscriberGnss::callbackObservation(const iri_gnss_msgs::Observation::ConstPtr& msg)
 {
+    setLastStamp(msg->header.stamp);
+
     ROS_DEBUG("callbackObs!");
     if (last_nav_ptr_==nullptr)
         return;
diff --git a/src/subscriber_gnss_fix.cpp b/src/subscriber_gnss_fix.cpp
index 59f0877808e0882e7d03256ab18a63438e4cc81f..29a69f5da2d55b68381f60736354c0aa7effe05d 100644
--- a/src/subscriber_gnss_fix.cpp
+++ b/src/subscriber_gnss_fix.cpp
@@ -28,6 +28,8 @@ void SubscriberGnssFix::initialize(ros::NodeHandle& nh, const std::string& topic
 
 void SubscriberGnssFix::callback(const sensor_msgs::NavSatFix::ConstPtr& msg)
 {
+    setLastStamp(msg->header.stamp);
+
     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 70127b5dd0470bf7d81800df4386be88fb5ebb1c..0d77429cf79df4d1c66ba6de4b9ba43cf1aab850 100644
--- a/src/subscriber_gnss_fix_novatel_publisher_ecef.cpp
+++ b/src/subscriber_gnss_fix_novatel_publisher_ecef.cpp
@@ -23,6 +23,8 @@ void SubscriberGnssFixNovatelPublisherEcef::initialize(ros::NodeHandle& nh, cons
 
 void SubscriberGnssFixNovatelPublisherEcef::callback(const novatel_oem7_msgs::BESTPOS::ConstPtr& _msg)
 {
+    setLastStamp(_msg->header.stamp);
+
     computeAndPublish(Eigen::Vector3d(_msg->lat * M_PI / 180.0,
                                       _msg->lon * M_PI / 180.0,
                                       _msg->hgt+_msg->undulation),
diff --git a/src/subscriber_gnss_fix_publisher_ecef.cpp b/src/subscriber_gnss_fix_publisher_ecef.cpp
index 6edf1a6b07626c6481b131d3891e38ba51827e2c..afa33164cf9910d5a0be45ff265aaf71c5e110c5 100644
--- a/src/subscriber_gnss_fix_publisher_ecef.cpp
+++ b/src/subscriber_gnss_fix_publisher_ecef.cpp
@@ -58,6 +58,8 @@ void SubscriberGnssFixPublisherEcef::initialize(ros::NodeHandle& nh, const std::
 
 void SubscriberGnssFixPublisherEcef::callback(const sensor_msgs::NavSatFix::ConstPtr& _msg)
 {
+    setLastStamp(_msg->header.stamp);
+
     computeAndPublish(Eigen::Vector3d(_msg->latitude * M_PI / 180.0,
                                       _msg->longitude * M_PI / 180.0,
                                       _msg->altitude),
diff --git a/src/subscriber_gnss_novatel.cpp b/src/subscriber_gnss_novatel.cpp
index a903c61e8d817c45cd41b71e2d45f2d730c23a15..118c1e14d95ecd71d572558599799068fd3d6e1a 100644
--- a/src/subscriber_gnss_novatel.cpp
+++ b/src/subscriber_gnss_novatel.cpp
@@ -23,6 +23,8 @@ void SubscriberGnssNovatel::initialize(ros::NodeHandle& nh, const std::string& t
 
 void SubscriberGnssNovatel::callback(const novatel_oem7_msgs::Oem7RawMsg& msg)
 {
+    setLastStamp(msg->header.stamp);
+
     GnssUtils::RawDataType res = receiver_->addDataStream(msg.message_data);
 
     //std::cout << "res = " << (int) res << std::endl;
diff --git a/src/subscriber_gnss_ublox.cpp b/src/subscriber_gnss_ublox.cpp
index d4f43b3f1931ace3e2e0caf8d3e011edceaa6927..53fb70406143800f1602f6044959538f28597c63 100644
--- a/src/subscriber_gnss_ublox.cpp
+++ b/src/subscriber_gnss_ublox.cpp
@@ -24,6 +24,8 @@ void SubscriberGnssUblox::initialize(ros::NodeHandle& nh, const std::string& top
 void SubscriberGnssUblox::callback(const std_msgs::UInt8MultiArray& msg)
 {
     auto stamp = ros::Time::now();
+    setLastStamp(stamp);
+
     GnssUtils::RawDataType res = receiver_->addDataStream(msg.data);
 
     //std::cout << "res = " << (int) res << std::endl;