Adding serial_gnss_device node

README.md

@@ -72,6 +72,27 @@ This node currently only has three topics of interest:
 * **/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 always needed when using a virtual NTRIP server or an NTRIP device
 * **/fix**: This serves the same exact purpose as `/nmea`, but facilitates receiving global position that is not in NMEA format
 
+
+#### Connect to a GNSS "device" via serial
+
+This is a separate node to be launched in parallel with the `ntrip_client` node, and is useful for GNSS devices connected over serial that are expecting an RTCM message, and outputting a NMEA sentence over serial. 
+
+```bash
+ros2 launch ntrip_client serial_gnss_device_launch.py
+```
+
+Optional launch parameters:
+- **port**: Serial port that the GNSS device is connected on
+- **baudrate**: Baudrate to connect the serial port at. Default 115200.
+
+#### Topics
+
+This node currently only has 2 topics of interest:
+
+* **/rtcm**: This node will subscribe one this topic and receive as [RTCM messages](http://docs.ros.org/en/noetic/api/mavros_msgs/html/msg/RTCM.html) which it will write to the GNSS device.
+* **/nmea**: This node will publish the NMEA sentences received via serial from the GNSS device. 
+
+
 ## License
 ntrip_client is released under the MIT License - see the `LICENSE` file in the source distribution.
 

launch/serial_gnss_device_launch.py

@@ -0,0 +1,48 @@
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import SetEnvironmentVariable
+
+def generate_launch_description():
+      return LaunchDescription([
+        # Declare arguments with default values
+        DeclareLaunchArgument('namespace',             default_value='/'),
+        DeclareLaunchArgument('node_name',             default_value='serial_gnss_device'),
+        DeclareLaunchArgument('debug',                 default_value='false'),
+        DeclareLaunchArgument('port',                  default_value='/dev/ttyACM1'),
+        DeclareLaunchArgument('baudrate',              default_value='115200'),
+
+
+        # Pass an environment variable to the node
+        SetEnvironmentVariable(name='NTRIP_CLIENT_DEBUG', value=LaunchConfiguration('debug')),
+
+        # ******************************************************************
+        # Serial GNSS Device Node
+        # ******************************************************************
+        Node(
+              name=LaunchConfiguration('node_name'),
+              namespace=LaunchConfiguration('namespace'),
+              package='ntrip_client',
+              executable='serial_gnss_device_ros.py',
+              parameters=[
+                {
+                  # Required parameters used to connect to serial GNSS device
+                  'port': LaunchConfiguration('port'),
+                  'baudrate': LaunchConfiguration('baudrate'),
+
+                  # Not sure if this will be looked at by other ndoes, but this frame ID will be added to the RTCM messages published by this node
+                  'nmea_frame_id': 'odom',
+
+
+                  # Optional parameters that will allow for longer or shorter NMEA messages. Standard max length for NMEA is 82
+                  'nmea_max_length': 128,
+                  'nmea_min_length': 3
+                } 
+              ],
+        )
+      ])
+
+
+

scripts/serial_gnss_device_ros.py

@@ -0,0 +1,117 @@
+#!/usr/bin/env python
+import os
+import sys
+import json
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Header
+from rtcm_msgs.msg import Message
+from nmea_msgs.msg import Sentence
+
+from ntrip_client.serial_gnss_device import SerialGNSSDevice
+from ntrip_client.nmea_parser import NMEAParser, NMEA_DEFAULT_MAX_LENGTH, NMEA_DEFAULT_MIN_LENGTH
+class SerialGNSSDeviceROS(Node):
+  def __init__(self):
+    # Read a debug flag from the environment that should have been set by the launch file
+    try:
+      self._debug = json.loads(os.environ["NTRIP_CLIENT_DEBUG"].lower())
+    except:
+      self._debug = False
+
+    # Init the node and declare params
+    super().__init__('serial_gnss_device')
+    self.declare_parameters(
+      namespace='',
+      parameters=[
+        ('nmea_frame_id', 'odom'),
+        ('nmea_max_length', NMEA_DEFAULT_MAX_LENGTH),
+        ('nmea_min_length', NMEA_DEFAULT_MIN_LENGTH),
+        ('port', '/dev/ttyACM0'),
+        ('baudrate', 115200),
+      ]
+    )
+
+    if self._debug:
+      rclpy.logging.set_logger_level(self.get_logger().name, rclpy.logging.LoggingSeverity.DEBUG)
+
+    # Read some mandatory config
+    port = self.get_parameter('port').value
+    baudrate = self.get_parameter('baudrate').value
+
+    self._gnss_device = SerialGNSSDevice(
+      port=port, 
+      baudrate=baudrate,   
+      logerr=self.get_logger().error,
+      logwarn=self.get_logger().warning,
+      loginfo=self.get_logger().info,
+      logdebug=self.get_logger().debug
+    )
+
+    # Read an optional Frame ID from the config
+    self._nmea_frame_id = self.get_parameter('nmea_frame_id').value
+
+    # Get NMEA length config
+    self._gnss_device.nmea_parser.nmea_max_length = self.get_parameter('nmea_max_length').value
+    self._gnss_device.nmea_parser.nmea_min_length = self.get_parameter('nmea_min_length').value
+
+    # Setup NMEA publisher
+    self._nmea_pub = self.create_publisher(Sentence, 'nmea', 10)
+
+  def run(self):
+    # Connect to GNSS device
+    if not self._gnss_device.connect():
+      self.get_logger().error('Unable to connect to GNSS device')
+      return False
+
+    # Setup our subscribers
+    self._rtcm_sub = self.create_subscription(Message, 'rtcm', self.subscribe_rtcm, 10)
+
+    # Start the timer that will check for NMEA data
+    self._nmea_timer = self.create_timer(0.1, self.publish_nmea)
+    return True
+
+  def stop(self):
+    self.get_logger().info('Stopping NMEA publisher')
+    if self._nmea_timer:
+      self._nmea_timer.cancel()
+      self._nmea_timer.destroy()
+    self._gnss_device.disconnect()
+    self.get_logger().info('Shutting down node')
+    self.destroy_node()
+
+  def publish_nmea(self):
+    for nmea_sentence in self._gnss_device.recv_nmea():
+      self._nmea_pub.publish(self._create_nmea_sentence_nmea_packet(nmea_sentence))
+
+  def _create_nmea_sentence_nmea_packet(self, nmea_sentence):
+    return Sentence(
+      header=Header(
+        stamp = self.get_clock().now().to_msg(),
+        frame_id = self._nmea_frame_id
+      ),
+      sentence = nmea_sentence
+    )
+
+  def subscribe_rtcm(self, rtcm):
+    # Extract RTCM data from the mesage and send it to the gnss_device
+    self._gnss_device.send_rtmc(rtcm.message)
+
+if __name__ == '__main__':
+  # Start the node
+  rclpy.init()
+  node = SerialGNSSDeviceROS()
+  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()

setup.py

@@ -32,6 +32,7 @@
     scripts=[
       'scripts/ntrip_ros.py',
       'scripts/ntrip_ros_base.py',
-      'scripts/ntrip_serial_device_ros.py'
+      'scripts/ntrip_serial_device_ros.py',
+      'scripts/serial_gnss_device_ros.py'
     ]
 )

