From 8a8b61eca3db76b961a2bd9f581ccb7576033f25 Mon Sep 17 00:00:00 2001
From: Rob <87914992+robbiefish@users.noreply.github.com>
Date: Wed, 17 Aug 2022 12:29:42 -0400
Subject: [PATCH] ROS NMEA sentence min/max length (#19)

* ROS NMEA sentence variable length

* Removes unnecesarry imports
---
 launch/ntrip_client.launch       |  4 ++++
 scripts/ntrip_ros.py             |  3 +++
 src/ntrip_client/nmea_parser.py  | 16 ++++++++++------
 src/ntrip_client/ntrip_client.py |  8 ++++----
 4 files changed, 21 insertions(+), 10 deletions(-)

diff --git a/launch/ntrip_client.launch b/launch/ntrip_client.launch
index d1d3ae4..a86eed0 100644
--- a/launch/ntrip_client.launch
+++ b/launch/ntrip_client.launch
@@ -54,6 +54,10 @@
     <!-- Not sure if this will be looked at, but this frame ID will be added to the RTCM messages published by this node -->
     <param name="rtcm_frame_id" value="odom" />
 
+    <!-- Optional parameters that will allow for longer or shorter NMEA messages. Standard max length for NMEA is 82 -->
+    <param name="nmea_max_length" value="82" />
+    <param name="nmea_min_length" value="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" -->
     <param name="rtcm_message_package" value="$(arg rtcm_message_package)" />
 
diff --git a/scripts/ntrip_ros.py b/scripts/ntrip_ros.py
index 2ae57bc..3d38756 100755
--- a/scripts/ntrip_ros.py
+++ b/scripts/ntrip_ros.py
@@ -10,6 +10,7 @@
 from nmea_msgs.msg import Sentence
 
 from ntrip_client.ntrip_client import NTRIPClient
+from ntrip_client.nmea_parser import NMEA_DEFAULT_MAX_LENGTH, NMEA_DEFAULT_MIN_LENGTH
 
 # Try to import a couple different types of RTCM messages
 _MAVROS_MSGS_NAME = "mavros_msgs"
@@ -105,6 +106,8 @@ def __init__(self):
     self._client.ca_cert = rospy.get_param('~ca_cert', None)
 
     # Set parameters on the client
+    self._client.nmea_parser.nmea_max_length = rospy.get_param('~nmea_max_length', NMEA_DEFAULT_MAX_LENGTH)
+    self._client.nmea_parser.nmea_min_length = rospy.get_param('~nmea_min_length', NMEA_DEFAULT_MIN_LENGTH)
     self._client.reconnect_attempt_max = rospy.get_param('~reconnect_attempt_max', NTRIPClient.DEFAULT_RECONNECT_ATTEMPT_MAX)
     self._client.reconnect_attempt_wait_seconds = rospy.get_param('~reconnect_attempt_wait_seconds', NTRIPClient.DEFAULT_RECONNECT_ATEMPT_WAIT_SECONDS)
     self._client.rtcm_timeout_seconds = rospy.get_param('~rtcm_timeout_seconds', NTRIPClient.DEFAULT_RTCM_TIMEOUT_SECONDS)
diff --git a/src/ntrip_client/nmea_parser.py b/src/ntrip_client/nmea_parser.py
index 9a4e75d..70630b9 100644
--- a/src/ntrip_client/nmea_parser.py
+++ b/src/ntrip_client/nmea_parser.py
@@ -1,7 +1,7 @@
 import logging
 
-_NMEA_MAX_LENGTH = 82
-_NMEA_MIN_LENGTH = 3
+NMEA_DEFAULT_MAX_LENGTH = 82
+NMEA_DEFAULT_MIN_LENGTH = 3
 _NMEA_CHECKSUM_SEPERATOR = "*"
 
 class NMEAParser:
@@ -13,14 +13,18 @@ def __init__(self, logerr=logging.error, logwarn=logging.warning, loginfo=loggin
     self._loginfo = loginfo
     self._logdebug = logdebug
 
+    # Save some other config
+    self.nmea_max_length = NMEA_DEFAULT_MAX_LENGTH
+    self.nmea_min_length = NMEA_DEFAULT_MIN_LENGTH
+
   def is_valid_sentence(self, sentence):
     # Simple sanity checks
-    if len(sentence) > _NMEA_MAX_LENGTH:
-      self._logwarn('Received invalid NMEA sentence. Max length is {}, but sentence was {} bytes'.format(_NMEA_MAX_LENGTH, len(sentence)))
+    if len(sentence) > self.nmea_max_length:
+      self._logwarn('Received invalid NMEA sentence. Max length is {}, but sentence was {} bytes'.format(self.nmea_max_length, len(sentence)))
       self._logwarn('Sentence: {}'.format(sentence))
       return False
-    if len(sentence) < _NMEA_MIN_LENGTH:
-      self._logwarn('Received invalid NMEA sentence. We need at least {} bytes to parse but got {} bytes'.format(_NMEA_MIN_LENGTH, len(sentence)))
+    if len(sentence) < self.nmea_min_length:
+      self._logwarn('Received invalid NMEA sentence. We need at least {} bytes to parse but got {} bytes'.format(self.nmea_min_length, len(sentence)))
       self._logwarn('Sentence: {}'.format(sentence))
       return False
     if sentence[0] != '$' and sentence[0] != '!':
diff --git a/src/ntrip_client/ntrip_client.py b/src/ntrip_client/ntrip_client.py
index bd28352..7976bcd 100644
--- a/src/ntrip_client/ntrip_client.py
+++ b/src/ntrip_client/ntrip_client.py
@@ -53,13 +53,13 @@ def __init__(self, host, port, mountpoint, ntrip_version, username, password, lo
     self._server_socket = None
 
     # Setup some parsers to parse incoming messages
-    self._rtcm_parser = RTCMParser(
+    self.rtcm_parser = RTCMParser(
       logerr=logerr,
       logwarn=logwarn,
       loginfo=loginfo,
       logdebug=logdebug
     )
-    self._nmea_parser = NMEAParser(
+    self.nmea_parser = NMEAParser(
       logerr=logerr,
       logwarn=logwarn,
       loginfo=loginfo,
@@ -216,7 +216,7 @@ def send_nmea(self, sentence):
       sentence = sentence + '\r\n'
 
     # Check if it is a valid NMEA sentence
-    if not self._nmea_parser.is_valid_sentence(sentence):
+    if not self.nmea_parser.is_valid_sentence(sentence):
       self._logwarn("Invalid NMEA sentence, not sending to server")
       return
 
@@ -284,7 +284,7 @@ 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 []
+    return self.rtcm_parser.parse(data) if data else []
 
   def shutdown(self):
     # Set some state, and then disconnect