added check of publishers receiving messages

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;

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());
 

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),

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),

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;

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;