diff --git a/.github/workflows/remove_container_on_close.yaml b/.github/workflows/remove_container_on_close.yaml index 6979a81..0818555 100644 --- a/.github/workflows/remove_container_on_close.yaml +++ b/.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 }} - - diff --git a/.github/workflows/update-image.yml b/.github/workflows/update-image.yml index 541f1ed..d1ac1a0 100644 --- a/.github/workflows/update-image.yml +++ b/.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 - - \ No newline at end of file diff --git a/buildtools/docker-bake.hcl b/buildtools/docker-bake.hcl index ad450c9..367d9e5 100644 --- a/buildtools/docker-bake.hcl +++ b/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 = [ diff --git a/docs/docker-images/mavros2.md b/docs/docker-images/mavros2.md new file mode 100644 index 0000000..a7a3c00 --- /dev/null +++ b/docs/docker-images/mavros2.md @@ -0,0 +1,239 @@ +# `starling-mavros2` Container + +This container is the core Starling container. It contains the primary interface between the flight controller (simulated or real) and the users application written in ROS2. + +This container is mainly comprised of the MAVROS, MAVLINK/ROS2 ros node which provides a standard ROS2 interface to the user. This container performs an automated set of modifications to allow easy connection to SITL or the real vehicle. It also contains a set of general nodes which should be part of every user application. + +## Contents + +[TOC] + +## Overview + +The image is a wrapper around MAVROS, as such, most of the documentation for MAVROS is of use when working with this +image. The image has both `mavros` and `mavros-extras` installed so all plugins should be available. + +The published topics are namespaced to allow for running multiple vehicles on the same ROS network. By default, this +namespace will be `/vehicle_N`, where `N` is the MAVLink system ID of the vehicle. MAVROS topics are published within +this namespace, *e.g.* `/vehicle_1/mavros/state`. + +The image has been setup to automatically configure itself in some scenarios: + + - Running on a vehicle + - Running in Kubernetes + +There are also some additional general nodes running in the background. In particular: + + - Emergency Stop listener on the `/emergency_stop` topic of type `std_msgs/msg/Empty`. It is hard-wired to forcefully disarm the vehicle (stop motors) for both PX4 and Ardupilot in simulation and reality. + - Ping Listener on the `/ping_source` and `/ping_sink` topics which are used in conjunction with the `ping-monitor` to measure vehicle round trip time. + + +## Launch Process + +The initial process of starting the container goes through `ros_entrypoint.sh`. This file sources ROS2 and +runs a setup script `mavros_setup.sh` 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 filecontains 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 start a MAVROS2 Container instance based on the set numerical given by Kubernetes. + +To build this container clone the ProjectStarling repo and cd in system. + +```bash +cd system +make mavros2 # Builds docker container +docker run -it --rm uobflightlabstarling/starling-mavros2:latest +``` + +Default action of launching the container (make run) will be to launch the `mavros.launch.xml`. + +## Launch file + +The launch file `mavros.launch.xml` runs the following +Running the container will run the following ros nodes in parallel: + +This launch file starts the following: + +1. Mavros2 ROS2 node +2. The EStop Listener +3. The Ping Responder (To measure Round Trip Time) + + +## Contents + +The container is built out of `Dockerfile` which is the base container which is based on Ubuntu 20.04. It runs ROS2 Foxy and MAVROS 2 with extra nodes. + +> The `buildfromsource.Dockerfile` is an example Dockerfile for building MAVROS2 from source which has been kept for reference. + +The other files include: + +- `mavros_setup.sh` is sourced at the end of the Dockerfile if any environment variables need modification. +- `mavros.launch.xml` is a ros2 launch file, and launches both mavros and the extra nodes. It has 4 main 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`. +- `config` folder contains the parameter files which `mavros.launch.xml` reads in to setup what mavros2 functionality is available and how it is configured. See below. +- `starling_mavros_extras` folder contains the source code for any extra ROS2 nodes we wish to run inside this core container. +- `fastrtps_profiles.xml` is a ROS2 configuration file which specifies that it should always use UDP for sending data. + +## Configuration + +We have identified that mavros is a base requirement for any drone running onboard compute in order to communicate with bot h 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=` to `docker run` like so +> `docker run -e MAVROS_FCU_URL= -e MAVROS_TGT_SYSTEM=<#n>...` + +See Mavros for possible connection values + +It to gets 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.launch.xml` file. This behaviour +should be usable in almost all cases. + +The __ROS2__ `mavros.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. + +In addition there exists the configuration parameter files in `config` which are used by mavros to specify which mavros plugins are running, and their associated parameters. These configuration files are copied from `config` into the root directory of the container (`/`). See below for how to use custom configurations. + +During runtime, the `mavros_setup.sh` produces a modified version of these files to ensure that vehicle namespacing is understood. These are suffixed with `_mod` and should not be modified. + +## Configuration Options + +There are many configuration options available for this image to allow it to be used flexibly across different +deployment scenarios. The most important of these are those relating to the vehicle and GCS connections, and MAVROS +configuration. A full summary is given in the table below. + +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_PLUGINLISTS_PATH` | "/mavros_pluginlists_px4.yaml" | Path for MAVROS pluginlists file +`MAVROS_CONFIG_PATH` | "/mavros_config_px4.yaml" | Path for initial MAVROS configuration file +`MAVROS_MOD_CONFIG_PATH` | "/mavros_config_mod.yaml" | Path for modified MAVROS config to be written to + +### Configuring the Connection + +MAVROS is told to connect to a MAVLink source with a URL. This URL can be configured in a number of ways. The most +straightforward is to simply set the `MAVROS_FCU_URL` environment variable. This will override any other behaviour. + +```sh +docker run -e MAVROS_FCU_URL=serial:///dev/ttyUSB0:115200 uobflightlabstarling/starling-mavros +``` + +When setting `MAVROS_FCU_URL` directly, note 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. + +`MAVROS_FCU_URL` is can also be set automatically by the container depending on its environment. If there is a +`vehicle.config` file mounted in the image, the value of `VEHICLE_FCU_URL` from that file will be used as the `fcu_url`. +This is especially useful when deploying the container onto physical vehicles. The same warning about trailing slashes +above goes for the value put in `vehicle.config`. + +If there is no `vehicle.config` file, the image will configure itself based on the values of `MAVROS_FCU_CONN`, +`MAVROS_FCU_IP` and `MAVROS_FCU_UDP_BASE`. An additional parameter, `INSTANCE` is also used in the construction of the +URL. This is generated based on the container hostname and is intended for use in Kubernetes deployments to distinguish +multiple instances. `PX4_INSTANCE_BASE` can be used to offset the `fcu_url` will be constructed as below: +> `$MAVROS_FCU_CONN://$MAVROS_FCU_IP:$((MAVROS_FCU_UDP_BASE + INSTANCE))@/` + +With all values at default, this ends up as: +> `udp://127.0.0.1:14830@/` + +### Configuring the Target System + +Another important configuration option is the target system ID. This controls the target system that MAVROS sends in +some messages. As for the `fcu_url`, the value can be explicitly overridden, this time using the `MAVROS_TGT_SYSTEM` +environment variable. Setting this will overrise all other values. If it is set to an invalid system ID, MAVROS will +be set to use a target ID of `1`. + +If the environment variable is left at its default value of `auto`, a similar flow to the `fcu_url` occurs: if a +`vehicle.config` file exists, the value of `VEHICLE_MAVLINK_SYSID` from that file will be used. Otherwise, the value is +autogenerated based on the `INSTANCE` parameter derived from the container hostname. Note that the `INSTANCE` number is +0-indexed, while MAVLink system IDs start at `1`. Therefore, the system ID is set to one more than the `INSTANCE`. + +### Configuring the MAVROS Configuration + +Two sets of `config.yaml` and `pluginlists.yaml` files are installed in the root directory to provide alternatives for +PX4 and ArduPilot autopilots. These are named: `/mavros_config_px4.yaml` and `/mavros_pluginlists_px4.yaml` for the PX4 +versions and `/mavros_config_ap.yaml` and `/mavros_pluginlists_ap.yaml` for the ArduPilot versions. + +The easiest way to choose between the two is to set the `MAVROS_CONFIG_PATH` and `MAVROS_PLUGINLISTS_PATH` environment +variables. By default these point to the PX4 versions. To use the ArduPilot versions set both variables as below: + +```sh +docker run -e MAVROS_CONFIG_PATH=/mavros_config_ap.yaml -e MAVROS_PLUGINLISTS_PATH=/mavros_pluginlists_ap.yaml ... +``` + +It is also possible to mount alternative configurations into the image and use the environment variables to configure +MAVROS with them. + +TODO: Example of mounted configuration + +## 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 +``` + +## Implementation Details + +### 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` + +### Using custom mavros configurations + +By default we have only enabled a subset of all the mavros plugins which are available. This is so that we limit the data flow. If your application requires other plugins, this can really easily be done. + +First make a local copy of the `mavros_pluginlists....yaml` file and modify it to what you wish. The `plugin_denylist` specifies a list of plugins which should not run. The `plugin_allowlist` speicifes a list of plugins which should be run. Make sure not to change the name of the parameters (`mavros`). Then either manually using bind mounting, or within the docker-compose file, you should mount this file at `/mavros_pluginlists_px4.yaml`. This overwrites the default file. + +### 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 `/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 `/mavros`. However in order for the param file to match, we need to match the root mavros node name which is `/mavros/mavros`. + +Therefore the top of `mavros_pluginlists_px4.yaml` must specify the node name `/mavros/mavros`. The template pluginlists only specifies `mavros`, so the `mavros_setup.sh` dynamically renames the pluginlist yaml file with the correct namespace. + +## Advanced Topics + +### Adding additional MAVROS plugins + +This should be possible by mounting a volume with your plugin into the container. Assuming ROS tools are able to find +it, MAVROS should load the plugin (if directed by the pluginlists file). \ No newline at end of file diff --git a/mkdocs.yml b/mkdocs.yml index 7983d50..3823f5e 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -57,6 +57,7 @@ nav: - "Image Documentation": - "starling-ui": "docker-images/ui.md" - "starling-mavros": "docker-images/mavros.md" + - "starling-mavros2": "docker-images/mavros2.md" - "starling-controller-base": "docker-images/controller-base.md" - "starling-sim-base-core": "docker-images/sim-base-core.md" - "starling-sim-base-px4": "docker-images/sim-base-px4.md" diff --git a/system/Makefile b/system/Makefile index 9a0541a..15341c4 100644 --- a/system/Makefile +++ b/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 diff --git a/system/mavros/README.md b/system/mavros/README.md index 0536f7d..c7e2f09 100644 --- a/system/mavros/README.md +++ b/system/mavros/README.md @@ -1,140 +1,3 @@ # `starling-mavros` Container -This container holds both MAVROS and a ROS1/2 bridge. In the future, we expect to use the ROS2 version of MAVROS which -will eliminate the need for the ROS1/2 bridge. - -## 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=` to `docker run` like so -> `docker run -e MAVROS_FCU_URL= -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` +Please see [docs for more details](https://docs.starlinguas.dev/docker-images/mavros/) \ No newline at end of file diff --git a/system/mavros2/Dockerfile b/system/mavros2/Dockerfile new file mode 100644 index 0000000..cb43549 --- /dev/null +++ b/system/mavros2/Dockerfile @@ -0,0 +1,58 @@ +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 . /opt/ros/${ROS_DISTRO}/setup.sh \ + && ros2 run mavros install_geographiclib_datasets.sh + +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"] diff --git a/system/mavros2/README.md b/system/mavros2/README.md new file mode 100644 index 0000000..34ba24e --- /dev/null +++ b/system/mavros2/README.md @@ -0,0 +1,3 @@ +# `starling-mavros2` Container + +Please see [docs for more details](https://docs.starlinguas.dev/docker-images/mavros2/) \ No newline at end of file diff --git a/system/mavros2/buildfromsource.Dockerfile b/system/mavros2/buildfromsource.Dockerfile new file mode 100644 index 0000000..c484859 --- /dev/null +++ b/system/mavros2/buildfromsource.Dockerfile @@ -0,0 +1,85 @@ +# This Dockerfile is now not used +# However it is left in as future reference for how to build from mavros source +# If that is ever needed in the future + +FROM ros:foxy-ros-base-focal AS builder + +WORKDIR /ros_ws + +# Install mavros dependencies +# COPY mavros/libmavconn/package.xml /ros_ws/src/mavros/libmavconn/package.xml +# COPY mavros/mavros/package.xml /ros_ws/src/mavros/mavros/package.xml +# COPY mavros/mavros_msgs/package.xml /ros_ws/src/mavros/mavros_msgs/package.xml +# COPY mavros/mavros_extras/package.xml /ros_ws/src/mavros/mavros_extras/package.xml + +RUN git clone --depth 1 --branch 2.3.0 https://github.com/mavlink/mavros.git /ros_ws/src/mavros + +RUN . /opt/ros/${ROS_DISTRO}/setup.sh \ + && apt-get update \ + && rosdep install -y --from-paths src --ignore-src . + +# Install mavros itself +# COPY mavros /ros_ws/src/mavros + +RUN . /opt/ros/${ROS_DISTRO}/setup.sh \ + && colcon build + +# Start the main image build +FROM ros:foxy-ros-core-focal AS image + +WORKDIR /ros_ws + +# Copy over the build artifacts +COPY --from=builder /ros_ws/install /ros_ws/install + +# Install exec dependencies +RUN rm install/COLCON_IGNORE \ + && apt-get update \ + && apt-get install -y --no-install-recommends python3-rosdep \ + && rosdep init \ + && rosdep update --rosdistro $ROS_DISTRO \ + && . /opt/ros/${ROS_DISTRO}/setup.sh \ + && rosdep install -t exec -y --from-paths install --ignore-src \ + && apt-get remove -y python3-rosdep \ + && SUDO_FORCE_REMOVE=yes apt-get autoremove -y \ + && rm -rf /var/lib/apt/lists/* + +# Part of MAVROS setup for lib geographic +## Ensure geographiclib is properly installed with FindGeographicLib available +RUN apt-get update \ + && apt-get install -y --no-install-recommends \ + libgeographic-dev \ + geographiclib-tools \ + && rm -rf /var/lib/apt/lists/* \ + && /ros_ws/install/mavros/lib/mavros/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" + +# 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"] diff --git a/system/mavros2/config/mavros_config_ap.yaml b/system/mavros2/config/mavros_config_ap.yaml new file mode 100644 index 0000000..34f5526 --- /dev/null +++ b/system/mavros2/config/mavros_config_ap.yaml @@ -0,0 +1,257 @@ +# Common configuration for APM2 autopilot +# +# node: +startup_px4_usb_quirk: false + +# --- system plugins --- + +# sys_status & sys_time connection options +conn: + heartbeat_rate: 1.0 # send hertbeat rate in Hertz + heartbeat_mav_type: "ONBOARD_CONTROLLER" + timeout: 10.0 # hertbeat timeout in seconds + timesync_rate: 10.0 # TIMESYNC rate in Hertz (feature disabled if 0.0) + system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0) + +# sys_status +sys: + min_voltage: 10.0 # diagnostics min voltage + disable_diag: false # disable all sys_status diagnostics, except heartbeat + +# sys_time +time: + time_ref_source: "fcu" # time_reference source + timesync_mode: MAVLINK + timesync_avg_alpha: 0.6 # timesync averaging factor + +# --- mavros plugins (alphabetical order) --- + +# 3dr_radio +tdr_radio: + low_rssi: 40 # raw rssi lower level for diagnostics + +# actuator_control +# None + +# command +cmd: + use_comp_id_system_control: false # quirk for some old FCUs + +# dummy +# None + +# ftp +# None + +# global_position +global_position: + frame_id: "map" # origin frame + child_frame_id: "base_link" # body-fixed frame + rot_covariance: 99999.0 # covariance for attitude? + gps_uere: 1.0 # User Equivalent Range Error (UERE) of GPS sensor (m) + use_relative_alt: true # use relative altitude for local coordinates + tf: + send: false # send TF? + frame_id: "map" # TF frame_id + global_frame_id: "earth" # TF earth frame_id + child_frame_id: "base_link" # TF child_frame_id + +# imu_pub +imu: + frame_id: "base_link" + # need find actual values + linear_acceleration_stdev: 0.0003 + angular_velocity_stdev: 0.0003490659 // 0.02 degrees + orientation_stdev: 1.0 + magnetic_stdev: 0.0 + +# local_position +local_position: + frame_id: "map" + tf: + send: false + frame_id: "map" + child_frame_id: "base_link" + send_fcu: false + +# param +# None, used for FCU params + +# rc_io +# None + +# safety_area +safety_area: + p1: {x: 1.0, y: 1.0, z: 1.0} + p2: {x: -1.0, y: -1.0, z: -1.0} + +# setpoint_accel +setpoint_accel: + send_force: false + +# setpoint_attitude +setpoint_attitude: + reverse_thrust: false # allow reversed thrust + use_quaternion: false # enable PoseStamped topic subscriber + tf: + listen: false # enable tf listener (disable topic subscribers) + frame_id: "map" + child_frame_id: "target_attitude" + rate_limit: 50.0 + +# setpoint_raw +setpoint_raw: + thrust_scaling: 1.0 # specify thrust scaling (normalized, 0 to 1) for thrust (like PX4) + +# setpoint_position +setpoint_position: + tf: + listen: false # enable tf listener (disable topic subscribers) + frame_id: "map" + child_frame_id: "target_position" + rate_limit: 50.0 + mav_frame: LOCAL_NED + +# setpoint_velocity +setpoint_velocity: + mav_frame: LOCAL_NED + +# vfr_hud +# None + +# waypoint +mission: + pull_after_gcs: true # update mission if gcs updates + use_mission_item_int: true # use the MISSION_ITEM_INT message instead of MISSION_ITEM + # for uploading waypoints to FCU + +# --- mavros extras plugins (same order) --- + +# adsb +# None + +# debug_value +# None + +# distance_sensor +## Currently available orientations: +# Check http://wiki.ros.org/mavros/Enumerations +## +distance_sensor: + rangefinder_pub: + id: 0 + frame_id: "lidar" + #orientation: PITCH_270 # sended by FCU + field_of_view: 0.0 # XXX TODO + send_tf: false + sensor_position: {x: 0.0, y: 0.0, z: -0.1} + rangefinder_sub: + subscriber: true + id: 1 + orientation: PITCH_270 # only that orientation are supported by APM 3.4+ + +# image_pub +image: + frame_id: "px4flow" + +# fake_gps +fake_gps: + # select data source + use_mocap: true # ~mocap/pose + mocap_transform: false # ~mocap/tf instead of pose + mocap_withcovariance: false # ~mocap/pose uses PoseWithCovarianceStamped Message + use_vision: false # ~vision (pose) + use_hil_gps: true + gps_id: 4 + # origin (default: Zürich) + geo_origin: + lat: 47.3667 # latitude [degrees] + lon: 8.5500 # longitude [degrees] + alt: 408.0 # altitude (height over the WGS-84 ellipsoid) [meters] + eph: 2.0 + epv: 2.0 + horiz_accuracy: 0.5 + vert_accuracy: 0.5 + speed_accuracy: 0.0 + satellites_visible: 6 # virtual number of visible satellites + fix_type: 3 # type of GPS fix (default: 3D) + tf: + listen: false + send: false # send TF? + frame_id: "map" # TF frame_id + child_frame_id: "fix" # TF child_frame_id + rate_limit: 10.0 # TF rate + gps_rate: 5.0 # GPS data publishing rate + +# landing_target +landing_target: + listen_lt: false + mav_frame: "LOCAL_NED" + land_target_type: "VISION_FIDUCIAL" + image: + width: 640 # [pixels] + height: 480 + camera: + fov_x: 2.0071286398 # default: 115 [degrees] + fov_y: 2.0071286398 + tf: + send: true + listen: false + frame_id: "landing_target" + child_frame_id: "camera_center" + rate_limit: 10.0 + target_size: {x: 0.3, y: 0.3} + +# mocap_pose_estimate +mocap: + # select mocap source + use_tf: false # ~mocap/tf + use_pose: true # ~mocap/pose + +# odom +odometry: + frame_tf: + desired_frame: "ned" + estimator_type: 3 # check enum MAV_ESTIMATOR_TYPE in + +# px4flow +px4flow: + frame_id: "px4flow" + ranger_fov: 0.118682 # 6.8 degrees at 5 meters, 31 degrees at 1 meter + ranger_min_range: 0.3 # meters + ranger_max_range: 5.0 # meters + +# vision_pose_estimate +vision_pose: + tf: + listen: false # enable tf listener (disable topic subscribers) + frame_id: "map" + child_frame_id: "vision_estimate" + rate_limit: 10.0 + +# vision_speed_estimate +vision_speed: + listen_twist: true # enable listen to twist topic, else listen to vec3d topic + twist_cov: true # enable listen to twist with covariance topic + +# vibration +vibration: + frame_id: "base_link" + +# wheel_odometry +wheel_odometry: + count: 2 # number of wheels to compute odometry + use_rpm: false # use wheel's RPM instead of cumulative distance to compute odometry + wheel0: {x: 0.0, y: -0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m) + wheel1: {x: 0.0, y: 0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m) + send_raw: true # send wheel's RPM and cumulative distance (~/wheel_odometry/rpm, ~/wheel_odometry/distance) + send_twist: false # send geometry_msgs/TwistWithCovarianceStamped instead of nav_msgs/Odometry + frame_id: "map" # origin frame + child_frame_id: "base_link" # body-fixed frame + vel_error: 0.1 # wheel velocity measurement error 1-std (m/s) + tf: + send: true + frame_id: "map" + child_frame_id: "base_link" + +# vim:set ts=2 sw=2 et: diff --git a/system/mavros2/config/mavros_config_px4.yaml b/system/mavros2/config/mavros_config_px4.yaml new file mode 100644 index 0000000..8c7ebc3 --- /dev/null +++ b/system/mavros2/config/mavros_config_px4.yaml @@ -0,0 +1,95 @@ +mavros/altitude: + ros__parameters: + frame_id: map + use_sim_time: false + +mavros/cmd: + ros__parameters: + command_ack_timeout: 5.0 + use_comp_id_system_control: false + use_sim_time: false + +mavros/imu: + ros__parameters: + frame_id: "base_link" + # need find actual values + linear_acceleration_stdev: 0.0003 + angular_velocity_stdev: 0.0003490659 + orientation_stdev: 1.0 + magnetic_stdev: 0.0 + +mavros/global_position: + ros__parameters: + child_frame_id: base_link + frame_id: map + gps_uere: 1.0 + rot_covariance: 99999.0 + tf: + child_frame_id: base_link + frame_id: map + global_frame_id: earth + send: false + use_relative_alt: true + use_sim_time: false + +mavros/local_position: + ros__parameters: + frame_id: map + tf: + child_frame_id: base_link + frame_id: map + send: false + use_sim_time: false + +mavros/setpoint_attitude: + ros__parameters: + reverse_thrust: false + use_quaternion: false + use_sim_time: false + +mavros/setpoint_position: + ros__parameters: + mav_frame: LOCAL_NED + use_sim_time: false + +mavros/setpoint_raw: + ros__parameters: + thrust_scaling: 1.0 + use_sim_time: false + +mavros/setpoint_velocity: + ros__parameters: + mav_frame: LOCAL_NED + use_sim_time: false + +mavros/setpoint_accel: + ros__parameters: + send_force: false + use_sim_time: false + +mavros/distance_sensor: + ros__parameters: + base_frame_id: base_link + config: "{rangefinder: { + subscriber: true, + id: 0, + frame_id: rangefinder, + orientation: PITCH_270, + horizontal_fov_ratio: 0.5, + vertical_fov_ratio: 0.5} + }" + use_sim_time: false + + +mavros/vision_pose: + ros__parameters: + tf/child_frame_id: vision_estimate + tf/frame_id: map + tf/listen: false + tf/rate_limit: 10.0 + use_sim_time: false + +mavros/vibration: + ros__parameters: + frame_id: base_link + use_sim_time: false diff --git a/system/mavros2/config/mavros_pluginlists_ap.yaml b/system/mavros2/config/mavros_pluginlists_ap.yaml new file mode 100644 index 0000000..436752d --- /dev/null +++ b/system/mavros2/config/mavros_pluginlists_ap.yaml @@ -0,0 +1,48 @@ +mavros: + ros__parameters: + component_id: 191 + plugin_denylist: + - "*" + plugin_allowlist: + # - altitude + - command + - distance_sensor + # - ftp + # - global_position + # - imu + - local_position + # - manual_control + # - param + # - px4flow + # - rc_io + # - setpoint_accel + # - setpoint_attitude + - setpoint_position + # - setpoint_raw + # - setpoint_velocity + - sys_status + - sys_time + - vision_pose + - log_transfer + # - onboard_computer_status + # - vibration + +# Old format + +# plugin_blacklist: +# # common +# - actuator_control +# - ftp +# - safety_area +# - hil +# # extras +# - altitude +# - debug_value +# - image_pub +# - px4flow +# - vibration +# - vision_speed_estimate +# - wheel_odometry + +# plugin_whitelist: [] +# #- 'sys_* \ No newline at end of file diff --git a/system/mavros2/config/mavros_pluginlists_px4.yaml b/system/mavros2/config/mavros_pluginlists_px4.yaml new file mode 100644 index 0000000..c31ae3c --- /dev/null +++ b/system/mavros2/config/mavros_pluginlists_px4.yaml @@ -0,0 +1,28 @@ +mavros: + ros__parameters: + component_id: 191 + plugin_denylist: + - "*" + plugin_allowlist: + # - altitude + - command + - distance_sensor + # - ftp + # - global_position + # - imu + - local_position + # - manual_control + # - param + # - px4flow + # - rc_io + # - setpoint_accel + # - setpoint_attitude + - setpoint_position + # - setpoint_raw + # - setpoint_velocity + - sys_status + - sys_time + - vision_pose + - log_transfer + # - onboard_computer_status + # - vibration diff --git a/system/mavros2/docker-compose.yml b/system/mavros2/docker-compose.yml new file mode 100644 index 0000000..cf71b6f --- /dev/null +++ b/system/mavros2/docker-compose.yml @@ -0,0 +1,31 @@ + +version: '3' + +services: + simhost: # Must be called simhost + image: uobflightlabstarling/starling-sim-iris:latest + ports: + - "8080:8080" + + sitl: + image: uobflightlabstarling/starling-sim-px4-sitl:nightly + environment: + - "PX4_SIM_HOST=simhost" + - "PX4_OFFBOARD_HOST=mavros" + ports: + - "18570:18570/udp" + + mavros: + image: uobflightlabstarling/starling-mavros2:latest + # command: ros2 launch launch/mavros_bridge.launch.xml + environment: + - "MAVROS_TGT_SYSTEM=1" + - "MAVROS_FCU_IP=0.0.0.0" + - "MAVROS_GCS_URL=tcp-l://0.0.0.0:5760" + ports: + - "5760:5760" + + # rosbridge-suite: + # image: uobflightlabstarling/rosbridge-suite:latest + # ports: + # - "9090:9090" diff --git a/system/mavros2/fastrtps_profiles.xml b/system/mavros2/fastrtps_profiles.xml new file mode 100644 index 0000000..4d3a443 --- /dev/null +++ b/system/mavros2/fastrtps_profiles.xml @@ -0,0 +1,19 @@ + + + + + CustomUdpTransport + UDPv4 + + + + + + + CustomUdpTransport + + + false + + + \ No newline at end of file diff --git a/system/mavros2/mavros.launch.xml b/system/mavros2/mavros.launch.xml new file mode 100644 index 0000000..ae19e31 --- /dev/null +++ b/system/mavros2/mavros.launch.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/system/mavros2/mavros_setup.sh b/system/mavros2/mavros_setup.sh new file mode 100755 index 0000000..7e40054 --- /dev/null +++ b/system/mavros2/mavros_setup.sh @@ -0,0 +1,126 @@ +#!/bin/bash + +HAS_VEHICLE_CONFIG=false +TGT_SYSTEM_SET=false + +# If we have a vehicle.config file, assume we are running on a real vehicle +if [ -f "/etc/starling/vehicle.config" ]; then + echo "Found vehicle.config" + HAS_VEHICLE_CONFIG=true + # Source VEHICLE_MAVLINK_SYSID, VEHICLE_NAME, VEHICLE_FCU_URL and VEHICLE_FIRMWARE + source /etc/starling/vehicle.config +fi + +#TODO: VEHICLE_FIRMWARE should affect choice of default parameter file + +function get_instance { + # Attempt to get ordinal + ORDINAL="${HOSTNAME##*-}" + + if [ -z "$ORDINAL" ]; then + # Parsing ordinal failed, assume non-kubernetes container and use default of 0 + echo "0" + return 2 + fi + + if ! [ "$ORDINAL" -ge 0 ] 2>/dev/null; then + # Ordinal is not an integer, assume non-kubernetes container and use default of 0 + echo "0" + return 3 + fi + + echo $((PX4_INSTANCE_BASE + ORDINAL)) +} + +# If MAVROS_TGT_SYSTEM is auto, then set instance id to take MAVROS_TGT_SYSTEM_BASE + ORDINAL from StatefulSet hostname +# Hostname is of the form '-' +if [ "$MAVROS_TGT_SYSTEM" = "auto" ]; then + echo "MAVROS_TGT_SYSTEM set to auto" + if [ $HAS_VEHICLE_CONFIG = true ]; then + echo "MAVROS_TGT_SYSTEM set to VEHICLE_MAVLINK_SYSID ($VEHICLE_MAVLINK_SYSID) from /etc/starling/vehicle.config" + MAVROS_TGT_SYSTEM=$VEHICLE_MAVLINK_SYSID + # TODO: Add validity check for SYSID + else + set +e + INSTANCE=$(get_instance) + RESULT=$? + MAVROS_TGT_SYSTEM=$((INSTANCE + 1)) + set -e + if [ $RESULT -ne 0 ]; then + echo "Could not parse ordinal, using default instance of 0. MAVROS_TGT_SYSTEM will be 1" + else + echo "MAVROS_TGT_SYSTEM set to $MAVROS_TGT_SYSTEM (from hostname: $HOSTNAME)" + fi + fi +elif (($MAVROS_TGT_SYSTEM >= 1 && $MAVROS_TGT_SYSTEM <= 255 )); then + echo "MAVROS_TGT_SYSTEM setting as specified: $MAVROS_TGT_SYSTEM" + TGT_SYSTEM_SET=true +else + # TODO: Fail early here + echo "MAVROS_TGT_SYSTEM (set to $MAVROS_TGT_SYSTEM) is invalid, setting to 1. Must either be set to 'auto' where it will either look or number between 1 and 256" + MAVROS_TGT_SYSTEM=1 + TGT_SYSTEM_SET=true +fi + +export MAVROS_TGT_SYSTEM=$MAVROS_TGT_SYSTEM + + +if [ ! -v $VEHICLE_NAMESPACE ]; then + # If set Ensure VEHICLE_NAMESPACE is a valid topic name + # Replace all '-' with '_' + export VEHICLE_NAMESPACE=${VEHICLE_NAMESPACE//-/_} + echo "VEHICLE_NAMESPACE setting to $VEHICLE_NAMESPACE" +else + export VEHICLE_NAMESPACE="vehicle_$MAVROS_TGT_SYSTEM" + echo "VEHICLE_NAMESPACE not set, default to auto generated default based on mavros target ($MAVROS_TGT_SYSTEM) of $VEHICLE_NAMESPACE" +fi + + +if [ -v $MAVROS_FCU_URL ]; then + # If not set, then generate automatically + if ! [ $HAS_VEHICLE_CONFIG = true ]; then + # No vehicle.config, generate based on subparameters and the target system id dynamically + INSTANCE=$((MAVROS_TGT_SYSTEM - 1)) # Default is based on Mavros TGT System + if [ $TGT_SYSTEM_SET = false ]; then # If Mavros TGT System was set automatically, use get_instance. + set +e + INSTANCE=$(get_instance) + set -e + fi + MAVROS_FCU_URL="$MAVROS_FCU_CONN://$MAVROS_FCU_IP:$((MAVROS_FCU_UDP_BASE + INSTANCE))@/" + export MAVROS_FCU_URL=$MAVROS_FCU_URL + echo "MAVROS_FCU_URL automatically set to: $MAVROS_FCU_URL" + else + # Use value from vehicle.config + export MAVROS_FCU_URL=$VEHICLE_FCU_URL + echo "MAVROS_FCU_URL set to: $MAVROS_FCU_URL from vehicle.config" + fi +else + echo "MAVROS_FCU_URL was set to $MAVROS_FCU_URL" +fi + +# Modify mavros PX4 config for frame ids +MAVROS_MOD_CONFIG_PATH=${MAVROS_MOD_CONFIG_PATH:-"/mavros_config_mod.yaml"} +MAVROS_CONFIG_PATH=${MAVROS_CONFIG_PATH:-"/mavros_config.yaml"} +if [ ! -f "$MAVROS_MOD_CONFIG_PATH" ]; then + echo "Modified Mavros Config for $VEHICLE_NAMESPACE does not exist at $MAVROS_MOD_CONFIG_PATH. Generating specialised configuration for launch." + # This line: + # -E allow groupings defined by parenthesis to be used in replace by \1 \2 + # /\"map\"/! selects lines which do not contain "map" or "earth" (Global frame) + # The rest then adds $VEHICLE_NAMESPACE in as a prefix to whatever the frame_id was + # This captures both frame_id: and child_frame_id + # This then saves the output into $PX4_MOD_CONFIG_PATH + sed -E "/\"map\"|\"earth\"/! s/(frame_id: )\"(.+)\"/\1\"$VEHICLE_NAMESPACE\/\2\"/g" $MAVROS_CONFIG_PATH > $MAVROS_MOD_CONFIG_PATH + sed -i "s#mavros#/$VEHICLE_NAMESPACE/mavros#g" $MAVROS_MOD_CONFIG_PATH +else + echo "Modified Mavros Config for $VEHICLE_NAMESPACE already exists at $MAVROS_MOD_CONFIG_PATH. Using for launch" +fi + +# Modify mavros PX4 config for frame ids +MAVROS_MOD_PLUGINLISTS_PATH=${MAVROS_MOD_PLUGINLISTS_PATH:-"/mavros_pluginlists_mod.yaml"} +MAVROS_PLUGINLISTS_PATH=${MAVROS_PLUGINLISTS_PATH:-"/mavros_pluginlists_px4.yaml"} +if [ ! -f "$MAVROS_MOD_PLUGINLISTS_PATH" ]; then + echo "Modified Mavros Pluginlist for $VEHICLE_NAMESPACE does not exist at $MAVROS_PLUGINLISTS_PATH. Generating specialised configuration for launch." + sed -E "s#mavros#/$VEHICLE_NAMESPACE/mavros/mavros#g" $MAVROS_PLUGINLISTS_PATH > $MAVROS_MOD_PLUGINLISTS_PATH +else + echo "Modified Mavros Pluginlist for $VEHICLE_NAMESPACE already exists at $MAVROS_MOD_PLUGINLISTS_PATH. Using for launch" +fi diff --git a/system/mavros2/ros_entrypoint.sh b/system/mavros2/ros_entrypoint.sh new file mode 100755 index 0000000..c6d05e9 --- /dev/null +++ b/system/mavros2/ros_entrypoint.sh @@ -0,0 +1,13 @@ +#!/bin/bash +set -e + +# setup ros2 environment +source "/opt/ros/$ROS_DISTRO/setup.bash" + +# setup the local environment +source "/ros_ws/install/setup.bash" + +# Source the mavros_setup for any user defined edits to the environment +source "/ros_ws/mavros_setup.sh" + +exec "$@" diff --git a/system/mavros2/starling_mavros_extras/CMakeLists.txt b/system/mavros2/starling_mavros_extras/CMakeLists.txt new file mode 100644 index 0000000..a4bac66 --- /dev/null +++ b/system/mavros2/starling_mavros_extras/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 3.5) +project(starling_mavros_extras) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(mavros_msgs REQUIRED) +find_package(diagnostic_msgs REQUIRED) + +add_executable(estop src/estop.cpp include/estop.hpp) +ament_target_dependencies(estop + rclcpp std_msgs mavros_msgs +) +target_include_directories(estop PRIVATE include) + +add_executable(ping src/ping.cpp include/ping.hpp) +ament_target_dependencies(ping + rclcpp std_msgs +) +target_include_directories(ping PRIVATE include) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +install(TARGETS + estop + ping + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/system/mavros2/starling_mavros_extras/include/estop.hpp b/system/mavros2/starling_mavros_extras/include/estop.hpp new file mode 100644 index 0000000..df8eefa --- /dev/null +++ b/system/mavros2/starling_mavros_extras/include/estop.hpp @@ -0,0 +1,41 @@ +#ifndef ESTOP_HPP +#define ESTOP_HPP + +/* + * Trajectory follower + * Copyright (C) 2021 University of Bristol + * + * Author: Mickey Li (University of Bristol) + * + * Distributed under MIT License (available at https://opensource.org/licenses/MIT). + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + */ +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include "mavros_msgs/srv/command_long.hpp" + + +class EstopHandler : public rclcpp::Node +{ + public: + EstopHandler(); + + private: + void emergency_stop(); + rclcpp::Client::SharedPtr mavros_command_srv; + rclcpp::Subscription::SharedPtr estop_sub; +}; + + +#endif diff --git a/system/mavros2/starling_mavros_extras/include/ping.hpp b/system/mavros2/starling_mavros_extras/include/ping.hpp new file mode 100644 index 0000000..f054684 --- /dev/null +++ b/system/mavros2/starling_mavros_extras/include/ping.hpp @@ -0,0 +1,41 @@ +#ifndef PING_HPP +#define PING_HPP + +/* + * Copyright (C) 2022 University of Bristol + * + * Author: Mickey Li (University of Bristol) + * + * Distributed under MIT License (available at https://opensource.org/licenses/MIT). + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + */ + +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include + +class PingHandler : public rclcpp::Node +{ + public: + PingHandler(); + + private: + void handle_ping(const std_msgs::msg::Header::SharedPtr s); + + rclcpp::Subscription::SharedPtr ping_sub; + rclcpp::Publisher::SharedPtr ping_pub; + + std::string vehicle_id; +}; + + +#endif \ No newline at end of file diff --git a/system/mavros2/starling_mavros_extras/package.xml b/system/mavros2/starling_mavros_extras/package.xml new file mode 100644 index 0000000..0845439 --- /dev/null +++ b/system/mavros2/starling_mavros_extras/package.xml @@ -0,0 +1,22 @@ + + + + starling_mavros_extras + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + mavros_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/system/mavros2/starling_mavros_extras/src/estop.cpp b/system/mavros2/starling_mavros_extras/src/estop.cpp new file mode 100644 index 0000000..219fa0a --- /dev/null +++ b/system/mavros2/starling_mavros_extras/src/estop.cpp @@ -0,0 +1,53 @@ +/* + * Trajectory follower + * Copyright (C) 2021 University of Bristol + * + * Author: Mickey Li (University of Bristol) + * + * Distributed under MIT License (available at https://opensource.org/licenses/MIT). + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + */ +#include "estop.hpp" + +EstopHandler::EstopHandler() : + Node("estop_handler") +{ + this->mavros_command_srv = this->create_client("mavros/cmd/command", rmw_qos_profile_services_default); + this->estop_sub = this->create_subscription( + "/emergency_stop", 1, + [this](const std_msgs::msg::Empty::SharedPtr s){ + (void)s; + this->emergency_stop(); + } + ); + + RCLCPP_INFO(this->get_logger(), "ESTOP Initialised"); +} + +void EstopHandler::emergency_stop() { + RCLCPP_ERROR(this->get_logger(), "EMERGENCY STOP RECIEVED, SENDING KILL MESSAGE TO AUTOPILOT"); + // Kill Drone Rotors + auto commandCall = std::make_shared(); + commandCall->broadcast = false; + commandCall->command = 400; + commandCall->param2 = 21196.0; + while (!this->mavros_command_srv->wait_for_service(std::chrono::duration(0.1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for command service. Exiting."); + throw std::runtime_error("Interrupted while waiting for command service. Exiting."); + return; + } + RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); + } + auto result_future = this->mavros_command_srv->async_send_request(commandCall); +} + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + auto handler = std::make_shared(); + rclcpp::spin(handler); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/system/mavros2/starling_mavros_extras/src/ping.cpp b/system/mavros2/starling_mavros_extras/src/ping.cpp new file mode 100644 index 0000000..10e7f5f --- /dev/null +++ b/system/mavros2/starling_mavros_extras/src/ping.cpp @@ -0,0 +1,45 @@ +/* + * Copyright (C) 2021 University of Bristol + * + * Author: Mickey Li (University of Bristol) + * + * Distributed under MIT License (available at https://opensource.org/licenses/MIT). + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + */ +#include "ping.hpp" + +PingHandler::PingHandler() : + Node("ping_handler") +{ + this->declare_parameter("vehicle_id"); + rclcpp::Parameter id_param = this->get_parameter("vehicle_id"); + this->vehicle_id = id_param.as_string(); + + this->ping_sub = this->create_subscription( + "/ping_source", 1, + [this](const std_msgs::msg::Header::SharedPtr s){ + this->handle_ping(s); + } + ); + + this->ping_pub = this->create_publisher("/ping_sink", 10); + + RCLCPP_INFO(this->get_logger(), "Ping Initialised"); +} + +void PingHandler::handle_ping(const std_msgs::msg::Header::SharedPtr s) { + std_msgs::msg::Header msg; + msg.frame_id = this->vehicle_id; + msg.stamp = s->stamp; + this->ping_pub->publish(msg); +} + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + auto handler = std::make_shared(); + rclcpp::spin(handler); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file