Add Dockerfile (clydemcqueen#6)

* Fix a bug in install instructions: we must build ros_gz from source to use Gazebo Garden with Humble

* Small fixes to launch file, docs

* Add Dockerfile

* Build mavros from src, works better in a docker container (?)

* Remove exec_depend on mavros and ros_gz to avoiding building in CI

* ardupilot_gazebo now defaults to Gazebo Garden

* Add dependency on mavros_extras; use MAVROS binaries

.dockerignore

@@ -0,0 +1,3 @@
+.idea/
+cmake-build-*
+.cmake-build-*

README.md

@@ -34,12 +34,15 @@ planning and navigation. Several simple Nav2 plugins are provided to work in a 3
 
 See [orca_nav2](orca_nav2/README.md) for details.
 
-## Requirements
+## Installation
+
+See the [Dockerfile](docker/Dockerfile) for installation details.
 
+Install these packages:
 * [ROS2 Humble](https://docs.ros.org/en/humble/Installation.html)
 * [Gazebo Garden 7.1.0](https://gazebosim.org/docs/garden/install)
 * [ros_gz, built from source on the ros2 branch](https://github.com/gazebosim/ros_gz)
-* [ardupilot_gazebo, built from source on the ignition-garden branch](https://github.com/ArduPilot/ardupilot_gazebo/tree/ignition-garden)
+* [ardupilot_gazebo](https://github.com/ArduPilot/ardupilot_gazebo)
 * [ArduSub](https://ardupilot.org/dev/docs/building-setup-linux.html)
 
 Build ArduSub for SITL:
@@ -49,8 +52,6 @@ cd ~/ardupilot
 ./waf sub
 ~~~
 
-## Installation
-
 Populate the workspace:
 ~~~
 mkdir -p ~/colcon_ws/src
@@ -59,10 +60,10 @@ git clone https://github.com/clydemcqueen/orca4
 vcs import < orca4/workspace.repos
 ~~~
 
-Get dependencies:
+Get dependencies, ignoring Gazebo Garden rosdep keys:
 ~~~
 rosdep update
-rosdep install -y --from-paths . --ignore-src
+rosdep install -y --from-paths . --ignore-src --skip-keys="gz-transport12 gz-sim7 gz-math7 gz-msgs9"
 ~~~
 
 MAVROS depends on GeographicLib, and GeographicLib needs some datasets:

docker/Dockerfile

@@ -0,0 +1,158 @@
+FROM osrf/ros:humble-desktop AS base
+
+ARG USERNAME=orca4
+ARG USER_UID=1000
+ARG USER_GID=$USER_UID
+ARG DEBIAN_FRONTEND=noninteractive
+
+RUN apt-get update && apt-get upgrade -y
+
+# Install a few handy tools
+RUN apt-get update \
+  && apt-get -y --quiet --no-install-recommends install \
+    bash-completion \
+    build-essential \
+    git \
+    glmark2 \
+    gnupg \
+    iputils-ping \
+    lsb-release \
+    mlocate \
+    software-properties-common \
+    sudo \
+    wget \
+    vim \
+  && rm -rf /var/lib/apt/lists/*
+
+# Install Gazebo Garden
+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 \
+  && rm -rf /var/lib/apt/lists/*
+
+# Install NVIDIA software
+RUN apt-get update \
+ && apt-get -y --quiet --no-install-recommends install \
+    libglvnd0 \
+    libgl1 \
+    libglx0 \
+    libegl1 \
+    libxext6 \
+    libx11-6 \
+  && rm -rf /var/lib/apt/lists/* \
+ENV NVIDIA_VISIBLE_DEVICES=all
+ENV NVIDIA_DRIVER_CAPABILITIES=graphics,utility,compute
+ENV QT_X11_NO_MITSHM=1
+
+# Install some ardupilot and ardupilot_gazebo prereqs
+RUN apt-get update \
+ && apt-get -y --quiet --no-install-recommends install \
+    python3-wxgtk4.0 \
+    rapidjson-dev \
+    xterm \
+  && rm -rf /var/lib/apt/lists/*
+
+# Create a non-root user
+# Required for ArduSub install, but generally a good idea
+RUN groupadd --gid $USER_GID $USERNAME \
+  && useradd -s /bin/bash --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 "\n# Added by orca4 Dockerfile:" >> /home/$USERNAME/.bashrc \
+  && echo "source /usr/share/bash-completion/completions/git" >> /home/$USERNAME/.bashrc
+
+# Switch to our new user
+USER $USERNAME
+ENV USER=$USERNAME
+
+# Clone ArduSub code
+WORKDIR /home/$USERNAME
+RUN git clone https://github.com/ArduPilot/ardupilot.git --recurse-submodules
+
+# Install ArduSub prereqs (this also appends to .bashrc)
+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
+# Note: waf will capture all of the environment variables in ardupilot/.lock-waf_linux_build.
+# Any change to enviroment variables will cause a re-build.
+# To avoid this call sim_vehicle.py with the "--no-rebuild" option.
+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
+
+# Build ardupilot_gazebo
+RUN [ "/bin/bash" , "-c" , " \
+  cd ardupilot_gazebo \
+  && mkdir build \
+  && cd build \
+  && cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo \
+  && make -j4" ]
+
+# Break orca4 build into multiple stages to speed up 'docker build':
+# -- work that depends on workspace.repos
+# -- work that depends on orca4 package.xml files
+# -- everything else
+
+# Create colcon workspace and copy workspace.repos file
+WORKDIR /home/$USERNAME
+RUN mkdir -p colcon_ws/src/orca4
+COPY --chown=$USER_UID:$USER_GID workspace.repos colcon_ws/src/orca4
+
+# Get workspace repos
+WORKDIR /home/$USERNAME/colcon_ws/src
+RUN vcs import < orca4/workspace.repos
+
+# Run rosdep over workspace repos
+# Note: ros_gz asks for some Gazebo Garden keys which are not yet present in rosdistro.
+# This is fine, since Gazebo Garden has already been installed.
+WORKDIR /home/$USERNAME/colcon_ws
+RUN rosdep update \
+  && rosdep install -y --from-paths . --ignore-src --skip-keys="gz-transport12 gz-sim7 gz-math7 gz-msgs9"
+
+# Build everything so far
+RUN [ "/bin/bash" , "-c" , "\
+  source /opt/ros/humble/setup.bash \
+  && colcon build" ]
+
+# Copy orca4 package.xml files
+COPY --chown=$USER_UID:$USER_GID orca_base/package.xml src/orca4/orca_base/
+COPY --chown=$USER_UID:$USER_GID orca_bringup/package.xml src/orca4/orca_bringup/
+COPY --chown=$USER_UID:$USER_GID orca_description/package.xml src/orca4/orca_description/
+COPY --chown=$USER_UID:$USER_GID orca_msgs/package.xml src/orca4/orca_msgs/
+COPY --chown=$USER_UID:$USER_GID orca_nav2/package.xml src/orca4/orca_nav2/
+COPY --chown=$USER_UID:$USER_GID orca_shared/package.xml src/orca4/orca_shared/
+
+# Run rosdep over orca4 package.xml files
+RUN rosdep update \
+  && rosdep install -y --from-paths . --ignore-src --skip-keys="gz-transport12 gz-sim7 gz-math7 gz-msgs9"
+
+# MAVROS depends on GeographicLib, and GeographicLib needs some datasets
+RUN [ "/bin/bash" , "-c" , "\
+  wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh \
+  && chmod +x install_geographiclib_datasets.sh \
+  && sudo ./install_geographiclib_datasets.sh" ]
+
+# Copy everything
+COPY --chown=$USER_UID:$USER_GID . src/orca4
+
+# Build everything
+RUN [ "/bin/bash" , "-c" , "\
+  source /opt/ros/humble/setup.bash \
+  && colcon build" ]
+
+# Set up the environment
+WORKDIR /home/$USERNAME/colcon_ws
+RUN echo "export PATH=/home/$USERNAME/.local/bin:\$PATH" >> /home/$USERNAME/.bashrc \
+  && echo "export PATH=/home/$USERNAME/.local/lib/python3.10/site-packages:\$PATH" >> /home/$USERNAME/.bashrc \
+  && echo "source /home/$USERNAME/colcon_ws/src/orca4/setup.bash" >> /home/$USERNAME/.bashrc
+
+# Required to use the --console option on sim_vehicle.py:
+# RUN pip3 install matplotlib

docker/README.md

@@ -0,0 +1,16 @@
+To build the docker image:
+~~~
+./build.sh
+~~~
+
+To launch Gazebo, RViz, all nodes:
+~~~
+./run.sh
+ros2 launch orca_bringup sim_launch.py
+~~~
+
+To execute a mission:
+~~~
+docker exec -it orca4 /bin/bash
+ros2 run orca_bringup mission_runner.py
+~~~

docker/build.sh

@@ -0,0 +1,7 @@
+#!/usr/bin/env bash
+
+DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
+
+cd $DIR
+
+docker build -f $DIR/Dockerfile -t orca4:latest ..

docker/run.sh

@@ -0,0 +1,33 @@
+#!/usr/bin/env bash
+
+XAUTH=/tmp/.docker.xauth
+if [ ! -f $XAUTH ]
+then
+    xauth_list=$(xauth nlist $DISPLAY)
+    xauth_list=$(sed -e 's/^..../ffff/' <<< "$xauth_list")
+    if [ ! -z "$xauth_list" ]
+    then
+        echo "$xauth_list" | xauth -f $XAUTH nmerge -
+    else
+        touch $XAUTH
+    fi
+    chmod a+r $XAUTH
+fi
+
+# Specific for NVIDIA drivers, required for OpenGL >= 3.3
+docker run -it \
+    --rm \
+    --name orca4 \
+    -e DISPLAY \
+    -e QT_X11_NO_MITSHM=1 \
+    -e XAUTHORITY=$XAUTH \
+    -e NVIDIA_VISIBLE_DEVICES=all \
+    -e NVIDIA_DRIVER_CAPABILITIES=all \
+    -v "$XAUTH:$XAUTH" \
+    -v "/tmp/.X11-unix:/tmp/.X11-unix" \
+    -v "/etc/localtime:/etc/localtime:ro" \
+    -v "/dev/input:/dev/input" \
+    --privileged \
+    --security-opt seccomp=unconfined \
+    --gpus all \
+    orca4:latest

orca_base/src/base_controller.cpp

@@ -264,7 +264,9 @@ class BaseController : public rclcpp::Node
     }
   }
 
-  // Output from ArduSub EKF
+  // Output from ArduSub EKF. Troubleshooting tip: if this message never appears, make sure that
+  // the mavros and mavros_extras packages are installed and that all required MAVROS plugins are
+  // were loaded.
   void ardu_pose_cb(const geometry_msgs::msg::PoseStamped::ConstSharedPtr & msg)
   {
     if (state_ != State::BOOT) {

orca_bringup/README.md

@@ -5,14 +5,23 @@
 Launch ROV or AUV simulation in Gazebo.
 Calls [bringup.py](launch/bringup.py).
 
+To see parameters: `ros2 launch --show-args orca_bringup sim_launch.py`
+
+A few parameters:
+* `bag` bag interesting topics, default false
+* `gzclient` run the Gazebo GUI, default true
+* `rviz` run rviz2, default true
+
+E.g., run headless: `ros2 launch orca_bringup sim_launch.py gzclient:=false rviz:=false`
+
 ### [bringup.py](launch/bringup.py)
 
 Bring up all core ROV and AUV nodes, including ORB_SLAM2 and Nav2.
 Calls [navigation_launch.py](launch/navigation_launch.py).
 
 ### [navigation_launch.py](launch/navigation_launch.py)
 
-Nav2 navigation launch file, copied here so I can change the log output.
+Nav2 navigation launch file, modified to avoid launch the velocity smoother.
 
 ## Video pipeline
 

orca_bringup/launch/sim_launch.py

@@ -213,13 +213,12 @@ def generate_launch_description():
             ],
         ),
 
-        # Publish ground truth poses from Ignition Gazebo
+        # Publish ground truth pose from Ignition Gazebo
         Node(
             package='ros_gz_bridge',
             executable='parameter_bridge',
             arguments=[
                 '/model/orca4/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry',
-                '/world/sand/model/orca4/link/base_link/sensor/imu_sensor/imu@sensor_msgs/msg/Imu[gz.msgs.IMU',
             ],
             output='screen'
         ),

orca_bringup/package.xml

@@ -16,11 +16,14 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <exec_depend>nav2_bringup</exec_depend>
   <exec_depend>mavros</exec_depend>
   <exec_depend>mavros_extras</exec_depend>
-  <exec_depend>nav2_bringup</exec_depend>
-  <exec_depend>ros_gz_bridge</exec_depend>
-  <exec_depend>ros_gz_image</exec_depend>
+
+  <!-- These are required to run, but I don't want to build them in the CI -->
+  <!-- Ideally I would be able to install & use released versions -->
+  <!--<exec_depend>ros_gz_bridge</exec_depend>-->
+  <!--<exec_depend>ros_gz_image</exec_depend>-->
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_cmake_copyright</test_depend>

orca_bringup/params/sim_mavros_params.yaml

@@ -17,6 +17,9 @@ mavros:
     #     local_position handles the LOCAL_POSITION_NED msg (MAV id 32)
     # Also be sure to set the message interval to ~50ms for these 2 messages
 
+    # The vision_pose plugin is provided by mavros_extras.
+    # MAVROS will not complain if this package is missing.
+
     plugin_allowlist:
       - sys_status
       - command

workspace.repos

@@ -1,13 +1,22 @@
 repositories:
+
   bluerov2_ignition:
     type: git
     url: https://github.com/clydemcqueen/bluerov2_ignition
     version: main
+
   orb_slam_2_ros:
     type: git
     url: https://github.com/clydemcqueen/orb_slam_2_ros
     version: orca4_humble
+
   ros2_shared:
     type: git
     url: https://github.com/ptrmu/ros2_shared
     version: master
+
+  # ros_gz supports Humble + Garden only if built from source on the ros2 branch
+  ros_gz:
+    type: git
+    url: https://github.com/gazebosim/ros_gz
+    version: ros2