diff --git a/launch/ntrip_client.launch b/launch/ntrip_client.launch index 1225e72..ee23f2f 100644 --- a/launch/ntrip_client.launch +++ b/launch/ntrip_client.launch @@ -66,6 +66,8 @@ + + diff --git a/launch/ntrip_serial_device.launch b/launch/ntrip_serial_device.launch index c999044..9f75838 100644 --- a/launch/ntrip_serial_device.launch +++ b/launch/ntrip_serial_device.launch @@ -36,6 +36,8 @@ + + diff --git a/scripts/ntrip_ros.py b/scripts/ntrip_ros.py index 6e7d19e..22d2639 100755 --- a/scripts/ntrip_ros.py +++ b/scripts/ntrip_ros.py @@ -75,6 +75,8 @@ def __init__(self): self._client.nmea_parser.nmea_min_length = self._nmea_min_length self._client.reconnect_attempt_max = self._reconnect_attempt_max self._client.reconnect_attempt_wait_seconds = self._reconnect_attempt_wait_seconds + self._client.reconnect_backoff_base = self._reconnect_backoff_base + self._client.reconnect_backoff_max_seconds = self._reconnect_backoff_max_seconds self._client.rtcm_timeout_seconds = rospy.get_param('~rtcm_timeout_seconds', NTRIPClient.DEFAULT_RTCM_TIMEOUT_SECONDS) diff --git a/scripts/ntrip_serial_device_ros.py b/scripts/ntrip_serial_device_ros.py index 6fa64d3..c849b12 100755 --- a/scripts/ntrip_serial_device_ros.py +++ b/scripts/ntrip_serial_device_ros.py @@ -44,6 +44,8 @@ def __init__(self): self._client.nmea_parser.nmea_min_length = self._nmea_min_length self._client.reconnect_attempt_max = self._reconnect_attempt_max self._client.reconnect_attempt_wait_seconds = self._reconnect_attempt_wait_seconds + self._client.reconnect_backoff_base = self._reconnect_backoff_base + self._client.reconnect_backoff_max_seconds = self._reconnect_backoff_max_seconds if __name__ == '__main__': diff --git a/src/ntrip_client/ntrip_base.py b/src/ntrip_client/ntrip_base.py index b189529..fb6646e 100644 --- a/src/ntrip_client/ntrip_base.py +++ b/src/ntrip_client/ntrip_base.py @@ -1,5 +1,5 @@ -import time import logging +import rospy from .nmea_parser import NMEAParser from .rtcm_parser import RTCMParser @@ -10,6 +10,9 @@ class NTRIPBase: DEFAULT_RECONNECT_ATTEMPT_MAX = 10 DEFAULT_RECONNECT_ATEMPT_WAIT_SECONDS = 5 + DEFAULT_RECONNECT_BACKOFF_BASE = 1.8 + DEFAULT_RECONNECT_BACKOFF_MAX_SECONDS = 300 + def __init__(self, logerr=logging.error, logwarn=logging.warning, loginfo=logging.info, logdebug=logging.debug): # Bit of a strange pattern here, but save the log functions so we can be agnostic of ROS self._logerr = logerr @@ -34,10 +37,15 @@ def __init__(self, logerr=logging.error, logwarn=logging.warning, loginfo=loggin # Setup some state self._shutdown = False self._connected = False + # How many connection attempts have failed since we last connected? + # We don't consider connection successful until some valid data has been received. + self._reconnect_attempt_count = 0 # Public reconnect info self.reconnect_attempt_max = self.DEFAULT_RECONNECT_ATTEMPT_MAX self.reconnect_attempt_wait_seconds = self.DEFAULT_RECONNECT_ATEMPT_WAIT_SECONDS + self.reconnect_backoff_base = self.DEFAULT_RECONNECT_BACKOFF_BASE + self.reconnect_backoff_max_seconds = self.DEFAULT_RECONNECT_BACKOFF_MAX_SECONDS def connect(self): raise NotImplementedError("Must override connect") @@ -45,25 +53,48 @@ def connect(self): def disconnect(self): raise NotImplementedError("Must override disconnect") - def reconnect(self): - if self._connected: - while not self._shutdown: - self._reconnect_attempt_count += 1 - self.disconnect() - connect_success = self.connect() - if not connect_success and self._reconnect_attempt_count < self.reconnect_attempt_max: - self._logerr('Reconnect failed. Retrying in {} seconds'.format(self.reconnect_attempt_wait_seconds)) - time.sleep(self.reconnect_attempt_wait_seconds) - elif self._reconnect_attempt_count >= self.reconnect_attempt_max: - self._reconnect_attempt_count = 0 - self._logerr('Reconnect failed. Max attempts reached. Shutting down') - self.shutdown() - break - elif connect_success: - self._reconnect_attempt_count = 0 - break - else: + def _compute_reconnect_wait_time(self): + """ + Compute a time to sleep before attempting to reconnect. + + This is based on an exponential backoff, capped to a maximum. + + All of the initial wait times, the maximum and the base are configurable. + """ + return min( + self.reconnect_attempt_wait_seconds * (self.reconnect_backoff_base ** self._reconnect_attempt_count ), + self.reconnect_backoff_max_seconds + ) + + def reconnect(self, initial = False): + if not (self._connected or initial): self._logdebug('Reconnect called while not connected, ignoring') + return + + while not self._shutdown: + if not initial: + # If this isn't our initial connection attempt, + # disconnect and wait a reconnection interval + self.disconnect() + to_wait = self._compute_reconnect_wait_time() + self._logerr(f"Reconnecting in {to_wait:.1f} seconds") + rospy.sleep(self._compute_reconnect_wait_time()) + + initial = False + self._reconnect_attempt_count += 1 + + connect_success = self.connect() + if connect_success: + break + + if self._reconnect_attempt_count >= self.reconnect_attempt_max: + self._reconnect_attempt_count = 0 + self._logerr('Reconnect failed. Max attempts reached. Shutting down') + self.shutdown() + break + + def mark_successful_connection(self): + self._reconnect_attempt_count = 0 def send_nmea(self): raise NotImplementedError("Must override send_nmea") diff --git a/src/ntrip_client/ntrip_client.py b/src/ntrip_client/ntrip_client.py index c439e9b..a74ca9b 100644 --- a/src/ntrip_client/ntrip_client.py +++ b/src/ntrip_client/ntrip_client.py @@ -53,7 +53,6 @@ def __init__(self, host, port, mountpoint, ntrip_version, username, password, lo self.ca_cert = None # Private reconnect info - self._reconnect_attempt_count = 0 self._nmea_send_failed_count = 0 self._nmea_send_failed_max = 5 self._read_zero_bytes_count = 0 @@ -235,7 +234,13 @@ def recv_rtcm(self): self._first_rtcm_received = True # Send the data to the RTCM parser to parse it - return self.rtcm_parser.parse(data) if data else [] + parsed_packets = self.rtcm_parser.parse(data) if data else [] + + if parsed_packets: + # now we've received a packet, we can happily say that we are successfully connected + self.mark_successful_connection() + + return parsed_packets def _form_request(self): if self._ntrip_version != None and self._ntrip_version != '': diff --git a/src/ntrip_client/ntrip_ros_base.py b/src/ntrip_client/ntrip_ros_base.py index 120187e..90fdb3b 100755 --- a/src/ntrip_client/ntrip_ros_base.py +++ b/src/ntrip_client/ntrip_ros_base.py @@ -77,15 +77,15 @@ def __init__(self, name): self._nmea_min_length = rospy.get_param('~nmea_min_length', NMEA_DEFAULT_MIN_LENGTH) self._reconnect_attempt_max = rospy.get_param('~reconnect_attempt_max', NTRIPBase.DEFAULT_RECONNECT_ATTEMPT_MAX) self._reconnect_attempt_wait_seconds = rospy.get_param('~reconnect_attempt_wait_seconds', NTRIPBase.DEFAULT_RECONNECT_ATEMPT_WAIT_SECONDS) + self._reconnect_backoff_base = rospy.get_param('~reconnect_backoff_base', NTRIPBase.DEFAULT_RECONNECT_BACKOFF_BASE) + self._reconnect_backoff_max_seconds = rospy.get_param('~reconnect_backoff_max_seconds', NTRIPBase.DEFAULT_RECONNECT_BACKOFF_MAX_SECONDS) def run(self): # Setup a shutdown hook rospy.on_shutdown(self.stop) - # Connect the client - if not self._client.connect(): - rospy.logerr('Unable to connect to NTRIP server') - return 1 + # Start the client's reconnection loop + self._client.reconnect(initial = True) # Setup our subscriber self._nmea_sub = rospy.Subscriber('nmea', Sentence, self.subscribe_nmea, queue_size=10)