Mavros Update to Official ROS2 Port

.github/workflows/remove_container_on_close.yaml

@@ -21,6 +21,7 @@ jobs:
           - rosbridge-suite
           - starling-controller-base
           - starling-mavros
+          - starling-mavros2
           - starling-vicon
           - starling-sim-base-core
           - starling-sim-base-px4
@@ -39,5 +40,3 @@ jobs:
           organisation: uobflightlabstarling
           image: ${{matrix.image}}
           tag: ${{github.event.pull_request.head.ref || github.event.ref }}
-
-

.github/workflows/update-image.yml

@@ -28,20 +28,22 @@ jobs:
           DEFAULT_PRERELEASE_BUMP: none
           DRY_RUN: true # Dont push new tag here, only push on successful build
           PRERELEASE_SUFFIX: ${{ steps.extract_branch.outputs.branch }}
-      
+
   bake_images_stage_1:
     runs-on: ubuntu-latest
     needs: bump_version
     strategy:
       fail-fast: false
       matrix:
-        include: 
+        include:
           - target: rosbridge-suite
             arm64: false
           - target: starling-controller-base
             arm64: true
           - target: starling-mavros
             arm64: true
+          - target: starling-mavros2
+            arm64: true
           - target: starling-vicon
             arm64: true
           - target: starling-sim-base-core
@@ -98,7 +100,7 @@ jobs:
     needs: [bump_version, bake_images_stage_1]
     strategy:
       matrix:
-        include: 
+        include:
           - target: starling-sim-base-px4
             arm64: false
           - target: starling-sim-px4-sitl
@@ -155,7 +157,7 @@ jobs:
           push: true
           skip_setup: true
 
-    
+
   bake_images_stage_3:
     runs-on: ubuntu-latest
     needs: [bump_version, bake_images_stage_2]
@@ -258,5 +260,3 @@ jobs:
           tag: ${{ needs.bump_version.outputs.tag }}
           name: Release ${{ needs.bump_version.outputs.tag }}
           allowUpdates: true
-
-

buildtools/docker-bake.hcl

