From 04a0ce9ad624e8481c7617bbbf4cf69bf6676a60 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Fri, 24 Dec 2021 12:19:47 +0100
Subject: [PATCH] adapted to new subscriber API
---
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 | 8 +++++---
6 files changed, 10 insertions(+), 8 deletions(-)
diff --git a/src/subscriber_gnss.cpp b/src/subscriber_gnss.cpp
index 5ac0abb..af3d6f2 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 7b4dc93..ba3d7ab 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 a0e90d0..77a4e3c 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 8da4e48..eebf9b6 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 7bca69d..4081c70 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 80feae9..97a15c5 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);
}
}
--
GitLab