optionally reseting driver

include/subscriber_gnss_ublox.h

@@ -45,8 +45,13 @@ class SubscriberGnssUblox : public SubscriberGnssReceiver
 
         void callback(const std_msgs::UInt8MultiArray& msg);
 
+        double secondsSinceLastCallback() override;
+
     protected:
         long unsigned int n_msgs_;
+        ros::Time reseted_stamp_;
+        bool reset_driver_node_;
+        double max_topic_delay_, reset_duration_;
 };
 
 WOLF_REGISTER_SUBSCRIBER(SubscriberGnssUblox);

src/subscriber_gnss_ublox.cpp

@@ -13,6 +13,12 @@ SubscriberGnssUblox::SubscriberGnssUblox(const std::string& _unique_name,
                                std::make_shared<GnssUtils::UBloxRaw>()),
         n_msgs_(0)
 {
+    reset_driver_node_ = _server.getParam<bool>(prefix_ + "/reset_receiver");
+    if (reset_driver_node_)
+    {
+        max_topic_delay_ = _server.getParam<double>(prefix_ + "/reset_duration");
+        reset_duration_  = _server.getParam<double>(prefix_ + "/reset_duration");
+    }
 }
 
 SubscriberGnssUblox::~SubscriberGnssUblox()
@@ -29,6 +35,8 @@ void SubscriberGnssUblox::initialize(ros::NodeHandle& nh, const std::string& top
 
 void SubscriberGnssUblox::callback(const std_msgs::UInt8MultiArray& msg)
 {
+    ROS_DEBUG("SubscriberGnssUblox::callback message received!");
+
     auto stamp = ros::Time::now();
     setLastStamp(stamp);
     n_msgs_++;
@@ -45,4 +53,24 @@ void SubscriberGnssUblox::callback(const std_msgs::UInt8MultiArray& msg)
     }
 }
 
+double SubscriberGnssUblox::secondsSinceLastCallback()
+{
+    if (last_stamp_ == ros::Time(0))
+        return 0;
+
+    double seconds = (ros::Time::now() - last_stamp_).toSec();
+
+    if (reset_driver_node_ and
+        seconds > max_topic_delay_ and
+        (ros::Time::now() - reseted_stamp_).toSec() > reset_duration_)
+    {
+        ROS_WARN("SubscriberGnssUblox: not received raw data for more than %fs and reseted more than %fs ago, reseting ublox diver...", max_topic_delay_, reset_duration_);
+
+        system("rosnode kill ublox_gps");
+        reseted_stamp_ = ros::Time::now();
+    }
+
+    return seconds;
+}
+
 }