Implemented ISMC and SITL environment

.clang-tidy

@@ -15,7 +15,6 @@ Checks: >
   -misc-non-private-member-variables-in-classes,
   -modernize-return-braced-init-list,
   -modernize-use-trailing-return-type,
-  -performance-move-const-arg,
   -readability-braces-around-statements,
   -readability-identifier-length,
   -readability-magic-numbers,
@@ -25,8 +24,6 @@ Checks: >
   -bugprone-narrowing-conversions,
   -bugprone-easily-swappable-parameters,
   -bugprone-implicit-widening-of-multiplication-result,
-  -modernize-avoid-bind,
-  -modernize-use-nodiscard
 WarningsAsErrors: "*"
 CheckOptions:
   - key: readability-braces-around-statements.ShortStatementLines

.devcontainer.json

@@ -7,14 +7,15 @@
       "ROS_DISTRO": "rolling"
     }
   },
-  "remoteUser": "dev",
+  "remoteUser": "blue",
   "runArgs": [
     "--network=host",
     "--cap-add=SYS_PTRACE",
     "--security-opt=seccomp:unconfined",
     "--security-opt=apparmor:unconfined",
     "--volume=/dev:/dev",
-    "--privileged"
+    "--privileged",
+    "--volume=/run/user/1000:/run/user/1000"
   ],
   "containerEnv": {
     "DISPLAY": "${localEnv:DISPLAY}",
@@ -30,12 +31,15 @@
           "*.world": "xml",
           "*.xacro": "xml",
           "*.srdf": "xml",
-          "*.rviz": "yaml"
+          "*.rviz": "yaml",
+          "*.config": "xml",
+          "*.sdf": "xml"
         },
         "terminal.integrated.defaultProfile.linux": "bash",
         "files.insertFinalNewline": true,
         "files.trimTrailingWhitespace": true,
         "editor.formatOnSave": true,
+        "editor.tabSize": 2,
         "xml.format.maxLineWidth": 100,
         "json.format.enable": true,
         "python.linting.enabled": true,
@@ -45,14 +49,20 @@
         "python.linting.mypyEnabled": true,
         "python.formatting.provider": "black",
         "autoDocstring.startOnNewLine": false,
-        "autoDocstring.docstringFormat": "google",
+        "autoDocstring.docstringFormat": "google-notypes",
         "isort.args": ["--profile", "black"],
         "isort.check": true,
         "python.autoComplete.extraPaths": [
-          "/opt/ros/humble/lib/python3.10/site-packages/"
+          "/opt/ros/humble/lib/python3.10/site-packages/",
+          "/opt/ros/humble/local/lib/python3.10/dist-packages/",
+          "/opt/ros/rolling/lib/python3.10/site-packages/",
+          "/opt/ros/rolling/local/lib/python3.10/dist-packages/"
         ],
         "python.analysis.extraPaths": [
-          "/opt/ros/humble/lib/python3.10/site-packages/"
+          "/opt/ros/humble/lib/python3.10/site-packages/",
+          "/opt/ros/humble/local/lib/python3.10/dist-packages/",
+          "/opt/ros/rolling/lib/python3.10/site-packages/",
+          "/opt/ros/rolling/local/lib/python3.10/dist-packages/"
         ],
         "C_Cpp.default.intelliSenseMode": "linux-gcc-x86",
         "C_Cpp.clang_format_fallbackStyle": "Google",
@@ -84,9 +94,6 @@
         "[xml]": {
           "editor.defaultFormatter": "redhat.vscode-xml"
         },
-        "[yaml]": {
-          "editor.defaultFormatter": "redhat.vscode-yaml"
-        },
         "[markdown]": {
           "editor.rulers": [80],
           "editor.defaultFormatter": "DavidAnson.vscode-markdownlint"
@@ -114,5 +121,6 @@
         "ms-python.flake8"
       ]
     }
-  }
+  },
+  "postStartCommand": "vcs import < blue.repos"
 }

.docker/Dockerfile

@@ -4,12 +4,15 @@ FROM ros:$ROS_DISTRO-ros-base as ci
 LABEL maintainer="Evan Palmer"
 LABEL maintainer-email="evanp922@gmail.com"
 
+ENV DEBIAN_FRONTEND=noninteractive
 WORKDIR /root/ws_blue
 
 COPY . src/blue
 
-# Install the core apt packages
-RUN apt-get update && apt-get install -y --no-install-recommends \
+# Install apt packages
+RUN apt-get -q update \
+    && apt-get -q -y upgrade \
+    && apt-get -q install --no-install-recommends -y \
     git \
     wget \
     curl \
@@ -18,32 +21,147 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
     clang-format-14 \
     clang-tidy \
     clang-tools \
