From 53f7edd95a382b9d70f76446335ce5ece1dfd291 Mon Sep 17 00:00:00 2001 From: agennart Date: Wed, 10 Dec 2025 14:56:12 +0100 Subject: [PATCH 1/7] Setup RTCM timer in constructor Using the argument 'autostart=False' available from jazzy, this allows to create the timer without starting it. --- scripts/ntrip_ros_base.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/scripts/ntrip_ros_base.py b/scripts/ntrip_ros_base.py index aba74b6..59241a4 100755 --- a/scripts/ntrip_ros_base.py +++ b/scripts/ntrip_ros_base.py @@ -76,6 +76,7 @@ def __init__(self, name): # Setup the RTCM publisher self._rtcm_pub = self.create_publisher(self._rtcm_message_type, 'rtcm', 10) + self._rtcm_timer = self.create_timer(0.1, self.publish_rtcm, autostart=False) # Initialize the client self._client = NTRIPBase( @@ -101,14 +102,12 @@ def run(self): self._fix_sub = self.create_subscription(NavSatFix, 'fix', self.subscribe_fix, 10) # Start the timer that will check for RTCM data - self._rtcm_timer = self.create_timer(0.1, self.publish_rtcm) + self._rtcm_timer.reset() return True def stop(self): self.get_logger().info('Stopping RTCM publisher') - if self._rtcm_timer: - self._rtcm_timer.cancel() - self._rtcm_timer.destroy() + self._rtcm_timer.cancel() self.get_logger().info('Disconnecting NTRIP client') self._client.disconnect() self.get_logger().info('Shutting down node') From c9ca514430575a68c6d794dfd077207bd42d9377 Mon Sep 17 00:00:00 2001 From: agennart Date: Wed, 10 Dec 2025 14:57:52 +0100 Subject: [PATCH 2/7] Remove trailing spaces in ntrip_ros and ntrip_ros_base --- scripts/ntrip_ros.py | 4 ++-- scripts/ntrip_ros_base.py | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/scripts/ntrip_ros.py b/scripts/ntrip_ros.py index 38927e2..35c45e5 100755 --- a/scripts/ntrip_ros.py +++ b/scripts/ntrip_ros.py @@ -102,6 +102,6 @@ def __init__(self): raise e finally: node.stop() - + # Shutdown the node and stop rclpy - rclpy.shutdown() \ No newline at end of file + rclpy.shutdown() diff --git a/scripts/ntrip_ros_base.py b/scripts/ntrip_ros_base.py index 59241a4..7ea79fb 100755 --- a/scripts/ntrip_ros_base.py +++ b/scripts/ntrip_ros_base.py @@ -116,7 +116,7 @@ def stop(self): def subscribe_nmea(self, nmea): # Just extract the NMEA from the message, and send it right to the server self._client.send_nmea(nmea.sentence) - + def subscribe_fix(self, fix: NavSatFix): # Calculate the timestamp of the message timestamp_secs = fix.header.stamp.sec + fix.header.stamp.nanosec * 1e-9 @@ -135,7 +135,7 @@ def subscribe_fix(self, fix: NavSatFix): nmea_lat_direction = "S" if fix.longitude < 0: nmea_lon_direction = "W" - + # Convert the units of the latitude and longitude nmea_lat = NMEAParser.lat_dd_to_dmm(fix.latitude) nmea_lon = NMEAParser.lon_dd_to_dmm(fix.longitude) @@ -179,4 +179,4 @@ def _create_rtcm_msgs_rtcm_message(self, rtcm): frame_id=self._rtcm_frame_id ), message=rtcm - ) \ No newline at end of file + ) From 7e06ee37a571e98f1286e09bc5210d38e06da972 Mon Sep 17 00:00:00 2001 From: agennart Date: Wed, 10 Dec 2025 14:58:45 +0100 Subject: [PATCH 3/7] Do not self destroy node Let the destroy logic be called when the ntrip client node is dereferenced. This is done in order to prepare the ROS node to be re-configurable at runtime. In this case, when calling stop, we do not want the node to self destroy. --- scripts/ntrip_ros_base.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/scripts/ntrip_ros_base.py b/scripts/ntrip_ros_base.py index 7ea79fb..2810647 100755 --- a/scripts/ntrip_ros_base.py +++ b/scripts/ntrip_ros_base.py @@ -110,8 +110,6 @@ def stop(self): self._rtcm_timer.cancel() self.get_logger().info('Disconnecting NTRIP client') self._client.disconnect() - self.get_logger().info('Shutting down node') - self.destroy_node() def subscribe_nmea(self, nmea): # Just extract the NMEA from the message, and send it right to the server From 4f76934d1c91b059ac6211b7075c9feae9ce4323 Mon Sep 17 00:00:00 2001 From: agennart Date: Wed, 10 Dec 2025 15:22:20 +0100 Subject: [PATCH 4/7] Split parameter loading from ntrip client initialization This separates the parameter loading logic from the client initialization making it easier to update. --- scripts/ntrip_ros.py | 117 +++++++++++++++++++++++++++---------------- 1 file changed, 73 insertions(+), 44 deletions(-) diff --git a/scripts/ntrip_ros.py b/scripts/ntrip_ros.py index 35c45e5..84e99d6 100755 --- a/scripts/ntrip_ros.py +++ b/scripts/ntrip_ros.py @@ -31,61 +31,90 @@ def __init__(self): ] ) + # Initialize all internal variables in constructor + # Will be loaded in 'load_parameters' function. + self.host = None + self.port = None + self.mountpoint = None + self.ntrip_version = None + self.authenticate = None + self.username = None + self.password = None + self.ssl = None + self.cert = None + self.key = None + self.ca_cert = None + self.rtcm_timeout_seconds = None + + self.load_parameters() + + # Initialize the client + self._client = self.init_ntrip_client() + + + def load_parameters(self): + """Load ROS parameters.""" # Read some mandatory config - host = self.get_parameter('host').value - port = self.get_parameter('port').value - mountpoint = self.get_parameter('mountpoint').value + self.host = self.get_parameter('host').value + self.port = self.get_parameter('port').value + self.mountpoint = self.get_parameter('mountpoint').value # Optionally get the ntrip version from the launch file - ntrip_version = self.get_parameter('ntrip_version').value - if ntrip_version == 'None': - ntrip_version = None + self.ntrip_version = self.get_parameter('ntrip_version').value + if self.ntrip_version == 'None': + self.ntrip_version = None # If we were asked to authenticate, read the username and password - username = None - password = None - if self.get_parameter('authenticate').value: - username = self.get_parameter('username').value - password = self.get_parameter('password').value - if not username: - self.get_logger().error('Requested to authenticate, but param "username" was not set') - sys.exit(1) - if not password: - self.get_logger().error('Requested to authenticate, but param "password" was not set') - sys.exit(1) + self.username = None + self.password = None + self.authenticate = self.get_parameter('authenticate').value + if self.authenticate: + self.username = self.get_parameter('username').value + self.password = self.get_parameter('password').value + if not self.username or not self.password: + raise ValueError(f'Invalid username/password: {self.username}/{self.password}') - # Initialize the client - self._client = NTRIPClient( - host=host, - port=port, - mountpoint=mountpoint, - ntrip_version=ntrip_version, - username=username, - password=password, + self.ssl = self.get_parameter('ssl').value + self.cert = self.get_parameter('cert').value + if self.cert == 'None': + self.cert = None + self.key = self.get_parameter('key').value + if self.key == 'None': + self.key = None + self.ca_cert = self.get_parameter('ca_cert').value + if self.ca_cert == 'None': + self.ca_cert = None + + self.rtcm_timeout_seconds = self.get_parameter('rtcm_timeout_seconds').value + + + def init_ntrip_client(self): + """Initialize a NTRIP client using class internal variable.""" + client = NTRIPClient( + host=self.host, + port=self.port, + mountpoint=self.mountpoint, + ntrip_version=self.ntrip_version, + username=self.username, + password=self.password, logerr=self.get_logger().error, logwarn=self.get_logger().warning, loginfo=self.get_logger().info, logdebug=self.get_logger().debug - ) + ) + client.ssl = self.ssl + client.cert = self.cert + client.key = self.key + client.ca_cert = self.ca_cert + + client.nmea_parser.nmea_max_length = self._nmea_max_length + client.nmea_parser.nmea_min_length = self._nmea_min_length + client.reconnect_attempt_max = self._reconnect_attempt_max + client.reconnect_attempt_wait_seconds = self._reconnect_attempt_wait_seconds + client.rtcm_timeout_seconds = self.rtcm_timeout_seconds + + return client - # Get some SSL parameters for the NTRIP client - self._client.ssl = self.get_parameter('ssl').value - self._client.cert = self.get_parameter('cert').value - self._client.key = self.get_parameter('key').value - self._client.ca_cert = self.get_parameter('ca_cert').value - if self._client.cert == 'None': - self._client.cert = None - if self._client.key == 'None': - self._client.key = None - if self._client.ca_cert == 'None': - self._client.ca_cert = None - - # Get some timeout parameters for the NTRIP client - self._client.nmea_parser.nmea_max_length = self._nmea_max_length - 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.rtcm_timeout_seconds = self.get_parameter('rtcm_timeout_seconds').value if __name__ == '__main__': # Start the node From 0f1fe3df18af21f392135f2f5406e0c36e19bce5 Mon Sep 17 00:00:00 2001 From: agennart Date: Wed, 10 Dec 2025 15:26:02 +0100 Subject: [PATCH 5/7] Start client in ROS node So that the ROS node can manage its child attribute at runtime. In particular it can manages the ntrip_client in dedicated callback. --- scripts/ntrip_ros.py | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/scripts/ntrip_ros.py b/scripts/ntrip_ros.py index 84e99d6..245485d 100755 --- a/scripts/ntrip_ros.py +++ b/scripts/ntrip_ros.py @@ -50,6 +50,7 @@ def __init__(self): # Initialize the client self._client = self.init_ntrip_client() + self.run() def load_parameters(self): @@ -120,17 +121,11 @@ def init_ntrip_client(self): # Start the node rclpy.init() node = NTRIPRos() - if not node.run(): - sys.exit(1) try: # Spin until we are shut down rclpy.spin(node) except KeyboardInterrupt: pass - except BaseException as e: - raise e - finally: - node.stop() - # Shutdown the node and stop rclpy - rclpy.shutdown() + # Shutdown the node and stop rclpy + rclpy.shutdown() From e2ec0431b8c916986f957c44c4883204adb7d8c0 Mon Sep 17 00:00:00 2001 From: agennart Date: Wed, 10 Dec 2025 16:32:14 +0100 Subject: [PATCH 6/7] Add simple recovery callback and timer The recovery callback checks the connection status. When the client is disconnected for too long, try to stop the client and restart. --- scripts/ntrip_ros.py | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/scripts/ntrip_ros.py b/scripts/ntrip_ros.py index 245485d..bd220b9 100755 --- a/scripts/ntrip_ros.py +++ b/scripts/ntrip_ros.py @@ -28,6 +28,8 @@ def __init__(self): ('key', 'None'), ('ca_cert', 'None'), ('rtcm_timeout_seconds', NTRIPClient.DEFAULT_RTCM_TIMEOUT_SECONDS), + ('recovery_period_s', 5.0), + ('max_disconnected_count', 3), ] ) @@ -52,6 +54,12 @@ def __init__(self): self._client = self.init_ntrip_client() self.run() + # Initialize timer(s) + self._disconnected_count = 0 + self.recovery_timer = self.create_timer( + self.get_parameter('recovery_period_s').value, + self.recovery_callback) + def load_parameters(self): """Load ROS parameters.""" @@ -116,6 +124,17 @@ def init_ntrip_client(self): return client + def recovery_callback(self): + """Perform recovery of the NTRIP client + - Check if the NTRIP client is still connected + """ + if not self._client._connected: + self._disconnected_count += 1 + if self._disconnected_count >= self.get_parameter('max_disconnected_count').value: + self._disconnected_count = 0 + self.stop() + self.run() + if __name__ == '__main__': # Start the node From 11cde66695e2dbd4614df605deee3465e7ae06a7 Mon Sep 17 00:00:00 2001 From: agennart Date: Wed, 10 Dec 2025 15:47:14 +0100 Subject: [PATCH 7/7] Add on set parameter callback This is useful to detect update of the NTRIP configuration at runtime and automatically reload the NTRIP client when it is the case. --- scripts/ntrip_ros.py | 65 +++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 64 insertions(+), 1 deletion(-) diff --git a/scripts/ntrip_ros.py b/scripts/ntrip_ros.py index bd220b9..946b9bb 100755 --- a/scripts/ntrip_ros.py +++ b/scripts/ntrip_ros.py @@ -3,8 +3,11 @@ import os import sys import json +from typing import List import rclpy +from rclpy.parameter import Parameter +from rcl_interfaces.msg import SetParametersResult from ntrip_ros_base import NTRIPRosBase from ntrip_client.ntrip_client import NTRIPClient @@ -49,18 +52,19 @@ def __init__(self): self.rtcm_timeout_seconds = None self.load_parameters() + self.add_on_set_parameters_callback(self.on_set_parameters_callback) # Initialize the client self._client = self.init_ntrip_client() self.run() # Initialize timer(s) + self._ntrip_config_updated = False self._disconnected_count = 0 self.recovery_timer = self.create_timer( self.get_parameter('recovery_period_s').value, self.recovery_callback) - def load_parameters(self): """Load ROS parameters.""" # Read some mandatory config @@ -96,6 +100,54 @@ def load_parameters(self): self.rtcm_timeout_seconds = self.get_parameter('rtcm_timeout_seconds').value + def on_set_parameters_callback(self, parameters: List[Parameter]) -> SetParametersResult: + """Callback on parameter update + This allows to validate every parameter update and automatically + reload the necessary component when an update is triggered + """ + def is_string(name: str, param: Parameter) -> bool: + """Helper function to reduce verbosity""" + return param.name == name and param.type_ == Parameter.Type.STRING + + def is_bool(name: str, param: Parameter) -> bool: + """Helper function to reduce verbosity""" + return param.name == name and param.type_ == Parameter.Type.BOOL + + def is_integer(name: str, param: Parameter) -> bool: + """Helper function to reduce verbosity""" + return param.name == name and param.type_ == Parameter.Type.INTEGER + + self.get_logger().warn(f'{parameters}') + + for parameter in parameters: + if is_string('host', parameter): + self.host = parameter.value + elif is_integer('port', parameter): + self.port = parameter.value + elif is_string('mountpoint', parameter): + self.mountpoint = parameter.value + elif is_string('username', parameter): + self.username = parameter.value + elif is_string('password', parameter): + self.password = parameter.value + elif is_bool('ssl', parameter): + self.ssl = parameter.value + elif is_string('cert', parameter): + self.cert = parameter.value + self.cert = self.cert if self.cert != 'None' else None + elif is_string('key', parameter): + self.key = parameter.value + self.key = self.key if self.key != 'None' else None + elif is_string('ca_cert', parameter): + self.ca_cert = parameter.value + self.ca_cert = self.ca_cert if self.ca_cert != 'None' else None + else: + # parameters unrelated to the NTRIP configuration + # such as a timer period + pass + + self._ntrip_config_updated = True + return SetParametersResult(successful=True) def init_ntrip_client(self): """Initialize a NTRIP client using class internal variable.""" @@ -126,8 +178,19 @@ def init_ntrip_client(self): def recovery_callback(self): """Perform recovery of the NTRIP client + - Check if the configuration was updated through a parameter update - Check if the NTRIP client is still connected """ + if self._ntrip_config_updated: + self.stop() + # Re-initialize ntrip client with updated configuration + self._client = self.init_ntrip_client() + self.run() + self._ntrip_config_updated = False + + #return early, do not check if client is connected immediately + return + if not self._client._connected: self._disconnected_count += 1 if self._disconnected_count >= self.get_parameter('max_disconnected_count').value: