Merge pull request #26 from hmakelin/hmakelin-update-px4-v1.14

Update defaults for PX4 v1.14

Author: hmakelin

README.md

@@ -7,44 +7,47 @@ https://user-images.githubusercontent.com/22712178/187902004-480397cc-460f-4d57-
 
 GISNav is a ROS 2 package that enables map-based visual navigation for airborne drones **in a simulation environment**.
 
-GISNav provides an *accurate* **global** position for an airborne drone by visually comparing frames from the drone's 
-nadir-facing camera to a map of the drone's *approximate* global position retrieved from an underlying 
-GIS system.
+GISNav provides a *precise* global position for an airborne drone by visually comparing frames from the drone's 
+nadir-facing camera to a map of the drone's *approximate* global position retrieved from an underlying GIS system.
 
 # Mock GPS Example
 
-The below steps demonstrate how GISNav's `MockGPSNode` ROS 2 node enables GNSS-free flight with PX4 Autopilot's 
-[Mission mode][1] in a SITL simulation.
+The below steps demonstrate how GISNav enables GNSS-free flight with PX4 Autopilot's [Mission mode][1] in a SITL 
+simulation.
 
-You will need to have [NVIDIA Container Toolkit][2] for Docker installed.
+You will need to have the [docker compose plugin][2] and [NVIDIA Container Toolkit][3] installed.
 
 [1]: https://docs.px4.io/v1.12/en/flight_modes/mission.html
-
-[2]: https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/install-guide.html
+[2]: https://docs.docker.com/compose/install/linux/
+[3]: https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/install-guide.html
 
 ## Build and run SITL simulation
 
+Build the Docker images:
 ```bash
-git clone https://github.com/hmakelin/gisnav-docker.git
-cd gisnav-docker
-docker-compose build sitl
+git clone https://github.com/hmakelin/gisnav.git
+cd gisnav
+docker compose build px4 qgc mapserver micro-ros-agent gisnav
 ```
 
-Once the `sitl` image has been built, run the `mapserver` and `sitl` services:
-
+Run GISNav along with supporting services:
 ```bash
-docker-compose up -d mapserver sitl
+docker compose up px4 qgc mapserver micro-ros-agent gisnav 
 ```
 
-> **Note**:
-> * The build for the `sitl` image takes a long time.
-> * The `mapserver` container needs to download roughly 1 GB of high-resolution aerial imagery, so it may take some 
->   time until it starts serving the WMS endpoint.
-> * Prebuilt [sitl][4] and [mapserver][5] images for the demo are available in Docker Hub in case you have problems with
->   the build process.
+> **Note**
+> * The build for the `px4` and `gisnav` images may take a long time.
+> * The `mapserver` container needs to download roughly 1 GB of high-resolution aerial imagery when ran for the first 
+>   time, so it may take some time until it starts serving the WMS endpoint.
+> * If the Gazebo and QGroundControl windows do not appear on your screen you may need to expose your ``xhost`` to your 
+>   Docker containers (see e.g. [ROS GUI Tutorial][4]):
+>   ```bash
+>   for containerId in $(docker ps -f name=gisnav -q); do
+>     xhost +local:$(docker inspect --format='{{ .Config.Hostname }}' $containerId)
+>   done
+>   ```
 
-[4]: https://hub.docker.com/r/hmakelin/gisnav-sitl
-[5]: https://hub.docker.com/r/hmakelin/gisnav-mapserver
+[4]: http://wiki.ros.org/docker/Tutorials/GUI
 
 ## Upload flight plan via QGroundControl
 
@@ -54,11 +57,9 @@ Docker container, and then start the mission.
 
 ## Simulate GPS failure
 
-> **Warning** Do not attempt this on a real flight - simulation use only.
-
 Wait until the drone has risen to its final mission altitude. You should see a visualization of the GISNav-estimated 
-field of view projected on the ground appear. You can then try disabling GPS from the *nsh* console running on the drone
-through your [MAVLink Shell][6] *(accessible e.g. through QGroundControl > Analyze Tools > MAVLink Console)*:
+field of view projected on the ground appear. You can then try disabling GPS through your [MAVLink Shell][5] 
+*(accessible e.g. through QGroundControl > Analyze Tools > MAVLink Console)*:
 
 ```
 failure gps off
@@ -75,14 +76,14 @@ listener sensor_gps
 If the printed GPS message has a `satellites_used` field value of `255`, your PX4 is receiving the mock GPS node output 
 as expected.
 
-[6]: https://docs.px4.io/main/en/debug/mavlink_shell.html#qgroundcontrol
+[5]: https://docs.px4.io/main/en/debug/mavlink_shell.html#qgroundcontrol
 
 # Documentation
 
-See the [latest developer documentation][7] for information on how to setup a local environment for GISNav development, 
+See the [latest developer documentation][6] for information on how to setup a local environment for GISNav development, 
 for code examples and API documentation, and for contribution guidelines.
 
-[7]: https://hmakelin.github.io/gisnav
+[6]: https://hmakelin.github.io/gisnav
 
 # License
 

docker-compose.ardupilot.yaml

@@ -0,0 +1,5 @@
+version: "3.4"
+
+services:
+  gisnav:
+    command: bash -c "source ~/colcon_ws/install/setup.bash && ros2 launch gisnav ardupilot.launch.py"

docker-compose.yaml

@@ -0,0 +1,123 @@
+version: "3.4"
+
+services:
+  mapserver:
+    image: camptocamp/mapserver
+    network_mode: host
+    environment:
+      - NAIP_GDOWN_ID=16M_kbsLpF3t87KC2n9YgqGEBA5h0lG7U
+      - OSM_GDOWN_ID=1snGYjWxs71m6I-qxKsmUzch_bAfQtrEW
+    volumes:
+      - $PWD/docker/mapserver/mapfiles/mapserver.map:/etc/mapserver/mapserver.map:ro
+      - $PWD/docker/mapserver/setup_mapserver.sh:/etc/mapserver/setup_mapserver.sh:ro
+    entrypoint: /etc/mapserver/setup_mapserver.sh
+
+  mapproxy:
+    build:
+      context: docker/mapproxy
+      dockerfile: Dockerfile
+    network_mode: host
+
+  micro-ros-agent:
+    image: microros/micro-ros-agent:foxy
+    command: udp4 -p 8888
+    network_mode: host
+    volumes:
+      - /dev/shm:/dev/shm
+
+  mavros:
+    build:
+      context: docker/mavros
+      dockerfile: Dockerfile
+    environment:
+      - FASTRTPS_DEFAULT_PROFILES_FILE=/disable_shared_memory.xml
+    network_mode: host
+    volumes:
+      - /dev/shm:/dev/shm
+
+  qgc:
+    build:
+      context: docker/qgc
+      dockerfile: Dockerfile
+    environment:
+      - QT_X11_NO_MITSHM=1
+      - DISPLAY=${DISPLAY}
+    volumes:
+      - /tmp/.X11-unix:/tmp/.X11-unix:ro
+      - /dev/shm:/dev/shm
+      - /dev/dri:/dev/dri
+    network_mode: host
+    stdin_open: true
+    tty: true
+
+  gisnav:
+    build:
+      context: docker/gisnav
+      dockerfile: Dockerfile
+    environment:
+      - FASTRTPS_DEFAULT_PROFILES_FILE=/disable_shared_memory.xml
+    volumes:
+      - /dev/dri:/dev/dri
+    network_mode: host
+    stdin_open: true
+    tty: true
+    deploy:
+      resources:
+        reservations:
+          devices:
+            - driver: nvidia
+              count: 1
+              capabilities: [ gpu ]
+    command: bash -c "source /opt/ros/foxy/setup.bash && source ~/colcon_ws/install/setup.bash && ros2 launch gisnav px4.launch.py"
+
+  px4:
+    build:
+      context: docker/px4
+      dockerfile: Dockerfile
+    environment:
+      - QT_X11_NO_MITSHM=1
+      - DISPLAY=${DISPLAY}
+      - GAZEBO_HEADLESS=${GAZEBO_HEADLESS}
+    volumes:
+      - /tmp/.X11-unix:/tmp/.X11-unix:ro
+      - /dev/shm:/dev/shm
+      - /dev/dri:/dev/dri
+    network_mode: host
+    stdin_open: true
+    tty: true
+    deploy:
+      resources:
+        reservations:
+          devices:
+            - driver: nvidia
+              count: 1
+              capabilities: [ gpu ]
+
+  #ardupilot:
+  #  build:
+  #    context: https://github.com/ArduPilot/ardupilot.git
+  #    dockerfile: https://raw.githubusercontent.com/ArduPilot/ardupilot/master/Dockerfile
+
+  ardupilot:
+    build:
+      context: docker/ardupilot
+      dockerfile: Dockerfile
+    environment:
+      - QT_X11_NO_MITSHM=1
+      - DISPLAY=${DISPLAY}
+      - GAZEBO_HEADLESS=${GAZEBO_HEADLESS}
+    volumes:
+      - /tmp/.X11-unix:/tmp/.X11-unix:ro
+      - /dev/shm:/dev/shm
+      - /dev/dri:/dev/dri
+    network_mode: host
+    stdin_open: true
+    tty: true
+    #privileged: True
+    deploy:
+      resources:
+        reservations:
+          devices:
+            - driver: nvidia
+              count: 1
+              capabilities: [ gpu ]

docker/ardupilot/Dockerfile

@@ -0,0 +1,67 @@
+FROM ros:foxy
+
+LABEL maintainer="Harri Makelin <hmakelin@protonmail.com>"
+
+ARG USERNAME=gisnav
+ARG UID=1000
+ARG GID=$UID
+
+# Set 'gisnav' as default user and add it to sudo'ers
+RUN apt-get update --fix-missing && \
+    apt-get install -y sudo wget && \
+    groupadd --gid $GID $USERNAME && useradd --uid $UID --gid $GID -m $USERNAME && \
+    echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/$USERNAME
+USER $USERNAME
+
+# Install required tools
+RUN sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main"  \
+      > /etc/apt/sources.list.d/gazebo-stable.list' && \
+    wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - && \
+    sudo apt-get update && \
+    sudo DEBIAN_FRONTEND=noninteractive apt-get -y install git gitk git-gui python3-pip python-is-python3 gazebo11 \
+      gazebo11-common libgazebo11-dev libgazebo11 ros-foxy-gazebo-ros-pkgs ros-foxy-gazebo-plugins ros-foxy-gazebo-dev \
+      ros-foxy-gazebo-ros && \
+    pip install --upgrade pymavlink MAVProxy pexpect
+
+# Clone ArduPilot and update submodules
+RUN cd $HOME && \
+    git clone https://github.com/ArduPilot/ardupilot && \
+    cd ardupilot && \
+    git submodule update --init --recursive
+
+# Install ArduPilot prerequisites
+# The script requires the USER env variable be set to ${USERNAME}
+SHELL ["/bin/bash", "-c"]
+RUN cd $HOME/ardupilot/ && \
+    export USER=$USERNAME && \
+    sudo DEBIAN_FRONTEND=noninteractive apt-get install -y keyboard-configuration && \
+    Tools/environment_install/install-prereqs-ubuntu.sh -y
+
+# Update PATH
+ENV PATH="/usr/lib/ccache:/home/$USERNAME/.local/bin:${PATH}:/home/$USERNAME/ardupilot/Tools/autotest"
+
+# Install Gazebo plugin
+RUN cd $HOME && \
+    git clone https://github.com/hmakelin/ardupilot_gazebo && \
+    cd ardupilot_gazebo && \
+    mkdir build && \
+    cd build && \
+    cmake .. && \
+    make -j4 && \
+    sudo make install && \
+    echo 'source /usr/share/gazebo/setup.sh' >> ~/.bashrc && \
+    echo 'export GAZEBO_MODEL_PATH=~/ardupilot_gazebo/models' >> ~/.bashrc && \
+    echo 'export GAZEBO_RESOURCE_PATH=~/ardupilot_gazebo/worlds:${GAZEBO_RESOURCE_PATH}' >> ~/.bashrc
+
+# Make initial build
+RUN cd $HOME/ardupilot && \
+    source ~/.profile && \
+    $(sim_vehicle.py -v ArduCopter -f gazebo-iris -L KSQL_Airport | grep -m 1 "BUILD SUMMARY") || echo "Gazebo built."
+
+# Copy and apply configuration files, add SITL simulation demo location
+COPY * /
+RUN sudo mv -t $HOME/ /gazebo-iris.parm /Makefile && \
+    cat $HOME/gazebo-iris.parm >> $HOME/ardupilot/Tools/autotest/default_params/gazebo-iris.parm && \
+    echo "KSQL_Airport=37.523640,-122.255122,1.7,90" >> $HOME/ardupilot/Tools/autotest/locations.txt
+
+CMD cd $HOME && make sim_vehicle

docker/ardupilot/Makefile

@@ -0,0 +1,14 @@
+SHELL:=/bin/bash
+
+sim_vehicle: gazebo
+	sim_vehicle.py -v ArduCopter -f gazebo-iris -L KSQL_Airport -m '--cmd="module load GPSInput"'
+
+gazebo:
+	cd ${HOME} && \
+	if [ -z ${GAZEBO_HEADLESS} ]; then \
+		echo "Launching Gazebo client & server..."; \
+		gazebo --verbose worlds/ksql_airport.world & \
+	else \
+		echo "Launching Gazebo in headless mode..."; \
+		gzserver --verbose worlds/ksql_airport.world & \
+	fi

docker/ardupilot/gazebo-iris.parm

@@ -0,0 +1,13 @@
+# SIMULATION USE ONLY
+# ArduPilot mock GPS SITL simulation demo default parameters
+# Disable GPS arming check (GPS2 will not work until already flying) and use best GPS
+# Use MAVLink (14) for second GPS type
+# Use 2D position only with EK2 (no velocity estimates)
+ARMING_CHECK 0
+AHRS_EKF_TYPE 2
+EK2_ENABLE 1
+EK3_ENABLE 0
+EK2_GPS_TYPE 2
+GPS_TYPE2 14
+SIM_GPS2_TYPE 14
+GPS_AUTO_SWITCH 1

docker/gisnav/Dockerfile

@@ -0,0 +1,53 @@
+FROM ros:foxy
+
+ARG USERNAME=gisnav
+ARG UID=1000
+ARG GID=$UID
+
+RUN apt-get update --fix-missing && \
+    apt-get install sudo && \
+    groupadd --gid $GID $USERNAME && useradd --uid $UID --gid $GID -m $USERNAME && \
+    echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/$USERNAME
+USER $USERNAME
+
+# Install GISNav & colcon dependencies
+RUN mkdir -p $HOME/colcon_ws/src && \
+    cd $HOME/colcon_ws/src && \
+    git clone https://github.com/hmakelin/gisnav.git && \
+    git clone https://github.com/hmakelin/gisnav_msgs.git && \
+    git clone https://github.com/px4/px4_msgs.git && \
+    rosdep update && \
+    rosdep install --from-paths . -y --ignore-src
+
+# Install LoFTR
+RUN cd $HOME/colcon_ws/src/gisnav && \
+    git submodule update --init gisnav/pose_estimators/third_party/LoFTR && \
+    sudo apt-get -y install python3-pip && \
+    pip3 install gdown && \
+    mkdir weights && \
+    cd weights && \
+    $HOME/.local/lib/$USERNAME/gdown https://drive.google.com/uc?id=1M-VD35-qdB5Iw-AtbDBCKC7hPolFW9UY
+
+# Python dependencies
+RUN cd $HOME/colcon_ws/src/gisnav && \
+    pip3 install -r requirements.txt  && \
+    pip3 install --upgrade numpy python-dateutil
+
+# GeographicLib
+RUN sudo apt-get -y install ros-foxy-mavros ros-foxy-mavros-extras && \
+    cd $HOME && \
+    wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh && \
+    sudo bash ./install_geographiclib_datasets.sh
+
+# Build
+SHELL ["/bin/bash", "-c"]
+RUN cd $HOME/colcon_ws && \
+    source /opt/ros/foxy/setup.bash && \
+    colcon build
+
+# Clean
+RUN sudo apt-get clean && \
+    sudo rm -rf /var/lib/apt/lists/*
+
+COPY * /
+RUN sudo chmod 755 /disable_shared_memory.xml