+    python3-pip \
+    python3-dev \
     software-properties-common \
     && apt-get autoremove -y \
     && apt-get clean -y \
     && rm -rf /var/lib/apt/lists/*
 
+# Install Qualisys Python SDK
+# This isn't available as a rosdep, so we need to install
+# it manually
+RUN python3 -m pip install qtm
+
+# Install gstreamer
+RUN apt-get -q update \
+    && apt-get -q -y upgrade \
+    && apt-get -q install --no-install-recommends -y \
+    python3-gi \
+    gstreamer1.0-tools \
+    gir1.2-gstreamer-1.0 \
+    gir1.2-gst-plugins-base-1.0 \
+    gstreamer1.0-plugins-good \
+    gstreamer1.0-plugins-ugly \
+    gstreamer1.0-plugins-bad \
+    gstreamer1.0-libav \
+    && apt-get autoremove -y \
+    && apt-get clean -y \
+    && rm -rf /var/lib/apt/lists/*
+
 # Install all ROS dependencies
-RUN apt-get update \
+RUN apt-get -q update \
+    && apt-get -q -y upgrade \
     && rosdep update \
-    && DEBIAN_FRONTEND=noninteractive \
-    rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false \
+    && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false \
     && rm -rf src \
     && apt-get autoremove -y \
     && apt-get clean -y \
     && rm -rf /var/lib/apt/lists/*
 
-FROM ci as source
+# Configure a new non-root user
+ARG USERNAME=blue
+ARG USER_UID=1000
+ARG USER_GID=$USER_UID
 
+RUN groupadd --gid $USER_GID $USERNAME \
+    && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \
+    && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \
+    && chmod 0440 /etc/sudoers.d/$USERNAME \
+    && echo "source /usr/share/bash-completion/completions/git" >> /home/$USERNAME/.bashrc
+
+# Switch to the non-root user to install MAVROS dependencies
+USER $USERNAME
+ENV USER=$USERNAME
+
+# Install MAVROS dependencies
+WORKDIR /home/$USERNAME
+RUN wget https://raw.githubusercontent.com/mavlink/mavros/ros2/mavros/scripts/install_geographiclib_datasets.sh \
+    && chmod +x install_geographiclib_datasets.sh \
+    && sudo ./install_geographiclib_datasets.sh
+
+# Switch back to root user
+USER root
+ENV USER=root
+
+FROM ci as sim
+
+# Install Gazebo Garden: https://gazebosim.org/docs/garden/install_ubuntu
+RUN wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
+RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
+RUN apt-get update \
+    && apt-get -y --quiet --no-install-recommends install \
+    gz-garden \
+    && apt-get autoremove -y \
+    && apt-get clean -y \
+    && rm -rf /var/lib/apt/lists/*
+
+# Install ArduPilot and ardupilot_gazebo dependencies
+RUN apt-get -q update \
+    && apt-get -q -y upgrade \
+    && apt-get -q install --no-install-recommends -y \
+    python3-wxgtk4.0 \
+    rapidjson-dev \
+    xterm \
+    && apt-get autoremove -y \
+    && apt-get clean -y \
+    && rm -rf /var/lib/apt/lists/*
+
+# Switch to the non-root user to install ArduSub
+USER $USERNAME
+ENV USER=$USERNAME
+
+# Clone ArduSub
+# ArduSub is installed for simulation purposes ONLY
+# When deployed onto hardware, the native installation of ArduSub
+# (on the FCU) will be used.
+WORKDIR /home/$USERNAME
+RUN git clone https://github.com/ArduPilot/ardupilot.git --recurse-submodules
+
+# Install ArduSub dependencies
+WORKDIR /home/$USERNAME/ardupilot
+ENV SKIP_AP_EXT_ENV=1 SKIP_AP_GRAPHIC_ENV=1 SKIP_AP_COV_ENV=1 SKIP_AP_GIT_CHECK=1
+RUN Tools/environment_install/install-prereqs-ubuntu.sh -y
+
+# Build ArduSub
+WORKDIR /home/$USERNAME/ardupilot
+RUN modules/waf/waf-light configure --board sitl \
+    && modules/waf/waf-light build --target bin/ardusub
+
+# Clone ardupilot_gazebo code
+WORKDIR /home/$USERNAME
+RUN git clone https://github.com/ArduPilot/ardupilot_gazebo.git
+
+# Install ardupilot_gazebo plugin
+RUN [ "/bin/bash" , "-c" , " \
+    cd ardupilot_gazebo \
+    && mkdir build \
+    && cd build \
+    && cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo \
+    && make -j4" ]
+
+# Configure the ardupilot_gazebo environment variables
+RUN echo 'export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build:${GZ_SIM_SYSTEM_PLUGIN_PATH}' >> /home/$USERNAME/.bashrc \
+    && echo 'export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:${GZ_SIM_RESOURCE_PATH}' >> /home/$USERNAME/.bashrc
+
+# Switch back to the root user
+USER root
+ENV USER=root
+
+FROM sim as source
+
+ENV GZ_VERSION=garden
 ENV ROS_UNDERLAY /root/ws_blue/install
+ENV DEBIAN_FRONTEND=noninteractive
 WORKDIR $ROS_UNDERLAY/..
 
 COPY . src/blue
 
-RUN apt-get update \
+RUN apt-get -q update \
+    && apt-get -q -y upgrade \
+    && vcs import src < src/blue/blue.repos \
     && rosdep update \
-    && DEBIAN_FRONTEND=noninteractive \
-    rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false \
+    && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false --skip-keys="gz-transport12 gz-sim7 gz-math7 gz-msgs9" \
     && apt-get autoremove -y \
     && apt-get clean -y \
     && rm -rf /var/lib/apt/lists/*
@@ -56,26 +174,29 @@ RUN . "/opt/ros/${ROS_DISTRO}/setup.sh" \
     # Update /ros_entrypoint.sh to source the new workspace
     && sed -i "s#/opt/ros/\$ROS_DISTRO/setup.bash#$ROS_UNDERLAY/setup.sh#g" /ros_entrypoint.sh
 
-FROM ci as develop
+FROM sim as develop
 
+ENV GZ_VERSION=garden
 ENV ROS_UNDERLAY /root/ws_blue/install
+ENV DEBIAN_FRONTEND=noninteractive
 WORKDIR $ROS_UNDERLAY/..
 
 COPY . src/blue
 
-RUN apt-get update \
+RUN apt-get -q update \
+    && apt-get -q -y upgrade \
+    && vcs import src < src/blue/blue.repos \
     && rosdep update \
-    && DEBIAN_FRONTEND=noninteractive \
-    rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false \
+    && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false --skip-keys="gz-transport12 gz-sim7 gz-math7 gz-msgs9" \
     && rm -rf src \
     && apt-get autoremove -y \
     && apt-get clean -y \
     && rm -rf /var/lib/apt/lists/*
 
 # Install development tools
-RUN apt-get update && apt-get install -y --no-install-recommends \
-    python3-dev \
-    python3-pip \
+RUN apt-get -q update \
+    && apt-get -q -y upgrade \
+    && apt-get -q install --no-install-recommends -y \
     iputils-ping \
     net-tools \
     gdb \
@@ -84,27 +205,15 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
     && rm -rf /var/lib/apt/lists/*
 
 # Install debugging/linting Python packages
-RUN pip3 install \
+RUN python3 -m pip install \
     pre-commit \
     mypy \
     isort \
     flake8 \
-    black
-
-# Configure a new non-root user
-ARG USERNAME=dev
-ARG USER_UID=1000
-ARG USER_GID=$USER_UID
-
-RUN groupadd --gid $USER_GID $USERNAME \
-    && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \
-    && apt-get update \
-    && apt-get install -y sudo \
-    && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \
-    && chmod 0440 /etc/sudoers.d/$USERNAME \
-    && rm -rf /var/lib/apt/lists/* \
-    && echo "source /usr/share/bash-completion/completions/git" >> /home/$USERNAME/.bashrc \
-    && echo "if [ -f /opt/ros/${ROS_DISTRO}/setup.bash ]; then source /opt/ros/${ROS_DISTRO}/setup.bash; fi" >> /home/$USERNAME/.bashrc
+    black \
+    setuptools==58.2.0
 
 ARG WORKSPACE
-RUN echo "if [ -f ${WORKSPACE}/install/setup.bash ]; then source ${WORKSPACE}/install/setup.bash; fi" >> /home/$USERNAME/.bashrc
+RUN echo "if [ -f ${WORKSPACE}/install/setup.bash ]; then source ${WORKSPACE}/install/setup.bash; fi" >> /home/$USERNAME/.bashrc \
+    && echo "if [ -f /opt/ros/${ROS_DISTRO}/setup.bash ]; then source /opt/ros/${ROS_DISTRO}/setup.bash; fi" >> /home/$USERNAME/.bashrc \
+    && echo "source ${WORKSPACE}/env.sh" >> /home/$USERNAME/.bashrc

.dockerignore

@@ -1,2 +1,11 @@
 # ignore everything
 *
+
+# Except the following
+!blue_dynamics
+!blue_manager
+!blue_control
+!blue_bringup
+!blue_localization
+!blue_utils
+!blue.repos

.github/PULL_REQUEST_TEMPLATE.md

@@ -19,11 +19,6 @@ toward closing.
 
 - Fixes # (issue)
 
-## Files Changed
-
-Please provide a list of all files changed and a summary of the changes made to
-the respective files.
-
 ## Testing
 
 Please provide a clear and concise description of the testing performed.

.github/workflows/ci.yaml

@@ -20,6 +20,8 @@ jobs:
         env:
           - IMAGE: rolling-ci
             ROS_DISTRO: rolling
+          - IMAGE: iron-ci
+            ROS_DISTRO: iron
           - IMAGE: humble-ci
             ROS_DISTRO: humble
     steps:

.github/workflows/docker.yaml

@@ -61,7 +61,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        ROS_DISTRO: [humble, rolling]
+        ROS_DISTRO: [humble, iron, rolling]
     runs-on: ubuntu-latest
     permissions:
       packages: write

.gitignore

@@ -42,3 +42,13 @@ log/
 # python
 .mypy_cache/
 __pycache__
+
+# workspace.repos
+ros_gz/
+
+# ArduSub Sim
+eeprom.bin
+mav.parm
+mav.tlog
+mav.tlog.raw
+logs/