diff --git a/README.md b/README.md index 48b53bf..1d658e9 100644 --- a/README.md +++ b/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. diff --git a/launch/serial_gnss_device_launch.py b/launch/serial_gnss_device_launch.py new file mode 100644 index 0000000..32b05c1 --- /dev/null +++ b/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 + } + ], + ) + ]) + + + \ No newline at end of file diff --git a/scripts/serial_gnss_device_ros.py b/scripts/serial_gnss_device_ros.py new file mode 100644 index 0000000..effdbc6 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/setup.py b/setup.py index f2141f9..213dc0a 100644 --- a/setup.py +++ b/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' ] ) \ No newline at end of file diff --git a/src/ntrip_client/nmea_parser.py b/src/ntrip_client/nmea_parser.py index d02ce66..ba19068 100644 --- a/src/ntrip_client/nmea_parser.py +++ b/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 + \ No newline at end of file diff --git a/src/ntrip_client/ntrip_base.py b/src/ntrip_client/ntrip_base.py index f353018..348c48c 100644 --- a/src/ntrip_client/ntrip_base.py +++ b/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 diff --git a/src/ntrip_client/rtcm_parser.py b/src/ntrip_client/rtcm_parser.py index 6989f5b..ed9e2d2 100644 --- a/src/ntrip_client/rtcm_parser.py +++ b/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 + + diff --git a/src/ntrip_client/serial_gnss_device.py b/src/ntrip_client/serial_gnss_device.py new file mode 100644 index 0000000..b79dca1 --- /dev/null +++ b/src/ntrip_client/serial_gnss_device.py @@ -0,0 +1,126 @@ +#!/usr/bin/evn python + +import time +import logging +import serial + +from .nmea_parser import NMEAParser +from .rtcm_parser import RTCMParser + +class SerialGNSSDevice: + + # Public constants + DEFAULT_RECONNECT_ATTEMPT_MAX = 10 + DEFAULT_RECONNECT_ATEMPT_WAIT_SECONDS = 5 + + def __init__(self, port, baudrate, 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 + self._logdebug = logdebug + + # Setup some parsers to parse incoming messages + self.rtcm_parser = RTCMParser( + logerr=logerr, + logwarn=logwarn, + loginfo=loginfo, + logdebug=logdebug + ) + self.nmea_parser = NMEAParser( + logerr=logerr, + logwarn=logwarn, + loginfo=loginfo, + logdebug=logdebug + ) + + # Setup some state + self._shutdown = False + self._connected = False + + # Public reconnect info + self.reconnect_attempt_max = self.DEFAULT_RECONNECT_ATTEMPT_MAX + self.reconnect_attempt_wait_seconds = self.DEFAULT_RECONNECT_ATEMPT_WAIT_SECONDS + + # Save the connection info + self._port = port + self._baudrate = baudrate + + # Initialize this so we don't throw an exception when closing + self._device = None + + def connect(self): + # Attempt to open the serial port + try: + self._device = serial.Serial(self._port, self._baudrate) + except Exception as e: + self._logerr('Unable to open serial port {} at baudrate {}'.format(self._port, self._baudrate)) + self._logerr('Exception: {}'.format(str(e))) + return False + + # Right now, we can't check anything else, so assuming that the port is open, we succeeded. + self._loginfo('Connected to serial port {} at baudrate {}'.format(self._port, self._baudrate)) + self._connected = True + return True + + def disconnect(self): + # Disconnect the serial port + try: + if self._device: + self._device.close() + except Exception as e: + self._logdebug('Encountered exception when closing the serial port. This can likely be ignored.') + self._logdebug('Exception: {}'.format(str(e))) + + 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 + raise Exception("Reconnect was attempted {} times, but never succeeded".format(self._reconnect_attempt_count)) + elif connect_success: + self._reconnect_attempt_count = 0 + break + else: + self._logdebug('Reconnect called while not connected, ignoring') + + def recv_nmea(self): + if not self._connected: + self._logwarn('NMEA received before port was connected, returning empty string') + return [] + # Check how much data is available on the device + if self._device.in_waiting: + try: + data = self._device.read_all() + self._logdebug('Read {} bytes from serial device'.format(len(data))) + return self.nmea_parser.parse(data) + except Exception as e: + self._logerr('Unable to read from serial device, reconnecting...') + self.reconnect() + return [] + else: + return [] + + def send_rtmc(self, packet): + if not self._connected: + self._logwarn('RTCM sent before device was connected, returing empty list') + return [] + + # Check if RTCM packet is valid + if not self.rtcm_parser.is_valid_packet(packet): + self._logwarn('Invalid RTCM packet, not sending to GNSS device') + return + + try: + self._device.write(packet) + except Exception as e: + self._logerr('Unable to send RTCM packet to device, check COM port settings.') +