@@ -55,12 +55,12 @@ group "system" {
 target "ping-monitor" {
     context = "system/ping_monitor"
     tags = [
-        "${BAKE_REGISTRY}uobflightlabstarling/ping-monitor:${BAKE_VERSION}",
-        notequal("",BAKE_RELEASENAME) ? "${BAKE_REGISTRY}uobflightlabstarling/ping-monitor:${BAKE_RELEASENAME}": "",
+        "${BAKE_REGISTRY}${BAKE_PREFIX}/ping-monitor:${BAKE_VERSION}",
+        notequal("",BAKE_RELEASENAME) ? "${BAKE_REGISTRY}${BAKE_PREFIX}/ping-monitor:${BAKE_RELEASENAME}": "",
         ]
     platforms = ["linux/amd64"]
-    cache-to = [ notequal("",BAKE_CACHETO_NAME) ? "${BAKE_CACHETO_REGISTRY}uobflightlabstarling/ping-monitor:${BAKE_CACHETO_NAME}" : "" ]
-    cache-from = [ notequal("",BAKE_CACHEFROM_NAME) ? "${BAKE_CACHEFROM_REGISTRY}uobflightlabstarling/ping-monitor:${BAKE_CACHEFROM_NAME}" : "" ]
+    cache-to = [ notequal("",BAKE_CACHETO_NAME) ? "type=registry,mode=max,ref=${BAKE_CACHETO_REGISTRY}${BAKE_PREFIX}/ping-monitor:${BAKE_CACHETO_NAME}" : "" ]
+    cache-from = [ notequal("",BAKE_CACHEFROM_NAME) ? "${BAKE_CACHEFROM_REGISTRY}${BAKE_PREFIX}/ping-monitor:${BAKE_CACHEFROM_NAME}" : "" ]
 }
 
 target "rosbridge-suite" {
@@ -96,6 +96,18 @@ target "starling-mavros" {
     cache-from = [ notequal("",BAKE_CACHEFROM_NAME) ? "${BAKE_CACHEFROM_REGISTRY}${BAKE_PREFIX}/starling-mavros:${BAKE_CACHEFROM_NAME}" : "" ]
 }
 
+target "starling-mavros2" {
+    context = "system/mavros2"
+    dockerfile = "Dockerfile"
+    tags = [
+        "${BAKE_REGISTRY}${BAKE_PREFIX}/starling-mavros2:${BAKE_VERSION}",
+        notequal("",BAKE_RELEASENAME) ? "${BAKE_REGISTRY}${BAKE_PREFIX}/starling-mavros2:${BAKE_RELEASENAME}": "",
+        ]
+    platforms = ["linux/amd64", "linux/arm64"]
+    cache-to = [ notequal("",BAKE_CACHETO_NAME) ? "type=registry,mode=max,ref=${BAKE_CACHETO_REGISTRY}${BAKE_PREFIX}/starling-mavros2:${BAKE_CACHETO_NAME}" : "" ]
+    cache-from = [ notequal("",BAKE_CACHEFROM_NAME) ? "${BAKE_CACHEFROM_REGISTRY}${BAKE_PREFIX}/starling-mavros2:${BAKE_CACHEFROM_NAME}" : "" ]
+}
+
 target "mavp2p" {
     context = "system/mavp2p"
     tags = [

system/Makefile

@@ -11,6 +11,9 @@ controller-base:
 mavros:
 	$(BAKE) starling-mavros
 
+mavros2:
+	$(BAKE) starling-mavros2
+
 mavros_all_arch:
 	cd $(MAKEFILE_DIR)/.. && docker buildx bake --builder default --load -f $(BAKE_SCRIPT)
 
@@ -26,4 +29,4 @@ vicon:
 ping-monitor:
 	$(BAKE) ping-monitor
 
-.PHONY: mavros mavros_all_arch controller-base rosbridge-suite vicon mavp2p all ping-monitor
+.PHONY: mavros mavros2 mavros_all_arch controller-base rosbridge-suite vicon mavp2p all ping-monitor

system/mavros2/Dockerfile

@@ -0,0 +1,59 @@
+FROM ros:foxy-ros-base-focal
+
+WORKDIR /ros_ws
+
+# Install mavros from apt
+RUN apt-get update \
+    && apt-get install -y --no-install-recommends \
+        libboost-system-dev \
+        wget \
+        python3-pip \
+        ros-${ROS_DISTRO}-mavros\
+        ros-${ROS_DISTRO}-mavros-extras \
+        ros-${ROS_DISTRO}-mavros-msgs \
+        libgeographic-dev \
+        geographiclib-tools \
+    && rm -rf /var/lib/apt/lists/*
+
+## Ensure geographiclib is properly installed with FindGeographicLib available
+RUN wget https://raw.githubusercontent.com/mavlink/mavros/ros2/mavros/scripts/install_geographiclib_datasets.sh \
+    && bash install_geographiclib_datasets.sh \
+    && ln -s /usr/share/cmake/geographiclib/FindGeographicLib.cmake /usr/share/cmake-3.16/Modules/
+
+COPY ros_entrypoint.sh /ros_entrypoint.sh
+
+COPY mavros_setup.sh .
+
+COPY mavros.launch.xml /ros_ws/launch/mavros.launch.xml
+COPY config/mavros_config_*.yaml /
+COPY config/mavros_pluginlists_*.yaml /
+
+ENV MAVROS_MOD_CONFIG_PATH="/mavros_config_mod.yaml"
+ENV MAVROS_CONFIG_PATH="/mavros_config_px4.yaml"
+ENV MAVROS_MOD_PLUGINLISTS_PATH="/mavros_pluginlists_mod.yaml"
+ENV MAVROS_PLUGINLISTS_PATH="/mavros_pluginlists_px4.yaml"
+
+# Build extra custom nodes in Mavros
+COPY starling_mavros_extras /ros_ws/src/starling_mavros_extras
+RUN . /opt/ros/${ROS_DISTRO}/setup.sh \
+    && export CMAKE_PREFIX_PATH=$AMENT_PREFIX_PATH:$CMAKE_PREFIX_PATH \
+    && cd /ros_ws \
+    && colcon build --packages-select starling_mavros_extras \
+    && rm -r build
+
+# Add custom ROS DDS configuration (force UDP always)
+COPY fastrtps_profiles.xml /ros_ws/
+ENV FASTRTPS_DEFAULT_PROFILES_FILE /ros_ws/fastrtps_profiles.xml
+
+# TODO: Rename to FCU_PROTOCOL
+ENV MAVROS_FCU_CONN="udp"
+# TODO: Rename to FCU_HOST
+ENV MAVROS_FCU_IP="127.0.0.1"
+# TODO: Rename to PORT_BASE
+ENV MAVROS_FCU_UDP_BASE="14830"
+ENV MAVROS_TGT_SYSTEM="auto"
+ENV PX4_INSTANCE_BASE=0
+ENV MAVROS_TGT_FIRMWARE="px4"
+ENV MAVROS_GCS_URL="udp-pb://@:14550"
+
+CMD [ "ros2", "launch", "launch/mavros.launch.xml"]

system/mavros2/README.md

@@ -0,0 +1,147 @@
+# `starling-mavros2` Container
+
+This container is the native ROS2 MAVROS container. Most of the below still holds.
+
+## Contents
+[TOC]
+
+## Launch Process
+
+The initial process of starting the container goes through `ros_entrypoint.sh`. This file sets up both ROS versions and
+runs a setup script which configures the environment to tell MAVROS how to talk to the drone and where to publish its
+topics. The setup script works in a couple of different ways depending on where the container is running.
+
+If the container is running on a drone, it expects to be able to find the `/etc/starling/vehicle.config` file. This file
+contains some information that MAVROS needs to be able to communicate with the flight controller. An example
+`vehicle.config` file is included below.
+
+If the container is started as part of a Kubernetes StatefulSet deployment, the setup script will attemp# Mavros /ROS12 Bridge Container
+
+To run:
+```bash
+make build # Builds docker container
+make run # Runs docker container
+```
+
+Default action of launching the container (make run) will be to run `launch.sh` which will source mavros_setup.sh, and run the launch file.
+
+## Launch file
+
+The launch file `mavros_bridge.launch` runs the following
+Running the container will run 2 ros nodes in parallel:
+1. A Mavros node listening on fcu_url, targeting drone with sysid tgt_system
+   - `roslaunch mavros apm.launch fcu_url:=${MAVROS_FCU_URL} tgt_system:=${MAVROS_TGT_SYSTEM}`
+2. A ros1 bridge node with option to forward all mavros topics to ros2
+   - `ros2 run ros1_bridge dynamic_bridge --bridge-all-1to2-topics`
+
+## Contents
+There are two docker files:
+
+- `Dockerfile.mavros` is the base container which is based on Ubuntu 20.04. It runs ROS2 Foxy, ROS1 Noetic and MAVROS via the ROS1_bridge.
+- `mavros_setup.sh` is sourced at the end of the Dockerfile if any environment variables need modification.
+- `mavros_bridge.launch` is a ros1 launch file, and launches both mavros and the ros1 bridge. It has 4 parameters:
+   - fcu_url - Flight control url, point it at the physical autopilot, ardupilot sitl or px4 sitl, no default in launch file. Defailts to `MAVROS_FCU_URL="udp://127.0.0.1:14550@14555"` using launch.sh
+   - target_system - target id to filter for from mavlink stream, defaults to 1, or MAVROS_TGT_SYSTEM using launch.sh
+   - system_id - sysid of mavros node, defaults to 1
+   - firmware - `px4` or `apm`, name of the mavros launch file. Defaults to `px4`.
+
+## Configuration
+
+We have identified that mavros is a base requirement for any drone running onboard compute in order to communicate with both the autopilot and GCS.
+
+The mavros container exposes an environment variable `MAVROS_FCU_URL` which is passed to mavros's `fcu_url` option for configuring the mavlink connection to mavros. The default value is:
+> `MAVROS_FCU_URL=udp://127.0.0.1:14550@14555`
+
+It also exposes environment variabel `MAVROS_TGT_SYSTEM` which is the `SYSID` of the ardupilot instance that mavros wants to connect to. This is usually an integer value. However, an IP address can also be passed in and `mavros_setup.sh` will extract the last numver of the IPv4 address as the value. Default is 1
+
+To modify this at runtime, pass the `-e MAVROS_FCU_URL=<conenction string>` to `docker run` like so
+> `docker run -e MAVROS_FCU_URL=<conenction string> -e MAVROS_TGT_SYSTEM=<#n>...`
+
+See Mavros for possible connection values
+
+This container builds the ros1_bridge from scratch in order to incorporate the `mavros_msgs` package.
+t to get the
+ordinal of its containing pod from the hostname. It will then use this ordinal to set up the ports and the system ID to
+match those generated by a PX4 SITL instance set up with the same ordinal. If the setup script fails to get the ordinal
+from the hostname, it will attempt to connect to a PX4 SITL with `PX4_INSTANCE=0`.
+
+Once the setup script has run, the default behaviour is to launch the `mavros_bridge.launch.xml` file. This behaviour
+should be usable in almost all cases. This is a __ROS2__ launch file. It instructs `ros2 launch` to run the
+`ros1_bridge` node and an instance of __ROS1__'s `roslaunch`. This in turn launches the __ROS1__ `mavros.launch` file,
+which contains instructions to run the MAVROS node.
+
+The __ROS2__ `mavros_bridge.launch.xml` script defines a set of arguments to enable configuration of the MAVROS node.
+These are ususally filled by the environment variables defined below. If the configurability provided here is
+insufficient, the image can be run with a different command.
+
+## Environment Variables
+
+Name                  | Default Value      | Description
+----------------------|--------------------|------------
+`MAVROS_FCU_CONN`     | "udp"              | Protocol for autogenerated FCU URL
+`MAVROS_FCU_IP`       | "127.0.0.1"        | IP for autogenerated FCU_URL
+`MAVROS_FCU_UDP_BASE` | "14830"            | Base port for autogenerated FCU_URL
+`MAVROS_TGT_SYSTEM`   | "auto"             | Target system ID, if set to a number, this will __override__ the automatic behaviour
+`PX4_INSTANCE_BASE`   | 0                  | Base instance for autogenerated instance matching
+`MAVROS_TGT_FIRMWARE` | "px4"              | Firmware profile used by MAVROS. Only other valid value currently is "apm"
+`MAVROS_GCS_URL`      | "udp-pb://@:14550" | MAVROS URL for ground control station connection
+`MAVROS_FCU_URL`      | {unset}            | MAVROS URL for FCU connection. Set to __override__ automatic behaviour
+`VEHICLE_NAMESPACE`   | {unset}            | Namespace for mavros topics. Set to __override__ default value of `vehicle_${TGT_SYSTEM}`
+
+`MAVROS_FCU_URL` is autogenerated as:
+> `$MAVROS_FCU_CONN://$MAVROS_FCU_IP:$((MAVROS_FCU_UDP_BASE + INSTANCE))@/`
+
+If no Kubernetes deployment is detected, this ends up as:
+> `udp://127.0.0.1:14830@/`
+
+Similarly, if no Kubernetes is detected, `MAVROS_TGT_SYSTEM` will end up as `1`,
+
+When setting `MAVROS_FCU_URL` manually, remember that a query string (_e.g._ `?ids=n,240`) will be added during launch.
+You need to ensure that your input for `MAVROS_FCU_URL` supports this syntax. Of particular note is the need for a
+trailing `/` in most formats, but not for the `serial://` format. Also note that the plain file format does not support
+this. See [the MAVROS docs](https://github.com/mavlink/mavros/blob/master/mavros/README.md#connection-url) for more
+information on URL formats.
+
+## Build Process
+
+At the time of writing, there is not a binary distribution available for `mavros_msgs` under ROS2. Therefore, this
+package needs to be built from source. In addition, to ensure support for custom message types, `ros1_bridge` also needs
+to be built from source. The build process also includes some additional steps to complete the installation of MAVROS
+under ROS1.
+
+## Example `vehicle.config`
+
+Note that the extended form of the serial URL is required for MAVROS's target "query string" to work.
+
+```bash
+VEHICLE_FCU_URL=serial:///dev/px4fmu:115200
+VEHICLE_FIRMWARE=px4
+VEHICLE_MAVLINK_SYSID=23
+VEHICLE_NAME=clover23
+```
+
+## Behaviour Notes
+
+### Running on a vehicle
+
+Ensure `/etc/starling/vehicle.config` is mounted. The container is then configured from the contents of that file.
+
+### Running under Kubernetes StatefulSet
+
+The container is configured based on the detected ordinal from the hostname.
+
+`MAVROS_FCU_URL` is autogenerated as: `udp://127.0.0.1:$((14830 + ORDINAL))@`
+
+`MAVROS_TGT_SYSTEM` will end up as `$((ORDINAL + 1))`
+
+### Running isolated
+
+Default values will be used, equivalent to the Kubernetes case with `ORDINAL=0`
+
+### Weird Namespacing for mavros pluginlists
+
+In ROS2 a parameter file has an annoying specific format. The first entry should be the name of the node which the following ros2 parameters apply to. This must directly map to the node name.
+
+We would like our node to be under `<vehicle_namepsace>/mavros/...`. But by default the topics all come out on `...` essentially root. Therefore in the `mavros.launch.xml` we set the mavros node to use namespace `<vehicle_namepsace>/mavros`. However in order for the param file to match, we need to match the root mavros node name which is `<vehicle_namepsace>/mavros/mavros`.
+
+Therefore the top of `mavros_pluginlists_px4.yaml` must specify the node name `<vehicle_namepsace>/mavros/mavros`. The template pluginlists only specifies `mavros`, so the `mavros_setup.sh` dynamically renames the pluginlist yaml file with the correct namespace.