Integrate support for simulation with Gazebo and ArduSub

.devcontainer.json

@@ -14,7 +14,8 @@
     "--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,7 +31,9 @@
           "*.world": "xml",
           "*.xacro": "xml",
           "*.srdf": "xml",
-          "*.rviz": "yaml"
+          "*.rviz": "yaml",
+          "*.config": "xml",
+          "*.sdf": "xml"
         },
         "terminal.integrated.defaultProfile.linux": "bash",
         "files.insertFinalNewline": true,
@@ -118,5 +121,6 @@
         "ms-python.flake8"
       ]
     }
-  }
+  },
+  "postStartCommand": "vcs import < blue.repos"
 }

.docker/Dockerfile

@@ -29,6 +29,8 @@ RUN apt-get -q update \
     && 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
@@ -64,20 +66,16 @@ 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 upgrade -y \
     && 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
 
-# Switch to the non-root user to install MAVROS dependencies and ArduSub
+# Switch to the non-root user to install MAVROS dependencies
 USER $USERNAME
 ENV USER=$USERNAME
 
-# Set the working directory to the user's home directory
-WORKDIR /home/$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
@@ -86,8 +84,73 @@ RUN wget https://raw.githubusercontent.com/mavlink/mavros/ros2/mavros/scripts/in
 USER root
 ENV USER=root
 
-FROM ci as source
+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/..
@@ -96,8 +159,9 @@ COPY . src/blue
 
 RUN apt-get -q update \
     && apt-get -q -y upgrade \
+    && vcs import src < src/blue/blue.repos \
     && rosdep update \
-    && 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/*
@@ -110,8 +174,9 @@ 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/..
@@ -120,8 +185,9 @@ COPY . src/blue
 
 RUN apt-get -q update \
     && apt-get -q -y upgrade \
+    && vcs import src < src/blue/blue.repos \
     && rosdep update \
-    && 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 \
@@ -149,4 +215,5 @@ RUN python3 -m pip install \
 
 ARG WORKSPACE
 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 "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

@@ -8,3 +8,4 @@
 !blue_msgs
 !blue_bringup
 !blue_localization
+!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,9 @@ log/
 # python
 .mypy_cache/
 __pycache__
+
+# workspace.repos
+ros_gz/
+
+# ArduSub Sim
+eeprom.bin

blue.repos

@@ -0,0 +1,6 @@
+repositories:
+
+  ros_gz:
+    type: git
+    url: https://github.com/gazebosim/ros_gz
+    version: ros2

blue_bringup/config/bluerov2.yaml

@@ -0,0 +1,79 @@
+blue_manager:
+  ros__parameters:
+    num_thrusters: 6
+    mode_change_timeout: 1.0
+    mode_change_retries: 3
+
+ismc:
+  ros__parameters:
+    mass: 10.0
+    buoyancy: 100.06
+    weight: 98.1
+    inertia_tensor_coeff: [0.16, 0.16, 0.16]
+    added_mass_coeff: [-5.50, -12.70, -14.57, -0.12, -0.12, -0.12]
+    linear_damping_coeff: [-4.03, -6.22, -5.18, -0.07, -0.07, -0.07]
+    quadratic_damping_coeff: [-18.18, -21.66, -36.99, -1.55, -1.55, -1.55]
+    center_of_gravity: [0.0, 0.0, 0.0]
+    center_of_buoyancy: [0.0, 0.0, 0.2]
+    ocean_current: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+    num_thrusters: 6
+    tcm: [ 0.707, 0.707,  -0.707, -0.707, 0.0,   0.0,
+          -0.707, 0.707,  -0.707, 0.707,  0.0,   0.0,
+           0.0,   0.0,    0.0,    0.0,    1.0,   1.0,
+           0.051, -0.051, 0.051,  -0.051, 0.111, -0.111,
+           0.051, 0.051,  -0.051, -0.051, 0.002, -0.002,
+          -0.167, 0.167,  0.175,  -0.175, 0.0,   0.0   ]
+    msg_ids: [31, 32]
+    msg_rates: [100.0, 100.0]
+    control_loop_freq: 200.0
+
+aruco_marker_localizer:
+  ros__parameters:
+    camera_matrix:
+      [1078.17559, 0.0,        1010.57086,
+       0.0,        1076.46176, 463.06243,
+       0.0,        0.0,        1.0]
+    distortion_coefficients:
+      [0.019645, 0.007271, -0.004324, -0.001628, 0.000000]
+    projection_matrix:
+      [1108.25366, 0.0,        1003.75555, 0.0,
+       0.0,        1108.39001, 456.92861,  0.0,
+       0.0,        0.0,        1.0,        0.0]
+
+camera:
+  ros__parameters:
+    port: 5600
+
+qualisys_mocap:
+  ros__parameters:
+    ip: "192.168.0.0"
+    port: 22223
+    version: "1.22"
+    body: "bluerov"
+
+qualisys_localizer:
+  ros__parameters:
+    body: "bluerov" # This should be the same as the body parameter setting for the qualisys_mocap node
+
+mavros:
+  ros__parameters:
+    system_id: 255
+    plugin_allowlist:
+      - sys_status
+      - command
+      - imu
+      - local_position
+      - rc_io
+      - param
+      - vision_pose
+
+mavros_node:
+  ros__parameters:
+    fcu_url: "tcp://localhost"
+    gcs_url: "udp://@localhost:14550"
+
+mavros/local_position:
+  ros__parameters:
+    frame_id: "map"
+    tf:
+      send: false