diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 28e8a60db..d4d12c078 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -1,4 +1,4 @@ -FROM ghcr.io/ubcsailbot/sailbot_workspace/dev:fix-building-pre-base +FROM ghcr.io/ubcsailbot/sailbot_workspace/dev:moved-network-deps # Copy configuration files (e.g., .vimrc) from config/ to the container's home directory ARG USERNAME=ros diff --git a/.devcontainer/base-dev/base-dev.Dockerfile b/.devcontainer/base-dev/base-dev.Dockerfile index 3499485b3..22cb0cc18 100644 --- a/.devcontainer/base-dev/base-dev.Dockerfile +++ b/.devcontainer/base-dev/base-dev.Dockerfile @@ -194,14 +194,6 @@ RUN apt-get update \ && rosdep init || echo "rosdep already initialized" ENV DEBIAN_FRONTEND= -# install base python3 dependencies -RUN pip3 install \ - # from local pathfinding - plotly \ - pyproj \ - flask \ - shapely - # root bash configuration ENV ROS_WORKSPACE=/workspaces/sailbot_workspace COPY update-bashrc.sh /sbin/update-bashrc @@ -213,9 +205,6 @@ RUN chmod +x /sbin/update-bashrc \ # set timezone ENV TZ="America/Vancouver" -# customize ROS log format: https://docs.ros.org/en/humble/Concepts/About-Logging.html#environment-variables -ENV RCUTILS_CONSOLE_OUTPUT_FORMAT="[{severity}] [{time}] [{name}:{line_number}]: {message}" - FROM base as local-base # install virtual iridium dependencies @@ -255,6 +244,17 @@ RUN apt-get update \ && rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/* ENV DEBIAN_FRONTEND= +# install rapidyaml for diagnostics +ENV DEBIAN_FRONTEND=noninteractive +RUN wget https://github.com/biojppm/rapidyaml/releases/download/v0.5.0/rapidyaml-0.5.0-src.tgz +RUN tar -xzf rapidyaml-0.5.0-src.tgz && \ + cd rapidyaml-0.5.0-src && \ + cmake -S "." -B ./build/Release/ryml-build "-DCMAKE_INSTALL_PREFIX=/usr" -DCMAKE_BUILD_TYPE=Release && \ + cmake --build ./build/Release/ryml-build --parallel --config Release && \ + cmake --build ./build/Release/ryml-build --config Release --target install && \ + rm -rf *rapidyaml* +ENV DEBIAN_FRONTEND= + FROM local-base as ros-dev # From https://github.com/athackst/dockerfiles/blob/32a872348af0ad25ec4a6e6184cb803357acb6ab/ros2/humble.Dockerfile @@ -354,33 +354,11 @@ RUN apt-get update \ clangd \ clang-tidy \ cmake \ - googletest \ - libboost-all-dev \ - libprotobuf-dev \ - protobuf-compiler \ && apt-get autoremove -y \ && apt-get clean -y \ && rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/* ENV DEBIAN_FRONTEND= -# install rapidyaml for diagnostics -ENV DEBIAN_FRONTEND=noninteractive -RUN wget https://github.com/biojppm/rapidyaml/releases/download/v0.5.0/rapidyaml-0.5.0-src.tgz -RUN tar -xzf rapidyaml-0.5.0-src.tgz && \ - cd rapidyaml-0.5.0-src && \ - cmake -S "." -B ./build/Release/ryml-build "-DCMAKE_INSTALL_PREFIX=/usr" -DCMAKE_BUILD_TYPE=Release && \ - cmake --build ./build/Release/ryml-build --parallel --config Release && \ - cmake --build ./build/Release/ryml-build --config Release --target install && \ - rm -rf *rapidyaml* -ENV DEBIAN_FRONTEND= - -# install dev python3 dependencies -RUN pip3 install \ - # to be able to run juypter notebooks - ipykernel \ - # for integration_tests package - types-PyYAML - # install other helpful apt packages ENV DEBIAN_FRONTEND=noninteractive RUN apt-get update \ @@ -392,3 +370,8 @@ RUN apt-get update \ && apt-get clean -y \ && rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/* ENV DEBIAN_FRONTEND= + +# install dev python3 dependencies +RUN pip3 install \ + # for juypter notebooks + ipykernel diff --git a/.devcontainer/base-dev/update-bashrc.sh b/.devcontainer/base-dev/update-bashrc.sh index 36efa2dc4..0a08e0118 100644 --- a/.devcontainer/base-dev/update-bashrc.sh +++ b/.devcontainer/base-dev/update-bashrc.sh @@ -11,6 +11,8 @@ echo "" >> $HOME/.bashrc echo "# set up ROS environment" >> $HOME/.bashrc echo "source /usr/share/colcon_cd/function/colcon_cd.sh" >> $HOME/.bashrc echo "export _colcon_cd_root=$ROS_WORKSPACE" >> $HOME/.bashrc +echo "# customize ROS log format: https://docs.ros.org/en/humble/Concepts/About-Logging.html#environment-variables" >> $HOME/.bashrc +echo "export RCUTILS_CONSOLE_OUTPUT_FORMAT='[{severity}] [{time}] [{name}:{line_number}]: {message}'" >> $HOME/.bashrc echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> $HOME/.bashrc echo "if [ -f $ROS_WORKSPACE/install/local_setup.bash ]" >> $HOME/.bashrc echo "then" >> $HOME/.bashrc diff --git a/.devcontainer/docs/README.md b/.devcontainer/docs/README.md index b135caac9..607a4c48c 100644 --- a/.devcontainer/docs/README.md +++ b/.devcontainer/docs/README.md @@ -1,6 +1,6 @@ # Docs Image -Used for running [our docs site](https://github.com/UBCSailbot/docs). +Used for running [our docs site](https://ubcsailbot.github.io/sailbot_workspace/main/). ## Features diff --git a/.devcontainer/website/README.md b/.devcontainer/website/README.md index cb8dc0ebc..d9778ef1e 100644 --- a/.devcontainer/website/README.md +++ b/.devcontainer/website/README.md @@ -1,6 +1,6 @@ # Website Image -Used for running [our website](https://github.com/UBCSailbot/website). +Used for running [our website](https://github.com/UBCSailbot/sailbot_workspace/tree/main/src/website). ## Features diff --git a/.devcontainer/website/website.Dockerfile b/.devcontainer/website/website.Dockerfile index 893136ada..0a19cdac8 100644 --- a/.devcontainer/website/website.Dockerfile +++ b/.devcontainer/website/website.Dockerfile @@ -1,7 +1,4 @@ -# Copied from https://github.com/microsoft/vscode-dev-containers/blob/5a084a93b0736ea86395ac99019a5b72a00b6341/containers/javascript-node-mongo/.devcontainer/Dockerfile -# [Choice] Node.js version (use -bullseye variants on local arm64/Apple Silicon): 18, 16, 14, 18-bullseye, 16-bullseye, 14-bullseye, 18-buster, 16-buster, 14-buster -ARG VARIANT=18 -FROM mcr.microsoft.com/vscode/devcontainers/javascript-node:0-${VARIANT} +FROM node:20-alpine # [Optional] Uncomment this section to install additional OS packages. # RUN apt-get update && export DEBIAN_FRONTEND=noninteractive \ @@ -17,4 +14,4 @@ FROM mcr.microsoft.com/vscode/devcontainers/javascript-node:0-${VARIANT} # Adapted from https://www.digitalocean.com/community/tutorials/how-to-build-a-node-js-application-with-docker WORKDIR /website EXPOSE 3005 -CMD npm install --legacy-peer-deps && npm run dev +CMD npm install && npm run dev diff --git a/.github/dependabot.yml b/.github/dependabot.yml index afb98ae0c..94198add2 100644 --- a/.github/dependabot.yml +++ b/.github/dependabot.yml @@ -1,7 +1,23 @@ +# To get started with Dependabot version updates, you'll need to specify which +# package ecosystems to update and where the package manifests are located. +# Please see the documentation for all configuration options: +# https://docs.github.com/github/administering-a-repository/configuration-options-for-dependency-updates + version: 2 updates: -- package-ecosystem: github-actions - directory: "/" - schedule: - interval: daily - open-pull-requests-limit: 10 + - package-ecosystem: "github-actions" # See documentation for possible values + directory: "/" # Location of package manifests + schedule: + interval: "weekly" + # # Create a group of dependencies to be updated together in one pull request + # groups: + # # Specify a name for the group, which will be used in pull request titles + # # and branch names + # gh-actions: + # # Define patterns to include dependencies in the group (based on + # # dependency name) + # patterns: + # # - "rubocop" # A single dependency name + # # - "rspec*" # A wildcard string that matches multiple dependency names + # - "*" # A wildcard that matches all dependencies in the package + # # ecosystem. Note: using "*" may open a large pull request diff --git a/docs/assets/images/sailbot_workspace/workflow/sailbot_bug.png b/docs/assets/images/sailbot_workspace/workflow/sailbot_bug.png index d98be96e2..a84a83a03 100644 Binary files a/docs/assets/images/sailbot_workspace/workflow/sailbot_bug.png and b/docs/assets/images/sailbot_workspace/workflow/sailbot_bug.png differ diff --git a/docs/current/sailbot_workspace/how_to.md b/docs/current/sailbot_workspace/how_to.md index e8956e018..ae29e0ff0 100644 --- a/docs/current/sailbot_workspace/how_to.md +++ b/docs/current/sailbot_workspace/how_to.md @@ -33,8 +33,8 @@ For prefixes that are words, you will have to append a space to them to bring up We have containerized the following applications for a variety of reasons: - [MongoDB database](https://www.mongodb.com/){target=_blank} -- [Docs site](https://github.com/UBCSailbot/docs){target=_blank} -- [Website](https://github.com/UBCSailbot/website){target=_blank} +- [Docs site](https://ubcsailbot.github.io/sailbot_workspace/main/){target=_blank} +- [Website](https://github.com/UBCSailbot/sailbot_workspace/tree/main/src/website){target=_blank} ### Running containerized applications diff --git a/docs/reference/markdown.md b/docs/reference/markdown.md index 2781dcbe7..a14c56b8d 100644 --- a/docs/reference/markdown.md +++ b/docs/reference/markdown.md @@ -55,12 +55,12 @@ experience is required to contribute to our docs. Material for MkDocs supports powerful features purpose-built to take technical documentation to the next level. Feel free to browse this site to see how we use these features, exploring their syntax in the -[source code](https://github.com/UBCSailbot/docs/tree/main/docs){target=_blank}. Since GitHub renders Markdown files automatically -you will need to click the "Raw" button to view their contents. +[source code](https://github.com/UBCSailbot/sailbot_workspace/tree/main/docs){target=_blank}. +Since GitHub renders Markdown files automatically you will need to click the "Raw" button to view their contents. !!! note "Material-Flavoured Markdown" - Material for MkDocs' flavour of Markdown extends upon vanilla Markdown, adding features such as admonitions + Material for MkDocs' flavour of Markdown extends upon vanilla Markdown, adding features such as admonitions (like this note) and content tabs. Refer to the [official Material for MkDocs reference page](https://squidfunk.github.io/mkdocs-material/reference/){target=_blank} for more information on the available features. @@ -85,7 +85,7 @@ resources are good for rendering Markdown: === ":logo: Material for MkDocs" - UBC Sailbot Docs: To preview your changes when working on this site, - refer to the [run instructions in the `README.md`](https://github.com/UBCSailbot/docs#run){target=_blank}. + refer to the [How to work with containerized applications](../current/sailbot_workspace/how_to.md#work-with-containerized-applications){target=_blank}. - Material for MkDocs sites in general: If you ever decide to write your own documentation using Material for MkDocs, refer to the [official "Getting Started" guide](https://squidfunk.github.io/mkdocs-material/getting-started/){target=_blank}. @@ -98,10 +98,10 @@ to browse around for the solution that suits your needs. We lint our Markdown files to reduce errors and increase readability. In particular, we use two tools: 1. [markdownlint](https://github.com/DavidAnson/markdownlint){target=_blank} is -used to enforce a style guide. Its configuration file for this repository is [`.markdownlint.json`](https://github.com/UBCSailbot/docs/blob/main/.markdownlint.json){target=_blank}. +used to enforce a style guide. Its configuration file for this repository is [`.markdownlint.json`](https://github.com/UBCSailbot/sailbot_workspace/blob/main/.markdownlint.json){target=_blank}. If you use VS Code, there is a [markdownlint extension](https://marketplace.visualstudio.com/items?itemName=DavidAnson.vscode-markdownlint){target=_blank}. 2. [markdown-link-check](https://github.com/tcort/markdown-link-check){target=_blank} is -used to check for broken links. Its configuration file for this repository is [`.markdown-link-check.json`](https://github.com/UBCSailbot/docs/blob/main/.markdown-link-check.json){target=_blank}. +used to check for broken links. Its configuration file for this repository is [`.markdown-link-check.json`](https://github.com/UBCSailbot/sailbot_workspace/blob/main/.markdown-link-check.json){target=_blank}. [^1]: diff --git a/mkdocs.yml b/mkdocs.yml index f5d24c27b..c1cc6a49a 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -4,9 +4,9 @@ site_url: https://UBCSailbot.github.io/sailbot_workspace/ site_author: UBCSailbot Software Team # Repository -repo_name: UBCSailbot/docs -repo_url: https://github.com/UBCSailbot/docs -edit_uri: "edit/main/docs/" +repo_name: UBCSailbot/sailbot_workspace +repo_url: https://github.com/UBCSailbot/sailbot_workspace +edit_uri: "edit/main/sailbot_workspace/" # Configuration theme: diff --git a/sailbot.code-workspace b/sailbot.code-workspace index 24223190c..3589961d4 100644 --- a/sailbot.code-workspace +++ b/sailbot.code-workspace @@ -414,7 +414,7 @@ "label": "lint_cmake", "detail": "Run lint on cmake files.", "type": "shell", - "command": "LINTER=lint_cmake LOCAL_RUN=true .github/actions/ament-lint/run.sh", + "command": "LINTER=lint_cmake LOCAL_RUN=true .github/actions/run-in-container/ament-lint.sh", "problemMatcher": [ "$ament_lint_cmake" ], @@ -427,7 +427,7 @@ "label": "clang-tidy", "detail": "Run clang-tidy static analysis", "type": "shell", - "command": "LOCAL_RUN=true .github/actions/clang-tidy/run.sh", + "command": "LOCAL_RUN=true .github/actions/run-in-container/clang-tidy.sh", "problemMatcher": [], "presentation": { "panel": "dedicated", @@ -438,7 +438,7 @@ "label": "flake8", "detail": "Run flake8 on python files.", "type": "shell", - "command": "LINTER=flake8 LOCAL_RUN=true .github/actions/ament-lint/run.sh", + "command": "LINTER=flake8 LOCAL_RUN=true .github/actions/run-in-container/ament-lint.sh", "problemMatcher": [ "$ament_flake8" ], @@ -451,7 +451,7 @@ "label": "mypy", "detail": "Run mypy on python files.", "type": "shell", - "command": "LINTER=mypy LOCAL_RUN=true .github/actions/ament-lint/run.sh", + "command": "LINTER=mypy LOCAL_RUN=true .github/actions/run-in-container/ament-lint.sh", "problemMatcher": [ "$ament_mypy", ], @@ -464,7 +464,7 @@ "label": "xmllint", "detail": "Run xmllint on xml files.", "type": "shell", - "command": "LINTER=xmllint LOCAL_RUN=true .github/actions/ament-lint/run.sh", + "command": "LINTER=xmllint LOCAL_RUN=true .github/actions/run-in-container/ament-lint.sh", "problemMatcher": [ "$ament_xmllint", ], diff --git a/setup.sh b/setup.sh index ab0425f87..b65ac1040 100755 --- a/setup.sh +++ b/setup.sh @@ -1,6 +1,20 @@ #!/bin/bash set -e +# Create/overwrite the custom rosdep list file +CUSTOM_ROSDEP_LIST="/etc/ros/rosdep/sources.list.d/20-sailbot.list" +CUSTOM_ROSDEP_FILE="custom-rosdep.yaml" +echo "# sailbot" | sudo tee $CUSTOM_ROSDEP_LIST > /dev/null +for DIR in $ROS_WORKSPACE/src/*; do + if [ -d "$DIR" ]; then + FILE="$DIR/$CUSTOM_ROSDEP_FILE" + if [ -f $FILE ]; then + echo "Adding $FILE to $CUSTOM_ROSDEP_LIST" + echo "yaml file://$FILE" | sudo tee --append $CUSTOM_ROSDEP_LIST > /dev/null + fi + fi +done + sudo apt-get update rosdep update --rosdistro $ROS_DISTRO rosdep install --from-paths src --ignore-src -y --rosdistro $ROS_DISTRO diff --git a/src/boat_simulator/README.md b/src/boat_simulator/README.md index 26ce3f565..24ea423bc 100644 --- a/src/boat_simulator/README.md +++ b/src/boat_simulator/README.md @@ -1,7 +1,5 @@ # UBC Sailbot Boat Simulator -[![Tests](https://github.com/UBCSailbot/boat_simulator/actions/workflows/tests.yml/badge.svg)](https://github.com/UBCSailbot/boat_simulator/actions/workflows/tests.yml) - UBC Sailbot's boat simulator for the new project. This repository contains a ROS package `boat_simulator`. This README contains only setup and run instructions. Further information on the boat simulator can be found on the software team's [docs website](https://ubcsailbot.github.io/sailbot_workspace/main/current/boat_simulator/overview/). diff --git a/src/boat_simulator/boat_simulator/common/sensors.py b/src/boat_simulator/boat_simulator/common/sensors.py index a03940815..d1bf9d3b1 100644 --- a/src/boat_simulator/boat_simulator/common/sensors.py +++ b/src/boat_simulator/boat_simulator/common/sensors.py @@ -1,24 +1,43 @@ -from dataclasses import dataclass - -from typing import Optional, Any +from typing import Any from numpy.typing import NDArray - +from typing import List +import numpy as np from boat_simulator.common.types import Scalar, ScalarOrArray from boat_simulator.common.generators import ( - ConstantGenerator, MVGaussianGenerator, GaussianGenerator, ) -WindSensorGenerators = Optional[MVGaussianGenerator | ConstantGenerator] -GPSGenerators = Optional[GaussianGenerator | ConstantGenerator] - -@dataclass class Sensor: - """Interface for sensors in the Boat Simulation.""" + """ + + Interface for sensors in the Boat Simulation. + + Data delay and noise models are supported. + + Delay model will delay sensor value updates by one update cycle: + - Sensor has initial data x0 at t = 0 + - Sensor provided new data x_1 at t = 1 + - Sensor provided new data x_2 at t = 2. At t = 2, x1 is registered into the sensor. + - Sensor provided new data x_i at t = i. At t = i, x_{i-1} is registered into the sensor. + + Noise model will add noise to sensor values drawn from a + Gaussian or Multi-variate Gaussian distribution. + """ + + def __init__(self, enable_delay: bool = False, enable_noise: bool = False) -> None: + """ + + Args: + enable_noise (bool): Enables noise for fields. False by default. + enable_delay (bool): Enables delay for fields. False by default. + """ + + self.enable_delay = enable_delay + self.enable_noise = enable_noise def update(self, **kwargs): """ @@ -29,6 +48,7 @@ def update(self, **kwargs): Raises: ValueError: If kwarg is not a defined attribute in Sensor """ + for attr_name, attr_val in kwargs.items(): if attr_name in self.__annotations__: setattr(self, attr_name, attr_val) @@ -60,98 +80,181 @@ def read(self, key: str) -> Any: ) -@dataclass class WindSensor(Sensor): """ Abstraction for wind sensor. - # TODO: Add delay functions. - Properties: wind (ScalarOrArray): Wind x, y components or single value - wind_noisemaker (Optional[MVGaussianGenerator | ConstantGenerator]): - Noise function to emulate sensor noise in wind data reading + enable_noise (bool): Enables noise for fields. False by default. + enable_delay (bool): Enables delay for fields. False by default. """ wind: ScalarOrArray - wind_noisemaker: WindSensorGenerators = None + + def __init__( + self, + wind: ScalarOrArray, + wind_noise_stdev: List[Scalar] = [1.0, 1.0], + enable_noise: bool = False, + enable_delay: bool = False, + ) -> None: + super().__init__(enable_noise=enable_noise, enable_delay=enable_delay) + self._wind = wind + + # TODO: Refactor the initialization of data fields and their respective delay controls. + # Warning: this is not easy! + + self.wind_queue_next: bool = False + self.wind_next_value: ScalarOrArray = wind + self.wind_noisemaker: MVGaussianGenerator = MVGaussianGenerator( + mean=np.array([0, 0]), cov=np.diag(np.power(wind_noise_stdev, 2)) + ) @property # type: ignore def wind(self) -> ScalarOrArray: # TODO: Ensure attribute value and noisemakers are using the same value shape. # - wind scalars should add with noise scalars. # - wind vectors should add with noise vectors. - # Could consider using a __post_init__ function for this + return ( self._wind + self.wind_noisemaker.next() # type: ignore - if self.wind_noisemaker is not None + if self.enable_noise else self._wind ) @wind.setter def wind(self, wind: ScalarOrArray): - self._wind = wind + + if not self.enable_delay: + self._wind = wind + return + + if self.wind_queue_next: + self._wind = self.wind_next_value + else: + self.wind_queue_next = True + + self.wind_next_value = wind -@dataclass class GPS(Sensor): """ Abstraction for GPS. - # TODO: Add delay functions. - Properties: lat_lon (NDArray): Boat latitude and longitude (2x1 array) speed (Scalar): Boat speed heading (Scalar): Boat heading - lat_lon_noisemaker (Optional[GaussianGenerator | ConstantGenerator]): - Noise function to emulate sensor noise in latitude and longitude readings - speed_noisemaker (Optional[GaussianGenerator | ConstantGenerator]): - Noise function to emulate sensor noise in speed readings - heading_noisemaker (Optional[GaussianGenerator | ConstantGenerator]): - Noise function to emulate sensor noise in heading readings + enable_noise (bool): Enables noise for fields. False by default. + enable_delay (bool): Enables delay for fields. False by default. """ lat_lon: NDArray speed: Scalar heading: Scalar - lat_lon_noisemaker: GPSGenerators = None - speed_noisemaker: GPSGenerators = None - heading_noisemaker: GPSGenerators = None + def __init__( + self, + lat_lon: NDArray, + speed: Scalar, + heading: Scalar, + lat_lon_noise_stdev: Scalar = 1, + speed_noise_stdev: Scalar = 1, + heading_noise_stdev: Scalar = 1, + enable_noise: bool = False, + enable_delay: bool = False, + ): + super().__init__(enable_noise=enable_noise, enable_delay=enable_delay) + self._lat_lon = lat_lon + self._speed = speed + self._heading = heading + + # TODO: Refactor the initialization of data fields and their respective delay controls. + # Warning: this is not easy! + + # Delay Controls + self.lat_lon_queue_next: bool = False + self.lat_lon_next_value: NDArray = lat_lon + + self.speed_queue_next: bool = False + self.speed_next_value: Scalar = speed + + self.heading_queue_next: bool = False + self.heading_next_value: Scalar = heading + + self.lat_lon_noisemaker: GaussianGenerator = GaussianGenerator( + mean=0, stdev=lat_lon_noise_stdev + ) + self.speed_noisemaker: GaussianGenerator = GaussianGenerator( + mean=0, stdev=speed_noise_stdev + ) + self.heading_noisemaker: GaussianGenerator = GaussianGenerator( + mean=0, stdev=heading_noise_stdev + ) @property # type: ignore def lat_lon(self) -> NDArray: return ( self._lat_lon + self.lat_lon_noisemaker.next() - if self.lat_lon_noisemaker is not None + if self.enable_noise else self._lat_lon ) @lat_lon.setter def lat_lon(self, lat_lon: NDArray): - self._lat_lon = lat_lon + + if not self.enable_delay: + self._lat_lon = lat_lon + return + + if self.lat_lon_queue_next: + self._lat_lon = self.lat_lon_next_value + else: + self.lat_lon_queue_next = True + + self.lat_lon_next_value = lat_lon @property # type: ignore def speed(self) -> Scalar: return ( self._speed + self.speed_noisemaker.next() # type: ignore - if self.speed_noisemaker is not None + if self.enable_noise else self._speed ) @speed.setter def speed(self, speed: Scalar): - self._speed = speed + + if not self.enable_delay: + self._speed = speed + return + + if self.speed_queue_next: + self._speed = self.speed_next_value + else: + self.speed_queue_next = True + + self.speed_next_value = speed @property # type: ignore def heading(self) -> Scalar: return ( self._heading + self.heading_noisemaker.next() # type: ignore - if self.heading_noisemaker is not None + if self.enable_noise else self._heading ) @heading.setter def heading(self, heading: Scalar): - self._heading = heading + + if not self.enable_delay: + self._heading = heading + return + + if self.heading_queue_next: + self._heading = self.heading_next_value + else: + self.heading_queue_next = True + + self.heading_next_value = heading diff --git a/src/boat_simulator/boat_simulator/common/unit_conversions.py b/src/boat_simulator/boat_simulator/common/unit_conversions.py index 701b443a5..d41706742 100644 --- a/src/boat_simulator/boat_simulator/common/unit_conversions.py +++ b/src/boat_simulator/boat_simulator/common/unit_conversions.py @@ -131,6 +131,9 @@ class ConversionFactors(Enum): km_to_nautical_mi = nautical_mi_to_km.inverse() # Time + sec_to_ms = ConversionFactor(factor=1000) + ms_to_sec = sec_to_ms.inverse() + min_to_sec = ConversionFactor(factor=60) sec_to_min = min_to_sec.inverse() @@ -199,7 +202,9 @@ def __init__(self, **kwargs: EnumAttr): belonging to `ConversionFactors`. """ for attr_name, attr_val in kwargs.items(): - assert isinstance(attr_val, Enum) and isinstance(attr_val.value, ConversionFactor) + assert isinstance(attr_val, Enum) and isinstance( + attr_val.value, ConversionFactor + ) setattr(self, attr_name, attr_val) def convert(self, **kwargs: ScalarOrArray) -> Dict[str, ScalarOrArray]: @@ -223,7 +228,9 @@ def convert(self, **kwargs: ScalarOrArray) -> Dict[str, ScalarOrArray]: for attr_name, attr_val in kwargs.items(): attr = getattr(self, attr_name, None) - assert attr is not None, f"Attribute name {attr} not found in UnitConverter." + assert ( + attr is not None + ), f"Attribute name {attr} not found in UnitConverter." conversion_factor = attr.value converted_values[attr_name] = conversion_factor.forward_convert(attr_val) diff --git a/src/boat_simulator/boat_simulator/nodes/physics_engine/fluid_forces.py b/src/boat_simulator/boat_simulator/nodes/physics_engine/fluid_forces.py index c479bdb2d..770e8f343 100644 --- a/src/boat_simulator/boat_simulator/nodes/physics_engine/fluid_forces.py +++ b/src/boat_simulator/boat_simulator/nodes/physics_engine/fluid_forces.py @@ -2,9 +2,12 @@ from typing import Tuple +import matplotlib.patches as patches +import matplotlib.pyplot as plt +import numpy as np from numpy.typing import NDArray -from boat_simulator.common.types import Scalar +from boat_simulator.common.utils import Scalar class MediumForceComputation: @@ -18,9 +21,7 @@ class MediumForceComputation: `drag_coefficients` (NDArray): An array of shape (n, 2) where each row contains a pair (x, y) representing an angle of attack, in degrees, and its corresponding drag coefficient. - `areas` (NDArray): An array of shape (n, 2) where each row contains a pair (x, y), - representing an angle of attack, in degrees, and its corresponding area, in square - meters (m^2). + `areas` (Scalar): Corresponding area, in square meters (m^2). `fluid_density` (Scalar): The density of the fluid acting on the medium, expressed in kilograms per cubic meter (kg/m^3). """ @@ -29,7 +30,7 @@ def __init__( self, lift_coefficients: NDArray, drag_coefficients: NDArray, - areas: NDArray, + areas: Scalar, fluid_density: Scalar, ): self.__lift_coefficients = lift_coefficients @@ -37,6 +38,40 @@ def __init__( self.__areas = areas self.__fluid_density = fluid_density + def calculate_attack_angle(self, apparent_velocity: NDArray, orientation: Scalar) -> Scalar: + """Calculates the angle of attack formed between the orientation angle of the medium + and the direction of the apparent velocity, bounded between -180 and 180 degrees. + + Args: + apparent_velocity (NDArray): The apparent (relative) velocity between the fluid and + the medium, expressed in meters per second (m/s). + orientation (Scalar): The orientation angle of the medium in degrees. + + Returns: + Scalar: The angle of attack formed between the orientation angle of the medium and + the direction of the apparent velocity, expressed in degrees + and bounded between -180 and 180 degrees. + """ + # Check if the apparent velocity is [0, 0] + if np.all(apparent_velocity == 0): + # Directly return the normalized orientation as the angle of attack + # Normalize orientation to be within [-180, 180) + return ((orientation + 180) % 360) - 180 + + # Calculate the angle in degrees of the apparent velocity + angle_of_attack_raw = np.rad2deg(np.arctan2(apparent_velocity[1], apparent_velocity[0])) + + # Adjust orientation to be in the range of [-180, 180) + orientation = ((orientation + 180) % 360) - 180 + + # Calculate the raw angle of attack by subtracting the orientation from the velocity angle + angle_of_attack = angle_of_attack_raw - orientation + + # Normalize the angle of attack to [-180, 180) range + angle_of_attack = ((angle_of_attack + 180) % 360) - 180 + + return angle_of_attack + def compute(self, apparent_velocity: NDArray, orientation: Scalar) -> Tuple[NDArray, NDArray]: """Computes the lift and drag forces experienced by a medium immersed in a fluid. @@ -52,11 +87,97 @@ def compute(self, apparent_velocity: NDArray, orientation: Scalar) -> Tuple[NDAr by the medium, both expressed in newtons (N). """ - # TODO: Implement this method. + attack_angle = self.calculate_attack_angle(apparent_velocity, orientation) + lift_coefficient, drag_coefficient = self.interpolate(attack_angle) + velocity_magnitude = np.linalg.norm(apparent_velocity) + + # Calculate the lift and drag forces - raise NotImplementedError() + lift_force_magnitude = self.__calculate_fluid_force_magnitude( + lift_coefficient, velocity_magnitude + ) + drag_force_magnitude = self.__calculate_fluid_force_magnitude( + drag_coefficient, velocity_magnitude + ) - def interpolate(self, attack_angle: Scalar) -> Tuple[Scalar, Scalar, Scalar]: + drag_force_unit_vector = apparent_velocity / velocity_magnitude + drag_force_unit_vector = self.__rotate_vector(drag_force_unit_vector, orientation) + + # Rotate the lift and drag forces by 90 degrees to obtain the lift and drag forces + + # Convention used here is that the positive x-axis is 0 degrees + # and the positive y-axis is 90 degrees + # Positive rotation is counter clockwise + + is_drag_in_first_or_third_quadrant = ( + drag_force_unit_vector[0] > 0 and drag_force_unit_vector[1] > 0 + ) or (drag_force_unit_vector[0] < 0 and drag_force_unit_vector[1] < 0) + + is_drag_in_second_or_fourth_quadrant = ( + drag_force_unit_vector[0] > 0 and drag_force_unit_vector[1] < 0 + ) or (drag_force_unit_vector[0] < 0 and drag_force_unit_vector[1] > 0) + + # Rotate the lift force direction based on the quadrant of the drag force + if is_drag_in_first_or_third_quadrant: + # Rotate counter clockwise to get lift direction + lift_force_direction = np.array( + [-drag_force_unit_vector[1], drag_force_unit_vector[0]] + ) + elif is_drag_in_second_or_fourth_quadrant: + # Rotate clockwise to get lift direction + lift_force_direction = np.array( + [drag_force_unit_vector[1], -drag_force_unit_vector[0]] + ) + else: + # Should not happen if drag force direction is properly normalized + # This could be a fallback for an unexpected case + lift_force_direction = np.array([0, 0]) + + # Rotate the lift and drag forces back to the original orientation + lift_force_direction = self.__rotate_vector( + lift_force_direction, orientation, clockwise=False + ) + drag_force_unit_vector = self.__rotate_vector( + drag_force_unit_vector, orientation, clockwise=False + ) + + lift_force = lift_force_magnitude * lift_force_direction + drag_force = drag_force_magnitude * drag_force_unit_vector + + return lift_force, drag_force + + def __calculate_fluid_force_magnitude( + self, coefficient: Scalar, velocity_magnitude: Scalar + ) -> Scalar: + """Calculates the magnitude of fluid forces based on coefficient and velocity.""" + return 0.5 * self.__fluid_density * coefficient * self.__areas * (velocity_magnitude**2) + + def __rotate_vector(self, v: NDArray, theta_degrees: Scalar, clockwise=True) -> NDArray: + """ + Rotates a vector by a specified angle in degrees. + + Args: + v (np.array): The vector to be rotated. + theta_degrees (float): The rotation angle in degrees. + clockwise (bool, optional): Determines the direction of rotation. If True (default), + rotates the vector clockwise. If False, rotates the vector + counterclockwise. + + Returns: + np.array: The rotated vector. + """ + theta_radians = np.deg2rad(theta_degrees) + sign = 1 if clockwise else -1 + rotation_matrix = np.array( + [ + [np.cos(theta_radians), sign * np.sin(theta_radians)], + [-sign * np.sin(theta_radians), np.cos(theta_radians)], + ] + ) + v_rotated = np.dot(rotation_matrix, v) + return v_rotated + + def interpolate(self, attack_angle: Scalar) -> Tuple[Scalar, Scalar]: """Performs linear interpolation to estimate the lift and drag coefficients, as well as the associated area upon which the fluid acts, based on the provided angle of attack. @@ -65,16 +186,147 @@ def interpolate(self, attack_angle: Scalar) -> Tuple[Scalar, Scalar, Scalar]: the medium and the direction of the apparent velocity, expressed in degrees. Returns: - Tuple[Scalar, Scalar, Scalar]: A tuple representing the computed parameters. The + Tuple[Scalar, Scalar]: A tuple representing the computed parameters. The first scalar denotes the lift coefficient, the second scalar represents the drag coefficient, and the third scalar indicates the surface area upon which the fluid acts. Both lift and drag coefficients are unitless, while the area is expressed in square meters (m^2). """ - # TODO: Implement this method using `np.interp`. + lift_coefficient = np.interp( + attack_angle, self.__lift_coefficients[:, 0], self.__lift_coefficients[:, 1] + ) + drag_coefficient = np.interp( + attack_angle, self.__drag_coefficients[:, 0], self.__drag_coefficients[:, 1] + ) + return lift_coefficient, drag_coefficient + + def _draw_boat(ax, position, orientation): + """Draws a simplified boat shape on the given axes, ensuring it aligns + with the orientation line.""" + boat_length = 1.0 + boat_width = 0.3 + + # Center the shape around (0, 0) + front_extension = 3 * boat_length / 4 + rear_extension = -boat_length / 4 + + # Calculate the offset to center the shape + offset = front_extension + rear_extension + + boat_shape = np.array( + [ + [front_extension - offset, 0], + [boat_length / 2 - offset, boat_width / 2], + [rear_extension - offset, 0], + [boat_length / 2 - offset, -boat_width / 2], + [front_extension - offset, 0], + ] + ) + + # Rotation matrix for anticlockwise rotation + rotation_matrix = np.array( + [ + [np.cos(np.deg2rad(orientation)), np.sin(np.deg2rad(orientation))], + [np.sin(np.deg2rad(orientation)), np.cos(np.deg2rad(orientation))], + ] + ) + + # Apply rotation + rotated_boat = np.dot(boat_shape, rotation_matrix) + + # Translate boat to its position + translated_boat = rotated_boat + np.array(position) + + # Draw the boat + ax.plot(translated_boat[:, 0], translated_boat[:, 1], "k") + + # Ensure the orientation line is drawn correctly + # Calculate a point along the orientation direction + direction = np.array([np.cos(np.deg2rad(orientation)), np.sin(np.deg2rad(orientation))]) + line_start = np.array(position) + line_end = ( + line_start + direction * boat_length + ) # Extend the line out from the boat's position + + # Draw orientation line + ax.plot( + [line_start[0], line_end[0]], [line_start[1], line_end[1]], "black", linestyle="--" + ) + + def visualize_forces( + self, apparent_velocity, lift_force, drag_force, position=[0, 0], orientation=0 + ): + """Visualizes the sailboat, apparent velocity, lift force, and drag force.""" + fig, ax = plt.subplots() + attack_angle = self.calculate_attack_angle(apparent_velocity, orientation) + # Normalize forces for visualization + norm_apparent_velocity = apparent_velocity / np.linalg.norm(apparent_velocity) + norm_lift_force = lift_force / np.linalg.norm(lift_force) + norm_drag_force = drag_force / np.linalg.norm(drag_force) + + # Draw the boat + MediumForceComputation._draw_boat(ax, position, orientation) + # Plot forces and velocity + ax.quiver( + position[0], + position[1], + norm_apparent_velocity[0], + norm_apparent_velocity[1], + color="blue", + scale=5, + label="Apparent Velocity", + pivot="tip", + ) + ax.quiver( + position[0], + position[1], + norm_lift_force[0], + norm_lift_force[1], + color="red", + scale=5, + label="Lift Force", + ) + ax.quiver( + position[0], + position[1], + norm_drag_force[0], + norm_drag_force[1], + color="green", + scale=5, + label="Drag Force", + ) + orientation_rad = np.deg2rad(orientation) # Convert orientation to radians + ax.axline((0, 0), slope=np.tan(orientation_rad), color="black", linestyle="--") + + # Calculate angle for drag force + drag_angle = np.arctan2(norm_drag_force[1], norm_drag_force[0]) + + # Determine start and end angles for the arc + start_angle = np.rad2deg(orientation_rad) + end_angle = np.rad2deg(drag_angle) + + # Draw arc to represent angle between orientation and drag force + radius = 0.05 + arc = patches.Arc( + position, + 2 * radius, + 2 * radius, + angle=0, + theta1=min(start_angle, end_angle), + theta2=max(start_angle, end_angle), + color="purple", + label="Angle Arc", + ) + ax.add_patch(arc) - raise NotImplementedError() + ax.axis("equal") + ax.legend() + plt.title("Forces Acting on Sailboat for Attack Angle: " + str(round(attack_angle))) + plt.xlabel("X-axis") + plt.ylabel("Y-axis") + plt.grid(True) + plt.show() @property def lift_coefficients(self) -> NDArray: @@ -85,7 +337,7 @@ def drag_coefficients(self) -> NDArray: return self.__drag_coefficients @property - def areas(self) -> NDArray: + def areas(self) -> Scalar: return self.__areas @property diff --git a/src/boat_simulator/package.xml b/src/boat_simulator/package.xml index 55401ed73..cec78dd46 100644 --- a/src/boat_simulator/package.xml +++ b/src/boat_simulator/package.xml @@ -11,6 +11,7 @@ ament_flake8 ament_pep257 python3-pytest + python3-matplotlib custom_interfaces @@ -20,6 +21,7 @@ python3-numpy python3-scipy + python3-matplotlib ament_python diff --git a/src/boat_simulator/tests/unit/common/test_gps_sensor.py b/src/boat_simulator/tests/unit/common/test_gps_sensor.py index 60f2568d8..90a3ab982 100644 --- a/src/boat_simulator/tests/unit/common/test_gps_sensor.py +++ b/src/boat_simulator/tests/unit/common/test_gps_sensor.py @@ -1,9 +1,5 @@ from boat_simulator.common.sensors import GPS import numpy as np -from boat_simulator.common.generators import ( - ConstantGenerator, - GaussianGenerator, -) class TestGPS: @@ -11,96 +7,38 @@ def test_gps_init(self): lat_lon = np.array([1, 0]) speed = 100 heading = 1.09 - error_fn = None gps = GPS( lat_lon=lat_lon, speed=speed, heading=heading, - lat_lon_noisemaker=error_fn, - speed_noisemaker=error_fn, - heading_noisemaker=error_fn, ) assert (gps.lat_lon == lat_lon).all() assert gps.speed == speed assert gps.heading == heading - assert gps.lat_lon_noisemaker is error_fn - assert gps.speed_noisemaker is error_fn - assert gps.heading_noisemaker is error_fn - def test_gps_init_implicit_error_fn(self): - lat_lon = np.array([1, 0]) - speed = 100 - heading = 1.09 - - gps = GPS( - lat_lon=lat_lon, - speed=speed, - heading=heading, - ) - - assert (gps.lat_lon == lat_lon).all() - assert gps.speed == speed - assert gps.heading == heading - for noisemaker in [ - gps.lat_lon_noisemaker, - gps.speed_noisemaker, - gps.heading_noisemaker, - ]: - assert noisemaker is None - - def test_gps_read_no_error(self): + def test_gps_read_no_noise(self): lat_lon = np.array([1, 0]) speed = np.random.randint(0, 100) heading = np.random.rand() - gps = GPS( - lat_lon=lat_lon, - speed=speed, - heading=heading, - ) + gps = GPS(lat_lon=lat_lon, speed=speed, heading=heading, enable_noise=False) - assert (gps.read("lat_lon") == lat_lon).all() + assert np.all(gps.read("lat_lon") == lat_lon) assert gps.read("speed") == speed assert gps.read("heading") == heading - def test_gps_read_constant_error(self): - lat_lon = np.array([1, 0]) - speed = np.random.randint(0, 100) - heading = np.random.rand() - constant = 3.01 - error_fn = ConstantGenerator(constant=constant) - - gps = GPS( - lat_lon=lat_lon, - speed=speed, - heading=heading, - lat_lon_noisemaker=error_fn, - speed_noisemaker=error_fn, - heading_noisemaker=error_fn, - ) - - assert (gps.read("lat_lon") == lat_lon + constant).all() - assert gps.read("speed") == speed + constant - assert gps.read("heading") == heading + constant - - def test_gps_gaussian_error(self): + def test_gps_gaussian_noise(self): lat_lon = np.array([1, 0]) speed = np.random.randint(0, 100) heading = np.random.rand() mean = 0 - stdev = 1 - - error_fn = GaussianGenerator(mean=mean, stdev=stdev) gps = GPS( lat_lon=lat_lon, speed=speed, heading=heading, - lat_lon_noisemaker=error_fn, - speed_noisemaker=error_fn, - heading_noisemaker=error_fn, ) NUM_READINGS = 10000 @@ -117,9 +55,9 @@ def test_gps_gaussian_error(self): [speed, heading, lat_lon], ): sample_mean = np.mean(reading, axis=0) - assert np.isclose(sample_mean, mean + init_data, atol=0.1).all() + assert np.allclose(sample_mean, mean + init_data, atol=0.1) - def test_wind_sensor_update(self): + def test_gps_sensor_update(self): lat_lon = np.array([0, 0]) speed = 0 heading = 0 @@ -143,3 +81,17 @@ def test_wind_sensor_update(self): lat_lon_reading = gps.read("lat_lon") assert (lat_lon_reading == np.array([i, i])).all() gps.update(lat_lon=(lat_lon_reading + 1)) + + def test_gps_sensor_update_delay(self): + lat_lon = np.array([0, 0]) + speed0 = 0 + heading = 0 + + # Initialized data is read without delay + gps = GPS(lat_lon=lat_lon, speed=speed0, heading=heading, enable_delay=True) + assert gps.read("speed") == speed0 + + NUM_UPDATES = 3 + for i in range(NUM_UPDATES): + gps.update(speed=(i + 1)) + assert gps.read("speed") == i diff --git a/src/boat_simulator/tests/unit/common/test_wind_sensor.py b/src/boat_simulator/tests/unit/common/test_wind_sensor.py index aed0d1c88..f31512686 100644 --- a/src/boat_simulator/tests/unit/common/test_wind_sensor.py +++ b/src/boat_simulator/tests/unit/common/test_wind_sensor.py @@ -1,59 +1,29 @@ from boat_simulator.common.sensors import WindSensor import numpy as np -from boat_simulator.common.generators import ( - MVGaussianGenerator, - ConstantGenerator, -) class TestWindSensor: def test_wind_sensor_init(self): init_data = np.array([1, 0]) - error_fn = None ws = WindSensor( wind=init_data, - wind_noisemaker=error_fn, ) - assert ws.wind_noisemaker == error_fn assert np.all(ws.wind == init_data) - def test_wind_sensor_init_implicit_error_fn(self): + def test_wind_sensor_read_no_noise(self): init_data = np.array([1, 0]) - ws = WindSensor(wind=init_data) - - assert ws.wind_noisemaker is None - assert np.all(ws.wind == init_data) - - def test_wind_sensor_read_no_error(self): - init_data = np.array([1, 0]) - ws = WindSensor( - wind=init_data, - ) - read_data = ws.read("wind") - assert (init_data == read_data).all() - - def test_wind_sensor_read_constant_error(self): - init_data = np.array([1, 0]) - const_err = 0.1 - error_fn = ConstantGenerator(constant=0.1) ws = WindSensor( wind=init_data, - wind_noisemaker=error_fn, ) - read_data = ws.read("wind") - assert ((init_data + const_err) == read_data).all() + assert np.all(init_data == read_data) - def test_wind_sensor_read_mv_gaussian_error(self): - init_data = np.array([1, 0]) - mean = np.array([1, 1]) + def test_wind_sensor_read_mv_gaussian_noise(self): + init_data = np.array([0, 0]) + mean = np.array([0, 0]) cov = np.eye(2) - error_fn = MVGaussianGenerator(mean=mean, cov=cov) - ws = WindSensor( - wind=init_data, - wind_noisemaker=error_fn, - ) + ws = WindSensor(wind=init_data, enable_noise=True) NUM_READINGS = 10000 reading = np.zeros(shape=(NUM_READINGS, mean.size)) @@ -63,15 +33,35 @@ def test_wind_sensor_read_mv_gaussian_error(self): sample_mean = np.mean(reading, axis=0) sample_cov = np.cov(reading, rowvar=False) + assert np.allclose(sample_mean, mean, atol=0.2) assert np.allclose(sample_cov, cov, atol=0.2) - assert np.isclose(sample_mean, mean + init_data, 0.1).all() - def test_wind_sensor_update(self): - init_data = np.zeros(2) + def test_wind_sensor_update_no_delay(self): + init_data = np.array([0, 0]) ws = WindSensor(wind=init_data) NUM_READINGS = 100 for i in range(NUM_READINGS): wind = ws.read("wind") - assert (wind == np.array([i, i])).all() + assert np.all(wind == np.array([i, i])) ws.update(wind=(wind + 1)) + + def test_wind_sensor_update_with_delay(self): + """ + Attempt to constantly update wind sensor with new data. + Delay causes new data to be read in the next update cycle. + """ + + init_data = np.array([0, 0]) + + ws = WindSensor(wind=init_data, enable_delay=True) + + wind = ws.read("wind") + # Initialized data is read without delay + assert np.all(wind == init_data) + + NUM_UPDATES = 3 + for i in range(NUM_UPDATES): + ws.update(wind=np.array([i + 1, i + 1])) + wind = ws.read("wind") + assert np.all(wind == [i, i]) diff --git a/src/boat_simulator/tests/unit/nodes/physics_engine/test_fluid_forces.py b/src/boat_simulator/tests/unit/nodes/physics_engine/test_fluid_forces.py new file mode 100644 index 000000000..349df2dc6 --- /dev/null +++ b/src/boat_simulator/tests/unit/nodes/physics_engine/test_fluid_forces.py @@ -0,0 +1,102 @@ +import math + +import numpy as np +import pytest + +from boat_simulator.nodes.physics_engine.fluid_forces import MediumForceComputation + + +@pytest.fixture +def medium_force_setup(): + lift_coefficients = np.array([[0, 0], [5, 0.57], [10, 1.10], [15, 1.39], [20, 1.08]]) + drag_coefficients = np.array([[0, 0.013], [5, 0.047], [10, 0.144], [15, 0.279], [20, 0.298]]) + areas = 9.0 + fluid_density = 1.225 + + computation = MediumForceComputation( + lift_coefficients, drag_coefficients, areas, fluid_density + ) + return computation + + +def test_initialization(medium_force_setup): + assert isinstance(medium_force_setup.lift_coefficients, np.ndarray) + assert isinstance(medium_force_setup.drag_coefficients, np.ndarray) + assert isinstance(medium_force_setup.areas, (int, float)) + assert isinstance(medium_force_setup.fluid_density, (int, float)) + + +@pytest.mark.parametrize( + "apparent_velocity, orientation, expected_angle", + [ + # Test zero apparent velocity with various orientations, + # including edge cases and normalization + (np.array([0, 0]), 0, 0), + (np.array([0, 0]), 45, 45), + (np.array([0, 0]), 90, 90), + (np.array([0, 0]), 180, -180), # Normalized to -180 + (np.array([0, 0]), 270, -90), # Normalized to -90 + (np.array([0, 0]), 360, 0), + (np.array([0, 0]), -45, -45), # Test negative orientation + (np.array([0, 0]), 405, 45), # Orientation beyond 360 + (np.array([0, 0]), -405, -45), # Orientation below -360 + # Test non-zero apparent velocity for comprehensive angle of attack calculations + (np.array([1, 0]), 0, 0), + (np.array([0, 1]), 0, 90), + (np.array([-1, 0]), 0, -180), + (np.array([0, -1]), 0, -90), + (np.array([1, 1]), 45, 0), + (np.array([-1, -1]), 135, 90), + # Edge cases where orientation and velocity directions are opposite or identical + (np.array([1, 0]), 180, -180), + (np.array([-1, 0]), 180, 0), + (np.array([0, 1]), 270, -180), + (np.array([0, -1]), 90, -180), + # Additional tests with non-unit vectors + (np.array([2, 0]), 0, 0), # Horizontal vector, twice the unit length + (np.array([0, 2]), 0, 90), # Vertical vector, twice the unit length + (np.array([-2, 0]), 0, -180), # Left horizontal, twice the unit length + (np.array([3, 4]), 0, np.rad2deg(np.arctan2(4, 3))), # 3-4-5 triangle vector + (np.array([5, 5]), 45, 0), # Diagonal upward, aligned with orientation + ], +) +def test_calculate_attack_angle( + apparent_velocity, orientation, expected_angle, medium_force_setup +): + attack_angle = medium_force_setup.calculate_attack_angle(apparent_velocity, orientation) + assert np.isclose( + attack_angle, expected_angle, atol=1e-7 + ), f"Expected {expected_angle}, got {attack_angle}" + + +# Test taken from https://www1.grc.nasa.gov/beginners-guide-to-aeronautics/foilsimstudent/ +@pytest.mark.parametrize( + "orientation, expected_lift, expected_drag, apparent_velocity", + [ + # Tests for attack angle 0 + (0, 0, 140, np.array([44 * math.cos(0), 44 * math.sin(0)])), + # # # Tests for attack angle 5 + (0, 6262, 509, np.array([44 * math.cos(np.deg2rad(5)), 44 * math.sin(np.deg2rad(5))])), + # # Tests for attack angle 10 + (0, 11934, 1568, np.array([44 * math.cos(np.deg2rad(10)), 44 * math.sin(np.deg2rad(10))])), + # # Tests for attack angle 15 + (0, 15162, 3035, np.array([44 * math.cos(np.deg2rad(15)), 44 * math.sin(np.deg2rad(15))])), + # Tests for attack angle 20 + ( + 0, + 11768, + 3249, + np.array([44 * math.cos(np.deg2rad(20)), 44 * math.sin(np.deg2rad(20))]), + ), + ], +) +def test_compute_forces( + medium_force_setup, orientation, expected_lift, expected_drag, apparent_velocity +): + lift_force, drag_force = medium_force_setup.compute(apparent_velocity, orientation) + assert np.isclose( + np.linalg.norm(lift_force), expected_lift, rtol=0.05 + ), f"Expected {expected_lift}, got {np.linalg.norm(lift_force)}" + assert np.isclose( + np.linalg.norm(drag_force), expected_drag, rtol=0.05 + ), f"Expected {expected_drag}, got {np.linalg.norm(drag_force)}" diff --git a/src/controller/README.md b/src/controller/README.md index 05c831a2c..899411661 100644 --- a/src/controller/README.md +++ b/src/controller/README.md @@ -1,7 +1,5 @@ # Controller -[![Tests](https://github.com/UBCSailbot/controller/actions/workflows/tests.yml/badge.svg)](https://github.com/UBCSailbot/controller/actions/workflows/tests.yml) - UBC Sailbot's controller for the new project. This repository contains a ROS package `controller`. This README contains only setup and run instructions. Further information on the controller can be found on the software team's [docs website](https://ubcsailbot.github.io/sailbot_workspace/main/current/controller/overview/). diff --git a/src/controller/tests/unit/wingsail/common/test_lut.py b/src/controller/tests/unit/wingsail/common/test_lut.py index dc7c1d907..f0c3436e4 100644 --- a/src/controller/tests/unit/wingsail/common/test_lut.py +++ b/src/controller/tests/unit/wingsail/common/test_lut.py @@ -30,7 +30,7 @@ def test_unknown_interpolation_exception(self): [ [[10000, 10000, 10000], [1, 1, 1]], [10000, 10000, 10000], - [[0, 1], 10000, 10000], + np.array([[0, 1], 10000, 10000], dtype=object), np.array([[10000, 10000, 10000], [1, 1, 1]]), np.array([10000, 10000, 10000]), np.array([[[0, 1]], [[0, 1]], [[0, 1]]]), diff --git a/src/custom_interfaces/README.md b/src/custom_interfaces/README.md index b918e7bb0..a467ebe41 100644 --- a/src/custom_interfaces/README.md +++ b/src/custom_interfaces/README.md @@ -19,7 +19,7 @@ ROS messages and services used across many ROS packages in the project. Update diagram by editing diagrams/src/external_interfaces.puml and the PlantUML Export Diagram command in VSCode ---> -![External Interface Diagram](https://github.com/UBCSailbot/custom_interfaces/blob/main/diagrams/out/external_interfaces.png?raw=true) +![External Interface Diagram](https://github.com/UBCSailbot/sailbot_workspace/blob/main/src/custom_interfaces/diagrams/out/external_interfaces.png?raw=true) ### Project-wide Internal Interfaces @@ -36,7 +36,7 @@ Update diagram by editing diagrams/src/external_interfaces.puml and the PlantUML ## Boat Simulator Interfaces -ROS messages and services used in our [boat simulator](https://github.com/UBCSailbot/boat_simulator). +ROS messages and services used in our [boat simulator](https://github.com/UBCSailbot/sailbot_workspace/tree/main/src/boat_simulator). ### Boat Simulator External Interfaces diff --git a/src/custom_interfaces/diagrams/out/external_interfaces.png b/src/custom_interfaces/diagrams/out/external_interfaces.png index 5634cb399..b20a87a3a 100644 Binary files a/src/custom_interfaces/diagrams/out/external_interfaces.png and b/src/custom_interfaces/diagrams/out/external_interfaces.png differ diff --git a/src/global_launch/config/README.md b/src/global_launch/config/README.md index b2daac721..60065e737 100644 --- a/src/global_launch/config/README.md +++ b/src/global_launch/config/README.md @@ -73,6 +73,24 @@ ROS parameters specific to the nodes in the local_pathfinding package. - _Acceptable Values_: `"bitstar"`, `"bfmtstar"`, `"fmtstar"`, `"informedrrtstar"`, `"lazylbtrrt"`, `"lazyprmstar"`, `"lbtrrt"`, `"prmstar"`, `"rrtconnect"`, `"rrtsharp"`, `"rrtstar"`, `"rrtxstatic"`, `"sorrtstar"` +## Controller Parameters + +ROS parameters specific to the nodes in the Controller. + +### wingsail_ctrl_node + +**`reynolds_number`** + +- _Description_: The Reynolds number of the wind. +- _Datatype_: `double` +- _Range_: `(0.0, MAX_DOUBLE)` + +**`angle_of_attack`** + +- _Description_: The angle of attack of the sail. +- _Datatype_: `double` +- _Range_: `(-180.0, 180.0]` + ## Boat Simulator Parameters ROS parameters specific to the nodes in the boat simulator. diff --git a/src/global_launch/config/globals.yaml b/src/global_launch/config/globals.yaml index b385741dd..7cec8befa 100644 --- a/src/global_launch/config/globals.yaml +++ b/src/global_launch/config/globals.yaml @@ -16,6 +16,12 @@ navigate_main: ros__parameters: path_planner: "rrtstar" +# controller parameters +wingsail_ctrl_node: + ros__parameters: + reynolds_number: [0.0, 1.0, 2.0] + angle_of_attack: [0.0, 1.0, 2.0] + # boat_simulator parameters low_level_control_node: ros__parameters: diff --git a/src/integration_tests/custom-rosdep.yaml b/src/integration_tests/custom-rosdep.yaml new file mode 100644 index 000000000..0be358f82 --- /dev/null +++ b/src/integration_tests/custom-rosdep.yaml @@ -0,0 +1,4 @@ +python3-pyyaml-types-pip: + ubuntu: + pip: + packages: [types-PyYAML] diff --git a/src/integration_tests/package.xml b/src/integration_tests/package.xml index f15345d2f..fa7ddc0f2 100644 --- a/src/integration_tests/package.xml +++ b/src/integration_tests/package.xml @@ -7,9 +7,13 @@ Henry Huang MIT + rclpy custom_interfaces + + python3-pyyaml-types-pip + ament_python diff --git a/src/local_pathfinding/README.md b/src/local_pathfinding/README.md index b92bfcdc3..333b45d71 100644 --- a/src/local_pathfinding/README.md +++ b/src/local_pathfinding/README.md @@ -1,7 +1,5 @@ # Local Pathfinding -[![Tests](https://github.com/UBCSailbot/local_pathfinding/actions/workflows/tests.yml/badge.svg)](https://github.com/UBCSailbot/local_pathfinding/actions/workflows/tests.yml) - UBC Sailbot's local pathfinding ROS package ## Run diff --git a/src/local_pathfinding/test/test_local_path.py b/src/local_pathfinding/test/test_local_path.py index 001c45f3c..0d5d2c754 100644 --- a/src/local_pathfinding/test/test_local_path.py +++ b/src/local_pathfinding/test/test_local_path.py @@ -12,7 +12,7 @@ def test_LocalPath_update_if_needed(): ais_ships=AISShips(), global_path=Path(), filtered_wind_sensor=WindSensor(), - planner="bitstar", + planner="rrtstar", ) assert PATH.waypoints is not None, "waypoints is not initialized" assert len(PATH.waypoints) > 1, "waypoints length <= 1" diff --git a/src/local_pathfinding/test/test_objectives.py b/src/local_pathfinding/test/test_objectives.py index 9289b650f..da31d4e7c 100644 --- a/src/local_pathfinding/test/test_objectives.py +++ b/src/local_pathfinding/test/test_objectives.py @@ -22,7 +22,7 @@ ais_ships=AISShips(), global_path=Path(), filtered_wind_sensor=WindSensor(), - planner="bitstar", + planner="rrtstar", ), ) diff --git a/src/local_pathfinding/test/test_ompl_path.py b/src/local_pathfinding/test/test_ompl_path.py index e7662566d..73013c310 100644 --- a/src/local_pathfinding/test/test_ompl_path.py +++ b/src/local_pathfinding/test/test_ompl_path.py @@ -15,7 +15,7 @@ ais_ships=AISShips(), global_path=Path(), filtered_wind_sensor=WindSensor(), - planner="bitstar", + planner="rrtstar", ), ) diff --git a/src/network_systems/.gitignore b/src/network_systems/.gitignore index ec6f8109d..38919a42c 100644 --- a/src/network_systems/.gitignore +++ b/src/network_systems/.gitignore @@ -1,5 +1,6 @@ # autogenerated files /lib/cmn_hdrs/ros_info.h +/launch/ros_info.py *.pyc # PlantUML diagram export directory diff --git a/src/network_systems/README.md b/src/network_systems/README.md index eb0bb74e0..c0d44c3ac 100755 --- a/src/network_systems/README.md +++ b/src/network_systems/README.md @@ -1,7 +1,5 @@ # Network Systems -[![Tests](https://github.com/UBCSailbot/network_systems/actions/workflows/tests.yml/badge.svg)](https://github.com/UBCSailbot/network_systems/actions/workflows/tests.yml) - This repository contains the source code for all of UBC Sailbot's Network Systems programs. It is made to work as part of [Sailbot Workspace](https://github.com/UBCSailbot/sailbot_workspace), and is **_not_** meant to be built as an independent project. diff --git a/src/network_systems/config/local_transceiver/local_transceiver_template.yaml b/src/network_systems/config/local_transceiver/local_transceiver_template.yaml new file mode 100644 index 000000000..fd1382e84 --- /dev/null +++ b/src/network_systems/config/local_transceiver/local_transceiver_template.yaml @@ -0,0 +1,6 @@ +# Template for the local_transceiver module +local_transceiver_node: + ros__parameters: + enabled: true + # The following parameters are optional. Defaults are set in local_transceiver_ros_intf.cpp + port: # String: Serial port that the Local Transceiver will use (default: /tmp/local_transceiver_test_port) diff --git a/src/network_systems/launch/main_launch.py b/src/network_systems/launch/main_launch.py index 101a9ffe3..9241dc518 100644 --- a/src/network_systems/launch/main_launch.py +++ b/src/network_systems/launch/main_launch.py @@ -1,6 +1,7 @@ """Launch file that runs all nodes for the network systems ROS package.""" import os +import sys from importlib.util import module_from_spec, spec_from_file_location from typing import List, Tuple @@ -12,6 +13,16 @@ from launch.some_substitutions_type import SomeSubstitutionsType from launch.substitutions import LaunchConfiguration +# Deal with Python import paths +SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__)) +sys.path.append(SCRIPT_DIR) +from ros_info import ( # noqa: E402 + CACHED_FIB_NODE, + CAN_TRANSCEIVER_NODE, + MOCK_AIS_NODE, + REMOTE_TRANSCEIVER_NODE, +) + # Local launch arguments and constants PACKAGE_NAME = "network_systems" NAMESPACE = "" @@ -75,6 +86,7 @@ def setup_launch(context: LaunchContext) -> List[Node]: launch_description_entities.append(get_mock_ais_description(context)) launch_description_entities.append(get_can_transceiver_description(context)) launch_description_entities.append(get_remote_transceiver_description(context)) + launch_description_entities.append(get_local_transceiver_description(context)) return launch_description_entities @@ -87,7 +99,7 @@ def get_cached_fib_description(context: LaunchContext) -> Node: Returns: Node: The node object that launches the cached_fib_node. """ - node_name = "cached_fib_node" + node_name = CACHED_FIB_NODE ros_parameters = [ global_launch_config, {"mode": LaunchConfiguration("mode")}, @@ -119,7 +131,7 @@ def get_mock_ais_description(context: LaunchContext) -> Node: Returns: Node: The node object that launches the mock_ais_node. """ - node_name = "mock_ais_node" + node_name = MOCK_AIS_NODE ros_parameters = [ global_launch_config, {"mode": LaunchConfiguration("mode")}, @@ -151,7 +163,7 @@ def get_can_transceiver_description(context: LaunchContext) -> Node: Returns: Node: The node object that launches the can_transceiver_node. """ - node_name = "can_transceiver_node" + node_name = CAN_TRANSCEIVER_NODE ros_parameters = [ global_launch_config, {"mode": LaunchConfiguration("mode")}, @@ -183,7 +195,7 @@ def get_remote_transceiver_description(context: LaunchContext) -> Node: Returns: Node: The node object that launches the remote_transceiver_node. """ - node_name = "remote_transceiver_node" + node_name = REMOTE_TRANSCEIVER_NODE ros_parameters = [ global_launch_config, {"mode": LaunchConfiguration("mode")}, @@ -204,3 +216,34 @@ def get_remote_transceiver_description(context: LaunchContext) -> Node: ) return node + + +def get_local_transceiver_description(context: LaunchContext) -> Node: + """Gets the launch description for the local_transceiver_node. + + Args: + context (LaunchContext): The current launch context. + + Returns: + Node: The node object that launches the local_transceiver_node. + """ + node_name = "local_transceiver_node" + ros_parameters = [ + global_launch_config, + {"mode": LaunchConfiguration("mode")}, + *LaunchConfiguration("config").perform(context).split(","), + ] + ros_arguments: List[SomeSubstitutionsType] = [ + "--log-level", + [f"{node_name}:=", LaunchConfiguration("log_level")], + ] + node = Node( + package=PACKAGE_NAME, + namespace=NAMESPACE, + executable="local_transceiver", + name=node_name, + parameters=ros_parameters, + ros_arguments=ros_arguments, + ) + + return node diff --git a/src/network_systems/lib/cmn_hdrs/CMakeLists.txt b/src/network_systems/lib/cmn_hdrs/CMakeLists.txt index bb66d729c..ade5a688c 100644 --- a/src/network_systems/lib/cmn_hdrs/CMakeLists.txt +++ b/src/network_systems/lib/cmn_hdrs/CMakeLists.txt @@ -1,8 +1,7 @@ # Generate ROS info header file -set(ROS_INFO_FILE ${CMAKE_SOURCE_DIR}/ros_info.txt) add_custom_command( OUTPUT ${CMAKE_SOURCE_DIR}/lib/cmn_hdrs/ros_info.h - COMMAND ${CMAKE_SOURCE_DIR}/scripts/autogen_ros_topics.sh ${ROS_INFO_FILE} + COMMAND python3 ${CMAKE_SOURCE_DIR}/scripts/gen_ros_info.py DEPENDS ${CMAKE_SOURCE_DIR} ${ROS_INFO_FILE} ) add_custom_target(ros_info_h DEPENDS ${CMAKE_CURRENT_LIST_DIR}/ros_info.h) diff --git a/src/network_systems/package.xml b/src/network_systems/package.xml index c0c2c28fd..f57ddba19 100755 --- a/src/network_systems/package.xml +++ b/src/network_systems/package.xml @@ -8,11 +8,19 @@ Henry Huang Apache License 2.0 + ament_cmake - rclcpp - std_msgs custom_interfaces + rclcpp ros2launch + std_msgs + + + boost + + protobuf-dev + + gtest ament_cmake diff --git a/src/network_systems/projects/can_transceiver/inc/can_frame_parser.h b/src/network_systems/projects/can_transceiver/inc/can_frame_parser.h index 9b6c935e5..6cf9992a6 100644 --- a/src/network_systems/projects/can_transceiver/inc/can_frame_parser.h +++ b/src/network_systems/projects/can_transceiver/inc/can_frame_parser.h @@ -5,6 +5,9 @@ #include #include +#include +#include +#include #include #include #include @@ -27,19 +30,9 @@ enum class CanId : canid_t { BMS_P_DATA_FRAME_1 = 0x31, BMS_P_DATA_FRAME_2 = 0x32, SAIL_WSM_CMD_FRAME_1 = 0x60, - SAIL_WSM_CMD_FRAME_2 = 0x61, - SAIL_ENCD_DATA_FRAME = 0x62, SAIL_WSM_DATA_FRAME_1 = 0x63, - SAIL_WSM_DATA_FRAME_2 = 0x64, SAIL_WIND_DATA_FRAME_1 = 0x65, - SAIL_NAV_CMD_FRAME = 0x66, - RUDR_CMD_FRAME = 0x70, - RUDR_DATA_FRAME_1 = 0x71, - RUDR_DATA_FRAME_2 = 0x72, PATH_GPS_DATA_FRAME_1 = 0x80, - PATH_GPS_DATA_FRAME_2 = 0x81, - PATH_GPS_DATA_FRAME_3 = 0x82, - PATH_GPS_DATA_FRAME_4 = 0x83, PATH_WIND_DATA_FRAME = 0x84, }; @@ -52,19 +45,9 @@ static const std::map CAN_DESC{ {CanId::BMS_P_DATA_FRAME_1, "BMS_P_DATA_FRAME_1 (Battery 1 data)"}, {CanId::BMS_P_DATA_FRAME_2, "BMS_P_DATA_FRAME_2 (Battery 2 data)"}, {CanId::SAIL_WSM_CMD_FRAME_1, "SAIL_WSM_CMD_FRAME_1 (Main sail command)"}, - {CanId::SAIL_WSM_CMD_FRAME_2, "SAIL_WSM_CMD_FRAME_2 (Jib Sail command)"}, - {CanId::SAIL_ENCD_DATA_FRAME, "SAIL_ENCD_DATA_FRAME (Sail encoder data)"}, {CanId::SAIL_WSM_DATA_FRAME_1, "SAIL_WSM_DATA_FRAME_1 (Main sail data)"}, - {CanId::SAIL_WSM_DATA_FRAME_2, "SAIL_WSM_DATA_FRAME_2 (Jib sail data)"}, {CanId::SAIL_WIND_DATA_FRAME_1, "SAIL_WIND_DATA_FRAME_1 (Mast wind sensor)"}, - {CanId::SAIL_NAV_CMD_FRAME, "SAIL_NAV_CMD_FRAME (Nav light commands)"}, - {CanId::RUDR_CMD_FRAME, "RUDR_CMD_FRAME (Rudder commands [BOTH RUDDERS])"}, - {CanId::RUDR_DATA_FRAME_1, "RUDR_DATA_FRAME_1 (Port rudder data)"}, - {CanId::RUDR_DATA_FRAME_2, "RUDR_DATA_FRAME_2 (Starboard rudder data)"}, {CanId::PATH_GPS_DATA_FRAME_1, "PATH_GPS_DATA_FRAME_1 (GPS latitude)"}, - {CanId::PATH_GPS_DATA_FRAME_2, "PATH_GPS_DATA_FRAME_2 (GPS longitude)"}, - {CanId::PATH_GPS_DATA_FRAME_3, "PATH_GPS_DATA_FRAME_3 (GPS other data)"}, - {CanId::PATH_GPS_DATA_FRAME_4, "PATH_GPS_DATA_FRAME_4 (GPS time reporting [ex. day of the year])"}, {CanId::PATH_WIND_DATA_FRAME, "PATH_WIND_DATA_FRAME (Hull wind sensor)"}}; /** @@ -224,4 +207,231 @@ class Battery final : public BaseFrame float volt_min_; // Minimum voltage of cells in the battery pack (unused) }; +/** + * @brief A sail class derived from the BaseFrame. Represents a sail command. + * + */ +class SailCmd final : public BaseFrame +{ +public: + static constexpr std::array SAIL_CMD_IDS = {CanId::SAIL_WSM_CMD_FRAME_1, CanId::SAIL_WSM_DATA_FRAME_1}; + static constexpr uint8_t CAN_BYTE_DLEN_ = 2; + static constexpr uint8_t BYTE_OFF_ANGLE = 0; + + /** + * @brief Explicitly deleted no-argument constructor + * + */ + SailCmd() = delete; + + /** + * @brief Construct a SailCmd object from a Linux CanFrame representation + * + * @param cf Linux CanFrame + */ + explicit SailCmd(const CanFrame & cf); + + /** + * @brief Construct a SailCmd object from a custom_interfaces ROS msg representation + * + * @param ros_sail_cmd custom_interfaces representation of a SailCmd + * @param id CanId of the SailCmd (use the rosIdxToCanId() method if unknown) + */ + explicit SailCmd(msg::SailCmd ros_sail_cmd, CanId id); + + /** + * @return the custom_interfaces ROS representation of the SailCmd object + */ + msg::SailCmd toRosMsg() const; + + /** + * @return the Linux CanFrame representation of the SailCmd object + */ + CanFrame toLinuxCan() const override; + + /** + * @return A string that can be printed or logged to debug a SailCmd object + */ + std::string debugStr() const override; + +private: + /** + * @brief Private helper constructor for SailCmd objects + * + * @param id CanId of the SailCmd + */ + explicit SailCmd(CanId id); + + /** + * @brief Check if the assigned fields after constructing a SailCmd object are within bounds. + * @throws std::out_of_range if any assigned fields are outside of expected bounds + */ + void checkBounds() const; + + float angle_; // Angle specified by the command +}; + +/** + * @brief //TODO: Add description + * + */ +class WindSensor final : public BaseFrame +{ +public: + static constexpr std::array WIND_SENSOR_IDS = { + CanId::SAIL_WIND_DATA_FRAME_1, CanId::PATH_WIND_DATA_FRAME}; + static constexpr uint8_t CAN_BYTE_DLEN_ = 4; + static constexpr uint8_t BYTE_OFF_ANGLE = 0; + static constexpr uint8_t BYTE_OFF_SPEED = 2; + + /** + * @brief Explicitly deleted no-argument constructor + * + */ + WindSensor() = delete; + + /** + * @brief Construct a Wind object from a Linux CanFrame representation + * + * @param cf Linux CanFrame + */ + explicit WindSensor(const CanFrame & cf); + + /** + * @brief Construct a WindSensor object from a custom_interfaces ROS msg representation + * + * @param ros_wind_sensor custom_interfaces representation of a WindSensor + * @param id CanId of the GPS (use the rosIdxToCanId() method if unknown) + */ + explicit WindSensor(msg::WindSensor ros_wind_sensor, CanId id); + + /** + * @return the custom_interfaces ROS representation of the WindSensor + */ + msg::WindSensor toRosMsg() const; + + /** + * @return the Linux CanFrame representation of the GPS object + */ + CanFrame toLinuxCan() const override; + + /** + * @return A string that can be printed or logged to debug a GPS object + */ + std::string debugStr() const override; + + /** + * @brief Factory method to convert the index of a wind sensor in the custom_interfaces ROS representation + * into a CanId if valid. + * + * @param bat_idx idx of the wind sensor in a custom_interfaces::msg::WindSensors array + * @return CanId if valid, std::nullopt if invalid + */ + static std::optional rosIdxToCanId(size_t wind_idx); + +private: + /** + * @brief Private helper constructor for GPS objects + * + * @param id CanId of the WindSensor Object + */ + explicit WindSensor(CanId id); + + /** + * @brief Check if the assigned fields after constructing a GPS object are within bounds. + * @throws std::out_of_range if any assigned fields are outside of expected bounds + */ + void checkBounds() const; + + int16_t wind_angle_; + float wind_speed_; +}; + +/** + * @brief GPS class derived from the BaseFrame. Represents GPS data. + * + */ +class GPS final : public BaseFrame +{ +public: + static constexpr std::array GPS_IDS = {CanId::PATH_GPS_DATA_FRAME_1}; + static constexpr uint8_t CAN_BYTE_DLEN_ = 24; + static constexpr uint32_t BYTE_OFF_LAT = 0; + static constexpr uint32_t BYTE_OFF_LON = 4; + static constexpr uint32_t BYTE_OFF_SEC = 8; + static constexpr uint32_t BYTE_OFF_MIN = 12; + static constexpr uint32_t BYTE_OFF_HOUR = 13; + static constexpr uint32_t BYTE_OFF_RESV = 14; + static constexpr uint32_t BYTE_OFF_HEADING = 16; + static constexpr uint32_t BYTE_OFF_SPEED = 20; + + /** + * @brief Explicitly deleted no-argument constructor + * + */ + GPS() = delete; + + /** + * @brief Construct a GPS object from a Linux CanFrame representation + * + * @param cf Linux CanFrame + */ + explicit GPS(const CanFrame & cf); + + /** + * @brief Construct a GPS object from a custom_interfaces ROS msg representation + * + * @param ros_gps custom_interfaces representation of a GPS + * @param id CanId of the GPS (use the rosIdxToCanId() method if unknown) + */ + explicit GPS(msg::GPS ros_gps, CanId id); + + /** + * @return the custom_interfaces ROS representation of the GPS object + */ + msg::GPS toRosMsg() const; + + /** + * @return the Linux CanFrame representation of the GPS object + */ + CanFrame toLinuxCan() const override; + + /** + * @return A string that can be printed or logged to debug a GPS object + */ + std::string debugStr() const override; + + /** + * @brief Factory method to convert the index of a GPS in the custom_interfaces ROS representation + * into a CanId if valid. + * + * @param gps_idx idx of the GPS in a custom_interfaces::msg::GPS array + * @return CanId if valid, std::nullopt if invalid + */ + static std::optional rosIdxToCanId(size_t gps_idx); + +private: + /** + * @brief Private helper constructor for GPS objects + * + * @param id CanId of the GPS + */ + explicit GPS(CanId id); + + /** + * @brief Check if the assigned fields after constructing a GPS object are within bounds. + * @throws std::out_of_range if any assigned fields are outside of expected bounds + */ + void checkBounds() const; + + float lat_; + float lon_; + float sec_; + float min_; + float hour_; + //float reserved; // Unused + float heading_; + float speed_; +}; + } // namespace CAN_FP diff --git a/src/network_systems/projects/can_transceiver/inc/can_transceiver.h b/src/network_systems/projects/can_transceiver/inc/can_transceiver.h index 8053dff05..273e95fe2 100644 --- a/src/network_systems/projects/can_transceiver/inc/can_transceiver.h +++ b/src/network_systems/projects/can_transceiver/inc/can_transceiver.h @@ -23,18 +23,18 @@ class CanTransceiver CanTransceiver(); /** - * @brief Construct a new Can Transceiver and connect it to an existing and open file descriptor - * @note Can only be used if simulating the CAN bus + * @brief Destroy the Canbus Intf object * - * @param fd */ - explicit CanTransceiver(int fd); + ~CanTransceiver(); /** - * @brief Close the opened CAN port and kill the receive() thread + * @brief Construct a new Can Transceiver and connect it to an existing and open file descriptor + * @note Can only be used if simulating the CAN bus * + * @param fd */ - ~CanTransceiver(); + explicit CanTransceiver(int fd); /** * @brief Send a CAN frame to the CAN port @@ -82,7 +82,6 @@ class CanTransceiver * Can be shutdown by setting shutdown_flag_ to true */ void receive(); - /** * @brief Call on successfully reading a new CAN data frame from hardware/simulator * diff --git a/src/network_systems/projects/can_transceiver/src/can_frame_parser.cpp b/src/network_systems/projects/can_transceiver/src/can_frame_parser.cpp index d32afaf5f..a058f0a6c 100644 --- a/src/network_systems/projects/can_transceiver/src/can_frame_parser.cpp +++ b/src/network_systems/projects/can_transceiver/src/can_frame_parser.cpp @@ -106,6 +106,7 @@ msg::HelperBattery Battery::toRosMsg() const msg::HelperBattery msg; msg.set__voltage(volt_); msg.set__current(curr_); + return msg; } @@ -167,4 +168,276 @@ void Battery::checkBounds() const // Battery private END // Battery END +// SailCmd START +// SailCmd public START + +SailCmd::SailCmd(const CanFrame & cf) : SailCmd(static_cast(cf.can_id)) +{ + int16_t raw_angle; + + std::memcpy(&raw_angle, cf.data + BYTE_OFF_ANGLE, sizeof(int16_t)); + + angle_ = static_cast(raw_angle); + + checkBounds(); +} + +SailCmd::SailCmd(msg::SailCmd ros_sail_cmd, CanId id) +: BaseFrame(id, CAN_BYTE_DLEN_), angle_(ros_sail_cmd.trim_tab_angle_degrees) +{ + checkBounds(); +} + +msg::SailCmd SailCmd::toRosMsg() const +{ + msg::SailCmd msg; + msg.set__trim_tab_angle_degrees(angle_); + return msg; +} + +CanFrame SailCmd::toLinuxCan() const +{ + int16_t raw_angle = static_cast(angle_); + + CanFrame cf = BaseFrame::toLinuxCan(); + std::memcpy(cf.data + BYTE_OFF_ANGLE, &raw_angle, sizeof(int16_t)); + + return cf; +} + +std::string SailCmd::debugStr() const +{ + std::stringstream ss; + ss << BaseFrame::debugStr() << "\n" + << "Trim tab angle (degrees): " << angle_; + return ss.str(); +} + +// SailCmd public END +// SailCmd private START + +SailCmd::SailCmd(CanId id) : BaseFrame(std::span{SAIL_CMD_IDS}, id, CAN_BYTE_DLEN_) {} + +void SailCmd::checkBounds() const +{ //fix min max angle + auto err = utils::isOutOfBounds(angle_, HEADING_LBND, HEADING_UBND); + if (err) { + std::string err_msg = err.value(); + throw std::out_of_range("Sail angle is out of bounds!\n" + debugStr() + "\n" + err_msg); + } +} + +// SailCmd private END +// SailCmd END + +// WindSensor START +// WindSensor public START + +WindSensor::WindSensor(const CanFrame & cf) : WindSensor(static_cast(cf.can_id)) +{ + int16_t raw_wind_speed; + int16_t raw_wind_dir; + + std::memcpy(&raw_wind_speed, cf.data + BYTE_OFF_SPEED, sizeof(int16_t)); + std::memcpy(&raw_wind_dir, cf.data + BYTE_OFF_ANGLE, sizeof(int16_t)); + + // convert knots to kmph before setting value + wind_speed_ = static_cast(raw_wind_speed * 1.852 / 10.0); // NOLINT(readability-magic-numbers) + wind_angle_ = static_cast(raw_wind_dir); + + checkBounds(); +} + +WindSensor::WindSensor(msg::WindSensor ros_wind_sensor, CanId id) +: BaseFrame(id, CAN_BYTE_DLEN_), wind_angle_(ros_wind_sensor.direction), wind_speed_(ros_wind_sensor.speed.speed) +{ + checkBounds(); +} + +msg::WindSensor WindSensor::toRosMsg() const +{ + msg::WindSensor msg; + msg::HelperSpeed speed; + msg.set__direction(static_cast(wind_angle_)); + speed.set__speed(wind_speed_); + msg.set__speed(speed); + return msg; +} + +CanFrame WindSensor::toLinuxCan() const +{ + // convert kmph to knots before setting value + int16_t raw_wind_speed = static_cast(wind_speed_ * 10 / 1.852); // NOLINT(readability-magic-numbers) + int16_t raw_wind_dir = static_cast(wind_angle_); + + CanFrame cf = BaseFrame::toLinuxCan(); + std::memcpy(cf.data + BYTE_OFF_SPEED, &raw_wind_speed, sizeof(int16_t)); + std::memcpy(cf.data + BYTE_OFF_ANGLE, &raw_wind_dir, sizeof(int16_t)); + + return cf; +} + +std::string WindSensor::debugStr() const +{ + std::stringstream ss; + ss << BaseFrame::debugStr() << "\n" + << "Wind speed (m/s): " << wind_speed_ << "\n" + << "Wind angle (degrees): " << wind_angle_; + return ss.str(); +} + +std::optional WindSensor::rosIdxToCanId(size_t wind_idx) +{ + if (wind_idx < WIND_SENSOR_IDS.size()) { + return WIND_SENSOR_IDS[wind_idx]; + } + return std::nullopt; +} +// WindSensor public END +// WindSensor private START + +WindSensor::WindSensor(CanId id) : BaseFrame(std::span{WIND_SENSOR_IDS}, id, CAN_BYTE_DLEN_) {} + +void WindSensor::checkBounds() const +{ + auto err = utils::isOutOfBounds(wind_angle_, WIND_DIRECTION_LBND, WIND_DIRECTION_UBND); + if (err) { + std::string err_msg = err.value(); + throw std::out_of_range("Wind angle is out of bounds!\n" + debugStr() + "\n" + err_msg); + } + err = utils::isOutOfBounds(wind_speed_, SPEED_LBND, SPEED_UBND); + if (err) { + std::string err_msg = err.value(); + throw std::out_of_range("Wind speed is out of bounds!\n" + debugStr() + "\n" + err_msg); + } +} + +// WindSensor private END +// WindSensor END + +// GPS START +// GPS public START +GPS::GPS(const CanFrame & cf) : GPS(static_cast(cf.can_id)) +{ + int32_t raw_lat; + int32_t raw_lon; + int32_t raw_sec; + int8_t raw_min; + int8_t raw_hour; + int32_t raw_heading; + int32_t raw_speed; + + std::memcpy(&raw_lat, cf.data + BYTE_OFF_LAT, sizeof(int32_t)); + std::memcpy(&raw_lon, cf.data + BYTE_OFF_LON, sizeof(int32_t)); + std::memcpy(&raw_sec, cf.data + BYTE_OFF_SEC, sizeof(int32_t)); + std::memcpy(&raw_min, cf.data + BYTE_OFF_MIN, sizeof(int8_t)); + std::memcpy(&raw_hour, cf.data + BYTE_OFF_HOUR, sizeof(int8_t)); + std::memcpy(&raw_heading, cf.data + BYTE_OFF_HEADING, sizeof(int32_t)); + std::memcpy(&raw_speed, cf.data + BYTE_OFF_SPEED, sizeof(int32_t)); + + lat_ = static_cast(raw_lat / 1000.0 - 90); //NOLINT(readability-magic-numbers) + lon_ = static_cast(raw_lon / 1000.0 - 180.0); //NOLINT(readability-magic-numbers) + sec_ = static_cast(raw_sec / 1000.0); //NOLINT(readability-magic-numbers) + min_ = static_cast(raw_min); + hour_ = static_cast(raw_hour); + heading_ = static_cast(raw_heading / 1000.0); //NOLINT(readability-magic-numbers) + speed_ = static_cast(raw_speed / 1000.0); //NOLINT(readability-magic-numbers) + + checkBounds(); +} + +GPS::GPS(msg::GPS ros_gps, CanId id) +: BaseFrame(id, CAN_BYTE_DLEN_), + lat_(ros_gps.lat_lon.latitude), + lon_(ros_gps.lat_lon.longitude), + sec_(0), // unused set to 0 + min_(0), // unused set to 0 + hour_(0), // unused set to 0 + heading_(ros_gps.heading.heading), + speed_(ros_gps.speed.speed) +{ + checkBounds(); +} + +msg::GPS GPS::toRosMsg() const +{ + msg::GPS msg; + msg::HelperLatLon lat_lon; + msg::HelperHeading heading; + msg::HelperSpeed speed; + lat_lon.set__latitude(lat_); + lat_lon.set__longitude(lon_); + heading.set__heading(heading_); + speed.set__speed(speed_); + msg.set__lat_lon(lat_lon); + msg.set__heading(heading); + msg.set__speed(speed); + return msg; +} + +CanFrame GPS::toLinuxCan() const +{ + int32_t raw_lat = static_cast((lat_ + 90.0) * 1000.0); //NOLINT(readability-magic-numbers) + int32_t raw_lon = static_cast((lon_ + 180.0) * 1000.0); //NOLINT(readability-magic-numbers) + int32_t raw_sec = static_cast(sec_ * 1000); //NOLINT(readability-magic-numbers) + int8_t raw_min = static_cast(min_); + int8_t raw_hour = static_cast(hour_); + int32_t raw_heading = static_cast(heading_ * 1000); //NOLINT(readability-magic-numbers) + int32_t raw_speed = static_cast(speed_ * 1000); //NOLINT(readability-magic-numbers) + + CanFrame cf = BaseFrame::toLinuxCan(); + std::memcpy(cf.data + BYTE_OFF_LAT, &raw_lat, sizeof(int32_t)); + std::memcpy(cf.data + BYTE_OFF_LON, &raw_lon, sizeof(int32_t)); + std::memcpy(cf.data + BYTE_OFF_SEC, &raw_sec, sizeof(int32_t)); + std::memcpy(cf.data + BYTE_OFF_MIN, &raw_min, sizeof(int8_t)); + std::memcpy(cf.data + BYTE_OFF_HOUR, &raw_hour, sizeof(int8_t)); + std::memcpy(cf.data + BYTE_OFF_HEADING, &raw_heading, sizeof(int32_t)); + std::memcpy(cf.data + BYTE_OFF_SPEED, &raw_speed, sizeof(int32_t)); + + return cf; +} + +std::string GPS::debugStr() const +{ + std::stringstream ss; + ss << BaseFrame::debugStr() << "\n" + << "Latitude (decimal degrees): " << lat_ << "\n" + << "Longitude (decimal degrees): " << lon_ << "\n" + << "Seconds (sec): " << sec_ << "\n" + << "Minutes (min): " << min_ << "\n" + << "Hours (hr): " << hour_ << "\n" + << "True heading (degrees): " << heading_ << "\n" + << "Speed (km/hr): " << speed_ << "\n"; + return ss.str(); +} + +//GPS public END +//GPS private START + +GPS::GPS(CanId id) : BaseFrame(std::span{GPS_IDS}, id, CAN_BYTE_DLEN_) {} + +void GPS::checkBounds() const +{ + auto err = utils::isOutOfBounds(lat_, LAT_LBND, LAT_UBND); + if (err) { + std::string err_msg = err.value(); + throw std::out_of_range("Latitude angle is out of bounds!\n" + debugStr() + "\n" + err_msg); + } + err = utils::isOutOfBounds(lon_, LON_LBND, LON_UBND); + if (err) { + std::string err_msg = err.value(); + throw std::out_of_range("Longitude is out of bounds!\n" + debugStr() + "\n" + err_msg); + } + err = utils::isOutOfBounds(heading_, HEADING_LBND, HEADING_UBND); + if (err) { + std::string err_msg = err.value(); + throw std::out_of_range("Heading is out of bounds!\n" + debugStr() + "\n" + err_msg); + } + err = utils::isOutOfBounds(speed_, SPEED_LBND, SPEED_UBND); + if (err) { + std::string err_msg = err.value(); + throw std::out_of_range("Speed is out of bounds!\n" + debugStr() + "\n" + err_msg); + } +} + } // namespace CAN_FP diff --git a/src/network_systems/projects/can_transceiver/src/can_transceiver_ros_intf.cpp b/src/network_systems/projects/can_transceiver/src/can_transceiver_ros_intf.cpp index 62277556e..158d6d7c3 100644 --- a/src/network_systems/projects/can_transceiver/src/can_transceiver_ros_intf.cpp +++ b/src/network_systems/projects/can_transceiver/src/can_transceiver_ros_intf.cpp @@ -27,7 +27,7 @@ using CAN_FP::CanId; class CanTransceiverIntf : public NetNode { public: - CanTransceiverIntf() : NetNode("can_transceiver_node") + CanTransceiverIntf() : NetNode(ros_nodes::CAN_TRANSCEIVER) { this->declare_parameter("enabled", true); @@ -62,8 +62,8 @@ class CanTransceiverIntf : public NetNode throw std::runtime_error(msg); } - ais_pub_ = this->create_publisher(AIS_SHIPS_TOPIC, QUEUE_SIZE); - batteries_pub_ = this->create_publisher(BATTERIES_TOPIC, QUEUE_SIZE); + ais_pub_ = this->create_publisher(ros_topics::AIS_SHIPS, QUEUE_SIZE); + batteries_pub_ = this->create_publisher(ros_topics::BATTERIES, QUEUE_SIZE); can_trns_->registerCanCbs({ std::make_pair( @@ -77,10 +77,10 @@ class CanTransceiverIntf : public NetNode if (mode == SYSTEM_MODE::DEV) { // Initialize the CAN Sim Intf mock_ais_sub_ = this->create_subscription( - MOCK_AIS_SHIPS_TOPIC, QUEUE_SIZE, + ros_topics::MOCK_AIS_SHIPS, QUEUE_SIZE, [this](msg::AISShips mock_ais_ships) { subMockAISCb(mock_ais_ships); }); mock_gps_sub_ = this->create_subscription( - MOCK_GPS_TOPIC, QUEUE_SIZE, [this](msg::GPS mock_gps) { subMockGpsCb(mock_gps); }); + ros_topics::MOCK_GPS, QUEUE_SIZE, [this](msg::GPS mock_gps) { subMockGpsCb(mock_gps); }); // TODO(lross03): register a callback for CanSimToBoatSim diff --git a/src/network_systems/projects/can_transceiver/test/test_can_transceiver.cpp b/src/network_systems/projects/can_transceiver/test/test_can_transceiver.cpp index 1dee56f6c..ddc0241a9 100644 --- a/src/network_systems/projects/can_transceiver/test/test_can_transceiver.cpp +++ b/src/network_systems/projects/can_transceiver/test/test_can_transceiver.cpp @@ -125,7 +125,326 @@ TEST_F(TestCanFrameParser, TestBatteryInvalid) } /** - * @brief Test CanTransceiver using a mock CAN descriptor + * @brief Test ROS<->CAN SailCmd translations work as expected for valid input values + * + */ +TEST_F(TestCanFrameParser, SailCmdTestValid) +{ + constexpr std::uint8_t NUM_SAILS = CAN_FP::SailCmd::SAIL_CMD_IDS.size(); + constexpr std::array expected_angles{12, 128}; + + for (size_t i = 0; i < NUM_SAILS; i++) { + CAN_FP::CanId id = CAN_FP::SailCmd::SAIL_CMD_IDS[i]; + float expected_angle = expected_angles[i]; + msg::SailCmd msg; + msg.set__trim_tab_angle_degrees(expected_angle); + CAN_FP::SailCmd sail_from_ros = CAN_FP::SailCmd(msg, id); + CAN_FP::CanFrame cf = sail_from_ros.toLinuxCan(); + + EXPECT_EQ(cf.can_id, static_cast(id)); + EXPECT_EQ(cf.len, CAN_FP::SailCmd::CAN_BYTE_DLEN_); + + int16_t raw_angle; + std::memcpy(&raw_angle, cf.data + CAN_FP::SailCmd::BYTE_OFF_ANGLE, sizeof(int16_t)); + + EXPECT_EQ(raw_angle, expected_angle); + + CAN_FP::SailCmd sail_from_can = CAN_FP::SailCmd(cf); + + EXPECT_EQ(sail_from_can.id_, id); + + msg::SailCmd msg_from_bat = sail_from_can.toRosMsg(); + + EXPECT_DOUBLE_EQ(msg_from_bat.trim_tab_angle_degrees, expected_angle); + } +} + +/** + * @brief Test the behavior of the SailCmd class when given invalid Id values + * + */ +TEST_F(TestCanFrameParser, TestSailCmdInvalid) +{ + CAN_FP::CanId invalid_id = CAN_FP::CanId::RESERVED; + + CAN_FP::CanFrame cf{.can_id = static_cast(invalid_id)}; + + EXPECT_THROW(CAN_FP::SailCmd tmp(cf), CAN_FP::CanIdMismatchException); + + std::vector invalid_angles{HEADING_LBND - 1, HEADING_UBND + 1}; + + CAN_FP::CanId valid_id = CAN_FP::CanId::SAIL_WSM_CMD_FRAME_1; + msg::SailCmd msg; + + for (float invalid_angle : invalid_angles) { + msg.set__trim_tab_angle_degrees(invalid_angle); + + EXPECT_THROW(CAN_FP::SailCmd tmp(msg, valid_id), std::out_of_range); + }; + + cf.can_id = static_cast(CAN_FP::CanId::SAIL_WSM_CMD_FRAME_1); + std::copy(std::begin(GARBAGE_DATA), std::end(GARBAGE_DATA), cf.data); + + EXPECT_THROW(CAN_FP::SailCmd tmp(cf), std::out_of_range); +} + +/** + * @brief Test ROS<->CAN Battery translations work as expected for valid input values + * + */ +TEST_F(TestCanFrameParser, WindSensorTestValid) +{ + constexpr std::uint8_t NUM_SENSORS = CAN_FP::WindSensor::WIND_SENSOR_IDS.size(); + constexpr std::array expected_speeds{8.9, 3.0}; + constexpr std::array expected_angles{89, 167}; + + for (size_t i = 0; i < NUM_SENSORS; i++) { + auto optId = CAN_FP::WindSensor::rosIdxToCanId(i); + + ASSERT_TRUE(optId.has_value()); + + CAN_FP::CanId id = optId.value(); + float expected_speed = expected_speeds[i]; + int16_t expected_angle = expected_angles[i]; + msg::WindSensor msg; + msg::HelperSpeed speed_msg; + + speed_msg.set__speed(expected_speed); + msg.set__speed(speed_msg); + msg.set__direction(expected_angle); + + CAN_FP::WindSensor sensor_from_ros = CAN_FP::WindSensor(msg, id); + CAN_FP::CanFrame cf = sensor_from_ros.toLinuxCan(); + + EXPECT_EQ(cf.can_id, static_cast(id)); + EXPECT_EQ(cf.len, CAN_FP::WindSensor::CAN_BYTE_DLEN_); + + int16_t raw_speed; + int16_t raw_angle; + std::memcpy(&raw_speed, cf.data + CAN_FP::WindSensor::BYTE_OFF_SPEED, sizeof(int16_t)); + std::memcpy(&raw_angle, cf.data + CAN_FP::WindSensor::BYTE_OFF_ANGLE, sizeof(int16_t)); + float converted_speed = static_cast(raw_speed) * 1.852 / 10.0; //NOLINT + + EXPECT_NEAR(converted_speed, expected_speeds[i], 0.1852); + EXPECT_EQ(raw_angle, expected_angles[i]); + + CAN_FP::WindSensor sensor_from_can = CAN_FP::WindSensor(cf); + + EXPECT_EQ(sensor_from_can.id_, id); + + msg::WindSensor msg_from_sensor = sensor_from_can.toRosMsg(); + + EXPECT_NEAR(msg_from_sensor.speed.speed, expected_speed, 0.1852); + EXPECT_DOUBLE_EQ(msg_from_sensor.direction, expected_angle); + } +} +/** + * @brief Test the behavior of the WindSensor class when given invalid input values + * + */ +TEST_F(TestCanFrameParser, TestWindSensorInvalid) +{ + auto optId = CAN_FP::WindSensor::rosIdxToCanId(NUM_WIND_SENSORS); + EXPECT_FALSE(optId.has_value()); + + CAN_FP::CanId invalid_id = CAN_FP::CanId::RESERVED; + + CAN_FP::CanFrame cf{.can_id = static_cast(invalid_id)}; + + EXPECT_THROW(CAN_FP::WindSensor tmp(cf), CAN_FP::CanIdMismatchException); + + std::vector invalid_angles{WIND_DIRECTION_LBND - 1, WIND_DIRECTION_UBND + 1}; + std::vector invalid_speeds{SPEED_LBND - 1, SPEED_UBND + 1}; + + optId = CAN_FP::WindSensor::rosIdxToCanId(0); + ASSERT_TRUE(optId.has_value()); + + CAN_FP::CanId valid_id = optId.value(); + msg::WindSensor msg; + + // Set a valid speed for this portion + for (int16_t invalid_angle : invalid_angles) { + msg.set__direction(invalid_angle); + msg::HelperSpeed tmp_speed_msg; + tmp_speed_msg.set__speed(SPEED_LBND); + msg.set__speed(tmp_speed_msg); + + EXPECT_THROW(CAN_FP::WindSensor tmp(msg, valid_id), std::out_of_range); + }; + + // Set a valid direction for this portion + for (float invalid_speed : invalid_speeds) { + msg.set__direction(WIND_DIRECTION_UBND); + msg::HelperSpeed tmp_speed_msg; + tmp_speed_msg.set__speed(invalid_speed); + msg.set__speed(tmp_speed_msg); + + EXPECT_THROW(CAN_FP::WindSensor tmp(msg, valid_id), std::out_of_range); + }; +} + +/** + * @brief Test ROS<->CAN GPS translations work as expected for valid input values + * + */ +TEST_F(TestCanFrameParser, GPSTestValid) +{ + constexpr std::array expected_lats{48.6, -57.3}; + constexpr std::array expected_lons{-32.1, 112.9}; + constexpr std::array expected_speeds{3.5, 8.0}; + constexpr std::array expected_headings{100.4, 43.2}; + constexpr std::array expected_raw_lats{138600, 32700}; + constexpr std::array expected_raw_lons{147900, 292900}; + constexpr std::array expected_raw_speeds{3500, 8000}; + constexpr std::array expected_raw_headings{100400, 43200}; + + for (size_t i = 0; i < 2; i++) { + CAN_FP::CanId id = CAN_FP::CanId::PATH_GPS_DATA_FRAME_1; + float expected_lat = expected_lats[i]; + float expected_lon = expected_lons[i]; + float expected_speed = expected_speeds[i]; + float expected_heading = expected_headings[i]; + + msg::GPS msg; + msg::HelperLatLon msg_latlon; + msg::HelperSpeed msg_speed; + msg::HelperHeading msg_heading; + msg_latlon.set__latitude(static_cast(expected_lat)); + msg_latlon.set__longitude(static_cast(expected_lon)); + msg.set__lat_lon(msg_latlon); + + msg_speed.set__speed(expected_speed); + msg.set__speed(msg_speed); + + msg_heading.set__heading(expected_heading); + msg.set__heading(msg_heading); + + CAN_FP::GPS gps_from_ros = CAN_FP::GPS(msg, id); + CAN_FP::CanFrame cf = gps_from_ros.toLinuxCan(); + + EXPECT_EQ(cf.can_id, static_cast(id)); + EXPECT_EQ(cf.len, CAN_FP::GPS::CAN_BYTE_DLEN_); + + int32_t raw_lat; + int32_t raw_lon; + int32_t raw_speed; + int32_t raw_heading; + std::memcpy(&raw_lat, cf.data + CAN_FP::GPS::BYTE_OFF_LAT, sizeof(int32_t)); + std::memcpy(&raw_lon, cf.data + CAN_FP::GPS::BYTE_OFF_LON, sizeof(int32_t)); + std::memcpy(&raw_speed, cf.data + CAN_FP::GPS::BYTE_OFF_SPEED, sizeof(int32_t)); + std::memcpy(&raw_heading, cf.data + CAN_FP::GPS::BYTE_OFF_HEADING, sizeof(int32_t)); + + EXPECT_NEAR(raw_lat, expected_raw_lats[i], 1); + EXPECT_NEAR(raw_lon, expected_raw_lons[i], 1); + EXPECT_EQ(raw_speed, expected_raw_speeds[i]); + EXPECT_EQ(raw_heading, expected_raw_headings[i]); + + CAN_FP::GPS gps_from_can = CAN_FP::GPS(cf); + + EXPECT_EQ(gps_from_can.id_, id); + + msg::GPS msg_from_gps = gps_from_can.toRosMsg(); + + EXPECT_NEAR(msg_from_gps.lat_lon.latitude, expected_lat, 1); + EXPECT_NEAR(msg_from_gps.lat_lon.longitude, expected_lon, 1); + EXPECT_DOUBLE_EQ(msg_from_gps.speed.speed, expected_speed); + EXPECT_DOUBLE_EQ(msg_from_gps.heading.heading, expected_heading); + } +} + +/** + * @brief Test the behavior of the GPS class when given invalid input values + * + */ +TEST_F(TestCanFrameParser, TestGPSInvalid) +{ + CAN_FP::CanId invalid_id = CAN_FP::CanId::RESERVED; + + CAN_FP::CanFrame cf{.can_id = static_cast(invalid_id)}; + + EXPECT_THROW(CAN_FP::GPS tmp(cf), CAN_FP::CanIdMismatchException); + + std::vector invalid_lons{LON_LBND - 1, LON_UBND + 1}; + std::vector invalid_lats{LAT_LBND - 1, LAT_UBND + 1}; + std::vector invalid_speeds{SPEED_LBND - 1, SPEED_UBND + 1}; + std::vector invalid_headings{HEADING_LBND - 1, HEADING_UBND + 1}; + + CAN_FP::CanId valid_id = CAN_FP::CanId::PATH_GPS_DATA_FRAME_1; + msg::GPS msg; + + // Set a valid speed for this portion + for (float invalid_lon : invalid_lons) { + msg::HelperLatLon msg_latlon; + msg::HelperSpeed msg_speed; + msg::HelperHeading msg_heading; + msg_latlon.set__latitude(LAT_UBND); + msg_latlon.set__longitude(invalid_lon); + msg.set__lat_lon(msg_latlon); + + msg_speed.set__speed(SPEED_UBND); + msg.set__speed(msg_speed); + + msg_heading.set__heading(HEADING_UBND); + msg.set__heading(msg_heading); + + EXPECT_THROW(CAN_FP::GPS tmp(msg, valid_id), std::out_of_range); + }; + + // Set a valid direction for this portion + for (float invalid_lat : invalid_lats) { + msg::HelperLatLon msg_latlon; + msg::HelperSpeed msg_speed; + msg::HelperHeading msg_heading; + msg_latlon.set__latitude(invalid_lat); + msg_latlon.set__longitude(LON_UBND); + msg.set__lat_lon(msg_latlon); + + msg_speed.set__speed(SPEED_UBND); + msg.set__speed(msg_speed); + + msg_heading.set__heading(HEADING_UBND); + msg.set__heading(msg_heading); + + EXPECT_THROW(CAN_FP::GPS tmp(msg, valid_id), std::out_of_range); + }; + + for (float invalid_speed : invalid_speeds) { + msg::HelperLatLon msg_latlon; + msg::HelperSpeed msg_speed; + msg::HelperHeading msg_heading; + msg_latlon.set__latitude(LAT_UBND); + msg_latlon.set__longitude(LON_UBND); + msg.set__lat_lon(msg_latlon); + + msg_speed.set__speed(invalid_speed); + msg.set__speed(msg_speed); + + msg_heading.set__heading(HEADING_UBND); + msg.set__heading(msg_heading); + + EXPECT_THROW(CAN_FP::GPS tmp(msg, valid_id), std::out_of_range); + }; + + for (float invalid_heading : invalid_headings) { + msg::HelperLatLon msg_latlon; + msg::HelperSpeed msg_speed; + msg::HelperHeading msg_heading; + msg_latlon.set__latitude(LAT_UBND); + msg_latlon.set__longitude(LON_UBND); + msg.set__lat_lon(msg_latlon); + + msg_speed.set__speed(SPEED_UBND); + msg.set__speed(msg_speed); + + msg_heading.set__heading(invalid_heading); + msg.set__heading(msg_heading); + + EXPECT_THROW(CAN_FP::GPS tmp(msg, valid_id), std::out_of_range); + }; +} + +/** + * @brief Test CanTransceiver using a tmp file * */ class TestCanTransceiver : public ::testing::Test diff --git a/src/network_systems/projects/example/src/cached_fib_ros_intf.cpp b/src/network_systems/projects/example/src/cached_fib_ros_intf.cpp index d032a2d64..c949c033f 100644 --- a/src/network_systems/projects/example/src/cached_fib_ros_intf.cpp +++ b/src/network_systems/projects/example/src/cached_fib_ros_intf.cpp @@ -1,5 +1,6 @@ // Include this module #include "cached_fib.h" +#include "cmn_hdrs/ros_info.h" #include "net_node.h" // Include ROS headers #include @@ -14,7 +15,7 @@ constexpr int INIT_FIB_SIZE = 5; class CachedFibNode : public NetNode { public: - explicit CachedFibNode(const std::size_t initSize) : NetNode("cached_fib_node"), c_fib_(initSize) + explicit CachedFibNode(const std::size_t initSize) : NetNode(ros_nodes::CACHED_FIB), c_fib_(initSize) { this->declare_parameter("enabled", false); bool enabled = this->get_parameter("enabled").as_bool(); diff --git a/src/network_systems/projects/local_transceiver/src/local_transceiver_ros_intf.cpp b/src/network_systems/projects/local_transceiver/src/local_transceiver_ros_intf.cpp index 52db8f1f0..26844e0e0 100644 --- a/src/network_systems/projects/local_transceiver/src/local_transceiver_ros_intf.cpp +++ b/src/network_systems/projects/local_transceiver/src/local_transceiver_ros_intf.cpp @@ -13,7 +13,7 @@ #include "net_node.h" /** - * Local Transceiver Interface Node + * @brief Connect the Local Transceiver to the ROS network * */ class LocalTransceiverIntf : public NetNode @@ -24,38 +24,67 @@ class LocalTransceiverIntf : public NetNode * * @param lcl_trns Local Transceiver instance */ - explicit LocalTransceiverIntf(std::shared_ptr lcl_trns) - : NetNode("local_transceiver_node"), lcl_trns_(lcl_trns) + explicit LocalTransceiverIntf() : NetNode("local_transceiver_node") + { - static constexpr int ROS_Q_SIZE = 5; - static constexpr auto TIMER_INTERVAL = std::chrono::milliseconds(500); - timer_ = this->create_wall_timer(TIMER_INTERVAL, std::bind(&LocalTransceiverIntf::pub_cb, this)); - pub_ = this->create_publisher(GLOBAL_PATH_TOPIC, ROS_Q_SIZE); - - // subscriber nodes - sub_wind_sensor = this->create_subscription( - WIND_SENSORS_TOPIC, ROS_Q_SIZE, - std::bind(&LocalTransceiverIntf::sub_wind_sensor_cb, this, std::placeholders::_1)); - sub_batteries = this->create_subscription( - BATTERIES_TOPIC, ROS_Q_SIZE, std::bind(&LocalTransceiverIntf::sub_batteries_cb, this, std::placeholders::_1)); - sub_data_sensors = this->create_subscription( - DATA_SENSORS_TOPIC, ROS_Q_SIZE, - std::bind(&LocalTransceiverIntf::sub_data_sensors_cb, this, std::placeholders::_1)); - sub_ais_ships = this->create_subscription( - AIS_SHIPS_TOPIC, ROS_Q_SIZE, std::bind(&LocalTransceiverIntf::sub_ais_ships_cb, this, std::placeholders::_1)); - sub_gps = this->create_subscription( - GPS_TOPIC, ROS_Q_SIZE, std::bind(&LocalTransceiverIntf::sub_gps_cb, this, std::placeholders::_1)); - sub_local_path_data = this->create_subscription( - LOCAL_PATH_DATA_TOPIC, ROS_Q_SIZE, - std::bind(&LocalTransceiverIntf::sub_local_path_data_cb, this, std::placeholders::_1)); + this->declare_parameter("enabled", true); + bool enabled_ = this->get_parameter("enabled").as_bool(); + + if (!enabled_) { + RCLCPP_INFO(this->get_logger(), "Local Transceiver is DISABLED"); + } else { + this->declare_parameter("mode", SYSTEM_MODE::DEV); + + rclcpp::Parameter mode_param = this->get_parameter("mode"); + std::string mode = mode_param.as_string(); + std::string default_port; + + if (mode == SYSTEM_MODE::PROD) { + //TODO(Jng468) placeholder + } else if (mode == SYSTEM_MODE::DEV) { + default_port = LOCAL_TRANSCEIVER_TEST_PORT; + } else { + std::string msg = "Error, invalid system mode" + mode; + throw std::runtime_error(msg); + } + + this->declare_parameter("port", default_port); + rclcpp::Parameter default_port_parm = this->get_parameter("port"); + std::string port = default_port_parm.as_string(); + + RCLCPP_INFO( + this->get_logger(), "Running Local Transceiver in mode: %s, with port: %s.", mode.c_str(), port.c_str()); + lcl_trns_ = std::make_unique(port, SATELLITE_BAUD_RATE); + + static constexpr int ROS_Q_SIZE = 5; + static constexpr auto TIMER_INTERVAL = std::chrono::milliseconds(500); + timer_ = this->create_wall_timer(TIMER_INTERVAL, std::bind(&LocalTransceiverIntf::pub_cb, this)); + pub_ = this->create_publisher(ros_topics::GLOBAL_PATH, ROS_Q_SIZE); + + // subscriber nodes + sub_wind_sensor = this->create_subscription( + ros_topics::WIND_SENSORS, ROS_Q_SIZE, + std::bind(&LocalTransceiverIntf::sub_wind_sensor_cb, this, std::placeholders::_1)); + sub_batteries = this->create_subscription( + ros_topics::BATTERIES, ROS_Q_SIZE, + std::bind(&LocalTransceiverIntf::sub_batteries_cb, this, std::placeholders::_1)); + sub_data_sensors = this->create_subscription( + ros_topics::DATA_SENSORS, ROS_Q_SIZE, + std::bind(&LocalTransceiverIntf::sub_data_sensors_cb, this, std::placeholders::_1)); + sub_ais_ships = this->create_subscription( + ros_topics::AIS_SHIPS, ROS_Q_SIZE, + std::bind(&LocalTransceiverIntf::sub_ais_ships_cb, this, std::placeholders::_1)); + sub_gps = this->create_subscription( + ros_topics::GPS, ROS_Q_SIZE, std::bind(&LocalTransceiverIntf::sub_gps_cb, this, std::placeholders::_1)); + sub_local_path_data = this->create_subscription( + ros_topics::LOCAL_PATH, ROS_Q_SIZE, + std::bind(&LocalTransceiverIntf::sub_local_path_data_cb, this, std::placeholders::_1)); + } } private: - // Local Transceiver instance - std::shared_ptr lcl_trns_; - // Publishing timer - rclcpp::TimerBase::SharedPtr timer_; - // String is a placeholder pub and sub msg type - we will definitely define custom message types + std::unique_ptr lcl_trns_; // Local Transceiver instance + rclcpp::TimerBase::SharedPtr timer_; // Publishing timer rclcpp::Publisher::SharedPtr pub_; rclcpp::Subscription::SharedPtr sub_wind_sensor; @@ -69,73 +98,45 @@ class LocalTransceiverIntf : public NetNode * @brief Callback function to publish to onboard ROS network * */ - void pub_cb(/*place*/) + void pub_cb(/*placeholder*/) { - // TODO(Jng468): complete, after receive is done - // std::string recent_data = lcl_trns_->receive(); //receives most recent data from remote server - // auto msg = custom_interfaces::msg::Path(); - // pub_->publish(msg); + // TODO(Jng468): complete this, after receive is done } /** * @brief Callback function to subscribe to the onboard ROS network for wind sensors */ - void sub_wind_sensor_cb(custom_interfaces::msg::WindSensors in_msg) - { - custom_interfaces::msg::WindSensors data = in_msg; - lcl_trns_->updateSensor(data); - } + void sub_wind_sensor_cb(custom_interfaces::msg::WindSensors in_msg) { lcl_trns_->updateSensor(in_msg); } /** * @brief Callback function to subscribe to the onboard ROS network for batteries */ - void sub_batteries_cb(custom_interfaces::msg::Batteries in_msg) - { - custom_interfaces::msg::Batteries data = in_msg; - lcl_trns_->updateSensor(data); - } + void sub_batteries_cb(custom_interfaces::msg::Batteries in_msg) { lcl_trns_->updateSensor(in_msg); } /** * @brief Callback function to subscribe to the onboard ROS network for generic sensors */ - void sub_data_sensors_cb(custom_interfaces::msg::GenericSensors in_msg) - { - custom_interfaces::msg::GenericSensors data = in_msg; - lcl_trns_->updateSensor(data); - } + void sub_data_sensors_cb(custom_interfaces::msg::GenericSensors in_msg) { lcl_trns_->updateSensor(in_msg); } /** * @brief Callback function to subscribe to the onboard ROS network for ais ships */ - void sub_ais_ships_cb(custom_interfaces::msg::AISShips in_msg) - { - custom_interfaces::msg::AISShips data = in_msg; - lcl_trns_->updateSensor(data); - } + void sub_ais_ships_cb(custom_interfaces::msg::AISShips in_msg) { lcl_trns_->updateSensor(in_msg); } /** * @brief Callback function to subscribe to the onboard ROS network for GPS */ - void sub_gps_cb(custom_interfaces::msg::GPS in_msg) - { - custom_interfaces::msg::GPS data = in_msg; - lcl_trns_->updateSensor(data); - } + void sub_gps_cb(custom_interfaces::msg::GPS in_msg) { lcl_trns_->updateSensor(in_msg); } - void sub_local_path_data_cb(custom_interfaces::msg::LPathData in_msg) - { - custom_interfaces::msg::LPathData data = in_msg; - lcl_trns_->updateSensor(data); - } + void sub_local_path_data_cb(custom_interfaces::msg::LPathData in_msg) { lcl_trns_->updateSensor(in_msg); } }; int main(int argc, char * argv[]) { bool err = false; rclcpp::init(argc, argv); - std::shared_ptr lcl_trns = std::make_shared("PLACEHOLDER", SATELLITE_BAUD_RATE); try { - std::shared_ptr node = std::make_shared(lcl_trns); + std::shared_ptr node = std::make_shared(); try { rclcpp::spin(node); } catch (std::exception & e) { diff --git a/src/network_systems/projects/mock_ais/src/mock_ais_ros_intf.cpp b/src/network_systems/projects/mock_ais/src/mock_ais_ros_intf.cpp index 853dfd230..d2695c4fc 100644 --- a/src/network_systems/projects/mock_ais/src/mock_ais_ros_intf.cpp +++ b/src/network_systems/projects/mock_ais/src/mock_ais_ros_intf.cpp @@ -20,7 +20,7 @@ class MockAisRosIntf : public NetNode { public: - MockAisRosIntf() : NetNode("mock_ais_node") + MockAisRosIntf() : NetNode(ros_nodes::MOCK_AIS) { static constexpr int ROS_Q_SIZE = 5; this->declare_parameter("enabled", false); @@ -57,7 +57,7 @@ class MockAisRosIntf : public NetNode // TODO(): Add ROS parameters so that we can use the MockAis constructor that takes SimShipConfig // Optionally use nested parameters: https://answers.ros.org/question/325939/declare-nested-parameter/ mock_ais_ = std::make_unique(seed, num_sim_ships, polaris_start_pos); - std::string polaris_gps_topic = mode == SYSTEM_MODE::DEV ? MOCK_GPS_TOPIC : GPS_TOPIC; + std::string polaris_gps_topic = mode == SYSTEM_MODE::DEV ? ros_topics::MOCK_GPS : ros_topics::GPS; // The subscriber callback is very simple so it's just the following lambda function sub_ = this->create_subscription( @@ -65,7 +65,7 @@ class MockAisRosIntf : public NetNode mock_ais_->updatePolarisPos({mock_gps.lat_lon.latitude, mock_gps.lat_lon.longitude}); }); - pub_ = this->create_publisher(MOCK_AIS_SHIPS_TOPIC, ROS_Q_SIZE); + pub_ = this->create_publisher(ros_topics::MOCK_AIS_SHIPS, ROS_Q_SIZE); timer_ = this->create_wall_timer( std::chrono::milliseconds(publish_rate_ms), std::bind(&MockAisRosIntf::pubShipsCB, this)); } else { diff --git a/src/network_systems/projects/remote_transceiver/inc/remote_transceiver.h b/src/network_systems/projects/remote_transceiver/inc/remote_transceiver.h index da0e05033..ca00bf474 100644 --- a/src/network_systems/projects/remote_transceiver/inc/remote_transceiver.h +++ b/src/network_systems/projects/remote_transceiver/inc/remote_transceiver.h @@ -16,7 +16,8 @@ using tcp = boost::asio::ip::tcp; namespace remote_transceiver { -constexpr int DEFAULT_NUM_IO_THREADS = 2; // Default number of HTTP requests that can be accepted in parallel +// Default number of HTTP requests that can be accepted in parallel +constexpr int DEFAULT_NUM_IO_THREADS = 2; // Production constants are all placheholders static const std::string PROD_DB_NAME = "PLACEHOLDER"; diff --git a/src/network_systems/projects/remote_transceiver/src/remote_transceiver_ros_intf.cpp b/src/network_systems/projects/remote_transceiver/src/remote_transceiver_ros_intf.cpp index 78be9a8b7..867af364a 100644 --- a/src/network_systems/projects/remote_transceiver/src/remote_transceiver_ros_intf.cpp +++ b/src/network_systems/projects/remote_transceiver/src/remote_transceiver_ros_intf.cpp @@ -6,6 +6,7 @@ #include #include +#include "cmn_hdrs/ros_info.h" #include "cmn_hdrs/shared_constants.h" #include "net_node.h" #include "remote_transceiver.h" @@ -18,7 +19,7 @@ class RemoteTransceiverRosIntf : public NetNode { public: - RemoteTransceiverRosIntf() : NetNode("remote_transceiver_node") + RemoteTransceiverRosIntf() : NetNode(ros_nodes::REMOTE_TRANSCEIVER) { this->declare_parameter("enabled", true); enabled_ = this->get_parameter("enabled").as_bool(); diff --git a/src/network_systems/projects/remote_transceiver/test/test_remote_transceiver.cpp b/src/network_systems/projects/remote_transceiver/test/test_remote_transceiver.cpp index 8b043bf39..415a155bb 100644 --- a/src/network_systems/projects/remote_transceiver/test/test_remote_transceiver.cpp +++ b/src/network_systems/projects/remote_transceiver/test/test_remote_transceiver.cpp @@ -37,7 +37,7 @@ class TestRemoteTransceiver : public ::testing::Test protected: static constexpr int NUM_THREADS = 4; // Need to wait after receiving an HTTP response from the server - static constexpr auto WAIT_AFTER_RES = std::chrono::milliseconds(20); + static constexpr auto WAIT_AFTER_RES = std::chrono::milliseconds(200); // Network objects that are shared amongst all HTTP test suites static bio::io_context io_; diff --git a/src/network_systems/ros_info.yaml b/src/network_systems/ros_info.yaml new file mode 100644 index 000000000..5e65d5c4b --- /dev/null +++ b/src/network_systems/ros_info.yaml @@ -0,0 +1,20 @@ +ros_topics: + AIS_SHIPS: ais_ships + BATTERIES: batteries + DESIRED_HEADING: desired_heading + DATA_SENSORS: data_sensors + FILTERED_WIND_SENSOR: filtered_wind_sensor + GLOBAL_PATH: global_path + GPS: gps + LOCAL_PATH: local_path_data + MOCK_AIS_SHIPS: mock_ais_ships + MOCK_GPS: mock_gps + MOCK_WIND_SENSORS: mock_wind_sensor + WIND_SENSORS: wind_sensors + +ros_nodes: + CAN_TRANSCEIVER: can_transceiver_node + CACHED_FIB: cached_fib_node + LOCAL_TRANSCEIVER: local_transceiver_node + MOCK_AIS: mock_ais_node + REMOTE_TRANSCEIVER: remote_transceiver_node diff --git a/src/network_systems/scripts/README.md b/src/network_systems/scripts/README.md index af6b510f2..0ecce8bfb 100644 --- a/src/network_systems/scripts/README.md +++ b/src/network_systems/scripts/README.md @@ -1,12 +1,12 @@ # Scripts -## Autogen ROS Topics +## Generate ROS Info ```shell -./autogen_ros_topics.sh +python3 gen_ros_info.py ``` -Given an input text file where each line is the name of a ROS topic, generates a C++ header file matching those names. +Takes [ros_info.yaml](../ros_info.yaml) and generates a C++ header file and Python file with constants defined within. ## Sailbot DB diff --git a/src/network_systems/scripts/autogen_ros_topics.sh b/src/network_systems/scripts/autogen_ros_topics.sh deleted file mode 100755 index 87a0c33de..000000000 --- a/src/network_systems/scripts/autogen_ros_topics.sh +++ /dev/null @@ -1,31 +0,0 @@ -#!/bin/bash - -# A simple shell script that takes as input a text file of names and converts them to -# C++ constants in a header file (network_systems/lib/cmn_hdrs/ros_info.h) -# REQUIREMENTS: -# - A text document with the names of required ROS topics. -# - Example use in command line: -# ./autogen_ros_topics.sh input.txt - -SCRIPT_DIR="$(dirname "$(readlink -f "$0")")" -OUTPUT_PATH=$SCRIPT_DIR/../lib/cmn_hdrs/ros_info.h - -# Checks that requirements above are fulfilled. -if [ $# -ne 1 ]; then - echo "Usage: $0 INPUT_FILE" - exit 1 -fi - -INPUT_FILE=$1 - -echo "// AUTOGENERATED FILE - DO NOT EDIT" > $OUTPUT_PATH -echo "#pragma once" >> $OUTPUT_PATH - -# Reads each line in the input text document -# Generates the name of the constant in required format -# Writes to output a C++ constant with the generated name and line -while read -r line; do - CAPITALIZED=$(echo $line | tr '[:lower:]' '[:upper:]') - VAR_NAME="${CAPITALIZED}_TOPIC" - echo "constexpr auto $VAR_NAME = \"$line\";" >> $OUTPUT_PATH -done < "$INPUT_FILE" diff --git a/src/network_systems/scripts/gen_ros_info.py b/src/network_systems/scripts/gen_ros_info.py new file mode 100644 index 000000000..122995c5e --- /dev/null +++ b/src/network_systems/scripts/gen_ros_info.py @@ -0,0 +1,63 @@ +import os + +import yaml + +NET_DIR = os.path.join( + os.getenv("ROS_WORKSPACE", default="/workspaces/sailbot_workspace"), "src/network_systems" +) +ROS_INFO_FILE = os.path.join(NET_DIR, "ros_info.yaml") + +GEN_HDR_FILE = os.path.join(NET_DIR, "lib/cmn_hdrs/ros_info.h") +GEN_FILE_WARN = f"FILE GENERATED BY {__file__} using {ROS_INFO_FILE} - DO NOT EDIT" + +GEN_HDR_PREAMBLE = f""" +// {GEN_FILE_WARN} +#pragma once +""" +GEN_HDR_FILE_TOPICS_START = """ +namespace ros_topics +{ +""" +GEN_HDR_FILE_TOPICS_END = "}; // namespace ros_topics\n" +GEN_HDR_FILE_NODES_START = """ +namespace ros_nodes +{ +""" +GEN_HDR_FILE_NODES_END = "}; // namespace ros_nodes\n" + +GEN_PYTHON_FILE = os.path.join(NET_DIR, "launch/ros_info.py") +GEN_PYTHON_PREAMBLE = f"# {GEN_FILE_WARN} # noqa: E501\n" + + +def populate_hdr(hdr_file_obj, info_target): + for k, v in info_target.items(): + hdr_file_obj.write(f'static constexpr auto {k} = "{v}";\n') + + +def populate_py_nodes(py_file_obj, info_target): + for k, v in info_target.items(): + py_file_obj.write(f'{k}_NODE = "{v}"\n') + + +def main(): + with open(ROS_INFO_FILE, "r") as f: + info = yaml.safe_load(f) + + with open(GEN_HDR_FILE, "w+") as out_hdr: + out_hdr.write(GEN_HDR_PREAMBLE) + + out_hdr.write(GEN_HDR_FILE_TOPICS_START) + populate_hdr(out_hdr, info["ros_topics"]) + out_hdr.write(GEN_HDR_FILE_TOPICS_END) + + out_hdr.write(GEN_HDR_FILE_NODES_START) + populate_hdr(out_hdr, info["ros_nodes"]) + out_hdr.write(GEN_HDR_FILE_NODES_END) + + with open(GEN_PYTHON_FILE, "w+") as out_py: + out_py.write(GEN_PYTHON_PREAMBLE) + populate_py_nodes(out_py, info["ros_nodes"]) + + +if __name__ == "__main__": + main() diff --git a/src/virtual_iridium/.gitignore b/src/virtual_iridium/.gitignore new file mode 100644 index 000000000..a1dc806a7 --- /dev/null +++ b/src/virtual_iridium/.gitignore @@ -0,0 +1,2 @@ +# python generated files +*.pyc diff --git a/src/virtual_iridium/python/imap_stuff.pyc b/src/virtual_iridium/python/imap_stuff.pyc deleted file mode 100644 index b73294f84..000000000 Binary files a/src/virtual_iridium/python/imap_stuff.pyc and /dev/null differ diff --git a/src/virtual_iridium/python/sbd_packets.pyc b/src/virtual_iridium/python/sbd_packets.pyc deleted file mode 100644 index 8e7a07ce0..000000000 Binary files a/src/virtual_iridium/python/sbd_packets.pyc and /dev/null differ diff --git a/src/virtual_iridium/python/smtp_stuff.pyc b/src/virtual_iridium/python/smtp_stuff.pyc deleted file mode 100644 index a1a49b70c..000000000 Binary files a/src/virtual_iridium/python/smtp_stuff.pyc and /dev/null differ diff --git a/src/website/package-lock.json b/src/website/package-lock.json index 47bd58f4a..6881e5f99 100644 --- a/src/website/package-lock.json +++ b/src/website/package-lock.json @@ -17,7 +17,7 @@ "leaflet-geometryutil": "^0.10.2", "mongodb": "^4.8.1", "mongoose": "^7.5.0", - "next": "latest", + "next": "^14.1.3", "react": "^18.2.0", "react-dom": "^18.2.0", "react-leaflet": "^4.2.1", @@ -1334,8 +1334,9 @@ "integrity": "sha512-xWGDIW6x921xtzPkhiULtthJHoJvBbF3q26fzloPCK0hsvxtPVelvftw3zjbHWSkR2km9Z+4uxbDDK/6Zw9B8w==" }, "node_modules/@next/env": { - "version": "13.4.19", - "license": "MIT" + "version": "14.1.3", + "resolved": "https://registry.npmjs.org/@next/env/-/env-14.1.3.tgz", + "integrity": "sha512-VhgXTvrgeBRxNPjyfBsDIMvgsKDxjlpw4IAUsHCX8Gjl1vtHUYRT3+xfQ/wwvLPDd/6kqfLqk9Pt4+7gysuCKQ==" }, "node_modules/@next/eslint-plugin-next": { "version": "14.0.2", @@ -1347,9 +1348,9 @@ } }, "node_modules/@next/swc-darwin-arm64": { - "version": "13.4.19", - "resolved": "https://registry.npmjs.org/@next/swc-darwin-arm64/-/swc-darwin-arm64-13.4.19.tgz", - "integrity": "sha512-vv1qrjXeGbuF2mOkhkdxMDtv9np7W4mcBtaDnHU+yJG+bBwa6rYsYSCI/9Xm5+TuF5SbZbrWO6G1NfTh1TMjvQ==", + "version": "14.1.3", + "resolved": "https://registry.npmjs.org/@next/swc-darwin-arm64/-/swc-darwin-arm64-14.1.3.tgz", + "integrity": "sha512-LALu0yIBPRiG9ANrD5ncB3pjpO0Gli9ZLhxdOu6ZUNf3x1r3ea1rd9Q+4xxUkGrUXLqKVK9/lDkpYIJaCJ6AHQ==", "cpu": [ "arm64" ], @@ -1362,9 +1363,9 @@ } }, "node_modules/@next/swc-darwin-x64": { - "version": "13.4.19", - "resolved": "https://registry.npmjs.org/@next/swc-darwin-x64/-/swc-darwin-x64-13.4.19.tgz", - "integrity": "sha512-jyzO6wwYhx6F+7gD8ddZfuqO4TtpJdw3wyOduR4fxTUCm3aLw7YmHGYNjS0xRSYGAkLpBkH1E0RcelyId6lNsw==", + "version": "14.1.3", + "resolved": "https://registry.npmjs.org/@next/swc-darwin-x64/-/swc-darwin-x64-14.1.3.tgz", + "integrity": "sha512-E/9WQeXxkqw2dfcn5UcjApFgUq73jqNKaE5bysDm58hEUdUGedVrnRhblhJM7HbCZNhtVl0j+6TXsK0PuzXTCg==", "cpu": [ "x64" ], @@ -1377,9 +1378,9 @@ } }, "node_modules/@next/swc-linux-arm64-gnu": { - "version": "13.4.19", - "resolved": "https://registry.npmjs.org/@next/swc-linux-arm64-gnu/-/swc-linux-arm64-gnu-13.4.19.tgz", - "integrity": "sha512-vdlnIlaAEh6H+G6HrKZB9c2zJKnpPVKnA6LBwjwT2BTjxI7e0Hx30+FoWCgi50e+YO49p6oPOtesP9mXDRiiUg==", + "version": "14.1.3", + "resolved": "https://registry.npmjs.org/@next/swc-linux-arm64-gnu/-/swc-linux-arm64-gnu-14.1.3.tgz", + "integrity": "sha512-USArX9B+3rZSXYLFvgy0NVWQgqh6LHWDmMt38O4lmiJNQcwazeI6xRvSsliDLKt+78KChVacNiwvOMbl6g6BBw==", "cpu": [ "arm64" ], @@ -1392,9 +1393,9 @@ } }, "node_modules/@next/swc-linux-arm64-musl": { - "version": "13.4.19", - "resolved": "https://registry.npmjs.org/@next/swc-linux-arm64-musl/-/swc-linux-arm64-musl-13.4.19.tgz", - "integrity": "sha512-aU0HkH2XPgxqrbNRBFb3si9Ahu/CpaR5RPmN2s9GiM9qJCiBBlZtRTiEca+DC+xRPyCThTtWYgxjWHgU7ZkyvA==", + "version": "14.1.3", + "resolved": "https://registry.npmjs.org/@next/swc-linux-arm64-musl/-/swc-linux-arm64-musl-14.1.3.tgz", + "integrity": "sha512-esk1RkRBLSIEp1qaQXv1+s6ZdYzuVCnDAZySpa62iFTMGTisCyNQmqyCTL9P+cLJ4N9FKCI3ojtSfsyPHJDQNw==", "cpu": [ "arm64" ], @@ -1407,9 +1408,9 @@ } }, "node_modules/@next/swc-linux-x64-gnu": { - "version": "13.4.19", - "resolved": "https://registry.npmjs.org/@next/swc-linux-x64-gnu/-/swc-linux-x64-gnu-13.4.19.tgz", - "integrity": "sha512-htwOEagMa/CXNykFFeAHHvMJeqZfNQEoQvHfsA4wgg5QqGNqD5soeCer4oGlCol6NGUxknrQO6VEustcv+Md+g==", + "version": "14.1.3", + "resolved": "https://registry.npmjs.org/@next/swc-linux-x64-gnu/-/swc-linux-x64-gnu-14.1.3.tgz", + "integrity": "sha512-8uOgRlYEYiKo0L8YGeS+3TudHVDWDjPVDUcST+z+dUzgBbTEwSSIaSgF/vkcC1T/iwl4QX9iuUyUdQEl0Kxalg==", "cpu": [ "x64" ], @@ -1422,9 +1423,9 @@ } }, "node_modules/@next/swc-linux-x64-musl": { - "version": "13.4.19", - "resolved": "https://registry.npmjs.org/@next/swc-linux-x64-musl/-/swc-linux-x64-musl-13.4.19.tgz", - "integrity": "sha512-4Gj4vvtbK1JH8ApWTT214b3GwUh9EKKQjY41hH/t+u55Knxi/0wesMzwQRhppK6Ddalhu0TEttbiJ+wRcoEj5Q==", + "version": "14.1.3", + "resolved": "https://registry.npmjs.org/@next/swc-linux-x64-musl/-/swc-linux-x64-musl-14.1.3.tgz", + "integrity": "sha512-DX2zqz05ziElLoxskgHasaJBREC5Y9TJcbR2LYqu4r7naff25B4iXkfXWfcp69uD75/0URmmoSgT8JclJtrBoQ==", "cpu": [ "x64" ], @@ -1437,9 +1438,9 @@ } }, "node_modules/@next/swc-win32-arm64-msvc": { - "version": "13.4.19", - "resolved": "https://registry.npmjs.org/@next/swc-win32-arm64-msvc/-/swc-win32-arm64-msvc-13.4.19.tgz", - "integrity": "sha512-bUfDevQK4NsIAHXs3/JNgnvEY+LRyneDN788W2NYiRIIzmILjba7LaQTfihuFawZDhRtkYCv3JDC3B4TwnmRJw==", + "version": "14.1.3", + "resolved": "https://registry.npmjs.org/@next/swc-win32-arm64-msvc/-/swc-win32-arm64-msvc-14.1.3.tgz", + "integrity": "sha512-HjssFsCdsD4GHstXSQxsi2l70F/5FsRTRQp8xNgmQs15SxUfUJRvSI9qKny/jLkY3gLgiCR3+6A7wzzK0DBlfA==", "cpu": [ "arm64" ], @@ -1452,9 +1453,9 @@ } }, "node_modules/@next/swc-win32-ia32-msvc": { - "version": "13.4.19", - "resolved": "https://registry.npmjs.org/@next/swc-win32-ia32-msvc/-/swc-win32-ia32-msvc-13.4.19.tgz", - "integrity": "sha512-Y5kikILFAr81LYIFaw6j/NrOtmiM4Sf3GtOc0pn50ez2GCkr+oejYuKGcwAwq3jiTKuzF6OF4iT2INPoxRycEA==", + "version": "14.1.3", + "resolved": "https://registry.npmjs.org/@next/swc-win32-ia32-msvc/-/swc-win32-ia32-msvc-14.1.3.tgz", + "integrity": "sha512-DRuxD5axfDM1/Ue4VahwSxl1O5rn61hX8/sF0HY8y0iCbpqdxw3rB3QasdHn/LJ6Wb2y5DoWzXcz3L1Cr+Thrw==", "cpu": [ "ia32" ], @@ -1467,9 +1468,9 @@ } }, "node_modules/@next/swc-win32-x64-msvc": { - "version": "13.4.19", - "resolved": "https://registry.npmjs.org/@next/swc-win32-x64-msvc/-/swc-win32-x64-msvc-13.4.19.tgz", - "integrity": "sha512-YzA78jBDXMYiINdPdJJwGgPNT3YqBNNGhsthsDoWHL9p24tEJn9ViQf/ZqTbwSpX/RrkPupLfuuTH2sf73JBAw==", + "version": "14.1.3", + "resolved": "https://registry.npmjs.org/@next/swc-win32-x64-msvc/-/swc-win32-x64-msvc-14.1.3.tgz", + "integrity": "sha512-uC2DaDoWH7h1P/aJ4Fok3Xiw6P0Lo4ez7NbowW2VGNXw/Xv6tOuLUcxhBYZxsSUJtpeknCi8/fvnSpyCFp4Rcg==", "cpu": [ "x64" ], @@ -2109,8 +2110,9 @@ } }, "node_modules/@swc/helpers": { - "version": "0.5.1", - "license": "Apache-2.0", + "version": "0.5.2", + "resolved": "https://registry.npmjs.org/@swc/helpers/-/helpers-0.5.2.tgz", + "integrity": "sha512-E4KcWTpoLHqwPHLxidpOqQbcrZVgi0rsmmZXUle1jXmJfuIf/UWpczUJ7MZZ5tlxytgJXyp0w4PGkkeLiuIdZw==", "dependencies": { "tslib": "^2.4.0" } @@ -2723,7 +2725,7 @@ "version": "3.1.3", "resolved": "https://registry.npmjs.org/anymatch/-/anymatch-3.1.3.tgz", "integrity": "sha512-KMReFUr0B4t+D+OBkjR3KYqvocp2XaSzO55UcB6mgQMd3KbcE+mWTyvVV7D/zsdEbNnV6acZUutkiHQXvTr1Rw==", - "dev": true, + "devOptional": true, "dependencies": { "normalize-path": "^3.0.0", "picomatch": "^2.0.4" @@ -2985,7 +2987,7 @@ "version": "2.2.0", "resolved": "https://registry.npmjs.org/binary-extensions/-/binary-extensions-2.2.0.tgz", "integrity": "sha512-jDctJ/IVQbZoJykoeHbhXpOlNBqGNcwXJKJog42E5HDPUwQTSdjCHdihjj0DlnheQ7blbT6dHOafNAiS8ooQKA==", - "dev": true, + "devOptional": true, "engines": { "node": ">=8" } @@ -3031,7 +3033,7 @@ "version": "3.0.2", "resolved": "https://registry.npmjs.org/braces/-/braces-3.0.2.tgz", "integrity": "sha512-b8um+L1RzM3WDSzvhm6gIz1yfTbBt6YTlcEKAvsmqCZZFw46z626lVj9j1yEPW33H5H+lBQpZMP1k8l+78Ha0A==", - "dev": true, + "devOptional": true, "dependencies": { "fill-range": "^7.0.1" }, @@ -3150,7 +3152,9 @@ } }, "node_modules/caniuse-lite": { - "version": "1.0.30001522", + "version": "1.0.30001596", + "resolved": "https://registry.npmjs.org/caniuse-lite/-/caniuse-lite-1.0.30001596.tgz", + "integrity": "sha512-zpkZ+kEr6We7w63ORkoJ2pOfBwBkY/bJrG/UZ90qNb45Isblu8wzDgevEOrRL1r9dWayHjYiiyCMEXPn4DweGQ==", "funding": [ { "type": "opencollective", @@ -3164,8 +3168,7 @@ "type": "github", "url": "https://github.com/sponsors/ai" } - ], - "license": "CC-BY-4.0" + ] }, "node_modules/chalk": { "version": "2.4.2", @@ -3190,7 +3193,7 @@ "version": "3.5.3", "resolved": "https://registry.npmjs.org/chokidar/-/chokidar-3.5.3.tgz", "integrity": "sha512-Dr3sfKRP6oTcjf2JmUmFJfeVMvXBdegxB0iVQ5eb2V10uFJUCAS8OByZdVAyVb8xXNz3GjjTgj9kLWsZTqE6kw==", - "dev": true, + "devOptional": true, "funding": [ { "type": "individual", @@ -3217,7 +3220,7 @@ "version": "5.1.2", "resolved": "https://registry.npmjs.org/glob-parent/-/glob-parent-5.1.2.tgz", "integrity": "sha512-AOIgSQCepiJYwP3ARnGx+5VnTu2HBYdzbGP45eLw1vr3zB3vZLeyed1sC9hnbcOc9/SrMyM5RPQrkGz4aS9Zow==", - "dev": true, + "devOptional": true, "dependencies": { "is-glob": "^4.0.1" }, @@ -4522,7 +4525,7 @@ "version": "7.0.1", "resolved": "https://registry.npmjs.org/fill-range/-/fill-range-7.0.1.tgz", "integrity": "sha512-qOo9F+dMUmC2Lcb4BbVvnKJxTPjCm+RRpe4gDuGrzkL7mEVl/djYSu2OdQ2Pa302N4oqkSg9ir6jaLWJ2USVpQ==", - "dev": true, + "devOptional": true, "dependencies": { "to-regex-range": "^5.0.1" }, @@ -4724,10 +4727,6 @@ "node": ">=10.13.0" } }, - "node_modules/glob-to-regexp": { - "version": "0.4.1", - "license": "BSD-2-Clause" - }, "node_modules/global-modules": { "version": "2.0.0", "resolved": "https://registry.npmjs.org/global-modules/-/global-modules-2.0.0.tgz", @@ -5019,7 +5018,7 @@ "version": "4.3.4", "resolved": "https://registry.npmjs.org/immutable/-/immutable-4.3.4.tgz", "integrity": "sha512-fsXeu4J4i6WNWSikpI88v/PcVflZz+6kMhUfIwc5SY+poQRPnaf5V7qds6SUyUN3cVxEzuCab7QIoLOQ+DQ1wA==", - "dev": true + "devOptional": true }, "node_modules/import-fresh": { "version": "3.3.0", @@ -5115,8 +5114,9 @@ } }, "node_modules/ip": { - "version": "2.0.0", - "license": "MIT" + "version": "2.0.1", + "resolved": "https://registry.npmjs.org/ip/-/ip-2.0.1.tgz", + "integrity": "sha512-lJUL9imLTNi1ZfXT+DU6rBBdbiKGBuay9B6xGSPVjUeQwaH1RIGqef8RZkUtHioLmSNpPR5M4HVKJGm1j8FWVQ==" }, "node_modules/ipaddr.js": { "version": "1.9.1", @@ -5174,7 +5174,7 @@ "version": "2.1.0", "resolved": "https://registry.npmjs.org/is-binary-path/-/is-binary-path-2.1.0.tgz", "integrity": "sha512-ZMERYes6pDydyuGidse7OsHxtbI7WVeUEozgR/g7rd0xUimYNlvZRE/K2MgZTjWy725IfelLeVcEM97mmtRGXw==", - "dev": true, + "devOptional": true, "dependencies": { "binary-extensions": "^2.0.0" }, @@ -5240,7 +5240,7 @@ "version": "2.1.1", "resolved": "https://registry.npmjs.org/is-extglob/-/is-extglob-2.1.1.tgz", "integrity": "sha512-SbKbANkN603Vi4jEZv49LeVJMn4yGwsbzZworEoyEiutsN3nJYdbO36zfhGJ6QEDpOZIFkDtnq5JRxmvl3jsoQ==", - "dev": true, + "devOptional": true, "engines": { "node": ">=0.10.0" } @@ -5285,7 +5285,7 @@ "version": "4.0.3", "resolved": "https://registry.npmjs.org/is-glob/-/is-glob-4.0.3.tgz", "integrity": "sha512-xelSayHH36ZgE7ZWhli7pW34hNbNl8Ojv5KVmkJD4hBdD3th8Tfk9vYasLM+mXWOZhFkgZfxhLSnrwRr4elSSg==", - "dev": true, + "devOptional": true, "dependencies": { "is-extglob": "^2.1.1" }, @@ -5318,7 +5318,7 @@ "version": "7.0.0", "resolved": "https://registry.npmjs.org/is-number/-/is-number-7.0.0.tgz", "integrity": "sha512-41Cifkg6e8TylSpdtTpeLVMqvSBEVzTttHvERD741+pnZ8ANv0004MRL43QKPDlK9cGvNp6NZWZUBlbGXYxxng==", - "dev": true, + "devOptional": true, "engines": { "node": ">=0.12.0" } @@ -6069,35 +6069,34 @@ } }, "node_modules/next": { - "version": "13.4.19", - "resolved": "https://registry.npmjs.org/next/-/next-13.4.19.tgz", - "integrity": "sha512-HuPSzzAbJ1T4BD8e0bs6B9C1kWQ6gv8ykZoRWs5AQoiIuqbGHHdQO7Ljuvg05Q0Z24E2ABozHe6FxDvI6HfyAw==", + "version": "14.1.3", + "resolved": "https://registry.npmjs.org/next/-/next-14.1.3.tgz", + "integrity": "sha512-oexgMV2MapI0UIWiXKkixF8J8ORxpy64OuJ/J9oVUmIthXOUCcuVEZX+dtpgq7wIfIqtBwQsKEDXejcjTsan9g==", "dependencies": { - "@next/env": "13.4.19", - "@swc/helpers": "0.5.1", + "@next/env": "14.1.3", + "@swc/helpers": "0.5.2", "busboy": "1.6.0", - "caniuse-lite": "^1.0.30001406", - "postcss": "8.4.14", - "styled-jsx": "5.1.1", - "watchpack": "2.4.0", - "zod": "3.21.4" + "caniuse-lite": "^1.0.30001579", + "graceful-fs": "^4.2.11", + "postcss": "8.4.31", + "styled-jsx": "5.1.1" }, "bin": { "next": "dist/bin/next" }, "engines": { - "node": ">=16.8.0" + "node": ">=18.17.0" }, "optionalDependencies": { - "@next/swc-darwin-arm64": "13.4.19", - "@next/swc-darwin-x64": "13.4.19", - "@next/swc-linux-arm64-gnu": "13.4.19", - "@next/swc-linux-arm64-musl": "13.4.19", - "@next/swc-linux-x64-gnu": "13.4.19", - "@next/swc-linux-x64-musl": "13.4.19", - "@next/swc-win32-arm64-msvc": "13.4.19", - "@next/swc-win32-ia32-msvc": "13.4.19", - "@next/swc-win32-x64-msvc": "13.4.19" + "@next/swc-darwin-arm64": "14.1.3", + "@next/swc-darwin-x64": "14.1.3", + "@next/swc-linux-arm64-gnu": "14.1.3", + "@next/swc-linux-arm64-musl": "14.1.3", + "@next/swc-linux-x64-gnu": "14.1.3", + "@next/swc-linux-x64-musl": "14.1.3", + "@next/swc-win32-arm64-msvc": "14.1.3", + "@next/swc-win32-ia32-msvc": "14.1.3", + "@next/swc-win32-x64-msvc": "14.1.3" }, "peerDependencies": { "@opentelemetry/api": "^1.1.0", @@ -6114,29 +6113,6 @@ } } }, - "node_modules/next/node_modules/postcss": { - "version": "8.4.14", - "resolved": "https://registry.npmjs.org/postcss/-/postcss-8.4.14.tgz", - "integrity": "sha512-E398TUmfAYFPBSdzgeieK2Y1+1cpdxJx8yXbK/m57nRhKSmk1GB2tO4lbLBtlkfPQTDKfe4Xqv1ASWPpayPEig==", - "funding": [ - { - "type": "opencollective", - "url": "https://opencollective.com/postcss/" - }, - { - "type": "tidelift", - "url": "https://tidelift.com/funding/github/npm/postcss" - } - ], - "dependencies": { - "nanoid": "^3.3.4", - "picocolors": "^1.0.0", - "source-map-js": "^1.0.2" - }, - "engines": { - "node": "^10 || ^12 || >=14" - } - }, "node_modules/normalize-package-data": { "version": "3.0.3", "resolved": "https://registry.npmjs.org/normalize-package-data/-/normalize-package-data-3.0.3.tgz", @@ -6156,7 +6132,7 @@ "version": "3.0.0", "resolved": "https://registry.npmjs.org/normalize-path/-/normalize-path-3.0.0.tgz", "integrity": "sha512-6eZs5Ls3WtCisHWp9S2GUy8dqkpGi4BVSz3GaqiE6ezub0512ESztXUwUB6C6IKbQkY2Pnb/mD4WYojCRwcwLA==", - "dev": true, + "devOptional": true, "engines": { "node": ">=0.10.0" } @@ -6426,7 +6402,7 @@ "version": "2.3.1", "resolved": "https://registry.npmjs.org/picomatch/-/picomatch-2.3.1.tgz", "integrity": "sha512-JU3teHTNjmE2VCGFzuY8EXzCDVwEqB2a8fsIvwaStHhAWJEeVd1o1QD80CU6+ZdEXXSLbSsuLwJjkCBWqRQUVA==", - "dev": true, + "devOptional": true, "engines": { "node": ">=8.6" }, @@ -6438,7 +6414,6 @@ "version": "8.4.31", "resolved": "https://registry.npmjs.org/postcss/-/postcss-8.4.31.tgz", "integrity": "sha512-PS08Iboia9mts/2ygV3eLpY5ghnUcfLV/EXTOW1E2qYxJKGGBUtNjN76FYHnMs36RmARn41bC0AZmn+rR0OVpQ==", - "dev": true, "funding": [ { "type": "opencollective", @@ -6853,7 +6828,7 @@ "version": "3.6.0", "resolved": "https://registry.npmjs.org/readdirp/-/readdirp-3.6.0.tgz", "integrity": "sha512-hOS089on8RduqdbhvQ5Z37A0ESjsqz6qnRcffsMU3495FuTdqSm+7bhJ29JvIOsBDEEnan5DPu9t3To9VRlMzA==", - "dev": true, + "devOptional": true, "dependencies": { "picomatch": "^2.2.1" }, @@ -7127,7 +7102,7 @@ "version": "1.69.5", "resolved": "https://registry.npmjs.org/sass/-/sass-1.69.5.tgz", "integrity": "sha512-qg2+UCJibLr2LCVOt3OlPhr/dqVHWOa9XtZf2OjbLs/T4VPSJ00udtgJxH3neXZm+QqX8B+3cU7RaLqp1iVfcQ==", - "dev": true, + "devOptional": true, "dependencies": { "chokidar": ">=3.0.0 <4.0.0", "immutable": "^4.0.0", @@ -7826,6 +7801,21 @@ "node": ">=8" } }, + "node_modules/stylelint/node_modules/typescript": { + "version": "5.4.2", + "resolved": "https://registry.npmjs.org/typescript/-/typescript-5.4.2.tgz", + "integrity": "sha512-+2/g0Fds1ERlP6JsakQQDXjZdZMM+rqpamFZJEKh4kwTIn3iDkgKtby0CeNd5ATNZ4Ry1ax15TMx0W2V+miizQ==", + "dev": true, + "optional": true, + "peer": true, + "bin": { + "tsc": "bin/tsc", + "tsserver": "bin/tsserver" + }, + "engines": { + "node": ">=14.17" + } + }, "node_modules/stylis": { "version": "4.2.0", "license": "MIT" @@ -7966,7 +7956,7 @@ "version": "5.0.1", "resolved": "https://registry.npmjs.org/to-regex-range/-/to-regex-range-5.0.1.tgz", "integrity": "sha512-65P7iz6X5yEr1cwcgvQxbbIw7Uk3gOy5dIdtZ4rDveLqhrdJP+Li/Hx6tyK0NEb+2GCyneCMJiGqrADCSNk8sQ==", - "dev": true, + "devOptional": true, "dependencies": { "is-number": "^7.0.0" }, @@ -8275,17 +8265,6 @@ "d3-timer": "^3.0.1" } }, - "node_modules/watchpack": { - "version": "2.4.0", - "license": "MIT", - "dependencies": { - "glob-to-regexp": "^0.4.1", - "graceful-fs": "^4.1.2" - }, - "engines": { - "node": ">=10.13.0" - } - }, "node_modules/webidl-conversions": { "version": "7.0.0", "license": "BSD-2-Clause", @@ -8447,13 +8426,6 @@ "funding": { "url": "https://github.com/sponsors/sindresorhus" } - }, - "node_modules/zod": { - "version": "3.21.4", - "license": "MIT", - "funding": { - "url": "https://github.com/sponsors/colinhacks" - } } } } diff --git a/src/website/package.json b/src/website/package.json index d92309816..229d6536a 100644 --- a/src/website/package.json +++ b/src/website/package.json @@ -20,7 +20,7 @@ "leaflet-geometryutil": "^0.10.2", "mongodb": "^4.8.1", "mongoose": "^7.5.0", - "next": "latest", + "next": "^14.1.3", "react": "^18.2.0", "react-dom": "^18.2.0", "react-leaflet": "^4.2.1", diff --git a/src/website/public/BoatIconFinal.png b/src/website/public/BoatIconFinal.png index f4f912dbf..51b35c4e6 100644 Binary files a/src/website/public/BoatIconFinal.png and b/src/website/public/BoatIconFinal.png differ diff --git a/src/website/public/NSEWCompass.png b/src/website/public/NSEWCompass.png index 59a8312e3..83283aed7 100644 Binary files a/src/website/public/NSEWCompass.png and b/src/website/public/NSEWCompass.png differ diff --git a/src/website/public/NSEWCompassBackdrop.png b/src/website/public/NSEWCompassBackdrop.png index 0ef342a07..80c016c64 100644 Binary files a/src/website/public/NSEWCompassBackdrop.png and b/src/website/public/NSEWCompassBackdrop.png differ diff --git a/src/website/public/SailbotLogo.png b/src/website/public/SailbotLogo.png index 62464612a..2dc14bd8a 100644 Binary files a/src/website/public/SailbotLogo.png and b/src/website/public/SailbotLogo.png differ diff --git a/src/website/public/vercel.svg b/src/website/public/vercel.svg index fbf0e25a6..09dabddd6 100644 --- a/src/website/public/vercel.svg +++ b/src/website/public/vercel.svg @@ -1,4 +1 @@ - - - \ No newline at end of file + \ No newline at end of file diff --git a/src/website/tests/package-lock.json b/src/website/tests/package-lock.json index 194755a28..6cc3ef659 100644 --- a/src/website/tests/package-lock.json +++ b/src/website/tests/package-lock.json @@ -12,7 +12,7 @@ "@cucumber/messages": "20.0.0", "@cucumber/pretty-formatter": "1.0.0", "amqp-ts": "1.8.0", - "axios": "0.27.2", + "axios": "0.28.0", "chai": "4.3.7", "deep-equal-in-any-order": "1.1.20", "mongoose": "^7.5.3", @@ -504,12 +504,13 @@ "integrity": "sha512-Oei9OH4tRh0YqU3GxhX79dM/mwVgvbZJaSNaRk+bshkj0S5cfHcgYakreBjrHwatXKbz+IoIdYLxrKim2MjW0Q==" }, "node_modules/axios": { - "version": "0.27.2", - "resolved": "https://registry.npmjs.org/axios/-/axios-0.27.2.tgz", - "integrity": "sha512-t+yRIyySRTp/wua5xEr+z1q60QmLq8ABsS5O9Me1AsE5dfKqgnCFzwiCZZ/cGNd1lq4/7akDWMxdhVlucjmnOQ==", + "version": "0.28.0", + "resolved": "https://registry.npmjs.org/axios/-/axios-0.28.0.tgz", + "integrity": "sha512-Tu7NYoGY4Yoc7I+Npf9HhUMtEEpV7ZiLH9yndTCoNhcpBH0kwcvFbzYN9/u5QKI5A6uefjsNNWaz5olJVYS62Q==", "dependencies": { - "follow-redirects": "^1.14.9", - "form-data": "^4.0.0" + "follow-redirects": "^1.15.0", + "form-data": "^4.0.0", + "proxy-from-env": "^1.1.0" } }, "node_modules/balanced-match": { @@ -883,9 +884,9 @@ "integrity": "sha512-GRnmB5gPyJpAhTQdSZTSp9uaPSvl09KoYcMQtsB9rQoOmzs9dH6ffeccH+Z+cv6P68Hu5bC6JjRh4Ah/mHSNRw==" }, "node_modules/follow-redirects": { - "version": "1.15.3", - "resolved": "https://registry.npmjs.org/follow-redirects/-/follow-redirects-1.15.3.tgz", - "integrity": "sha512-1VzOtuEM8pC9SFU1E+8KfTjZyMztRsgEfwQl44z8A25uy13jSzTj6dyK2Df52iV0vgHCfBwLhDWevLn95w5v6Q==", + "version": "1.15.5", + "resolved": "https://registry.npmjs.org/follow-redirects/-/follow-redirects-1.15.5.tgz", + "integrity": "sha512-vSFWUON1B+yAw1VN4xMfxgn5fTUiaOzAJCKBwIIgT/+7CuGy9+r+5gITvP62j3RmaD5Ph65UaERdOSRGUzZtgw==", "funding": [ { "type": "individual", @@ -1010,9 +1011,9 @@ } }, "node_modules/ip": { - "version": "2.0.0", - "resolved": "https://registry.npmjs.org/ip/-/ip-2.0.0.tgz", - "integrity": "sha512-WKa+XuLG1A1R0UWhl2+1XQSi+fZWMsYKffMZTTYsiZaUD8k2yDAj5atimTUD2TZkyCkNEeYE5NhFZmupOGtjYQ==" + "version": "2.0.1", + "resolved": "https://registry.npmjs.org/ip/-/ip-2.0.1.tgz", + "integrity": "sha512-lJUL9imLTNi1ZfXT+DU6rBBdbiKGBuay9B6xGSPVjUeQwaH1RIGqef8RZkUtHioLmSNpPR5M4HVKJGm1j8FWVQ==" }, "node_modules/is-arrayish": { "version": "0.3.2", @@ -1419,6 +1420,11 @@ "resolved": "https://registry.npmjs.org/property-expr/-/property-expr-2.0.5.tgz", "integrity": "sha512-IJUkICM5dP5znhCckHSv30Q4b5/JA5enCtkRHYaOVOAocnH/1BQEYTC5NMfT3AVl/iXKdr3aqQbQn9DxyWknwA==" }, + "node_modules/proxy-from-env": { + "version": "1.1.0", + "resolved": "https://registry.npmjs.org/proxy-from-env/-/proxy-from-env-1.1.0.tgz", + "integrity": "sha512-D+zkORCbA9f1tdWRK0RaCR3GPv50cMxcrz4X8k5LTSUD1Dkw47mKJEZQNunItRTkWwgtaUSo1RVFRIG9ZXiFYg==" + }, "node_modules/punycode": { "version": "2.3.0", "resolved": "https://registry.npmjs.org/punycode/-/punycode-2.3.0.tgz", diff --git a/src/website/tests/package.json b/src/website/tests/package.json index cd8d42913..646ed49df 100644 --- a/src/website/tests/package.json +++ b/src/website/tests/package.json @@ -11,7 +11,7 @@ "@cucumber/messages": "20.0.0", "@cucumber/pretty-formatter": "1.0.0", "amqp-ts": "1.8.0", - "axios": "0.27.2", + "axios": "0.28.0", "chai": "4.3.7", "deep-equal-in-any-order": "1.1.20", "mongoose": "^7.5.3",