From a3f5fb698c9ab2e7dc6494a155dcf8245fd19f07 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Thu, 18 Feb 2021 22:14:14 +0100
Subject: [PATCH] added check of publishers receiving messages
---
src/subscriber_gnss.cpp | 2 ++
src/subscriber_gnss_fix.cpp | 2 ++
src/subscriber_gnss_fix_novatel_publisher_ecef.cpp | 2 ++
src/subscriber_gnss_fix_publisher_ecef.cpp | 2 ++
src/subscriber_gnss_novatel.cpp | 2 ++
src/subscriber_gnss_ublox.cpp | 2 ++
6 files changed, 12 insertions(+)
diff --git a/src/subscriber_gnss.cpp b/src/subscriber_gnss.cpp
index 7099f02..3d4337f 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 59f0877..29a69f5 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 70127b5..0d77429 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 6edf1a6..afa3316 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 a903c61..118c1e1 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 d4f43b3..53fb704 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;
--
GitLab