diff --git a/include/subscriber_gnss_ublox.h b/include/subscriber_gnss_ublox.h index 8f5f6779f907365a56bf45e8baa0ee1f35e9ee56..6be1861aa8dc4ba0c8115f7d68330bf898ada469 100644 --- a/include/subscriber_gnss_ublox.h +++ b/include/subscriber_gnss_ublox.h @@ -39,11 +39,14 @@ class SubscriberGnssUblox : public SubscriberGnssReceiver const ParamsServer& _server, const SensorBasePtr _sensor_ptr); WOLF_SUBSCRIBER_CREATE(SubscriberGnssUblox); + virtual ~SubscriberGnssUblox(); virtual void initialize(ros::NodeHandle& nh, const std::string& topic); void callback(const std_msgs::UInt8MultiArray& msg); + protected: + long unsigned int n_msgs_; }; WOLF_REGISTER_SUBSCRIBER(SubscriberGnssUblox); diff --git a/src/subscriber_gnss_novatel.cpp b/src/subscriber_gnss_novatel.cpp index bd70676fe67cd4c4dd25f54acf93e2ddfa4fa3dd..574308997c0b2619007bbf367d9e787a96f0e2b8 100644 --- a/src/subscriber_gnss_novatel.cpp +++ b/src/subscriber_gnss_novatel.cpp @@ -18,7 +18,7 @@ void SubscriberGnssNovatel::initialize(ros::NodeHandle& nh, const std::string& t { SubscriberGnssReceiver::initialize(nh, topic); - sub_ = nh.subscribe(topic, 100, &SubscriberGnssNovatel::callback, this); + sub_ = nh.subscribe(topic, 10000, &SubscriberGnssNovatel::callback, this); } void SubscriberGnssNovatel::callback(const novatel_oem7_msgs::Oem7RawMsg& msg) diff --git a/src/subscriber_gnss_ublox.cpp b/src/subscriber_gnss_ublox.cpp index 53fb70406143800f1602f6044959538f28597c63..fedcec51707399a7ce917c20b8d94a4eebc500fe 100644 --- a/src/subscriber_gnss_ublox.cpp +++ b/src/subscriber_gnss_ublox.cpp @@ -10,21 +10,28 @@ SubscriberGnssUblox::SubscriberGnssUblox(const std::string& _unique_name, SubscriberGnssReceiver(_unique_name, _server, _sensor_ptr, - std::make_shared<GnssUtils::UBloxRaw>()) + std::make_shared<GnssUtils::UBloxRaw>()), + n_msgs_(0) { } +SubscriberGnssUblox::~SubscriberGnssUblox() +{ + WOLF_INFO("SubscriberGnssUblox destructor: n_msgs_ = ", n_msgs_); +} + void SubscriberGnssUblox::initialize(ros::NodeHandle& nh, const std::string& topic) { SubscriberGnssReceiver::initialize(nh, topic); - sub_ = nh.subscribe(topic, 100, &SubscriberGnssUblox::callback, this); + sub_ = nh.subscribe(topic, 10000, &SubscriberGnssUblox::callback, this); } void SubscriberGnssUblox::callback(const std_msgs::UInt8MultiArray& msg) { auto stamp = ros::Time::now(); setLastStamp(stamp); + n_msgs_++; GnssUtils::RawDataType res = receiver_->addDataStream(msg.data);