diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml new file mode 100644 index 0000000..3183cb2 --- /dev/null +++ b/.github/workflows/docker.yml @@ -0,0 +1,55 @@ +name: Create and publish a Docker image + +on: + push: + tags: ["*"] + +env: + REGISTRY: ghcr.io + IMAGE_NAME: ${{ github.repository }} + +jobs: + build-and-push-image: + runs-on: ubuntu-latest + permissions: + contents: read + packages: write + + steps: + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 + + - name: Checkout repository + uses: actions/checkout@v4 + with: + token: ${{ secrets.MY_REPO_PAT }} + submodules: recursive + + - name: Log in to the Container registry + uses: docker/login-action@v3 + with: + registry: ${{ env.REGISTRY }} + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + + - name: Docker meta + id: meta + uses: docker/metadata-action@v5 + with: + images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }} + tags: | + type=ref,event=branch + type=semver,pattern={{version}} + type=semver,pattern={{major}}.{{minor}} + + - name: Build and push Docker image + uses: docker/build-push-action@v5 + with: + context: . + push: true + tags: ${{ steps.meta.outputs.tags }} + labels: ${{ steps.meta.outputs.labels }} + file: Dockerfile + target: runtime + build-args: + BUILDKIT_CONTEXT_KEEP_GIT_DIR=true diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b164d0b --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +# Python cache +*pycache* + +# Logs +*logs* diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..14c4ecc --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "scripts/av_ntrip_credentials"] + path = scripts/av_ntrip_credentials + url = git@github.com:ipab-rad/av_ntrip_credentials.git diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..8eb49b3 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,160 @@ + +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit + +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.6.0 + hooks: + - id: check-added-large-files + - id: check-ast + - id: check-case-conflict + - id: check-docstring-first + - id: check-merge-conflict + - id: check-symlinks + - id: check-xml + - id: check-yaml + - id: debug-statements + - id: end-of-file-fixer + - id: mixed-line-ending + - id: trailing-whitespace + exclude_types: [rst] + - id: fix-byte-order-marker + + # Python hooks + - repo: https://github.com/asottile/pyupgrade + rev: v3.17.0 + hooks: + - id: pyupgrade + args: [--py36-plus] + + - repo: https://github.com/psf/black + rev: 24.8.0 + hooks: + - id: black + args: ["--line-length=79", -S] + + # CPP hooks + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v18.1.8 + hooks: + - id: clang-format + args: ['-fallback-style=none', '-i'] + + - repo: local + hooks: + - id: ament_cppcheck + name: ament_cppcheck + description: Static code analysis of C/C++ files. + entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + + - repo: local + hooks: + - id: ament_cpplint + name: ament_cpplint + description: Static code analysis of C/C++ files. + entry: ament_cpplint + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + args: ["--linelength=100", "--filter=-legal/copyright,-build/include_order"] + + # Cmake hooks + - repo: local + hooks: + - id: ament_lint_cmake + name: ament_lint_cmake + description: Check format of CMakeLists.txt files. + entry: ament_lint_cmake + language: system + files: CMakeLists\.txt$ + + - repo: https://github.com/cheshirekow/cmake-format-precommit + rev: v0.6.13 + hooks: + - id: cmake-format + + # Docs - RestructuredText hooks + - repo: https://github.com/PyCQA/doc8 + rev: v1.1.1 + hooks: + - id: doc8 + args: ['--max-line-length=100', '--ignore=D001'] + exclude: CHANGELOG\.rst$ + + - repo: https://github.com/pre-commit/pygrep-hooks + rev: v1.10.0 + hooks: + - id: rst-backticks + exclude: CHANGELOG\.rst$ + - id: rst-directive-colons + - id: rst-inline-touching-normal + + # Spellcheck in comments and docs + # skipping of *.svg files is not working... + - repo: https://github.com/codespell-project/codespell + rev: v2.3.0 + hooks: + - id: codespell + args: ['--write-changes', '--uri-ignore-words-list=ist', '-L manuel,ned'] + exclude: CHANGELOG\.rst|\.(svg|pyc|drawio|dae)$ + + # Check Github files + - repo: https://github.com/python-jsonschema/check-jsonschema + rev: 0.29.1 + hooks: + - id: check-github-workflows + args: ["--verbose"] + - id: check-github-actions + args: ["--verbose"] + - id: check-dependabot + args: ["--verbose"] + + # Bash prettify + - repo: https://github.com/lovesegfault/beautysh + rev: v6.2.1 + hooks: + - id: beautysh + + # ROS checks + - repo: https://github.com/tier4/pre-commit-hooks-ros + rev: v0.10.0 + hooks: + - id: flake8-ros + - id: prettier-xacro + - id: prettier-launch-xml + - id: prettier-package-xml + - id: ros-include-guard + - id: sort-package-xml + + # Dockerfiles + - repo: https://github.com/AleksaC/hadolint-py + rev: v2.12.1b3 + hooks: + - id: hadolint + args: ['--ignore=DL3008'] + +ci: + autofix_commit_msg: | + [pre-commit.ci] auto fixes from pre-commit.com hooks + + for more information, see https://pre-commit.ci + autofix_prs: false + autoupdate_branch: '' + autoupdate_commit_msg: '[pre-commit.ci] pre-commit autoupdate' + autoupdate_schedule: monthly + skip: [ament_cppcheck, ament_cpplint, ament_lint_cmake] + submodules: false diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..24585c1 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,50 @@ +# Use the official Ubuntu 24.04 as a base image +FROM ubuntu:24.04 AS base + +# Set environment variables to avoid interaction during package installation +ENV DEBIAN_FRONTEND=noninteractive + +# Install Python3, venv and pip +RUN apt-get update && \ + DEBIAN_FRONTEND=noninteractive apt-get -y install --no-install-recommends \ + python3 \ + python3-venv \ + python3-pip \ + && rm -rf /var/lib/apt/lists/* "$HOME"/.cache + +# Install required Python packages using venv + pip +RUN python3 -m venv /opt/ntrip_venv \ + && /opt/ntrip_venv/bin/pip install --no-cache-dir pyyaml colorlog pyrtcm + +# Set the environment variable so that the following commands use the venv +ENV PATH="/opt/ntrip_venv/bin:$PATH" + +# Create a working directory +WORKDIR /workspace/scripts + +FROM base AS dev + +# Install dev tools +RUN apt-get update \ + && DEBIAN_FRONTEND=noninteractive apt-get -y install --no-install-recommends \ + net-tools \ + nano \ + python-is-python3 \ + && rm -rf /var/lib/apt/lists/* "$HOME"/.cache + +# Run bash terminal by default +CMD ["bash"] + +FROM base AS runtime + +# Copy scripts dierectory +COPY ./scripts /workspace/scripts + +# Make sure the script is executable +RUN chmod +x /workspace/scripts/ntrip_client.py + +# Set the entrypoint to run the Python script and forward arguments +ENTRYPOINT ["python3", "ntrip_client.py"] + +# No default arguments +CMD [] diff --git a/README.md b/README.md index a7c1c08..292b726 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,61 @@ # av_ntrip_client + NTRIP client for RTK corrections in the AV's GNSS system. + +## Overview + +`av_ntrip_client` is a tool designed to connect the IPAB Autonomous Vehicle's Novatel PwrPak7d GNSS to the Smartnet NTRIP server, providing Real-Time Kinematic (RTK) corrections to enhance GNSS accuracy. The client is containerised using Docker for ease of deployment and use. + +## Features + +- Connects to the Smartnet NTRIP server for RTK corrections. +- Supports live GNSS GPS reading or fixed location data. +- Logs generated for troubleshooting and analysis. + +## Setup + +1. **Clone the Repository:** + + - **For General Public:** + ```bash + git clone https://github.com/ipab-rad/av_ntrip_client.git + ``` + + - **For Authorised Users:** + If you have authorised access to the `av_ntrip_client` repository, use: + ```bash + git clone --recurse-submodules git@github.com:ipab-rad/av_ntrip_client.git + ``` + +2. **Build and Run the Docker Container:** + + - **Development Mode:** + This mode mounts the current `scripts` directory into the container, allowing for live development. + ```bash + ./dev.sh + ``` + + - **Runtime Mode:** + This mode builds the Docker container and runs it with the specified options. The script will automatically start `ntrip_client.py`. You can pass arguments such as `--use-fix-location` `--debug` or `--help`. + ```bash + ./runtime.sh [options] + ``` + - Example: + ```bash + ./runtime.sh --use-fix-location --debug + ``` +## Usage + +Once the container is running, `ntrip_client.py` will automatically connect to the Smartnet NTRIP server using the provided credentials. Logs will be saved in the `scripts/logs` directory. + +## NTRIP Server Credentials + +The `scripts/av_ntrip_credentials` directory is a protected submodule due to licensing requirements of the Smartnet service. Access is restricted to authorised personnel only. + +## License + +This project is licensed under the terms of the Apache-2.0 [LICENCE](./LICENSE) file. + +## Contact + +For any issues or questions, please contact [Alejandor Bordallo](https://github.com/GreatAlexander) or [Hector Cruz](https://github.com/hect95) diff --git a/dev.sh b/dev.sh new file mode 100755 index 0000000..6c68614 --- /dev/null +++ b/dev.sh @@ -0,0 +1,49 @@ +#!/bin/bash +# ---------------------------------------------------------------- +# Build docker dev stage and add local code for live development +# ---------------------------------------------------------------- + +BASH_CMD="" + +# Function to print usage +usage() { + echo " +Usage: dev.sh [-b|bash] [-h|--help] + +Where: + -b | bash Open bash in docker container + -h | --help Show this help message + " + exit 1 +} + +# Parse command-line options +while [[ "$#" -gt 0 ]]; do + case $1 in + -b|bash) + BASH_CMD=bash + ;; + -h|--help) + usage + ;; + *) + echo "Unknown option: $1" + usage + ;; + esac + shift +done + +# Create logs dir +mkdir -p scripts/logs + +# Build the Docker image +docker build \ + -t av_ntrip_client:latest-dev \ + -f Dockerfile --target dev . + +# Run the Docker container and pass any provided arguments +docker run -it --rm --net host --privileged \ + -v /etc/localtime:/etc/localtime:ro \ + -v ./scripts:/workspace/scripts \ + av_ntrip_client:latest-dev $BASH_CMD diff --git a/runtime.sh b/runtime.sh new file mode 100755 index 0000000..afeb069 --- /dev/null +++ b/runtime.sh @@ -0,0 +1,60 @@ +#!/bin/bash +# --------------------------------------------------------------------------- +# Build Docker image, then run Ntrip Client (runtime or interactive bash) +# --------------------------------------------------------------------------- + + +# Function to print usage +usage() { + echo " +Usage: dev.sh [-b|bash] [--param-file] [--use-fix-location] [--debug ] [-h|--help] + +Where: + -b | bash Open bash in docker container + --param-file Path to the YAML file containing Ntrip server credentials + --use-fix-location Use fixed location for Ntrip requests + --debug Run Ntrip Client in debug mode + -h | --help Show this help message + " + exit 1 +} + +ENTRYPOINT="" +BASH_CMD="$@" + +# Parse command-line options +while [[ "$#" -gt 0 ]]; do + case $1 in + -b|bash) + ENTRYPOINT="--entrypoint /bin/bash" + BASH_CMD="" + ;; + --use-fix-location|--debug|--param-file) + # These flags are intended to be passed to + # the Docker container, so no action is needed here + ;; + -h|--help) + usage + ;; + *) + echo "Unknown option: $1" + usage + ;; + esac + shift +done + +# Create logs dir +mkdir -p scripts/logs + +# Build the Docker image +docker build \ + -t av_ntrip_client:latest \ + -f Dockerfile --target runtime . + +# Run the Docker container and pass any provided arguments +docker run -it --rm --net host --privileged \ + -v /etc/localtime:/etc/localtime:ro \ + -v ./scripts/logs:/workspace/scripts/logs \ + $ENTRYPOINT \ + av_ntrip_client:latest $BASH_CMD diff --git a/scripts/av_ntrip_credentials b/scripts/av_ntrip_credentials new file mode 160000 index 0000000..1be32e4 --- /dev/null +++ b/scripts/av_ntrip_credentials @@ -0,0 +1 @@ +Subproject commit 1be32e458ee9dc106267aa155ece987b3441c8eb diff --git a/scripts/example_params.yaml b/scripts/example_params.yaml new file mode 100644 index 0000000..4365284 --- /dev/null +++ b/scripts/example_params.yaml @@ -0,0 +1,11 @@ +gnss_host: 'YOUR_GPS_HOST_IP' +gnss_port: 1234 # Change accordingly +ntrip_host: 'YOUR_NTRIP_HOST' +ntrip_port: 4321 # Change accordingly +mountpoint: 'MSM_iMAX' # 'MSM_NEAR', 'MSM_iMAX' or 'MSM_VRS' +username: 'YOUR_NTRIP_USERNAME' +password: 'YOUR_NTRIP_PASSWORD' +# Your custom fix GPS coordinate +fix_latitude: 51.0 +fix_longitude: -3.8 +fix_altitude: 500.0 diff --git a/scripts/nmea_generator.py b/scripts/nmea_generator.py new file mode 100644 index 0000000..d3aa0d1 --- /dev/null +++ b/scripts/nmea_generator.py @@ -0,0 +1,71 @@ +""" +NMEA generator module. + +This module generates NMEA (GPGGA) sentence for GPS data simulation. +""" + +import time + + +class NMEAGenerator: + """ + A class to generate NMEA GPGGA sentence. + + It provides methods to generate and validate GPGGA + sentences. + """ + + def __init__(self, fix_latitude, fix_longitude, fix_altitude): + """Initialise class.""" + self.fix_latitude = fix_latitude + self.fix_longitude = fix_longitude + self.fix_altitude = fix_altitude + + def _calculate_checksum(self, nmea_str): + """Calculate the NMEA checksum.""" + checksum = 0 + for char in nmea_str: + checksum ^= ord(char) + return f'{checksum:02X}' + + def generate_gga_sentence(self): + """Generate NMEA GGA sentence.""" + lat_deg = int(self.fix_latitude) + lat_min = (self.fix_latitude - lat_deg) * 60 + lat_hemisphere = 'N' if self.fix_latitude >= 0 else 'S' + + lon_deg = int(abs(self.fix_longitude)) + lon_min = (abs(self.fix_longitude) - lon_deg) * 60 + lon_hemisphere = 'E' if self.fix_longitude >= 0 else 'W' + + current_time = time.strftime('%H%M%S', time.gmtime()) + gga = ( + f'GPGGA,{current_time},{lat_deg:02d}{lat_min:07.4f},' + f'{lat_hemisphere},{lon_deg:03d}{lon_min:07.4f},' + f'{lon_hemisphere},1,08,0.9,{self.fix_altitude:.1f},M,46.9,M,,' + ) + + checksum = self._calculate_checksum(gga) + return f'${gga}*{checksum}' + + def is_gpgga_data_valid(self, nmea_sentence: str) -> bool: + """Validate GPGGA data.""" + try: + # Split the sentence into its components + fields = nmea_sentence.split(',') + + # Check the length of the fields, should be 14 or 15 + if len(fields) < 14 or len(fields) > 15: + return False + + # Validate GPS quality indicator (field 6) + gps_qual = int(fields[6]) + if gps_qual == 0: + # If 0, data is not valid/reliable + return False + + # If all validations passed, return True + return True + + except (IndexError, ValueError): + return False diff --git a/scripts/ntrip_client.py b/scripts/ntrip_client.py new file mode 100755 index 0000000..54d782d --- /dev/null +++ b/scripts/ntrip_client.py @@ -0,0 +1,584 @@ +#!/usr/bin/env python +""" +NTRIP Client for requesting and transmitting RTCMv3 corrections. + +This script implements an NTRIP client for requesting and transmitting +RTCMv3 corrections to a Novatel GNSS receiver. It manages a single +connection to an NTRIP server and one to the Novatel GNSS receiver, +parses Novatel binary data, and handles RTCM messages. +""" + +import argparse +import base64 +import logging +import socket +import threading +import time +from collections import defaultdict +from datetime import datetime + +import colorlog + +from nmea_generator import NMEAGenerator + +from pyrtcm import RTCMReader, exceptions + +import yaml + + +class NtripClient: + """ + NTRIP client for requesting and transmitting RTCMv3 corrections. + + Manages a connection to an NTRIP server and a Novatel GNSS receiver. + Handles parsing of Novatel binary data and RTCM messages, and supports + options for using a fixed location and enabling debug logging. + """ + + def __init__(self, config_path, use_fix_location=False, debug_mode=False): + """ + Initialise the object with configuration and settings. + + Loads settings from `config_path`, and sets up sockets, logging, + and NMEA generator. + + Args: + config_path (str): Path to the configuration file. + use_fix_location (bool): Whether to use a fixed GPS location. + debug_mode (bool): Whether to enable debug mode. + """ + self.ntrip_socket = None + self.gnss_socket = None + self.ntrip_connected = False + self.nmea_request_sent = False + self.gnss_log_thread = None + self.stop_event = threading.Event() + self.use_fix_location = use_fix_location + self.debug_mode = debug_mode + self.received_rtcm_msgs_ids = defaultdict(int) + self.latest_nmea_data_valid = False + self.novatel_response_binary_dict = { + 'response_id': {'offset': 28, 'size': 4}, + 'checksum': {'size': 4}, + } + self.NOVATEL_OK_ID = 1 + self.SOCKET_BUFFER_SIZE = 2048 + # Indicates the start of an RTCM message + self.RTCM_DATA_PREAMBLE = 0xD3 + self.PAUSE_DURATION = 1.0 + + self.configure_logging() + + self.load_config(config_path) + + self.nmea_generator = NMEAGenerator( + self.fix_latitude, self.fix_longitude, self.fix_altitude + ) + + def configure_logging(self): + """Configure logging to support color and timestamped log file.""" + # Define the log format for the console + formatter = colorlog.ColoredFormatter( + '%(asctime)s - %(log_color)s%(levelname)s%(reset)s - %(message)s', + datefmt='%Y-%m-%d %H:%M:%S', + ) + + # Create a console handler with the colored formatter + console_handler = colorlog.StreamHandler() + console_handler.setFormatter(formatter) + + # Define the log format for the file + file_formatter = logging.Formatter( + '%(asctime)s - %(levelname)s - %(message)s', + datefmt='%Y-%m-%d %H:%M:%S', + ) + + # Generate a timestamped log file name + timestamp = datetime.now().strftime('%Y_%m_%d_%H_%M_%S') + log_filename = f'logs/{timestamp}_ntrip_client.log' + + # Create a file handler with the log filename and formatter + file_handler = logging.FileHandler(log_filename) + file_handler.setFormatter(file_formatter) + + # Determine the logging level based on debug_mode + if self.debug_mode: + logging.basicConfig( + level=logging.DEBUG, handlers=[console_handler, file_handler] + ) + else: + logging.basicConfig( + level=logging.INFO, handlers=[console_handler, file_handler] + ) + + def load_config(self, config_path): + """ + Load configuration from a YAML file. + + Reads the configuration file at `config_path` and sets instance + variables for GNSS and NTRIP settings, including credentials. + + Args: + config_path (str): Path to the YAML configuration file. + """ + try: + with open(config_path) as file: + config = yaml.safe_load(file) + self.gnss_host = config['gnss_host'] + self.gnss_port = config['gnss_port'] + self.ntrip_host = config['ntrip_host'] + self.ntrip_port = config['ntrip_port'] + self.mountpoint = config['mountpoint'] + self.username = config['username'] + self.password = config['password'] + self.fix_latitude = config['fix_latitude'] + self.fix_longitude = config['fix_longitude'] + self.fix_altitude = config['fix_altitude'] + + self.credentials = base64.b64encode( + f'{self.username}:{self.password}'.encode() + ).decode() + + except FileNotFoundError: + logging.error( + f'Configuration file at {config_path} does not exist.' + ) + raise SystemExit() + except yaml.YAMLError as e: + logging.error(f'Error parsing the configuration file: {e}') + raise SystemExit() + except KeyError as e: + logging.error(f'Missing required YAML parameter: {e}') + raise SystemExit() + + def connect_ntrip_server(self): + """Connect to the NTRIP server.""" + self.ntrip_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.ntrip_socket.settimeout(6) + + logging.info( + f'Attempting to connect to NTRIP server at' + f' {self.ntrip_host}:{self.ntrip_port}' + ) + + try: + self.ntrip_socket.connect((self.ntrip_host, self.ntrip_port)) + except socket.gaierror as e: + logging.error( + 'Unable to connect to NTRIP server:' + f' DNS resolution failed: {e}' + ) + return False + except socket.timeout: + logging.error( + 'Unable to connect to NTRIP server: Connection timed out' + ) + return False + except OSError as e: + logging.error(f'Unable to connect to NTRIP server: {e}') + return False + + request = ( + f'GET /{self.mountpoint} HTTP/1.0\r\n' + f'Host: {self.ntrip_host}\r\n' + f'Ntrip-Version: Ntrip/1.0\r\n' + f'User-Agent: NTRIP PythonClient/1.0\r\n' + f'Authorization: Basic {self.credentials}\r\n' + f'\r\n' + ) + self.ntrip_socket.send(request.encode()) + + try: + response = self.ntrip_socket.recv(self.SOCKET_BUFFER_SIZE).decode( + 'ISO-8859-1' + ) + except (OSError, socket.timeout) as e: + logging.error(f'Error getting response from NTRIP: {e}') + return False + + if any( + success in response + for success in ['ICY 200 OK', 'HTTP/1.0 200 OK', 'HTTP/1.1 200 OK'] + ): + self.ntrip_connected = True + logging.info('Successfully connected to NTRIP server.') + return True + + logging.error('Failed to connect to NTRIP server.') + return False + + def disconnect_ntrip_server(self): + """Disconnect from the NTRIP server.""" + self.ntrip_connected = False + if self.ntrip_socket: + try: + self.ntrip_socket.shutdown(socket.SHUT_RDWR) + except (OSError, socket.timeout) as e: + logging.error(f'Exception when shutting down the socket: {e}') + try: + self.ntrip_socket.close() + except (OSError, socket.timeout) as e: + logging.error(f'Exception when closing the socket: {e}') + + def connect_to_gnss(self): + """Connect to the GNSS receiver.""" + self.gnss_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.gnss_socket.settimeout(5) + + logging.info( + f'Attempting to connect to GNSS receiver at' + f' {self.gnss_host}:{self.gnss_port}' + ) + + try: + self.gnss_socket.connect((self.gnss_host, self.gnss_port)) + logging.info('Successfully connected to GNSS receiver.') + return True + except (OSError, socket.timeout) as e: + logging.error(f'Unable to connect to GNSS receiver: {e}') + return False + + def configure_gnss(self): + """Configure the GNSS port to log NMEA data.""" + configure_command = ( + '\r\n' + 'unlogall thisport\r\n' + 'log gpggalong ontime 0.1\r\n' + 'log gprmc ontime 0.1\r\n' + 'log gpgst ontime 0.2\r\n' + 'interfacemode rtcmv3 novatel\r\n' # Set RX and TX + ) + + logging.debug(f'Configuring GNSS {self.gnss_port} port') + + try: + self.gnss_socket.sendall(configure_command.encode('utf-8')) + + # Read the response to confirm configuration + response = self.gnss_socket.recv(self.SOCKET_BUFFER_SIZE).decode( + 'utf-8' + ) + logging.info('GNSS receiver successfully configured.') + logging.debug(f'GNSS response: {response}') + except (OSError, socket.timeout) as e: + logging.error(f'Failed to send configuration: {e}') + + def parse_novatel_binary(self, data): + """ + Parse Novatel binary data to log response IDs. + + Extracts and decodes the response ID from the binary data. Logs + the decoded response ID or a warning if the ID is unknown. + + This is based on Table 10 (1.5.3 Binary Response) of + Novatel OEM7 Command and Logs Manual (Rev: v1) + + Args: + data (bytes): Binary data from Novatel. + """ + # Extract relevant metadata from the response dictionary + resp_info = self.novatel_response_binary_dict['response_id'] + resp_id_end = resp_info['offset'] + resp_info['size'] + checksum_size = self.novatel_response_binary_dict['checksum']['size'] + + # Decode the response ID + resp_id = int.from_bytes( + data[resp_info['offset'] : resp_id_end], # noqa: E203 + byteorder='little', + ) + + # Extract the remaining response content, excluding the checksum + resp = data[resp_id_end:-checksum_size] # noqa: E203 + + # Log the response based on the response ID + if resp_id == self.NOVATEL_OK_ID: + logging.debug(f'GNSS response: {resp.decode()}') + else: + logging.warning( + f'GNSS returned an unexpected response: {resp.decode()}' + ) + + def split_data(self, data): + """ + Split the input data into binary and ASCII messages. + + Process a byte sequence containing mixed binary and ASCII data. + Extract and return the messages as separate lists. + + Args: + data (bytes): Byte sequence with mixed data. + + Returns: + tuple: (ascii_msgs, binary_msgs) + - ascii_msgs (list): Extracted ASCII messages. + - binary_msgs (list): Extracted binary messages. + """ + # Define the headers + binary_header = b'\xaaD\x12\x1c' + ascii_start = b'$GP' + + binary_msgs = [] + ascii_msgs = [] + + i = 0 + length = len(data) + + while i < length: + if data.startswith(binary_header, i): + # Found the start of a binary message + start = i + i += len(binary_header) + + # Find the next binary header or ASCII start + next_binary_index = data.find(binary_header, i) + next_ascii_index = data.find(ascii_start, i) + + if next_binary_index == -1: + next_binary_index = length + if next_ascii_index == -1: + next_ascii_index = length + + # Determine the end of the current binary message + end = min(next_binary_index, next_ascii_index) + + # Extract and store the binary message + binary_msgs.append(data[start:end]) + + # Move index past this message + i = end + + elif data.startswith(ascii_start, i): + # Found the start of an ASCII message + start = i + # Find the end of this ASCII message + end = data.find(b'\r\n', i) + if end == -1: + # If no end delimiter is found, + # process till the end of data + end = length + else: + end += len(b'\r\n') + # Extract and store the ASCII message + ascii_msgs.append(data[start:end]) + # Move index past this message + i = end + + else: + # Move to the next byte if no start of a message is found + i += 1 + + return ascii_msgs, binary_msgs + + def read_nmea_and_send_to_server(self): + """Read incoming messages from the GNSS receiver.""" + while not self.stop_event.is_set(): + + if not self.gnss_socket: + logging.warning('GNSS socket not connected.') + time.sleep(self.PAUSE_DURATION) + continue + + try: + message = self.gnss_socket.recv(self.SOCKET_BUFFER_SIZE) + if not message: + logging.warning('Empty msg received from GNSS') + continue + + # Split messages into binary and ASCII types + ascii_msgs, binary_msgs = self.split_data(message) + + # Get GPGGA msg from ASCII msgs + nmea_sentence = next( + (msg.decode() for msg in ascii_msgs if b'GPGGA' in msg), + None, + ) + + if nmea_sentence: + + if self.use_fix_location: + generated_sentence = ( + self.nmea_generator.generate_gga_sentence() + ) + self.latest_nmea_data_valid = True + self.send_nmea_to_ntrip_server(generated_sentence) + else: + + self.latest_nmea_data_valid = ( + self.nmea_generator.is_gpgga_data_valid( + nmea_sentence + ) + ) + + if self.latest_nmea_data_valid: + self.send_nmea_to_ntrip_server(nmea_sentence) + else: + logging.debug( + 'NMEA data is not valid. ' + 'Skipping sending it to NTRIP server.' + ) + + # Process binary messages + for binary_msg in binary_msgs: + self.parse_novatel_binary(binary_msg) + + except socket.timeout: + logging.warning( + 'Socket timeout occurred while reading GNSS data.' + ) + except OSError as e: + logging.error(f'Error reading GNSS data: {e}') + logging.warning(f'Received: {message}') + + def send_rtcm_to_gnss(self, rctm_sentence): + """Send a RTCMv3 message to the GNSS receiver.""" + if self.gnss_socket: + try: + self.gnss_socket.send(rctm_sentence) + logging.debug('RTCM message sent to GNSS') + + except (OSError, socket.timeout) as e: + logging.error(f'Error sending command to GNSS receiver: {e}') + else: + logging.warning('GNSS socket not connected. Command not sent.') + + def send_nmea_to_ntrip_server(self, nmea_sentence): + """Send NMEA sentence to NTRIP server in a separate thread.""" + if not self.ntrip_connected: + logging.debug('Not connected to NTRIP server. Cannot send NMEA.') + time.sleep(self.PAUSE_DURATION) + return + + request = f'{nmea_sentence}\r\n' + try: + self.ntrip_socket.send(request.encode()) + self.nmea_request_sent = True + except (OSError, socket.timeout) as e: + logging.error( + f'Error sending NMEA sentence to NTRIP server: {e}' + f'\nAttempting to reconnect..' + ) + self.nmea_request_sent = False + self.ntrip_connected = False + + def is_rtcm_data(self, response_data): + """Check if data is RTCM correction.""" + return response_data[0] == self.RTCM_DATA_PREAMBLE + + def read_rtcm_and_send_to_gnss(self): + """Read RTCM data and report back to GNSS.""" + try: + server_response = self.ntrip_socket.recv(self.SOCKET_BUFFER_SIZE) + + if not server_response: + logging.warning('NTRIP server replied with an empty message') + time.sleep(self.PAUSE_DURATION) + return + + if self.is_rtcm_data(server_response): + + try: + rtcm_msg = RTCMReader.parse(server_response) + logging.debug( + f'RTCM data (ID: {rtcm_msg.identity}) received' + ) + + # Keep track of RTCM message IDs + self.received_rtcm_msgs_ids[int(rtcm_msg.identity)] += 1 + + except exceptions.RTCMParseError as e: + logging.warning( + f'Error parsing RTCM data: {e}\n' + f'Msg:\n {server_response}' + ) + else: + logging.warning( + f'Non-RTCM msg received from Ntrip server:\n' + f'{server_response}' + ) + + self.send_rtcm_to_gnss(server_response) + + except socket.timeout: + logging.warning( + 'NTRIP server connection lost due to ' + 'timeout while reading RTCMv3. ' + 'Attempting to reconnect...' + ) + self.ntrip_connected = False + + def run(self): + """Run main execution loop.""" + while not self.connect_to_gnss(): + time.sleep(self.PAUSE_DURATION) + continue + + self.configure_gnss() + + try: + # Start reading GNSS logs in a separate thread + self.gnss_log_thread = threading.Thread( + target=self.read_nmea_and_send_to_server, daemon=True + ) + self.gnss_log_thread.start() + + while True: + if not self.ntrip_connected: + if not self.connect_ntrip_server(): + time.sleep(self.PAUSE_DURATION) + continue + + if not self.latest_nmea_data_valid: + logging.debug( + 'Waiting to receive valid NMEA data from GNSS ...' + ) + time.sleep(self.PAUSE_DURATION) + continue + + if not self.nmea_request_sent: + logging.debug( + 'Waiting for client to send valid NMEA data.' + ) + time.sleep(self.PAUSE_DURATION) + continue + + # Read RTCM data and send to the gnss in the main thread + self.read_rtcm_and_send_to_gnss() + + except KeyboardInterrupt: + logging.info('Interrupted by user. Disconnecting...') + logging.debug( + f'\nReceived RTCM msgs types:\n' + f'{self.received_rtcm_msgs_ids}' + ) + finally: + self.stop_event.set() + self.gnss_log_thread.join() + self.disconnect_ntrip_server() + + +if __name__ == '__main__': + + parser = argparse.ArgumentParser(description='Ntrip Client') + parser.add_argument( + '--param-file', + default='av_ntrip_credentials/smartnet_novatel_params.yaml', + help='Path to the YAML file containing Ntrip server credentials', + ) + parser.add_argument( + '--use-fix-location', + action='store_true', + help='Use fixed location for Ntrip requests', + ) + parser.add_argument( + '--debug', action='store_true', help='Enable debug mode' + ) + + args = parser.parse_args() + + client = NtripClient( + config_path=args.param_file, + use_fix_location=args.use_fix_location, + debug_mode=args.debug, + ) + + client.run()