src/ntrip_client/nmea_parser.py

@@ -18,6 +18,9 @@ def __init__(self, logerr=logging.error, logwarn=logging.warning, loginfo=loggin
     self.nmea_max_length = NMEA_DEFAULT_MAX_LENGTH
     self.nmea_min_length = NMEA_DEFAULT_MIN_LENGTH
 
+    self._caching_data = False
+    self._buffer = b''
+
   @staticmethod
   def checksum(sentence_no_checksum):
     calculated_checksum = 0
@@ -80,3 +83,44 @@ def is_valid_sentence(self, sentence):
 
     # Passed all checks
     return True
+
+  def parse(self, buffer):
+    # Add any data we have cached
+    if self._caching_data:
+      combined_buffer = self._buffer + buffer
+    else:
+      combined_buffer = buffer
+
+      # Loop over the passed buffer, and parse all available NMEA sentences
+    index = 0
+    nmea_sentences = []
+    while index < len(combined_buffer):
+      # Find the start of the NMEA sentence
+      if combined_buffer[index] == ord('$'):
+        # Check if we have the end of the sentence in the buffer, and if so, try to parse it
+        end_index = combined_buffer.find(b'\r\n', index)
+        if end_index != -1:
+          sentence = combined_buffer[index:end_index + 2].decode('utf-8')
+          if self.is_valid_sentence(sentence):
+            nmea_sentences.append(sentence)
+          else:
+            self._logwarn('Found potential NMEA sentence, but it was invalid. Sentence: {}'.format(sentence))
+          index = end_index + 2
+        else:
+          self._logdebug('Found beginning of NMEA sentence at {}, but there is not enough data in the buffer to find the end of the sentence'.format(index))
+          self._caching_data = True
+          self._buffer = buffer[index:]
+          break
+      index += 1
+
+    # If we didn't find a full sentence, cache this one for next time
+    if self._caching_data:
+      self._buffer += buffer  
+
+      if len(self._buffer) > self.nmea_max_length:
+        self._logwarn('Too much data buffered, trimming to {} bytes.'.format(self.nmea_max_length))
+        self._buffer = self._buffer[:self.nmea_max_length]
+
+    # Return the NMEA sentences we found
+    return nmea_sentences 
+

src/ntrip_client/ntrip_base.py

@@ -12,6 +12,7 @@ class NTRIPBase:
 
   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._reconnect_attempt_count = 0
     self._logerr = logerr
     self._logwarn = logwarn
     self._loginfo = loginfo

src/ntrip_client/rtcm_parser.py

@@ -38,6 +38,7 @@
 
 # If we find the beginning of a packet, but not the end, we will cache up to this number of bytes
 _MAX_BUFFER_SIZE = 1024 * 10
+_MAX_PACKET_LEGNTH = 1023
 
 class RTCMParser:
 
@@ -116,3 +117,27 @@ def _checksum(self, packet):
     for byte in packet:
       crc = ((crc << 8) & 0xFFFFFF) ^ _RTCM_CRC_LOOKUP[(crc >> 16) ^ byte]
     return crc
+
+  def is_valid_packet(self, packet):
+    if len(packet) > _MAX_PACKET_LEGNTH:
+      self._logwarn('Received invalid RTCM packet. Max length is {}, but packet was {} bytes'.format(_MAX_PACKET_LEGNTH, len(packet)))
+      return False
+    # Check for RTCM preamble
+    if packet[0] != _RTCM_3_2_PREAMBLE:
+      self._logwarn('Received invalid RTCM packet. First byte should be 0b11010011, but we received 0b{:08b}'.format(packet[0]))
+      self._logwarn('Packet: {}'.format(packet))
+      return False
+
+    actual_checksum = self._checksum(packet[:-3])
+    expected_checksum = packet[-3] << 16 | packet[-2] << 8 | packet[-1]
+
+    if actual_checksum != expected_checksum:
+      self._logwarn('Received invalid RTCM packet. Checksum mismatch')    
+      self._logwarn('Expected Checksum: 0x{:X}'.format(expected_checksum))
+      self._logwarn('Calculated Checksum: 0x{:X}'.format(actual_checksum))
+      return False
+
+    # Passed all checks
+    return True
+
+