Feature/ros2 ntrip rate control

.gitignore

@@ -9,3 +9,4 @@ __pycache__
 
 # VIM swap files
 .*.sw*
+/.vscode

README.md

@@ -34,27 +34,37 @@ The following command will launch the node. Keep in mind each instance needs to
 ros2 launch ntrip_client ntrip_client_launch.py
 ```
 
+-- or override defaults from cmd line --
+
+```bash
+ros2 launch ntrip_client ntrip_client_launch.py host:=rtk2go.com mountpoint:=MyRealMtPt ntrip_server_hz:=1 \ 
+ authenticate:=true username:[email protected] password:=none namespace:=mybot
+```
+
 Optional launch parameters:
+- **namespace**: Prepend this namespace to your node and topic names. Default: /
 - **host**: Hostname or IP address of the NTRIP server to connect to.
 - **port**: Port to connect to on the server. Default: `2101`
 - **mountpoint**: Mountpoint to connect to on the NTRIP server.
 - **ntrip_version**: Value to use for the `Ntrip-Version` header in the initial HTTP request to the caster.
-- **authenticate**: Whether to authenticate with the server, or send an unauthenticated request. If set to true, `username`, and `password` must be supplied.
+- **ntrip_server_hz**: The frequency to communicate with the NTRIP server. Some servers, like rtk2go.com, will ban you if you request data too frequently. For rtk2go, use ntrip_server_hz:=1 Default is 10.
+- **authenticate**: Whether or not to authenticate with the server, or send an unauthenticated request. If set to true, `username`, and `password` must be supplied.
 - **username**: Username to use when authenticating with the NTRIP server. Only used if `authenticate` is true
 - **password**: Password to use when authenticating with the NTRIP server. Only used if `authenticate` is true
 - **ssl**: Whether to connect with SSL. cert, key, and ca_cert options will only take effect if this is true
 - **cert**: If the NTRIP caster is configured to use cert based authentication, you can use this option to specify the client certificate
 - **key**: If the NTRIP caster is configured to use cert based authentication, you can use this option to specify the private key
 - **ca_cert**: If the NTRIP caster uses self signed certs, or you need to use a different CA chain, this option can be used to specify a CA file
-- **rtcm_message_packege**: Changes the type of ROS RTCM message published by this node. Defaults to `mavros_msgs`, but also supports `rtcm_msgs`
+- **rtcm_message_package**: Changes the type of ROS RTCM message published by this node. Defaults to `mavros_msgs`, but also supports `rtcm_msgs`
 
 #### Topics
 
-This node currently only has two topics of interest:
+This node currently only has three topics of interest:
 
 * **/rtcm**: This node will publish the RTCM corrections received from the server to this topic as [RTCM messages](http://docs.ros.org/en/noetic/api/mavros_msgs/html/msg/RTCM.html). These messages can be consumed by nodes such as the [microstrain_inertial_driver](https://github.com/LORD-MicroStrain/microstrain_inertial)
     * **NOTE**: The type of message can be switched between [`mavros_msgs/RTCM`](https://github.com/mavlink/mavros/blob/ros2/mavros_msgs/msg/RTCM.msg) and [`rtcm_msgs/Message`](https://github.com/tilk/rtcm_msgs/blob/master/msg/Message.msg) using the `rtcm_message_package` parameter
-* **/nmea**: This node will subscribe on this topic and receive [NMEA sentence messages](http://docs.ros.org/en/api/nmea_msgs/html/msg/Sentence.html) which it will forward to the NTRIP server. This is only needed when using a virtual NTRIP server
+* **/nmea**: This node will subscribe on this topic and receive [NMEA sentence messages](http://docs.ros.org/en/api/nmea_msgs/html/msg/Sentence.html) which it will forward to the NTRIP server. This is needed when using a virtual NTRIP server or for some NTRIP servers that require it. This message should be produceed by your node talking to the gps receiver.
+* **/ntrip_server_hz**: This node will publish the frequency of communications with the NTRIP server to help check compliance with usage policies. 
 
 ## Docker Integration
 

launch/ntrip_client_launch.py

@@ -15,8 +15,9 @@ def generate_launch_description():
           DeclareLaunchArgument('port',                  default_value='2101'),
           DeclareLaunchArgument('mountpoint',            default_value='VTRI_RTCM3'),
           DeclareLaunchArgument('ntrip_version',         default_value='None'),
+          DeclareLaunchArgument('ntrip_server_hz',       default_value='1'), # set to 1 for rtk2go
           DeclareLaunchArgument('authenticate',          default_value='True'),
-          DeclareLaunchArgument('username',              default_value='user'),
+          DeclareLaunchArgument('username',              default_value='user'), # use email address for rtk2go
           DeclareLaunchArgument('password',              default_value='pass'),
           DeclareLaunchArgument('ssl',                   default_value='False'),
           DeclareLaunchArgument('cert',                  default_value='None'),
@@ -44,6 +45,9 @@ def generate_launch_description():
 
                     # Optional parameter that will set the NTRIP version in the initial HTTP request to the NTRIP caster.
                     'ntrip_version': LaunchConfiguration('ntrip_version'),
+
+                    # Rate to request correction messages. Some servers will sandbox clients that request too often
+                    'ntrip_server_hz': LaunchConfiguration('ntrip_server_hz'),
 
                     # If this is set to true, we will read the username and password and attempt to authenticate. If not, we will attempt to connect unauthenticated
                     'authenticate': LaunchConfiguration('authenticate'),
@@ -66,23 +70,27 @@ def generate_launch_description():
                     'rtcm_frame_id': 'odom',
 
                     # Optional parameters that will allow for longer or shorter NMEA messages. Standard max length for NMEA is 82
-                    'nmea_max_length': 82,
+                    'nmea_max_length': 100,
                     'nmea_min_length': 3,
 
                     # Use this parameter to change the type of RTCM message published by the node. Defaults to "mavros_msgs", but we also support "rtcm_msgs"
                     'rtcm_message_package': LaunchConfiguration('rtcm_message_package'),
 
                     # Will affect how many times the node will attempt to reconnect before exiting, and how long it will wait in between attempts when a reconnect occurs
                     'reconnect_attempt_max': 10,
-                    'reconnect_attempt_wait_seconds': 5,
+                    'reconnect_attempt_wait_seconds': 10, #was 5, changed per rtk2go reqs
 
                     # How many seconds is acceptable in between receiving RTCM. If RTCM is not received for this duration, the node will attempt to reconnect
-                    'rtcm_timeout_seconds': 4
+                    'rtcm_timeout_seconds': 10 #was 4 changed for rtk2go reqs
                   }
                 ],
                 # Uncomment the following section and replace "/gx5/nmea/sentence" with the topic you are sending NMEA on if it is not the one we requested
                 #remappings=[
                 #  ("nmea", "/gx5/nmea/sentence")
                 #],
+
+                remappings=[
+                #  ("nmea", "/ublox_gps_node/fix")
+                ],
           )
       ])

scripts/ntrip_ros.py

@@ -7,7 +7,7 @@
 
 import rclpy
 from rclpy.node import Node
-from std_msgs.msg import Header
+from std_msgs.msg import Header, String
 from nmea_msgs.msg import Sentence
 
 from ntrip_client.ntrip_client import NTRIPClient
@@ -42,6 +42,7 @@ def __init__(self):
         ('port', 2101),
         ('mountpoint', 'mount'),
         ('ntrip_version', 'None'),
+        ('ntrip_server_hz', 10), # set to 1hz for rtk2go
         ('authenticate', False),
         ('username', ''),
         ('password', ''),
@@ -69,6 +70,12 @@ def __init__(self):
     if ntrip_version == 'None':
       ntrip_version = None
 
+    # Set the rate at which RTCM requests and NMEA messages are sent
+    self.rtcm_request_rate = 1.0 / self.get_parameter('ntrip_server_hz').value
+
+    # Initialize variables to store the most recent NMEA message
+    self._latest_nmea = None
+
     # Set the log level to debug if debug is true
     if self._debug:
       rclpy.logging.set_logger_level(self.get_logger().name, rclpy.logging.LoggingSeverity.DEBUG)
@@ -111,6 +118,9 @@ def __init__(self):
     # Setup the RTCM publisher
     self._rtcm_pub = self.create_publisher(self._rtcm_message_type, 'rtcm', 10)
 
+    # Setup a server frequency confirmation publisher
+    self._rate_confirm_pub = self.create_publisher(String, 'ntrip_server_hz', 10)
+
     # Initialize the client
     self._client = NTRIPClient(
       host=host,
@@ -149,11 +159,13 @@ def run(self):
     if not self._client.connect():
       self.get_logger().error('Unable to connect to NTRIP server')
       return False
-    # Setup our subscriber
+
+    # Setup the subscriber for NMEA data
     self._nmea_sub = self.create_subscription(Sentence, 'nmea', self.subscribe_nmea, 10)
 
-    # Start the timer that will check for RTCM data
-    self._rtcm_timer = self.create_timer(0.1, self.publish_rtcm)
+    # Start the timer that will send both RTCM requests and NMEA data at the configured rate
+    self._rtcm_timer = self.create_timer(self.rtcm_request_rate, self.send_rtcm_and_nmea)
+
     return True
 
   def stop(self):
@@ -167,13 +179,23 @@ def stop(self):
     self.destroy_node()
 
   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)
+    # Cache the latest NMEA sentence
+    self._latest_nmea = nmea.sentence
+
+  def send_rtcm_and_nmea(self):
+    # Send cached NMEA data if available
+    if self._latest_nmea is not None:
+      self._client.send_nmea(self._latest_nmea)
 
-  def publish_rtcm(self):
+    # Request and publish RTCM data
     for raw_rtcm in self._client.recv_rtcm():
       self._rtcm_pub.publish(self._create_rtcm_message(raw_rtcm))
 
+    # Publish a confirmation message to indicate the send_rtcm_and_nmea call
+    confirmation_msg = String()
+    confirmation_msg.data = "RTCM request and NMEA receive set at rate: {} Hz".format(1.0 / self.rtcm_request_rate)
+    self._rate_confirm_pub.publish(confirmation_msg)
+
   def _create_mavros_msgs_rtcm_message(self, rtcm):
     return mavros_msgs_RTCM(
       header=Header(

src/ntrip_client/ntrip_client.py

@@ -27,8 +27,8 @@ class NTRIPClient:
 
   # Public constants
   DEFAULT_RECONNECT_ATTEMPT_MAX = 10
-  DEFAULT_RECONNECT_ATEMPT_WAIT_SECONDS = 5
-  DEFAULT_RTCM_TIMEOUT_SECONDS = 4
+  DEFAULT_RECONNECT_ATEMPT_WAIT_SECONDS = 10 #was 5 - changed for rtk2go reqs
+  DEFAULT_RTCM_TIMEOUT_SECONDS = 10 #was 4 - changed for rtk2go reqs
 
   def __init__(self, host, port, mountpoint, ntrip_version, username, password, 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
@@ -149,6 +149,7 @@ def connect(self):
       self._logwarn('Received unauthorized response from the server. Check your username, password, and mountpoint to make sure they are correct.')
       known_error = True
     elif not self._connected and (self._ntrip_version == None or self._ntrip_version == ''):
+      self._logwarn(response) # show the response
       self._logwarn('Received unknown error from the server. Note that the NTRIP version was not specified in the launch file. This is not necesarilly the cause of this error, but it may be worth checking your NTRIP casters documentation to see if the NTRIP version needs to be specified.')
       known_error = True
 
@@ -195,7 +196,7 @@ def reconnect(self):
           self._logerr('Reconnect to http://{}:{} failed. Retrying in {} seconds'.format(self._host, self._port, 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._reconnect_attempt_count = 0 #this hides the retry count from the excepton?
           raise Exception("Reconnect was attempted {} times, but never succeeded".format(self._reconnect_attempt_count))
           break
         elif connect_success: