diff --git a/.gitignore b/.gitignore index 757fee3..748423e 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,225 @@ -/.idea \ No newline at end of file +/.idea + +# don't track per-user compose settings +compose.override.yml + +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[codz] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py.cover +*.lcov +.hypothesis/ +.pytest_cache/ +cover/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +.pybuilder/ +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +# For a library or package, you might want to ignore these files since the code is +# intended to run in multiple environments; otherwise, check them in: +# .python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +# Pipfile.lock + +# UV +# Similar to Pipfile.lock, it is generally recommended to include uv.lock in version control. +# This is especially recommended for binary packages to ensure reproducibility, and is more +# commonly ignored for libraries. +# uv.lock + +# poetry +# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. +# This is especially recommended for binary packages to ensure reproducibility, and is more +# commonly ignored for libraries. +# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control +# poetry.lock +# poetry.toml + +# pdm +# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. +# pdm recommends including project-wide configuration in pdm.toml, but excluding .pdm-python. +# https://pdm-project.org/en/latest/usage/project/#working-with-version-control +# pdm.lock +# pdm.toml +.pdm-python +.pdm-build/ + +# pixi +# Similar to Pipfile.lock, it is generally recommended to include pixi.lock in version control. +# pixi.lock +# Pixi creates a virtual environment in the .pixi directory, just like venv module creates one +# in the .venv directory. It is recommended not to include this directory in version control. +.pixi/* +!.pixi/config.toml + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm +__pypackages__/ + +# Celery stuff +celerybeat-schedule* +celerybeat.pid + +# Redis +*.rdb +*.aof +*.pid + +# RabbitMQ +mnesia/ +rabbitmq/ +rabbitmq-data/ + +# ActiveMQ +activemq-data/ + +# SageMath parsed files +*.sage.py + +# Environments +.env +.envrc +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# pytype static type analyzer +.pytype/ + +# Cython debug symbols +cython_debug/ + +# PyCharm +# JetBrains specific template is maintained in a separate JetBrains.gitignore that can +# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore +# and can be added to the global gitignore or merged into this file. For a more nuclear +# option (not recommended) you can uncomment the following to ignore the entire idea folder. +# .idea/ + +# Abstra +# Abstra is an AI-powered process automation framework. +# Ignore directories containing user credentials, local state, and settings. +# Learn more at https://abstra.io/docs +.abstra/ + +# Visual Studio Code +# Visual Studio Code specific template is maintained in a separate VisualStudioCode.gitignore +# that can be found at https://github.com/github/gitignore/blob/main/Global/VisualStudioCode.gitignore +# and can be added to the global gitignore or merged into this file. However, if you prefer, +# you could uncomment the following to ignore the entire vscode folder +# .vscode/ +# Temporary file for partial code execution +tempCodeRunnerFile.py + +# Ruff stuff: +.ruff_cache/ + +# PyPI configuration file +.pypirc + +# Marimo +marimo/_static/ +marimo/_lsp/ +__marimo__/ + +# Streamlit +.streamlit/secrets.toml \ No newline at end of file diff --git a/Env.Containerfile b/Env.Containerfile index 577bb0a..15d88f4 100644 --- a/Env.Containerfile +++ b/Env.Containerfile @@ -4,36 +4,41 @@ FROM python:${PYTHON_VERSION} # Set noninteractive frontend for apt ENV DEBIAN_FRONTEND=noninteractive +ENV UV_SYSTEM_PYTHON=1 -# Install dependencies -RUN apt-get update && \ - apt-get install -y --no-install-recommends python3-wxgtk4.0 && \ - rm -rf /var/lib/apt/lists/* +# INSTALL UV +COPY --from=ghcr.io/astral-sh/uv:latest /uv /uvx /bin/ -# Ensure pip is up to date -RUN python -m pip install --upgrade pip +# Install System Dependencies +RUN apt-get update && apt-get install -y --no-install-recommends \ + git \ + curl \ + libgl1 \ + libglib2.0-0 \ + python3-wxgtk4.0 \ + && rm -rf /var/lib/apt/lists/* -# INSTALL POETRY - -ENV POETRY_HOME=/etc/poetry \ - POETRY_VERSION=1.8.5 +# INSTALL PROJECTAIRSIM +RUN mkdir /pyenv +WORKDIR /tmp +RUN git clone --filter=blob:none --no-checkout https://github.com/iamaisim/ProjectAirSim.git \ + && cd ProjectAirSim \ + && git sparse-checkout init --cone \ + && git sparse-checkout set client/python/projectairsim \ + && git checkout 3302010393ac896e8dffc16cbbe2ec1e05d844e3 \ + && mv client/python/projectairsim /pyenv/projectairsim \ + && cd / \ + && rm -rf /tmp/ProjectAirSim -RUN curl -sSL https://install.python-poetry.org | python3 - -ENV PATH="$POETRY_HOME/bin:$PATH" - # INSTALL DEPENDENCIES - -RUN mkdir /pyenv WORKDIR /pyenv -COPY pyproject.toml ./ +COPY ./pyproject.toml ./ -# install stuff to global python environment instead of creating a virtualenv -# the container is our virtual environment -ENV POETRY_VIRTUALENVS_CREATE=false -RUN poetry install --no-interaction --no-ansi +RUN --mount=type=cache,target=/root/.cache/uv \ + uv lock && \ + uv sync --no-install-project -# additional, non-essential packages/libraries -RUN apt-get update && apt-get install -y tmux iproute2 +RUN uv pip install --system -e /pyenv/projectairsim -RUN pip install pre-commit +RUN uv pip install --system pre-commit \ No newline at end of file diff --git a/IARC_AIRSIM_README.md b/IARC_AIRSIM_README.md new file mode 100644 index 0000000..7b884fa --- /dev/null +++ b/IARC_AIRSIM_README.md @@ -0,0 +1,241 @@ +# Running IARC on Project AirSim + +Cold-boot runbook for flying the IARC state machine against Project AirSim: Unreal on +Windows, ArduPilot SITLs and the flight code in podman containers under WSL2, and +(optionally) the Android app driving it. + +Companion docs: + +- [MULTIDRONE_HOWTO.md](./MULTIDRONE_HOWTO.md) — why the port arithmetic looks the way it + does, what the two containers are, how `ArduWorld`/`MultidroneWorld` inject per-drone + configs. Read this when something doesn't line up. +- [PI_SIM_RUNBOOK.md](./PI_SIM_RUNBOOK.md) — the variant where the flight code runs on four + Raspberry Pis instead of inside the `env` container. + +## Prerequisites + +Install these once, following the team docs at +: + +- Unreal Engine **5.2.x** and the Project AirSim Unreal project (Windows) +- IARC-10 (with SIM submodule), Iarc2025App, and SIM Unreal repo cloned in. Note: Must be on MST wifi to clone SIM Unreal repo +- WSL2 with `podman` and `podman-compose` +- `socat` in WSL (`sudo apt install -y socat`) — only if you're running the app +- Android Studio / `adb` on Windows — only if you're running the app + +Clone this repo inside WSL and make sure the `simulation/` submodule is checked out. (NOTE/TODO: IARC 10 does not officially have SIM sub module. We need to do this still) + +## Architecture + +``` +Windows WSL2 Android emulator +------- ---- ---------------- +Unreal + Project AirSim <----> env container the app + (scene, physics, sensors) interfaces/iarc.py + run.py x N (dronekit) + sim container + arducopter SITL x N +``` + +Three separate connections have to come up, in this order: + +1. `iarc.py` → Unreal, over the Project AirSim API at `PAS_HOST`. +2. Unreal → SITL, UDP sensor packets to `SITL_HOST:9003+10i`; SITL → Unreal, servo output + to `AIRSIM_HOST:9002+10i`. +3. flight code → SITL, dronekit over TCP `5762+10i`. + +`NUM_DRONES` must match between `iarc.py` and the `sim` container. Both derive their port +assignments from it independently and **nothing checks that they agree** — a mismatch shows +up as the 300 s `no MAVLink from the SITL` timeout. + +## 0. Collect the two addresses (every boot) + +Only `172.27.192.1` is stable. The WSL VM address is DHCP and changes on every +`wsl --shutdown` or reboot; a stale value is the most common cause of a failed run. + +```powershell +wsl hostname -I +ipconfig +``` + +| Name | Where it comes from | Last seen | +| -------------------------- | ------------------------------------ | ----------------------- | +| `SITL_HOST` | first address from `wsl hostname -I` | `172.27.193.57` | +| `PAS_HOST` / `AIRSIM_HOST` | `ipconfig` → `vEthernet (WSL)` IPv4 | `172.27.192.1` (stable) | + +`AIRSIM_HOST` is already pinned in `simulation/.env`, which podman-compose auto-loads, so +you usually only have to supply `SITL_HOST` by hand. + +If you have `networkingMode=mirrored` set in `.wslconfig`, WSL and Windows share +`127.0.0.1` and you can drop both variables entirely. + +## 1. Unreal + +1. Launch the Unreal simulation project (Unreal Engine 5.2.x). +2. Open the **IARC level** — Ctrl+Space opens the content drawer to find it. +3. Click **Play**. + +This has to be up first: `iarc.py` connects to `PAS_HOST` as its very first action. + +## 2. Terminal 1 — the `env` container and the orchestrator + +In a WSL terminal: + +```bash +cd ~/IARC-10/simulation && ./run_container.sh shutdown +``` + +Worth running first in case a previous run left containers up. + +```bash +./run_container.sh env +``` + +That builds if needed and drops you into a shell inside the container, with the repo bind +mounted at `/IARC`. Inside it: + +```bash +cd /IARC/simulation/interfaces && NUM_DRONES=4 SITL_HOST=172.27.193.57 PAS_HOST=172.27.192.1 python iarc.py +``` + +Wait for `Empty scene loaded.` and the `(press enter once the sim container is up)` prompt. + +**Do not press Enter yet.** `iarc.py` loads an empty scene first on purpose: the SITL needs +_a_ scene to pull data from at startup, but a drone that spawns before its SITL exists +softlocks and only a full sim restart recovers it. The prompt is the gap between those two +requirements. + +Useful variables: + +| Variable | Default | What it does | +| -------------------- | --------------------- | --------------------------------------------------------------------------------------------------------------- | +| `NUM_DRONES` | `1` | Drones to spawn. Must match the `sim` container. | +| `SITL_HOST` | `127.0.0.1` | Where Unreal sends sensor UDP — the WSL VM IP. | +| `PAS_HOST` | `127.0.0.1` | Where the Project AirSim API lives — the `vEthernet (WSL)` IP. | +| `MISSION_CONFIG` | `mission_config.json` | Passed to `run.py --config`. | +| `SPAWN_FLIGHT_CODE` | `1` | Set to `0` to hold the scene open and run flight code elsewhere (see [PI_SIM_RUNBOOK.md](./PI_SIM_RUNBOOK.md)). | +| `SITL_MAVLINK_HOST` | `$SITL_HOST` | Only set this if the SITL exposes MAVLink TCP on a different address than it receives UDP on. | +| `DRONE_SEPARATION_M` | `3.0` | Spacing between spawn points; without it they collide on takeoff. | +| `SITL_START_DELAY` | unset | Seconds to wait blindly instead of prompting. | + +## 3. Terminal 2 — the SITLs + +A second WSL terminal, from `simulation/` (that's where `compose.yml` and `.env` live): + +```bash +cd ~/IARC-10/simulation && NUM_DRONES=4 AIRSIM_HOST=172.27.192.1 ./run_container.sh sim +``` + +This attaches to a tmux session with one window per drone. Cycle through **all** of them +(`Ctrl-b n`) and wait until each reports: + +``` +Waiting for heartbeat from tcp:127.0.0.1:5760 +``` + +The first run of this container builds ArduCopter from source, so give it a few minutes. + +## 4. Back to terminal 1 — spawn and fly + +Press Enter. `iarc.py` then: + +1. Loads `scene_iarc.jsonc` with `NUM_DRONES` actors, rewriting each one's ArduPilot + endpoints to match the SITL instance it belongs to. +2. Creates a `Drone` handle per actor, which starts the sensor streams — this is what + unblocks each SITL's physics loop and makes it finally open its MAVLink port. +3. Waits for real MAVLink frames on `5762 + 10i` for every drone. +4. Launches `uv run run.py --airsim -i ` per drone, all sharing one `FLIGHT_LOG_RUN` + so `tools/analyze_flight.py Logs/` can put them on a single timeline. + +### Port map + +For drone index `i` (0-based; mission config IDs are 1-based, so `i = id - 1`): + +| Port | Purpose | +| ------------ | --------------------------------------- | +| `5760 + 10i` | SITL serial0, claimed by MAVProxy | +| `5762 + 10i` | SITL serial1, what dronekit connects to | +| `9003 + 10i` | AirSim → SITL, sensor data | +| `9002 + 10i` | SITL → AirSim, servo output | +| `5001 + i` | interdrone comms | + +## 5. Optional — the Android app + +Emulator only. The chain is fiddly because the emulator, Windows, and WSL each have their +own idea of `127.0.0.1`, and WSL's localhost forwarding mirrors container ports onto +Windows `127.0.0.1` **only** — never onto the Wi-Fi LAN IP. + +In the mission config, set `"app_opperable": true`. + +### App → drone + +In Windows PowerShell: + +```bash +adb reverse tcp:5001 tcp:5001 +``` + +Then in the app set **Drone 1 IP** to `127.0.0.1`. + +Alternatively, skip `adb` entirely and set **Drone 1 IP** to `10.0.2.2` — the emulator's +fixed NAT alias for the Windows host loopback, which is exactly where WSL mirrors port 5001. Either works; don't do both and then wonder which one is live. + +> `10.0.2.2`, not `10.0.0.2`. The digits matter — `10.0.0.2` is an arbitrary private +> address with nothing on it, and it is the mistake that cost us an afternoon. + +### Drone → app + +Leave the app's **App IP** field **blank**. That field is a bind address, not an announce +address, so anything other than an address on the emulated device (`10.0.2.16`) makes the +listen server fail to bind — it will read `Listen server: not bound`. + +Turn the **emulator loopback toggle ON**, then build the relay. Admin PowerShell, once per +boot: + +```bash +netsh interface portproxy add v4tov4 listenaddress=172.27.192.1 listenport=5100 connectaddress=127.0.0.1 connectport=5100 +``` + +WSL, outside the container: + +```bash +socat TCP-LISTEN:5100,bind=127.0.0.1,fork,reuseaddr TCP:172.27.192.1:5100 +``` + +Full chain: drone dials WSL `127.0.0.1:5100` → socat → `172.27.192.1:5100` → portproxy → +Windows `127.0.0.1:5100` → adb → app. The `env` container is `network_mode: host`, so it +shares WSL's loopback and the socat listener is visible to it with no extra plumbing. + +Get the forward path (app → drone) working on its own before adding any of this. + +## 6. Shutdown + +Ctrl-C the flight code, then: + +```bash +cd ~/IARC-10/simulation && ./run_container.sh shutdown +``` + +Stop Play in Unreal. Kill the `socat` process if you started one. + +## Troubleshooting + +| Symptom | Cause | +| -------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------- | +| `no MAVLink from the SITL ... after 300s` | Stale `SITL_HOST` after a WSL restart; or `NUM_DRONES` differs between the two terminals; or the WSL Hyper-V firewall is dropping the sensor UDP. | +| Connecting to Unreal hangs or refuses | Unreal isn't in Play mode, or `PAS_HOST` is wrong. | +| Drones spawn but never move; SITL prints `No sensor message received in last 1s, resending servos` | The SITL isn't getting sensor packets — check `AIRSIM_HOST` and that Unreal is bound where the SITL is sending. | +| Drone softlocked right after spawn | Enter was pressed before the SITLs were up. Restart the whole sim; there is no partial recovery. | +| `PreArm: Need Position Estimate` for a long time | Normal. The EKF needs GPS lock; it clears on its own. | +| App: `Connection to 10.0.0.2:5001 failed` | Typo — it is `10.0.2.2`. | +| App: `Listen server: not bound` | The App IP field has a non-device address in it. Clear it. | +| `env` container rebuilds its venv every run (~2.5 min) | `UV_PROJECT_ENVIRONMENT` isn't taking effect; check the `iarc_uv` named volume still exists. | +| Other issue not listed | Ask another member for help or Claude. Then document the issue here! | + +## Known rough edges + +- The two `NUM_DRONES` values are unchecked and silently disagree. +- Three files have to agree on the port arithmetic: `sim_start_drones.sh`, + `interfaces/iarc.py`, and `state_machine/drone.py`. +- `SITL_HOST`, the portproxy rule, and the app's addresses all need re-editing after a WSL + restart. `networkingMode=mirrored` in `.wslconfig` collapses most of that. diff --git a/MULTIDRONE_HOWTO.md b/MULTIDRONE_HOWTO.md new file mode 100644 index 0000000..f201d4d --- /dev/null +++ b/MULTIDRONE_HOWTO.md @@ -0,0 +1,192 @@ +# How to use Multiple Drones with Project Airsim + +## Quick start: N drones through the IARC flight code + +This is the current path, driven by [interfaces/iarc.py](./interfaces/iarc.py). It runs the +real state machine with interdrone comms, one process per drone. The standalone dronekit +example described further down predates it and is kept for reference. + +`NUM_DRONES` must match on both sides -- `iarc.py` and `sim_start_drones.sh` derive their +port assignments from it independently, and nothing checks that they agree. + +1. Start the Unreal / ProjectAirSim simulation on Windows. +2. In the `env` container, start the orchestrator: + + ```shell + NUM_DRONES=2 MISSION_CONFIG=mission_config_2drone.json \ + SITL_HOST= PAS_HOST= \ + python iarc.py + ``` + + It loads the empty scene, then blocks waiting for the SITLs. +3. In a second terminal, start the SITLs: + + ```shell + NUM_DRONES=2 AIRSIM_HOST= ./run_container.sh sim + ``` + + This attaches to a tmux session with one window per drone. Wait until every window + reports `Waiting for heartbeat from tcp:127.0.0.1:5760`. +4. Press Enter in the first terminal. It spawns the drones, waits for MAVLink on each + drone's port, then launches `run.py --airsim -i ` per drone. + +`SITL_HOST` is the WSL VM's IP (`hostname -I` inside WSL) and `PAS_HOST` / `AIRSIM_HOST` +are the Windows-side `vEthernet (WSL)` adapter IP (`ipconfig` on Windows). Both default to +loopback when everything shares one network namespace. Step 4's MAVLink wait dials +`SITL_MAVLINK_HOST`, which defaults to `SITL_HOST`; set it explicitly only if the SITL +container exposes its MAVLink TCP ports on a different address than the one it receives +AirSim's sensor UDP on. + +### Port map + +For drone index `i` (0-based; mission config IDs are 1-based, so `i = id - 1`): + +| Port | Purpose | Set by | +| --- | --- | --- | +| `5760 + 10i` | SITL serial0, claimed by MAVProxy | `--instance` | +| `5762 + 10i` | SITL serial1, what dronekit connects to | `--instance`, read in `state_machine/drone.py` | +| `9003 + 10i` | AirSim -> SITL, sensor data | `--instance`, `ardupilot-udp-port` | +| `9002 + 10i` | SITL -> AirSim, servo output | `--instance`, `local-host-udp-port` | +| `5001 + i` | interdrone comms | `drone_info` in the mission config | + +The SITL side of all four comes from `--instance` alone -- `arducopter --help` describes it +as adding `10*instance` to *all* port numbers. The scene config side is computed to match in +`ArduWorld._build_actors`. Note that `--sim-port-in`/`--sim-port-out` are options on the +`arducopter` binary, not on `sim_vehicle.py`; they are not needed here and setting them +would double up with the offset `--instance` already applies. + +Three files have to agree on this arithmetic: `sim_start_drones.sh`, `interfaces/iarc.py`, +and `state_machine/drone.py`. + +## Environment Setup + +The environment setup process is unchanged from the normal sim environment setup: + +## What are the Docker containers for?? + +### `env` Container + +The `env` container ([./Env.Containerfile](./Env.Containerfile)) essentially acts as a virtual environment in which to run flight code, such as the SUAS code itself or one of the test scrips in `/tests`. If you can run flight code locally on your machine, you do not need to use this container. It only exists to simplify the environment setup process. + +To start the `env` container, run the following in a bash-compatible terminal (with `podman` and `podman-compose`), such as WSL: + +```shell +./run_container.sh env +``` + +Running the container in this way automatically attaches to it. + +### `sim` Container + +The `sim` container ([./Sim.Containerfile](./Sim.Containerfile)) acts as a universal environment capable of running Ardupilot's `sim_vehicle.py` command ([click here for more info](https://ardupilot.org/dev/docs/using-sitl-for-ardupilot-testing.html)), which starts a SITL for the drone(s). By default, this script is automatically called when this container is launched, meaning that starting this container is equivalent to starting the drone SITL. + +```shell +./run_container.sh sim +``` + +Running the container in this way automatically attaches to it. + +#### How does `sim` work? + +Upon startup, `sim` runs the `sim_start_drones.sh` script, which automatically starts multiple drones (or just one by default). The number of drones is configured using the `NUM_DRONES` environment variable set when starting the container: + +```shell +NUM_DRONES=10 ./run_container.sh sim +``` + +Each drone is started using the following command and run in individual `tmux` windows: + +```sh +# i is just an index from a for loop +/ardupilot/Tools/autotest/sim_vehicle.py -v ArduCopter -f airsim-copter -w --instance $i +``` + +The `--instance` argument is 0-based and automatically increments relevant ports by 10 per instance. In dronekit terms, this means the first drone's connection string is `tcp:127.0.0.1:5762`, the second's is `tcp:127.0.0.1:5772`, the third's is `tcp:127.0.0.1:5782`, and so on. In terms of Airsim settings, the first drone's control ports (for `ardupilot-udp-port` and `local-host-udp-port`, respectively) are `9003` and `9002`, the second drone's are `9013` and `9012`, the third's are `9023` and `9022`, and so on. It is highly recommended that these are automated, which is what our current example does. + +## How the Multidrone Example Works + +The example in question is [/tests/ProjectAirsimMultidrone.py](/tests/ProjectAirsimMultidrone.py). It provides a single- or multidrone-environment with minimal terminal-based controls. The relevant config files are `scene_ardu_empty.jsonc`, `scene_ardu_quadrotor_template.jsonc`, and `robot_ardu_quadrotor.jsonc`. The file expects to be run from the SUAS repositoy root. + +The `main` function begins by connecting to the Unreal simulation, so the Unreal simulation should be started first. Then, an _empty_ scene is initialized with + +```python +World(client, "scene_ardu_empty.jsonc", delay_after_load_sec=2, sim_config_path="./simulation/sim_config") +``` + +This is done because `sim_vehicle.py` (i.e., the `sim` container) expects a scene to be loaded before starting, since the SITL downloads some scene data when starting. If no scene is not initialized, the download will fail, potentially softlocking the SITL. + +Next, the `main` procedure waits for user input. During this time, the `sim` container should be started, since the next steps require their existence to prevent the Unreal simulation from softlocking. Once the `sim` containter has fully started, the user may press enter to continue. + +Now, our actual scene, including all drones, is finally initialized: + +```python +world = MultidroneWorld(client, "scene_ardu_quadrotor.jsonc", delay_after_load_sec=2, sim_config_path="./simulation/sim_config", drone_grid=drone_grid) +``` + +A custom class, `MultidroneWorld`, is used to automatically generate a grid of drones based on `drone_grid`, a row-column pair/tuple (see [What is MultidroneWorld and why does it exist?](#what-is-multidroneworld-and-why-does-it-exist)). This will initialize all simulated drones in the Unreal simulation, which the SITLs running in the `sim` container will promptly connect to. + +> If the drones are spawned into the Unreal simulation before the SITLs are started, they will softlock, having failed to connect to an SITL immediately. It does not retry this connection, and you must restart the entire simulation to fix this. This is why we prompt the user to continue; it gives them time to start the SITL before reaching this step. + +> Thus, the SITL expects an initialized simulation scene to function and the simultaion scene (the drones, specifically) expects an SITL to be running. This is a bit of a Catch-22, but it's fixed by initializing that empty scene first, as the SITL does not need a scene with drones in it to start, only a scene. + +Next, the user is prompted to continue again, which is meant to give time for the SITLs to connect to started drones before connection attempts are made. After some time, you may continue to the connection step. + +Drones are controlled and connected to via `dronekit`. To make multidrone simulation easier, a custom `DronekitDrone` class exists to streamline interactions and commands. To improve performance (or attempt to), each drone is ran in its own subprocess. + +As connections are made, drones should slowly begin taking off. Once all drones take off, you can start providing instructions: `n` for North, `e` for East, `s` for South, `w` for West, `u` for up, and `d` for down. Multiple instructions can be provided in one batch, such as `nnnnnnnnuuuuuuwwwww`. To land drones, submit `q`, `quit`, or `die`. + +### Running the Example (in steps) + +1. Start the Unreal simulation +2. Run the example code (make sure the `drone_grid` variable matches the number of drones you are starting) +3. Once the first continue prompt halts execution, run the `sim` container + 1. make sure to run it with the correct number of drones, such as `NUM_DRONES=4 ./run_container.sh sim` + 2. press continue (Enter) once the sim container fully starts +4. Once teh second continue prompt halts execution, wait for the drones to initialize + 1. there is no exact science to this, but the more drone you are running, the longer you'll have to wait (probably) + 2. press continue once you think the drones are initalize +5. wait for all drones to connect and take off, then start controlling the drones + +## What is MultidroneWorld and why does it exist? + +Unlike legacy AirSim, ProjectAirSim uses a multi-file configuration structure (see the [official config docs](https://github.com/iamaisim/ProjectAirSim/blob/main/docs/config.md)). This means that each drone would have to have an individual config file, as the control ports we need to change per drone are stored in the robot config files. To circumvent this, we have created the `MultidroneWorld` class, which is a subclass of ProjectAirSim's `World` class, that intercepts the config loading process to inject new robot config settings automatically rather than storing and loading many files. + +`MultidroneWorld` only overrides the `__init__` function of `World`, and it is nearly identical to `World`'s config, but it modifies the loaded config file (stored as a Python dictionary) before it's used to reload the Unreal scene. It does this based on a new `drone_grid` argument, which is a `(num_rows, num_cols)` tuple. `MultdroneWorld` expects the loaded scene config file to contain one robot already, and it uses that robot's settings as a template for all generated drones. In other words, it deep-copies the drone already existing in the scene and increments its control ports accordingly (matching how the `--instance` argument of `sim_vehicle.py` increments its ports by ten). Additionally, each drone's `xyz` offsets are incremented such that spawned drones form a grid. Their names are of the form `Drone_{row_index}_{col_index}` (in case you want to access them using ProjectAirSim's Python package). + +## Expanding Beyond the Example + +Here are the main take aways on how to use multidrone with ProjectAirsim: + +1. before anything, start the Unreal simulation +2. before starting the `sim` container (or the drone's SITLs), make sure an empty scene (i.e., a scene with no drones in it) is initialized + 1. this is done via Python using the ProjectAirSim `World` class (see the example) + 2. **Note:** this does not need to be done in the same script that your flight code is in---the Unreal simulation is running an API, and the Python code just connects to it; that is, closing your Python code will not undo any initializations you've already made, so it can be a separate script if you'd like + 3. once this is complete, start the SITLs +3. once the SITLs have started, initialize a scene with drones in it + 1. use the `MultidroneWorld` class to automatically add new drones in a grid/matrix shape, using the single drone already provided in the scene config as a base +4. wait for the simulated drones and the SITLs to connect/initialize, then connect to the drones using `dronekit` +5. fly the drones + +It may be smart to have a "simulation init" Python script that initializes the scene properly, then run your flight code as usual after everything is ready. + +## Further Areas of Development + +- find a way to better automate these steps --- it'd be nice to be able to start a multi-drone simulation with a single command or something +- make `MultidroneWorld` more customizable?? + - I was thinking that we could create different callable classes that automatically modify the config dict differently, such as doing different shapes and such + - for instance (for existing grid setup): + + ```python + class DroneGridConfig: + def __init__(self, num_rows, num_cols): + self.num_rows = num_rows + self.num_cols = num_cols + + def __call__(self, config: dict) -> dict: + # generate new config dict + + func = DroneGridConfig(2, 2) + # would be used like func(scene_config) in MultidroneWorld's init + + world = MultidroneWorld(..., config_func=func) + ``` diff --git a/PI_SIM_RUNBOOK.md b/PI_SIM_RUNBOOK.md new file mode 100644 index 0000000..0dde1dd --- /dev/null +++ b/PI_SIM_RUNBOOK.md @@ -0,0 +1,181 @@ +# Pi + ProjectAirSim demo runbook + +Flight code on four Raspberry Pis, SITLs and ProjectAirSim on the Windows/WSL host. This is +the command sequence from a cold boot. For what the pieces are and why the port arithmetic +looks like it does, see [MULTIDRONE_HOWTO.md](./MULTIDRONE_HOWTO.md). + +Architecture: `simulation/interfaces/iarc.py` stands the scene up and holds it open +(`SPAWN_FLIGHT_CODE=0`); each Pi runs `run.py --airsim -i ` and opens dronekit over TCP +back to the host at `5762 + 10*(id-1)`. Pi Zero 2 Ws cannot run SITL themselves. Interdrone +comms ride the batman mesh on `bat0` and never touch the host. + +## 0. Windows -- collect the three addresses + +Nothing here is stable across a reboot except `172.27.192.1`. Do this every time. + +```shell +wsl hostname -I +ipconfig +``` + +| Name | Where it comes from | Last seen | +| --- | --- | --- | +| `WSL_IP` | first address from `wsl hostname -I` | `172.27.193.57` | +| `PAS_HOST` / `AIRSIM_HOST` | `ipconfig` -> `vEthernet (WSL)` IPv4 | `172.27.192.1` (stable) | +| `LAN_IP` | `ipconfig` -> Wi-Fi adapter IPv4; what the Pis dial | `10.106.89.115` | + +`WSL_IP` is DHCP and changes on every `wsl --shutdown` or reboot. A stale value is the single +most common cause of the 300 s `no MAVLink from the SITL` timeout in step 5. + +Then **launch Unreal / ProjectAirSim**. `iarc.py` connects to `PAS_HOST` as its first action, +so this has to be up before anything else. + +## 1. Admin PowerShell -- only when `WSL_IP` changed + +WSL2 is NAT'd, so LAN clients cannot reach the SITLs without portproxy entries (or +`networkingMode=mirrored` in `.wslconfig`). The entries persist in the registry across +reboots, but they point at the old `WSL_IP` -- hence delete-then-add: + +```powershell +$wsl = ((wsl hostname -I) -split '\s+')[0] +foreach ($p in 5762,5772,5782,5792) { + netsh interface portproxy delete v4tov4 listenaddress=0.0.0.0 listenport=$p | Out-Null + netsh interface portproxy add v4tov4 listenaddress=0.0.0.0 listenport=$p connectaddress=$wsl connectport=$p +} +net stop iphlpsvc; net start iphlpsvc +netsh interface portproxy show v4tov4 +``` + +The `iphlpsvc` bounce goes **after** the add, not before -- it is what makes a freshly added +entry take effect. + +One-time, persists across reboots. Both are guarded, so re-running is safe: + +```powershell +if (-not (Get-NetFirewallHyperVRule -Name "WSL-mavlink" -ErrorAction SilentlyContinue)) { + New-NetFirewallHyperVRule -Name "WSL-mavlink" -DisplayName "WSL MAVLink 5760-5800" ` + -Direction Inbound -VMCreatorId '{40E0AC32-46A5-438A-A0B2-2B479E8F2E90}' ` + -Protocol TCP -LocalPorts 5760-5800 -Action Allow +} +if (-not (Get-NetFirewallRule -Name "LAN-mavlink-in" -ErrorAction SilentlyContinue)) { + New-NetFirewallRule -Name "LAN-mavlink-in" -DisplayName "MAVLink from Pis 5760-5800" ` + -Direction Inbound -Protocol TCP -LocalPort 5760-5800 -Action Allow -Profile Private,Public +} +``` + +The Hyper-V rule covers Windows -> WSL VM. The ordinary rule covers Pi -> Windows on the +Wi-Fi adapter, where the portproxy listener lives. + +## 2. WSL terminal 1 -- orchestrator + +```shell +cd ~/IARC-10/simulation && ./run_container.sh env +``` + +Then inside the `env` container: + +```shell +cd /IARC/simulation/interfaces +NUM_DRONES=4 SPAWN_FLIGHT_CODE=0 SITL_HOST= PAS_HOST=172.27.192.1 python iarc.py +``` + +Wait for `Empty scene loaded.` and the `(press enter once the sim container is up)` prompt. +**Do not press Enter yet** -- a drone that spawns before its SITL exists softlocks and only a +full sim restart recovers it. + +`MISSION_CONFIG` is irrelevant here: with `SPAWN_FLIGHT_CODE=0` the mission config that +matters is each Pi's own copy. + +## 3. WSL terminal 2 -- SITLs + +```shell +cd ~/IARC-10/simulation && NUM_DRONES=4 AIRSIM_HOST=172.27.192.1 ./run_container.sh sim +``` + +Must be run from `simulation/` -- that is where `compose.yml` and `.env` live. `.env` already +pins `AIRSIM_HOST=172.27.192.1`, so the inline value is redundant but kept as documentation. +`NUM_DRONES` must match terminal 1; nothing checks that it does. + +This attaches to a tmux session with one window per drone. Cycle through all four +(`Ctrl-b n`) and wait until **each** reports +`Waiting for heartbeat from tcp:127.0.0.1:5760`. + +## 4. WSL terminal 3 -- verify the listeners + +```shell +ss -ltn | grep -E ':(5762|5772|5782|5792)' +``` + +Expect four rows bound to `0.0.0.0`. The `sim` container is `network_mode: host`, so its +listeners show up in the WSL host's namespace directly, and the portproxy from step 1 reaches +them with no relay in between. + +Only if these show `127.0.0.1` do you need a relay -- and then it is one per port, bound to +the WSL interface: + +```shell +for p in 5762 5772 5782 5792; do + socat TCP-LISTEN:$p,bind=$(hostname -I | awk '{print $1}'),fork,reuseaddr TCP:127.0.0.1:$p & +done +``` + +It cannot bind `0.0.0.0:$p` -- that collides with the SITL's own loopback listener. A single +socat on some unrelated port (e.g. `TCP-LISTEN:15762`) does nothing: no other link in the +chain references that port. + +## 5. Back to terminal 1 + +Press Enter. `iarc.py` spawns the four drones, which starts the sensor streams that unblock +the SITLs' physics loops, which is what finally makes them open their MAVLink ports. It then +polls 5762/5772/5782/5792 in turn (300 s timeout each). Wait for: + +``` +Scene is up and all SITLs are emitting MAVLink. +``` + +That line is the go signal for the Pis. Optional reachability check from any Pi first: + +```shell +nc -vz 5762 +``` + +## 6. Each Pi + +One-time cleanup, so `sudo` is not needed. A `Permission denied` unlinking files under +`.venv/` means an earlier `sudo uv run` left root-owned files there: + +```shell +sudo chown -R $USER:$USER ~/IARC-10/.venv && uv sync +``` + +If it comes back, something ran `uv` under `sudo` again -- repeat the chown rather than +re-adding `sudo`. `--airsim` touches no privileged device; only real mode needs `/dev/ttyS0`. + +Then, per Pi, with `` = 1, 2, 3, 4. **Each Pi gets its own id** -- it selects both the +`drone_info` entry and the SITL port `5762 + 10*(id-1)`: + +```shell +SITL_MAVLINK_HOST= uv run run.py --airsim -i +``` + +Preconditions on each Pi: + +- `drone-flight@.service` must not be running. It runs real mode (`/dev/ttyS0`, no + `--airsim`) and is wrong for sim demos: `sudo systemctl stop drone-flight@`. +- Stash `mission_config.json` before `git pull`. `batman-mesh-setup.sh` rewrites it at boot + with the `169.254.97.x` bat0 addresses; the committed version has `127.0.0.x`, which works + only when all four processes share one host and breaks interdrone across four Pis. +- MST-GUEST does not isolate clients, so Pi -> host TCP works on it. `wlan1` carries MAVLink, + `wlan0`/`bat0` carry the mesh. + +## Failure modes + +| Symptom | Cause | +| --- | --- | +| `no MAVLink from the SITL ... after 300s` | stale `SITL_HOST`/portproxy after a WSL IP change; or `NUM_DRONES` mismatch between terminals 1 and 2 | +| Instant `ECONNREFUSED` on a Pi (vs. a ~90 s dronekit timeout) | the Pi dialled its own loopback -- stale checkout without `SITL_MAVLINK_HOST` support | +| Pi connects, no interdrone traffic | `mission_config.json` overwritten with the committed `127.0.0.x` addresses | +| SITL windows stuck on `No sensor message received in last 1s` | drones never spawned, or AirSim's UDP is not reaching `SITL_HOST` | + +The dronekit connection address is logged at INFO, and console logging drops to WARNING after +`flight_log.configure()` -- so it appears in `Logs//drone_.log`, not on stdout. diff --git a/README.md b/README.md index 055b077..3a9a717 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,13 @@ Missouri S&T Multirotor Drone Design Team's simulation code environments. ## Table of Contents + - [Prerequisites](#prerequisites) +### Running the IARC sim + +See [IARC_AIRSIM_README.md](./IARC_AIRSIM_README.md) for the full Project AirSim runbook. + ### Prerequisites -You will need to have followed the installation instructions on our [GitHub page](https://missourimrr.github.io/docs/simulation/) to be able to use this code. \ No newline at end of file + +You will need to have followed the installation instructions on our [GitHub page](https://missourimrr.github.io/docs/simulation/) to be able to use this code. diff --git a/Sim.Containerfile b/Sim.Containerfile index 1c2707a..3209693 100644 --- a/Sim.Containerfile +++ b/Sim.Containerfile @@ -25,11 +25,29 @@ RUN pip install -U numpy # add GolfCourse location to ArduPilot locations RUN echo '# Multirotor Locations\nGolfCourse=37.9490953,-91.7848293,0,0' >> /ardupilot/Tools/autotest/locations.txt -COPY ./sim_start_drones.sh /ardupilot/Tools/autotest/ +COPY ./simulation/sim_start_drones.sh /ardupilot/Tools/autotest/ +COPY ./simulation/templates/multidrone.parm /ardupilot/Tools/autotest/ -# Environment Variables +# Strip carriage returns in case the build context was checked out on Windows with CRLF +# endings. A CRLF shebang makes exec fail with "No such file or directory" naming the +# script itself, which is a genuinely confusing way to spend an afternoon. .gitattributes +# should already prevent this; this is the belt to that pair of braces. +RUN sed -i 's/\r$//' /ardupilot/Tools/autotest/sim_start_drones.sh \ + && chmod +x /ardupilot/Tools/autotest/sim_start_drones.sh + +# ENVIRONMENT VARIABLES + +# The variables below are passed to sim_start_drones.sh within the container (see below) +# NOTE: DO NOT CHANGE THESE HERE (unless defaults change); set the env variables when +# starting the sim container (including when using run_container.sh) + +# OUT_PORT/HOST - sets the port/IP of the sim ENV OUT_PORT=14550 ENV OUT_HOST=127.0.0.1 + +# NUM_DRONES - the number of drones to start +# for multi-drone simulations, it's recommended to use update_airsim_settings.ps1 to automatically +# configure settings correctly. ENV NUM_DRONES=1 -CMD /ardupilot/Tools/autotest/sim_start_drones.sh $NUM_DRONES $OUT_PORT $OUT_HOST +CMD /ardupilot/Tools/autotest/sim_start_drones.sh $NUM_DRONES $OUT_PORT $OUT_HOST \ No newline at end of file diff --git a/compose.yml b/compose.yml index a7e0a27..09f73b3 100644 --- a/compose.yml +++ b/compose.yml @@ -6,20 +6,57 @@ services: dockerfile: simulation/Env.Containerfile container_name: multirotor_env network_mode: host + environment: + # Keep the project venv off the Windows bind mount. Left at the default /IARC/.venv + # it is rebuilt on every single run (~2.5 min): the venv is shared with the Windows + # host, so its bin/python3 symlink does not resolve inside the container and uv + # discards it. The uv cache lives under the same volume so wheel installs can + # hardlink instead of full-copying. + - UV_PROJECT_ENVIRONMENT=/opt/uv/venv + - UV_CACHE_DIR=/opt/uv/cache volumes: - type: bind - source: ./ - target: /SUAS - working_dir: /SUAS + source: .. + target: /IARC + # Named volume, so the venv survives `podman-compose run --rm`. + - iarc_uv:/opt/uv + working_dir: /IARC command: bash stdin_open: true tty: true sim: + environment: # See Sim.Containerfile for explanations + - OUT_PORT=14550 + - OUT_HOST=127.0.0.1 + # NUM_DRONES must match the drone count passed to simulation/interfaces/iarc.py, + # since both sides derive their ports from it independently. + - NUM_DRONES=${NUM_DRONES:-1} + # AIRSIM_HOST is where the Unreal/ProjectAirSim server is reachable from inside WSL. + # Leave it unset when Unreal runs on the same host; set it to the Windows-side WSL + # adapter IP (see `ipconfig`, "vEthernet (WSL)") when Unreal runs on Windows, e.g. + # NUM_DRONES=2 AIRSIM_HOST=172.27.192.1 ./run_container.sh sim + - AIRSIM_HOST=${AIRSIM_HOST:-127.0.0.1} build: context: .. dockerfile: simulation/Sim.Containerfile container_name: multirotor_sim network_mode: host - command: python /ardupilot/Tools/autotest/sim_vehicle.py -v ArduCopter -f airsim-copter --out=127.0.0.1:14550 + volumes: + # Bind-mounted over the copy baked in by Sim.Containerfile, so edits to the port + # arithmetic take effect on the next run instead of needing an image rebuild. + - type: bind + source: ./sim_start_drones.sh + target: /ardupilot/Tools/autotest/sim_start_drones.sh + # sim_start_drones.sh launches NUM_DRONES SITL instances in tmux windows, assigning + # each one its own AirSim UDP port pair and MAVLink TCP ports. It handles the N=1 case + # too, so single- and multi-drone runs share one code path. + # Invoked via `bash` rather than executed directly: the bind mount above comes off a + # Windows drive, where the exec bit does not reliably survive. + # --sim-address is sim_vehicle.py's own option; passing it via -A instead would leave + # sim_vehicle's default 127.0.0.1 on the command line as a confusing duplicate. + command: bash /ardupilot/Tools/autotest/sim_start_drones.sh stdin_open: true - tty: true \ No newline at end of file + tty: true + +volumes: + iarc_uv: diff --git a/interfaces/iarc.py b/interfaces/iarc.py new file mode 100644 index 0000000..0a7362a --- /dev/null +++ b/interfaces/iarc.py @@ -0,0 +1,338 @@ +import collections +import collections.abc +import copy +import os + +# commentjson (a projectairsim dependency) still uses the pre-3.10 collections aliases +collections.MutableMapping = collections.abc.MutableMapping + +import socket +import subprocess +import time +from pathlib import Path + +from projectairsim import Drone, ProjectAirSimClient, World +from projectairsim.utils import load_scene_config_as_dict, projectairsim_log + +# this file lives at /simulation/interfaces/iarc.py +PROJECT_ROOT = Path(__file__).resolve().parents[2] +SIM_CONFIG_PATH = str(PROJECT_ROOT / "simulation" / "sim_config") + +EMPTY_SCENE = "/IARC/simulation/sim_config/scene_ardu_empty.jsonc" +IARC_SCENE = "/IARC/simulation/sim_config/scene_iarc.jsonc" + +# How many drones to fly. Must match NUM_DRONES passed to the sim container, since both +# sides derive their port assignments from it independently. +NUM_DRONES = int(os.environ.get("NUM_DRONES", "1")) + +# Mission config the flight code reads. It must contain a drone_info entry for every drone +# ID this script spawns (1..NUM_DRONES). +MISSION_CONFIG = os.environ.get("MISSION_CONFIG", "mission_config.json") + +# Whether this script also runs the flight code. Set to 0 when the flight code lives +# somewhere else -- e.g. one Raspberry Pi per drone, each connecting back to the SITLs on +# this host. In that case this script only stands the scene up and holds it open. +SPAWN_FLIGHT_CODE = os.environ.get("SPAWN_FLIGHT_CODE", "1") not in ("0", "false", "False") + +# Where the ArduPilot SITL is reachable from the machine running Unreal. When Unreal runs +# on Windows and the SITL runs in a WSL container, this is the WSL VM's IP (`hostname -I` +# inside WSL); both default to loopback for an all-on-one-host setup. +SITL_HOST = os.environ.get("SITL_HOST", "127.0.0.1") +# Address the Unreal side binds to receive actuator packets from the SITL. 0.0.0.0 accepts +# them regardless of which interface they arrive on. +AIRSIM_BIND_HOST = os.environ.get("AIRSIM_BIND_HOST", "0.0.0.0") + +# Port arithmetic. MUST stay in sync with simulation/sim_start_drones.sh and with the port +# computed in state_machine/drone.py. For drone index i (0-based): +# dronekit talks to SITL serial1 at 5762 + 10i; serial0 (5760 + 10i) is taken by MAVProxy +# AirSim sends sensor data to 9003 + 10i +# AirSim listens for servos on 9002 + 10i +SITL_MAVLINK_PORT = int(os.environ.get("SITL_MAVLINK_PORT", "5762")) +# Where *this* process reaches the SITLs' MAVLink TCP ports. Usually the same host AirSim +# sends sensors to, but kept separate because the two can differ when the SITL container +# publishes MAVLink on a different address than the one it receives UDP on. +SITL_MAVLINK_HOST = os.environ.get("SITL_MAVLINK_HOST", SITL_HOST) +PORT_STRIDE = 10 +SITL_WAIT_SEC = 300 + +# Metres between adjacent drones in the scene, along the scene's x axis. Without this they +# spawn on top of each other and collide on takeoff. +DRONE_SEPARATION_M = float(os.environ.get("DRONE_SEPARATION_M", "3.0")) + +# Log directory name shared by every drone in this run. All the flight-code processes are +# children of this one, so setting it here is what lets tools/analyze_flight.py put them on +# a single timeline -- without it each process invents its own id and the logs scatter. +# Logs land in /IARC/Logs, a bind mount of the repo, so they show up on the host as well. +FLIGHT_LOG_RUN = os.environ.get("FLIGHT_LOG_RUN") or time.strftime( + "run_%Y%m%d_%H%M%S", time.gmtime() +) + +# The chase camera is a 1280x720 stream per drone. One is useful for watching the run; N of +# them is a large GPU cost for no benefit, so it is kept only on the first drone. +CHASE_CAMERA_ID = "Chase" + + +def drone_name(drone_id: int) -> str: + """Scene actor name for a drone ID. IDs are 1-based to match mission_config.json.""" + return f"Drone{drone_id}" + + +class ArduWorld(World): + """World that rewrites each robot's ArduPilot endpoints before loading the scene. + + ProjectAirSim reads robot configs straight off disk, so the only place to override the + SITL addresses without maintaining a per-machine copy of robot_ardu_quadrotor.jsonc is + between the config being parsed and the scene being loaded. Same interception trick as + MultidroneWorld in simulation/multidrone_world.py. + + For multi-drone runs this also clones the single robot declared in the scene config + into `num_drones` actors, incrementing each one's UDP ports and spawn position so they + line up with the SITL instances started by sim_start_drones.sh. + """ + + def __init__( + self, + client: ProjectAirSimClient, + scene_config_name: str, + delay_after_load_sec: int = 0, + sim_config_path: str = "sim_config/", + sim_instance_idx: int = -1, + num_drones: int = 1, + ): + self.client = client + self.sim_config_path = sim_config_path + self.sim_instance_idx = sim_instance_idx + self.parent_topic = "/Sim/SceneBasicDrone" # default-scene's ID + + self.sim_config = None + self.home_geo_point = None + + config_dict, config_paths = load_scene_config_as_dict( + scene_config_name, sim_config_path, sim_instance_idx + ) + + config_dict["actors"] = self._build_actors(config_dict.get("actors", []), num_drones) + + self.scene_config_path = config_paths[0] + self.robot_config_paths = config_paths[1] + self.envactor_config_paths = config_paths[2] + self.load_scene(config_dict, delay_after_load_sec=delay_after_load_sec) + + def _build_actors(self, actors: list, num_drones: int) -> list: + """Expand the scene's single robot template into `num_drones` configured actors.""" + if not actors: + return actors + + template = actors[0] + start_x, start_y, z = map(float, template["origin"]["xyz"].split()) + + built = [] + for index in range(num_drones): + actor = copy.deepcopy(template) + actor["name"] = drone_name(index + 1) + actor["origin"]["xyz"] = " ".join( + str(v) for v in (start_x + index * DRONE_SEPARATION_M, start_y, z) + ) + + settings = actor.get("robot-config", {}).get("controller", {}).get("ardupilot-settings") + if settings is None: + projectairsim_log().warning( + f"Actor '{actor['name']}' has no ardupilot-settings; leaving it as-is." + ) + built.append(actor) + continue + + # The template carries drone 0's ports, so offset from those rather than from a + # hardcoded base -- that keeps robot_ardu_quadrotor.jsonc the single source of + # truth for the starting port numbers. + settings["ardupilot-udp-port"] += index * PORT_STRIDE + settings["local-host-udp-port"] += index * PORT_STRIDE + settings["ardupilot-ip"] = SITL_HOST + settings["local-host-ip"] = AIRSIM_BIND_HOST + + if index > 0: + self._disable_chase_camera(actor) + + projectairsim_log().info( + f"Actor '{actor['name']}': sending sensors to SITL at " + f"{settings['ardupilot-ip']}:{settings['ardupilot-udp-port']}, " + f"listening for control on " + f"{settings['local-host-ip']}:{settings['local-host-udp-port']}" + ) + built.append(actor) + + return built + + @staticmethod + def _disable_chase_camera(actor: dict) -> None: + """Turn off the chase camera on an actor to keep the GPU cost of N drones sane.""" + for sensor in actor.get("robot-config", {}).get("sensors", []): + if sensor.get("id") == CHASE_CAMERA_ID: + sensor["enabled"] = False + + +def wait_for_sitl_launch() -> None: + """Pause until the user confirms the ArduPilot SITL has been launched. + + This cannot be automated by probing a port. AirSim::recv_fdm() blocks in a retry loop + until the first sensor packet arrives from AirSim, so a freshly launched SITL has not + yet opened any of its MAVLink TCP ports -- it sits there printing "No sensor message + received in last 1s, resending servos". Those ports only appear *after* a drone spawns + and starts feeding it. So the only observable signal at this point is the sim + container's own output. + + Set SITL_START_DELAY to a number of seconds to wait blindly instead of prompting. + """ + delay = os.environ.get("SITL_START_DELAY") + if delay is not None: + projectairsim_log().info(f"Waiting {delay}s for the SITL to launch...") + time.sleep(float(delay)) + return + + projectairsim_log().info( + f"Empty scene loaded. Now start {NUM_DRONES} SITL(s) in another terminal:\n" + f" NUM_DRONES={NUM_DRONES} AIRSIM_HOST= ./run_container.sh sim\n" + "Wait until every tmux window reports 'Waiting for heartbeat from tcp:127.0.0.1:5760', " + "then press Enter here to spawn the drones." + ) + input("(press enter once the sim container is up) ") + + +def wait_for_mavlink(host: str = SITL_MAVLINK_HOST, port: int = SITL_MAVLINK_PORT) -> None: + """Block until the SITL is actually emitting MAVLink. + + A successful TCP connect is not enough: the listening socket is open from the moment + the SITL starts, but nothing is written to it while the physics loop is stuck in + recv_fdm() waiting on AirSim. So read until a MAVLink frame header shows up (0xFE for + v1, 0xFD for v2), which only happens once the drone is feeding the SITL sensor data. + """ + projectairsim_log().info(f"Waiting for SITL MAVLink heartbeats on {host}:{port}...") + deadline = time.monotonic() + SITL_WAIT_SEC + while time.monotonic() < deadline: + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock: + sock.settimeout(2.0) + if sock.connect_ex((host, port)) == 0: + try: + data = sock.recv(64) + except (socket.timeout, OSError): + data = b"" + if any(b in data for b in (b"\xfd", b"\xfe")): + projectairsim_log().info(f"SITL on {host}:{port} is emitting MAVLink.") + return + time.sleep(2) + raise TimeoutError( + f"no MAVLink from the SITL on {host}:{port} after {SITL_WAIT_SEC}s. The SITL is " + "stuck in recv_fdm() waiting on AirSim sensor packets, so AirSim's UDP is not " + f"reaching {SITL_HOST}, or {host} is not where its MAVLink ports are exposed. Check " + "the WSL Hyper-V firewall inbound rule, SITL_HOST, SITL_MAVLINK_HOST, --sim-address, " + "and that NUM_DRONES matches on both sides." + ) + + +def run_iarc_code(drone_ids: list[int]) -> None: + """Run one flight-code process per drone and wait for them all to finish. + + Each process gets its own -i so it picks up the matching drone_info entry (and so + Drone.use_settings computes the matching SITL port). They talk to each other over the + interdrone loopback addresses declared in the mission config. + """ + project_root = "/IARC" + processes: list[tuple[int, subprocess.Popen]] = [] + + # Inherited by every child, so all drones write into the same run directory. + child_env = dict(os.environ, FLIGHT_LOG_RUN=FLIGHT_LOG_RUN) + projectairsim_log().info( + f"Flight logs for this run: /IARC/Logs/{FLIGHT_LOG_RUN} " + f"(analyze with: python tools/analyze_flight.py Logs/{FLIGHT_LOG_RUN})" + ) + + try: + for drone_id in drone_ids: + command = ["uv", "run", "run.py", "--airsim", "-i", str(drone_id)] + if MISSION_CONFIG: + command += ["--config", MISSION_CONFIG] + projectairsim_log().info(f"Launching flight code for drone {drone_id}: {command}") + processes.append( + (drone_id, subprocess.Popen(command, cwd=project_root, text=True, env=child_env)) + ) + + for drone_id, process in processes: + code = process.wait() + if code == 0: + print(f"Drone {drone_id} flight script executed successfully!") + else: + print(f"Drone {drone_id} flight script exited with code {code}") + except FileNotFoundError: + print("Error: 'uv' is not installed") + finally: + for _, process in processes: + if process.poll() is None: + process.terminate() + + +def wait_for_external_flight_code() -> None: + """Hold the scene open while the flight code runs elsewhere. + + Disconnecting the client does not tear the Unreal scene down, but this process owns the + Drone handles created in main(), so exiting here would drop the sensor streams the SITLs + depend on. Block until interrupted instead. + """ + projectairsim_log().info( + "Scene is up and all SITLs are emitting MAVLink. Start the flight code on each " + "drone's machine now. On the Pi for drone , with the LAN address of " + "the machine running Unreal:\n" + " SITL_MAVLINK_HOST= uv run run.py --airsim -i \n" + f"Drone connects to :{SITL_MAVLINK_PORT} + {PORT_STRIDE}*(-1), " + f"i.e. {', '.join(str(SITL_MAVLINK_PORT + i * PORT_STRIDE) for i in range(NUM_DRONES))} " + f"for drones 1..{NUM_DRONES}.\n" + "Press Ctrl-C here once the run is over." + ) + try: + while True: + time.sleep(1) + except KeyboardInterrupt: + projectairsim_log().info("Shutting the scene down.") + + +def main(): + # Initialize Project AirSim Client + client = ProjectAirSimClient(address=os.environ.get("PAS_HOST", "127.0.0.1")) + drone_ids = list(range(1, NUM_DRONES + 1)) + + try: + print(f"Connecting to projectAirSim for {NUM_DRONES} drone(s)...") + client.connect() + + # 1. An empty scene, so the SITL has something to pull scene data from on startup. + World(client, EMPTY_SCENE, delay_after_load_sec=2) + + # 2. The SITLs, which must be running before any drone spawns -- a drone that finds + # no SITL on spawn softlocks and cannot recover. + wait_for_sitl_launch() + + # 3. The real scene. Spawning the drones starts the sensor streams that unblock the + # SITLs' physics loops, which in turn makes them open their MAVLink ports. + world = ArduWorld(client, IARC_SCENE, delay_after_load_sec=2, num_drones=NUM_DRONES) + for drone_id in drone_ids: + Drone(client, world, drone_name(drone_id)) + + # 4. Only now can anything speak MAVLink to the SITLs. + for index in range(NUM_DRONES): + wait_for_mavlink(port=SITL_MAVLINK_PORT + index * PORT_STRIDE) + + # 5. The mission. run.py opens its own dronekit connection to the SITL rather than + # reusing the ProjectAirSim handle above, which only carries sensor/telemetry + # topics. + if SPAWN_FLIGHT_CODE: + run_iarc_code(drone_ids) + else: + wait_for_external_flight_code() + + except Exception as err: + projectairsim_log().error(f"Exception occurred: {err}", exc_info=True) + finally: + client.disconnect() + + +if __name__ == "__main__": + main() diff --git a/interfaces/suas.py b/interfaces/suas.py new file mode 100644 index 0000000..e523f3a --- /dev/null +++ b/interfaces/suas.py @@ -0,0 +1,165 @@ +import asyncio + +import collections +import collections.abc + +# Add the missing attribute back to the collections module +collections.MutableMapping = collections.abc.MutableMapping + +from projectairsim import ProjectAirSimClient, Drone, World +from projectairsim.utils import projectairsim_log +from projectairsim.image_utils import ImageDisplay + +import asyncio +import multiprocessing as mp +import time +import sys + +from dronekit import LocationGlobalRelative, VehicleMode, connect + +from ..multidrone_world import MultidroneWorld + +class DronekitDrone: + + def __init__(self, connection_string): + self._connection_string = connection_string + self._drone = None + + def connect(self, timeout=30): + print("connecting", self._connection_string) + vehicle = connect(self._connection_string, wait_ready=True, timeout=timeout) + + # Get some vehicle attributes (state) + print("Get some vehicle attribute values:") + print(" GPS: %s" % vehicle.gps_0) + print(" Battery: %s" % vehicle.battery) + print(" Last Heartbeat: %s" % vehicle.last_heartbeat) + print(" Is Armable?: %s" % vehicle.is_armable) + print(" System status: %s" % vehicle.system_status.state) + print(" Mode: %s" % vehicle.mode.name) + + while not vehicle.is_armable: + print("Waiting for vehicle to initialize...") + time.sleep(1) + + vehicle.parameters["ARMING_CHECK"] = 0 + vehicle.mode = VehicleMode("GUIDED") + vehicle.armed = True + + self._drone = vehicle + + def takeoff(self, alt): + self._drone.simple_takeoff(alt) + self._takeoff_alt = alt + + def translate(self, dlat, dlon, dalt): + loc = self._drone.location.global_relative_frame + lat, lon, alt = loc.lat, loc.lon, loc.alt + + self._drone.simple_goto( + LocationGlobalRelative(lat + dlat, lon + dlon, alt + dalt) + ) + + def goto(self, lat, lon, alt): + self._drone.simple_goto(LocationGlobalRelative(lat, lon, alt)) + + @property + def took_off(self): + return self._drone.location.global_relative_frame.alt >= self._takeoff_alt * 0.9 + + @property + def loc(self): + return self._drone.location.global_relative_frame + + def land(self): + self._drone.mode = VehicleMode("LAND") + + def close(self): + self._drone.close() + +def run_drone(connection_string, queue, timeout=30): + drone = DronekitDrone(connection_string) + drone.connect(timeout) + time.sleep(3) + + while True: + cmd = queue.get() + + if cmd is None: + drone.land() + drone.close() + break + elif cmd == "takeoff": + drone.takeoff(20) + while not drone.took_off: + print("Waiting for drone to finish takeoff...") + time.sleep(1) + else: + drone.translate(*cmd) + +def run_suas_code(): + project_root = "/SUAS" + command = ["uv", "run", "run.py", "--airsim"] + try: + process = subprocess.run( + command, + cwd=project_root, + check=True, + text=True, + capture_output=False + ) + print("Flight script executed successfully!") + except subprocess.CalledProcessError as err: + print(f"The simulation failed with exit code: {err}") + except FileNotFoundError: + print("Error: 'uv' is not installed") + + +def main(): + # Initialize Project AirSim Client + client = ProjectAirSimClient() + + try: + print("Connecting to projectAirSim...") + client.connect() + # Load the world and vehicle defined in your JSONC + world = World(client, "scene_ardu_empty.jsonc", delay_after_load_sec=2, sim_config_path="./simulation/sim_config") + + input("Start your sim container now. Press enter to continue (add drones to scene)") + + # SET DRONE GRID HERE + drone_grid = (4, 4) + processes = [] + + world = MultidroneWorld(client, "scene_ardu_quadrotor.jsonc", delay_after_load_sec=2, + sim_config_path="./simulation/sim_config", drone_grid=drone_grid) + + input("Press enter to start connections (may need to wait a while for drones to get ready)") + # Create a World object to interact with the sim world and load a scene + base_port = 5762 + drone_count = drone_grid[0] * drone_grid[1] + queues = [mp.Queue() for _ in range(drone_count)] + + # start drone processes, assign connection string + for port, queue in zip(range(base_port, base_port + 10 * drone_count, 10), queues): + proc = mp.Process(target=run_drone, args=(f"tcp:127.0.0.1:{port}", queue, 120)) + proc.start() + + processes.append(proc) + + for queue in queues: + queue.put("takeoff") + + # Execute the flight logic + run_suas_code() + + except Exception as err: + projectairsim_log().error(f"Exception occurred: {err}", exc_info=True) + finally: + client.disconnect() + + for p in processes: + p.join() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/multidrone_world.py b/multidrone_world.py new file mode 100644 index 0000000..451f418 --- /dev/null +++ b/multidrone_world.py @@ -0,0 +1,83 @@ +import copy +import random + +from projectairsim import ProjectAirSimClient +from projectairsim.utils import load_scene_config_as_dict +from projectairsim.world import World + + +class MultidroneWorld(World): + + # ProjectAirsim has reading config from a file deeply integrated into it + # You can't even trick it using a buffer or something. + # Thus, the easiest strat (without creating a million temp files) is + # to essentially intercept the initialization process after the config is + # read to a dict and inject our generated settings there. + # Most of the below code is the same as the normal World class __init__ + def __init__( + self, + client: ProjectAirSimClient, + scene_config_name: str = "", + delay_after_load_sec: int = 0, + sim_config_path: str = "sim_config/", + sim_instance_idx: int = -1, + drone_grid: tuple[int, int] | None = None, + x_sep: float = 3.0, + y_sep: float = 3.0, + ): + """ProjectAirSim World Interface. + + Args: + client (ProjectAirSimClient): ProjectAirSim client object + scene_config (str): Name of the scene config JSON file to load in the sim + delay_after_load_sec (int): Time in seconds to wait after the scene is loaded + sim_config_path (string): Relative path to search for the scene_config + sim_instance_idx (int): the instance index of the simulation (for distributed sim only) + """ + self.client = client + self.sim_config_path = sim_config_path + self.sim_instance_idx = sim_instance_idx + self.parent_topic = "/Sim/SceneBasicDrone" # default-scene's ID + + self.sim_config = None + self.home_geo_point = None + if scene_config_name: + config_loaded, config_paths = load_scene_config_as_dict( + scene_config_name, + sim_config_path, + sim_instance_idx, + ) + config_dict = config_loaded + + if drone_grid is not None: + row, col = drone_grid + template = config_dict["actors"][0] + template["name"] = "Drone_0_0" + start_x, start_y, z = map(float, template["origin"]["xyz"].split()) + + for r in range(row): + for c in range(col): + # don't remake existing drone (i.e., the template) + if r == 0 and c == 0: + continue + + new_drone = copy.deepcopy(template) + new_drone["name"] = f"Drone_{r}_{c}" + + new_drone["origin"]["xyz"] = " ".join(map(str, [start_x + c * x_sep, start_y + r * y_sep, z])) + + drone_num = col * r + c + ardu_settings = new_drone["robot-config"]["controller"]["ardupilot-settings"] + ardu_settings["ardupilot-udp-port"] += 10 * drone_num + ardu_settings["local-host-udp-port"] += 10 * drone_num + + config_dict["actors"].append(new_drone) + + self.scene_config_path = config_paths[0] + self.robot_config_paths = config_paths[1] + self.envactor_config_paths = config_paths[2] + self.load_scene(config_dict, delay_after_load_sec=delay_after_load_sec) + random.seed() + self.import_ned_trajectory( + "null_trajectory", [0, 1], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0] + ) \ No newline at end of file diff --git a/sim_config/robot_ardu_quadrotor.jsonc b/sim_config/robot_ardu_quadrotor.jsonc new file mode 100644 index 0000000..ac9e284 --- /dev/null +++ b/sim_config/robot_ardu_quadrotor.jsonc @@ -0,0 +1,464 @@ +{ + "physics-type": "fast-physics", + "links": [ + { + "name": "Frame", + "inertial": { + "mass": 1.0, + "inertia": { + "type": "geometry", + "geometry": { + "box": { + "size": "0.180 0.110 0.040" + } + } + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "box": { + "size": "0.180 0.110 0.040" + } + } + } + }, + "collision": { + "restitution": 0.1, + "friction": 0.5 + }, + "visual": { + "geometry": { + "type": "unreal_mesh", + "name": "/Drone/Quadrotor1" + } + } + }, + { + "name": "Prop_FL", + "inertial": { + "origin": { + "xyz": "0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 0.1143, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "/Drone/PropellerRed" + } + } + }, + { + "name": "Prop_FR", + "inertial": { + "origin": { + "xyz": "0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 0.1143, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "/Drone/PropellerRed" + } + } + }, + { + "name": "Prop_RL", + "inertial": { + "origin": { + "xyz": "-0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 0.1143, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "-0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "/Drone/PropellerWhite" + } + } + }, + { + "name": "Prop_RR", + "inertial": { + "origin": { + "xyz": "-0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 0.1143, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "-0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "/Drone/PropellerWhite" + } + } + } + ], + "joints": [ + { + "id": "Frame_Prop_FL", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_FL", + "axis": "0 0 1" + }, + { + "id": "Frame_Prop_FR", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_FR", + "axis": "0 0 1" + }, + { + "id": "Frame_Prop_RL", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_RL", + "axis": "0 0 1" + }, + { + "id": "Frame_Prop_RR", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_RR", + "axis": "0 0 1" + } + ], + "controller": { + "id": "ArduPilot_Controller", + "type": "ardupilot-api", + "ardupilot-settings": { + "ardupilot-ip": "127.0.0.1", + "ardupilot-udp-port": 9003, // AirSim sends to the SITL(ardupilot) + + "local-host-ip": "127.0.0.1", + "local-host-udp-port": 9002, // ardupilot sends to Unreal(AirSim) + + "use-distance-sensor": false, + "actuator-order": [ + { + "id": "Prop_FR_actuator" + }, + { + "id": "Prop_RL_actuator" + }, + { + "id": "Prop_FL_actuator" + }, + { + "id": "Prop_RR_actuator" + } + ] + } + }, + "actuators": [ + { + "name": "Prop_FL_actuator", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_FL", + "origin": { + "xyz": "0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "clock-wise", + "normal-vector": "0.0 0.0 -1.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + }, + { + "name": "Prop_FR_actuator", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_FR", + "origin": { + "xyz": "0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "counter-clock-wise", + "normal-vector": "0.0 0.0 -1.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + }, + { + "name": "Prop_RL_actuator", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_RL", + "origin": { + "xyz": "-0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "counter-clock-wise", + "normal-vector": "0.0 0.0 -1.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + }, + { + "name": "Prop_RR_actuator", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_RR", + "origin": { + "xyz": "-0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "clock-wise", + "normal-vector": "0.0 0.0 -1.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + } + ], + "sensors": [ + { + "id": "Chase", + "type": "camera", + "enabled": true, + "parent-link": "Frame", + "capture-interval": 0.03, + "capture-settings": [ + { + "image-type": 0, + "width": 1280, + "height": 720, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": true, + "pixels-as-float": false, + "compress": false, + "target-gamma": 2.5 + } + ], + "gimbal": { + "lock-roll": true, + "lock-pitch": true, + "lock-yaw": false + }, + "origin": { + "xyz": "-10.0 0.0 -1.0", + "rpy-deg": "0 -11.46 0" + } + }, + { + // Matched to the Raspberry Pi AI Camera (Sony IMX500) so ground footprints and + // pixel-to-ground maths carry over from the sim to the real airframe. + // + // The published spec gives only a diagonal FoV of 78.3 deg. With the sensor's + // 4056x3040 array of 1.55 um pixels (6.287 x 4.712 mm, 7.857 mm diagonal) that works + // out to 66.16 deg horizontal and 52.05 deg vertical. + // + // fov-degrees is the HORIZONTAL angle; the vertical one follows from width:height, so + // the 4:3 render aspect is what makes the vertical come out right (52.08 deg, 0.03 deg + // off). The old 400x225 was 16:9, which gave a 40.25 deg vertical -- nearly 12 deg + // narrow -- no matter what fov-degrees said. + // + // Deliberately no "gimbal" block: the real camera is bolted to the frame and tilts + // with the airframe. Stabilising it here would hide exactly the attitude compensation + // vision/common/drone_coordinates.py exists to do. + "id": "DownCamera", + "type": "camera", + "enabled": true, + "parent-link": "Frame", + "capture-interval": 0.02, + "capture-settings": [ + { + "image-type": 0, + "width": 640, + "height": 480, + "fov-degrees": 66.16, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false, + "target-gamma": 2.5 + }, + { + "image-type": 1, + "width": 640, + "height": 480, + "fov-degrees": 66.16, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + }, + { + "image-type": 2, + "width": 640, + "height": 480, + "fov-degrees": 66.16, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + }, + { + "image-type": 3, + "width": 640, + "height": 480, + "fov-degrees": 66.16, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + } + ], + "noise-settings": [ + { + "enabled": false, + "image-type": 1, + "rand-contrib": 0.2, + "rand-speed": 100000.0, + "rand-size": 500.0, + "rand-density": 2, + "horz-wave-contrib": 0.03, + "horz-wave-strength": 0.08, + "horz-wave-vert-size": 1.0, + "horz-wave-screen-size": 1.0, + "horz-noise-lines-contrib": 1.0, + "horz-noise-lines-density-y": 0.01, + "horz-noise-lines-density-xy": 0.5, + "horz-distortion-contrib": 1.0, + "horz-distortion-strength": 0.002 + } + ], + "origin": { + "xyz": "0 0.0 0.0", + "rpy-deg": "0 -90 0" + } + }, + { + "id": "IMU1", + "type": "imu", + "enabled": true, + "parent-link": "Frame", + "accelerometer": { + "velocity-random-walk": 2.353e-3, + "tau": 800, + "bias-stability": 3.53e-4, + "turn-on-bias": "0 0 0" + }, + "gyroscope": { + "angle-random-walk": 8.72644e-5, + "tau": 500, + "bias-stability": 2.23014e-5, + "turn-on-bias": "0 0 0" + } + }, + { + "id": "GPS", + "type": "gps", + "enabled": true, + "parent-link": "Frame" + }, + { + "id": "Barometer", + "type": "barometer", + "enabled": true, + "parent-link": "Frame" + }, + { + "id": "Magnetometer", + "type": "magnetometer", + "enabled": true, + "parent-link": "Frame" + } + ] +} diff --git a/sim_config/robot_quadrotor_fastphysics.jsonc b/sim_config/robot_quadrotor_fastphysics.jsonc index 3242386..d230415 100644 --- a/sim_config/robot_quadrotor_fastphysics.jsonc +++ b/sim_config/robot_quadrotor_fastphysics.jsonc @@ -469,6 +469,14 @@ "type": "magnetometer", "enabled": false, "parent-link": "Frame" + }, + { + "mavlink": { + "type": "Mavlink", + "udp-ip": "127.0.0.1", + "udp-port": 14550, + "use-serial": false + } } ] } \ No newline at end of file diff --git a/sim_config/scene_ardu_empty.jsonc b/sim_config/scene_ardu_empty.jsonc new file mode 100644 index 0000000..b41fc6f --- /dev/null +++ b/sim_config/scene_ardu_empty.jsonc @@ -0,0 +1,21 @@ +{ + "id": "SUAS_scene", + "actors": [], + "clock": { + "type": "steppable", + "step-ns": 3000000, + "real-time-update-rate": 3000000, + "pause-on-start": false + }, + "home-geo-point": { + "latitude": 47.641468, + "longitude": -122.140165, + "altitude": 122.0 + }, + "segmentation": { + "initialize-ids": true, + "ignore-existing": false, + "use-owner-name": true + }, + "scene-type": "UnrealNative" +} \ No newline at end of file diff --git a/sim_config/scene_ardu_quadrotor_template.jsonc b/sim_config/scene_ardu_quadrotor_template.jsonc new file mode 100644 index 0000000..a338466 --- /dev/null +++ b/sim_config/scene_ardu_quadrotor_template.jsonc @@ -0,0 +1,31 @@ +{ + "id": "SUAS_scene", + "actors": [ + { + "type": "robot", + "name": "Drone1", + "origin": { + "xyz": "0.0 0.0 -15.0", + "rpy-deg": "0 0 0" + }, + "robot-config": "/SUAS/simulation/sim_config/robot_ardu_quadrotor.jsonc" // Change /SUAS/ to whatever directory + } + ], + "clock": { + "type": "steppable", + "step-ns": 3000000, + "real-time-update-rate": 3000000, + "pause-on-start": false + }, + "home-geo-point": { // Current coordinates are the golf course + "latitude": 37.94894101091474, + "longitude": -91.78455965055593, + "altitude": 15.0 + }, + "segmentation": { + "initialize-ids": true, + "ignore-existing": false, + "use-owner-name": true + }, + "scene-type": "UnrealNative" +} \ No newline at end of file diff --git a/sim_config/scene_iarc.jsonc b/sim_config/scene_iarc.jsonc new file mode 100644 index 0000000..a9befb6 --- /dev/null +++ b/sim_config/scene_iarc.jsonc @@ -0,0 +1,31 @@ +{ + "id": "IARC_scene", + "actors": [ + { + "type": "robot", + "name": "Drone1", + "origin": { + "xyz": "0.0 0.0 -15.0", + "rpy-deg": "0 0 0" + }, + "robot-config": "/IARC/simulation/sim_config/robot_ardu_quadrotor.jsonc" + } + ], + "clock": { + "type": "steppable", + "step-ns": 3000000, + "real-time-update-rate": 3000000, + "pause-on-start": false + }, + "home-geo-point": { // Golf course coordinates + "latitude": 37.94894101091474, + "longitude": -91.78455965055593, + "altitude": 15.0 + }, + "segmentation": { + "initialize-ids": true, + "ignore-existing": false, + "use-owner-name": true + }, + "scene-type": "UnrealNative" +} \ No newline at end of file diff --git a/sim_config/scene_suas.jsonc b/sim_config/scene_suas.jsonc index 5d7f866..daddc18 100644 --- a/sim_config/scene_suas.jsonc +++ b/sim_config/scene_suas.jsonc @@ -3,69 +3,12 @@ "actors": [ { "type": "robot", - "name": "Drone1", + "name": "Drone", "origin": { - "reference-frame": "Frame", - "xyz": "0 0 -15", + "xyz": "0.0 0.0 -15.0", "rpy-deg": "0 0 0" }, - "robot-config": "robot_quadrotor_fastphysics.jsonc", - "start-landed": true - }, - { - "type": "robot", - "name": "Drone2", - "origin": { - "reference-frame": "Frame", - "xyz": "0 10 -15", - "rpy-deg": "0 0 0" - }, - "robot-config": "robot_quadrotor_fastphysics.jsonc", - "start-landed": true - }, - { - "type": "robot", - "name": "Drone3", - "origin": { - "reference-frame": "Frame", - "xyz": "0 5 -15", - "rpy-deg": "0 0 0" - }, - "robot-config": "robot_quadrotor_fastphysics.jsonc", - "start-landed": true - }, - { - "type": "robot", - "name": "Drone4", - "origin": { - "reference-frame": "Frame", - "xyz": "0 15 -15", - "rpy-deg": "0 0 0" - }, - "robot-config": "robot_quadrotor_fastphysics.jsonc", - "start-landed": true - }, - { - "type": "robot", - "name": "Drone5", - "origin": { - "reference-frame": "Frame", - "xyz": "0 20 -15", - "rpy-deg": "0 0 0" - }, - "robot-config": "robot_quadrotor_fastphysics.jsonc", - "start-landed": true - }, - { - "type": "robot", - "name": "Drone1111", - "origin": { - "reference-frame": "Frame", - "xyz": "0 20 -15", - "rpy-deg": "0 0 0" - }, - "robot-config": "robot_quadrotor_fastphysics.jsonc", - "start-landed": true + "robot-config": "/SUAS/simulation/sim_config/robot_ardu_quadrotor.jsonc" } ], "clock": { @@ -74,10 +17,10 @@ "real-time-update-rate": 3000000, "pause-on-start": false }, - "home-geo-point": { - "latitude": 37.9490953, - "longitude": -91.7848293, - "altitude": 100.0 + "home-geo-point": { // Golf course coordinates + "latitude": 37.94894101091474, + "longitude": -91.78455965055593, + "altitude": 15.0 }, "segmentation": { "initialize-ids": true, diff --git a/sim_start_drones.sh b/sim_start_drones.sh index 18a17aa..53965fd 100644 --- a/sim_start_drones.sh +++ b/sim_start_drones.sh @@ -1,28 +1,77 @@ #!/bin/bash +# Starts NCOPTERS ArduPilot SITL instances, one per tmux window, each wired to its own +# drone in the ProjectAirSim scene. +# +# Port arithmetic MUST stay in sync with simulation/interfaces/iarc.py. Drone i (0-based): +# MAVLink TCP 5760 + 10i serial0, claimed by MAVProxy +# MAVLink TCP 5762 + 10i serial1, what dronekit connects to (state_machine/drone.py) +# UDP in 9003 + 10i AirSim -> SITL, sensor data +# UDP out 9002 + 10i SITL -> AirSim, servo output +# +# All four come from --instance: the SITL binary's own help says it "adds 10*instance to +# all port numbers", which covers the AirSim UDP pair as well as the MAVLink TCP ports. +# Do not try to set the UDP ports explicitly here -- --sim-port-in/--sim-port-out are +# options on the arducopter binary, not on sim_vehicle.py, so they would have to go +# through -A, and they would then double up with the offset --instance already applied. + +set -u + SESSION_NAME="MultipleRotors" -NCOPTERS="${1:-1}" -OUT_PORT="${2:-14550}" -OUT_HOST="${3:-127.0.0.1}" +NCOPTERS="${1:-${NUM_DRONES:-1}}" +OUT_PORT="${2:-${OUT_PORT:-14550}}" +OUT_HOST="${3:-${OUT_HOST:-127.0.0.1}}" + +# Where the Unreal/ProjectAirSim server is reachable from inside this container. When +# Unreal runs on Windows and this container runs in WSL, set it to the Windows-side WSL +# adapter IP (`ipconfig` -> "vEthernet (WSL)"), e.g. AIRSIM_HOST=172.27.192.1. +AIRSIM_HOST="${AIRSIM_HOST:-127.0.0.1}" +PORT_STRIDE=10 -tmux kill-session -t "$SESSION_NAME" +echo "Starting $NCOPTERS drone(s), AirSim at $AIRSIM_HOST" + +tmux kill-session -t "$SESSION_NAME" 2>/dev/null # Start a new detached tmux session with a placeholder window tmux new-session -d -s "$SESSION_NAME" -n "init" -echo "Starting $NCOPTERS drones..." - -for ((i = 0; i < $NCOPTERS; i++)); do +for ((i = 0; i < NCOPTERS; i++)); do WINDOW_NAME="Drone_$i" + # Each instance needs its own working directory: eeprom.bin, logs and terrain cache + # are written to cwd, and instances sharing a directory corrupt each other's state. + DRONE_DIR="/dronedata/drone$i" + mkdir -p "$DRONE_DIR" + + # Give each instance its own GCS forwarding port, otherwise every SITL blindly UDPs + # into the same 14550 and the streams interleave into garbage. + INSTANCE_OUT_PORT=$((OUT_PORT + i * PORT_STRIDE)) + + echo "Starting drone $i: sim in $((9003 + i * PORT_STRIDE)), sim out $((9002 + i * PORT_STRIDE)), mavlink tcp $((5762 + i * PORT_STRIDE))" + tmux new-window -d -t "$SESSION_NAME" -n "$WINDOW_NAME" - tmux send-keys -t "$SESSION_NAME:$WINDOW_NAME" "/ardupilot/Tools/autotest/sim_vehicle.py -v ArduCopter -f airsim-copter --instance $i" Enter - sleep 1 + tmux send-keys -t "$SESSION_NAME:$WINDOW_NAME" \ + "cd $DRONE_DIR; python /ardupilot/Tools/autotest/sim_vehicle.py \ + -v ArduCopter \ + -f airsim-copter \ + -w \ + --add-param-file=/ardupilot/Tools/autotest/multidrone.parm \ + --instance $i \ + --auto-sysid \ + --sim-address=$AIRSIM_HOST \ + --out=$OUT_HOST:$INSTANCE_OUT_PORT" Enter + + # Wait until this instance finishes building before starting the next. Concurrent waf + # builds in the same tree collide, and this is more reliable than guessing a delay. + sleep 0.1 + while (( $(tmux capture-pane -t "$SESSION_NAME:$WINDOW_NAME.0" -pS - | grep -c "BUILD SUMMARY") < 1 )); do + sleep 0.3 + done done # Kill the initial placeholder window tmux kill-window -t "$SESSION_NAME:init" # Attach to the session -tmux attach-session -t "$SESSION_NAME" \ No newline at end of file +tmux attach-session -t "$SESSION_NAME" diff --git a/templates/legacy-airsim-settings-ardupilot.json b/templates/legacy-airsim-settings-ardupilot.json new file mode 100644 index 0000000..f7f2cfa --- /dev/null +++ b/templates/legacy-airsim-settings-ardupilot.json @@ -0,0 +1,43 @@ +{ + "SettingsVersion": 1.2, + "LogMessagesVisible": true, + "SimMode": "Multirotor", + "OriginGeopoint": { + "Latitude": 37.948692, + "Longitude": -91.784160, + "Altitude": 583 + }, + "CameraDefaults": { + "CaptureSettings": [ + { + "ImageType": 0, + "Width": 1920, + "Height": 1080, + "FOV_Degrees": 90, + "AutoExposureSpeed": 1.0, + "AutoExposureBias": 0.0, + "ExposureCompensation": -2.0, + "ManualExposure": true, + "ShutterSpeed": 0.01, + "ISO": 100, + "Aperture": 16 + } + ] + }, + "Vehicles": { + "Copter": { + "VehicleType": "ArduCopter", + "UseSerial": false, + "LocalHostIp": "127.0.0.1", + "UdpIp": "127.0.0.1", + "UdpPort": 9003, + "ControlPort": 9002, + "Cameras" : { + "bottom_center": { + "X": 0.00, "Y": 0.00, "Z": 0.00, + "Pitch": 270.0, "Roll": 0.0, "Yaw": 0.0 + } + } + } + } +} \ No newline at end of file diff --git a/templates/legacy-airsim-settings-multidrone.json b/templates/legacy-airsim-settings-multidrone.json new file mode 100644 index 0000000..b16e8de --- /dev/null +++ b/templates/legacy-airsim-settings-multidrone.json @@ -0,0 +1,28 @@ +{ + "SettingsVersion": 1.2, + "LogMessagesVisible": true, + "SimMode": "Multirotor", + "OriginGeopoint": { + "Latitude": 37.948692, + "Longitude": -91.784160, + "Altitude": 583 + }, + "ClockSpeed": 1, + "PhysicsEngineName": "FastPhysicsEngine", + "PhysicsSettings": { + "UpdateRate": 100 + }, + "CameraDefaults": { + "CaptureSettings": [] + }, + "Vehicles": { + "Copter": { + "VehicleType": "ArduCopter", + "UseSerial": false, + "LocalHostIp": "127.0.0.1", + "UdpIp": "127.0.0.1", + "UdpPort": 9003, + "ControlPort": 9002 + } + } +} \ No newline at end of file diff --git a/templates/multidrone.parm b/templates/multidrone.parm new file mode 100644 index 0000000..9959b58 --- /dev/null +++ b/templates/multidrone.parm @@ -0,0 +1,23 @@ +# Use single IMU to reduce EKF load +EK3_IMU_MASK 1 +EK3_ENABLE 1 + +# Reduce estimator CPU usage +EK3_GPS_TYPE 3 +EK3_SRC1_POSXY 3 +EK3_SRC1_VELXY 3 +EK3_SRC1_POSZ 1 +EK3_SRC1_VELZ 3 + +# Lower simulation sensor load +INS_FAST_SAMPLE 0 + +# Improve SITL timing +SIM_SPEEDUP 1 +SIM_RATE_HZ 100 +SCHED_LOOP_RATE 100 + +# Stabilize takeoff behavior +ATC_RAT_RLL_P 0.12 +ATC_RAT_PIT_P 0.12 +ATC_RAT_YAW_P 0.2 \ No newline at end of file diff --git a/templates/no-unreal-compose.override.yml b/templates/no-unreal-compose.override.yml new file mode 100644 index 0000000..3da090d --- /dev/null +++ b/templates/no-unreal-compose.override.yml @@ -0,0 +1,7 @@ +version: "3" +services: + sim: + # based off command taken from old installation guide + # alternatively, you can replace this command with "bash" to open an interactive terminal instead + command: python /ardupilot/Tools/autotest/sim_vehicle.py -v copter -L GolfCourse --map + \ No newline at end of file diff --git a/templates/nvidia-compose.override.yml b/templates/nvidia-compose.override.yml new file mode 100644 index 0000000..a375cfe --- /dev/null +++ b/templates/nvidia-compose.override.yml @@ -0,0 +1,10 @@ +version: "3" +services: + env: + deploy: + resources: + reservations: + devices: + - driver: nvidia + count: all + capabilities: [gpu] diff --git a/tests/ControllableFlight.py b/tests/AirSimControllableFlight.py similarity index 100% rename from tests/ControllableFlight.py rename to tests/AirSimControllableFlight.py diff --git a/tests/ProjectAirSimControllableFlight.py b/tests/ProjectAirSimControllableFlight.py new file mode 100644 index 0000000..be714eb --- /dev/null +++ b/tests/ProjectAirSimControllableFlight.py @@ -0,0 +1,180 @@ +import socket # For sending python commands directly to UE4 +import dronekit +from dronekit import LocationGlobalRelative, VehicleMode, connect +# from flight.camera import CameraAirSim + +import asyncio +# import cv2 +import numpy as np + +# from projectairsim import ProjectAirSimClient, Drone, World +# from projectairsim.utils import projectairsim_log, unpack_image +# from projectairsim.image_utils import ImageDisplay + +address = "127.0.0.1:14550" + +# client = ProjectAirSimClient(address=other_address) +# +# client.connect() +vehicle = dronekit.connect(address, wait_ready=True, timeout=90) + +# world = World(client=client, scene_config_name="/SUAS/simulation/sim_config/scene_suas.jsonc") +# drones = [ +# Drone(client=client, world=world, name=f"Drone{i}") +# for i in range(1, 1) +# ] +# drone = Drone(client=client, world=world, name="Drone1") + +async def main(): + # DroneKit uses different commands than ProjectAirSim + print("Basic pre-arm checks") + while not vehicle.is_armable: + print(" Waiting for vehicle to initialise...") + await asyncio.sleep(1) + + print("Arming motors") + vehicle.mode = VehicleMode("GUIDED") + vehicle.armed = True + + while not vehicle.armed: + print(" Waiting for arming...") + await asyncio.sleep(1) + + print("Taking off!") + vehicle.simple_takeoff(10) # Take off to 10m + + # Control drone + # type n to move north, s south, e east, w west, u up, and d down + # you can put multiple at once, such as nnneeeuuu to do multiple commands at once + # type q to quit/land + lat, lon, alt = 0.0, 0.0, 0.0 + while True: + command: str = input("Give instructions: ").lower() + + + if command.lower() in ("die", "q", "quit"): + projectairsim_log().info("land_async: starting") + land_task = await drone.land_async() + await land_task + projectairsim_log().info("land_async: completed") + break + + lat, lon, alt = 0.0, 0.0, 0.0 + for cmd in command: + if cmd == "n": + lat += 1 + elif cmd == "s": + lat -= 1 + elif cmd == "e": + lon += 1 + elif cmd == "w": + lon -= 1 + elif cmd == "u": + alt -= 5 + elif cmd == "d": + alt += 5 + # elif cmd == "p": + # print("Attempting to capture photo") + # await takepic(cam) + # print("Photo captured") + # elif cmd == "m": + # print("Attempting release!") + # send_rpc_msg("RELEASE") + + new_move = await drone2.move_by_velocity_async( + v_north=lat, v_east=lon, v_down=alt, duration=1.0 + ) + while not new_move.done(): + await asyncio.sleep(0.01) + + projectairsim_log().info("new_move: started") + + # Close vehicle object before exiting script + drone.disarm() + drone.disable_api_control() + # sock.close() # Release the socket server + +async def multi_main(): + # Set the drone to be ready to fly + for drone in drones: drone.enable_api_control() + for drone in drones: drone.arm() + + projectairsim_log().info("takeoff_async: starting") + for drone in drones: + takeoff_task = ( + await drone.takeoff_async() + ) # schedule an async task to start the command + + # Example 1: Wait on the result of async operation using 'await' keyword + await takeoff_task + projectairsim_log().info("takeoff_async: completed") + + # Command the drone to move up in NED coordinate system at 1 m/s for 4 seconds + for drone in drones: + move_up_task = await drone.move_by_velocity_async( + v_north=0.0, v_east=0.0, v_down=-1.0, duration=4.0 + ) + projectairsim_log().info("Move-Up invoked") + + await move_up_task + projectairsim_log().info("Move-Up completed") + + # Control drone + # type n to move north, s south, e east, w west, u up, and d down + # you can put multiple at once, such as nnneeeuuu to do multiple commands at once + # type q to quit/land + lat, lon, alt = 0.0, 0.0, 0.0 + while True: + command: str = input("Give instructions: ").lower() + + for drone in drones: + if command.lower() in ("die", "q", "quit"): + projectairsim_log().info("land_async: starting") + land_task = await drone.land_async() + await land_task + projectairsim_log().info("land_async: completed") + break + + lat, lon, alt = 0.0, 0.0, 0.0 + for cmd in command: + if cmd == "n": + lat += 1 + elif cmd == "s": + lat -= 1 + elif cmd == "e": + lon += 1 + elif cmd == "w": + lon -= 1 + elif cmd == "u": + alt -= 5 + elif cmd == "d": + alt += 5 + # elif cmd == "p": + # print("Attempting to capture photo") + # await takepic(cam) + # print("Photo captured") + # elif cmd == "m": + # print("Attempting release!") + # send_rpc_msg("RELEASE") + + for drone2 in drones: + new_move = await drone2.move_by_velocity_async( + v_north=lat, v_east=lon, v_down=alt, duration=1.0 + ) + while not new_move.done(): + await asyncio.sleep(0.01) + + projectairsim_log().info("new_move: started") + + # Close vehicle object before exiting script + for drone in drones: drone.disarm() + for drone in drones: drone.disable_api_control() + # sock.close() # Release the socket server + + +# --- RPC Server Send --- +def send_rpc_msg(cmd): + sock.send(cmd.encode('utf-8')) + +if __name__ == "__main__": + asyncio.run(main()) diff --git a/tests/ProjectAirsimMultidrone.py b/tests/ProjectAirsimMultidrone.py new file mode 100644 index 0000000..ef15fa1 --- /dev/null +++ b/tests/ProjectAirsimMultidrone.py @@ -0,0 +1,207 @@ +""" +Copyright (C) Microsoft Corporation. +Copyright (C) 2025 IAMAI CONSULTING CORP +MIT License. + +Demonstrates flying a quadrotor drone with camera sensors. +""" + +import asyncio + +import collections +import collections.abc + +# Add the missing attribute back to the collections module +collections.MutableMapping = collections.abc.MutableMapping + +from projectairsim import ProjectAirSimClient, Drone, World +from projectairsim.utils import projectairsim_log +from projectairsim.image_utils import ImageDisplay + +import asyncio +import multiprocessing as mp +import time +import sys + +from dronekit import LocationGlobalRelative, VehicleMode, connect + +from multidrone_world import MultidroneWorld + + +class DronekitDrone: + + def __init__(self, connection_string): + self._connection_string = connection_string + self._drone = None + + def connect(self, timeout=30): + print("connecting", self._connection_string) + vehicle = connect(self._connection_string, wait_ready=True, timeout=timeout) + + # Get some vehicle attributes (state) + print("Get some vehicle attribute values:") + print(" GPS: %s" % vehicle.gps_0) + print(" Battery: %s" % vehicle.battery) + print(" Last Heartbeat: %s" % vehicle.last_heartbeat) + print(" Is Armable?: %s" % vehicle.is_armable) + print(" System status: %s" % vehicle.system_status.state) + print(" Mode: %s" % vehicle.mode.name) + + while not vehicle.is_armable: + print("Waiting for vehicle to initialize...") + time.sleep(1) + + vehicle.parameters["ARMING_CHECK"] = 0 + vehicle.mode = VehicleMode("GUIDED") + vehicle.armed = True + + self._drone = vehicle + + def takeoff(self, alt): + self._drone.simple_takeoff(alt) + self._takeoff_alt = alt + + def translate(self, dlat, dlon, dalt): + loc = self._drone.location.global_relative_frame + lat, lon, alt = loc.lat, loc.lon, loc.alt + + self._drone.simple_goto(LocationGlobalRelative(lat + dlat, lon + dlon, alt + dalt)) + + def goto(self, lat, lon, alt): + self._drone.simple_goto(LocationGlobalRelative(lat, lon, alt)) + + @property + def took_off(self): + return self._drone.location.global_relative_frame.alt >= self._takeoff_alt * 0.9 + + @property + def loc(self): + return self._drone.location.global_relative_frame + + def land(self): + self._drone.mode = VehicleMode("LAND") + + def close(self): + self._drone.close() + + +def run_drone(connection_string, queue, timeout=30): + drone = DronekitDrone(connection_string) + drone.connect(timeout) + time.sleep(3) + + while True: + cmd = queue.get() + + if cmd is None: + drone.land() + drone.close() + break + elif cmd == "takeoff": + drone.takeoff(20) + while not drone.took_off: + print("Waiting for drone to finish takeoff...") + time.sleep(1) + else: + drone.translate(*cmd) + + +# Async main function to wrap async drone commands +async def main(): + # Create a Project AirSim client + client = ProjectAirSimClient() + + try: + # Connect to simulation environment + client.connect() + + # we need to init an empty scene BEFORE starting the sim container (drone SITL or something like that) since it wants to download scene data + # this download is impossible if the scene isn't initialized at all, and it just gives up if the download fails + World( + client, + "scene_ardu_empty.jsonc", + delay_after_load_sec=2, + sim_config_path="./simulation/sim_config", + ) + + # block and wait for sim container to start + input("Start your sim container now. Press enter to continue (add drones to scene)") + + # SET DRONE GRID HERE + drone_grid = (4, 4) + processes = [] + + # this reinitializes the scene to contain the drones + # the reason we don't do this first is that Project Airsim really wants the drone SITL(s) to be started before the drones are created + # it becomes this weird thing where the SITL wants the sim to be started first, but the drones in the sim wants the SITL to be started first + # hence, the empty scene stuff + world = MultidroneWorld( + client, + "scene_ardu_quadrotor.jsonc", + delay_after_load_sec=2, + sim_config_path="./simulation/sim_config", + drone_grid=drone_grid, + ) + + input("Press enter to start connections (may need to wait a while for drones to get ready)") + # Create a World object to interact with the sim world and load a scene + base_port = 5760 + drone_count = drone_grid[0] * drone_grid[1] + queues = [mp.Queue() for _ in range(drone_count)] + + # start drone processes, assign connection string + for port, queue in zip(range(base_port, base_port + 10 * drone_count, 10), queues): + proc = mp.Process(target=run_drone, args=(f"tcp:127.0.0.1:{port}", queue, 120)) + proc.start() + + processes.append(proc) + + for queue in queues: + queue.put("takeoff") + + # basic drone control: n for north, e for east, etc.; u for up, d for down; can put multiple instructions per entry (e.g., "nnnnneeeeeeuuuu") + # "quit", "q", or "die" to end connections + while True: + # print("Loc:", lat, lon, alt) + lat = lon = alt = 0 + command: str = input("Give instructions: ").lower() + + if command.lower() in ("die", "q", "quit"): + for queue in queues: + queue.put(None) + break + + for cmd in command: + match cmd: + case "n": + lat += 0.0001 + case "s": + lat -= 0.0001 + case "e": + lon += 0.0001 + case "w": + lon -= 0.0001 + case "u": + alt += 5 + case "d": + alt -= 5 + + for queue in queues: + queue.put((lat, lon, alt)) + + # ------------------------------------------------------------------------------ + + # logs exception on the console + except Exception as err: + projectairsim_log().error(f"Exception occurred: {err}", exc_info=True) + + finally: + # Always disconnect from the simulation environment to allow next connection + client.disconnect() + + for p in processes: + p.join() + + +if __name__ == "__main__": + asyncio.run(main()) # Runner for async main function diff --git a/tests/ardupilot_quadrotor.py b/tests/ardupilot_quadrotor.py new file mode 100644 index 0000000..f7d4862 --- /dev/null +++ b/tests/ardupilot_quadrotor.py @@ -0,0 +1,91 @@ +import time +from dronekit import connect, VehicleMode, APIException +from projectairsim import ProjectAirSimClient, Drone, World +from projectairsim.utils import projectairsim_log + +def stable_connect(): + vehicle = None + while not vehicle: + try: + print("Attempting to reach ArduPilot...") + # Use a longer timeout and wait_ready=False to prevent early exit + vehicle = connect('127.0.0.1:14550', wait_ready=False, timeout=60) + except (APIException, Exception) as e: + print(f"SITL not ready yet ({e}). Retrying in 2s...") + time.sleep(2) + + print("Link established. Waiting for parameters...") + vehicle.wait_ready(True, timeout=60) + return vehicle + +def run_dronekit_logic(): + """Handles the ArduPilot flight commands via DroneKit""" + vehicle = stable_connect() + vehicle.parameters['ARMING_CHECK'] = 0 + + # Now wait for the vehicle to be truly ready + vehicle.wait_ready(True, timeout=60) + print("Vehicle ready!") + + try: + print("Basic pre-arm checks...") + # Wait for vehicle to be armable + while not vehicle.is_armable: + print(" Waiting for vehicle to initialize...") + time.sleep(1) + + print("Arming motors") + vehicle.mode = VehicleMode("GUIDED") + vehicle.armed = True + + print("Taking off!") + target_altitude = 50 + vehicle.simple_takeoff(target_altitude) + + # Wait until the vehicle reaches a safe height + while True: + print(f" Altitude: {vehicle.location.global_relative_frame.alt}") + if vehicle.location.global_relative_frame.alt >= target_altitude * 0.95: + print("Reached target altitude") + break + time.sleep(1) + + print("Hovering for 5 seconds...") + time.sleep(5) + + print("Landing...") + vehicle.mode = VehicleMode("RTL") + + # Wait for the drone to touch down + while vehicle.armed: + print(f" Landing... Altitude: {vehicle.location.global_relative_frame.alt}") + time.sleep(1) + + print("Landed and Disarmed.") + + finally: + print("Closing vehicle object") + vehicle.close() + +def main(): + # Initialize Project AirSim Client + client = ProjectAirSimClient() + + try: + print("Connecting...") + client.connect() + # Load the world and vehicle defined in your JSONC + world = World(client, "/SUAS/simulation/sim_config/scene_ardu_quadrotor.jsonc", delay_after_load_sec=2) + drone = Drone(client, world, "Drone1") + + # Execute the flight logic + print("Running dronekit logic...") + run_suas_code() + + except Exception as err: + projectairsim_log().error(f"Exception occurred: {err}", exc_info=True) + finally: + client.disconnect() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/tests/coordinate_flight_helper.py b/tests/coordinate_flight_helper.py new file mode 100644 index 0000000..8fd4c8f --- /dev/null +++ b/tests/coordinate_flight_helper.py @@ -0,0 +1,220 @@ +"""Flight-side half of the coordinate accuracy test. Run by drone_coordinates_accuracy.py. + +The env container installs projectairsim into the system Python and the flight dependencies +into the uv project venv, and no single interpreter has both. Rather than asking anyone to +patch their container, the test is split the way the rest of the repo already splits it: +interfaces/iarc.py runs on the system Python and spawns `uv run run.py` for the flight code. +This module is that child -- it talks to the SITL over DroneKit and answers questions from +its parent, and deliberately imports nothing from projectairsim. + +Protocol: one JSON object per line on stdin, one reply per command on stdout. Replies carry +a `@@` prefix so DroneKit's own chatter on the same stream cannot be mistaken for one. + + {"cmd": "takeoff", "alt_m": 3.048} -> @@{"ok": true} + {"cmd": "yaw", "heading_deg": 45} -> @@{"ok": true, "heading": 45.0} + {"cmd": "pose"} -> @@{"ok": true, "pose": {"lat":..., "yaw_deg":...}} + {"cmd": "land"} -> @@{"ok": true} + {"cmd": "quit"} -> @@{"ok": true} + +Every reply is either `{"ok": true, ...}` or `{"ok": false, "error": "..."}`; the parent is +never left waiting on a command that failed. + +Run standalone for a smoke test: + echo '{"cmd":"pose"}' | uv run python simulation/tests/coordinate_flight_helper.py +""" + +from __future__ import annotations + +import argparse +import json +import math +import sys +import time + +import dronekit +from pymavlink import mavutil + +REPLY_PREFIX = "@@" + +SETTLE_TIMEOUT_SEC = 45.0 +SETTLE_SPEED_M_S = 0.25 +SETTLE_ALT_BAND_M = 0.35 + +YAW_RATE_DEG_S = 25.0 +YAW_TIMEOUT_SEC = 30.0 +YAW_TOLERANCE_DEG = 5.0 + + +def log(message: str) -> None: + """Progress goes to stderr, which the parent forwards but never parses.""" + print(f"[flight] {message}", file=sys.stderr, flush=True) + + +def reply(**payload) -> None: + print(REPLY_PREFIX + json.dumps(payload), flush=True) + + +class FlightSide: + """Owns the DroneKit connection and executes one command at a time.""" + + def __init__(self, address: str, timeout: float): + log(f"connecting to {address}") + self.vehicle = dronekit.connect(address, wait_ready=True, timeout=timeout) + # AirSim's simulated sensors do not always satisfy the full preflight suite, and the + # test is about camera geometry rather than arming logic. + self.vehicle.parameters["ARMING_CHECK"] = 0 + log("connected") + + def close(self) -> None: + self.vehicle.close() + + def takeoff(self, alt_m: float) -> dict: + vehicle = self.vehicle + while not vehicle.is_armable: + log("waiting for the vehicle to initialise...") + time.sleep(1) + + vehicle.mode = dronekit.VehicleMode("GUIDED") + vehicle.armed = True + while not vehicle.armed or vehicle.mode.name != "GUIDED": + log("waiting for arming...") + time.sleep(1) + + log(f"taking off to {alt_m:.2f} m") + # No overshoot margin: the hover altitude is the measurement's baseline, and a drone + # descending out of a margin would still be moving when the markers are photographed. + vehicle.simple_takeoff(alt_m) + deadline = time.monotonic() + SETTLE_TIMEOUT_SEC * 2 + while time.monotonic() < deadline: + if (vehicle.location.global_relative_frame.alt or 0.0) >= alt_m * 0.95: + break + time.sleep(0.5) + + settled = self.settle(alt_m) + return {"alt_m": vehicle.location.global_relative_frame.alt, "settled": settled} + + def settle(self, target_alt_m: float) -> bool: + """Wait for the hover to stop moving, so pose and photograph describe one instant.""" + deadline = time.monotonic() + SETTLE_TIMEOUT_SEC + while time.monotonic() < deadline: + altitude = self.vehicle.location.global_relative_frame.alt or 0.0 + velocity = self.vehicle.velocity or [0.0, 0.0, 0.0] + speed = max(abs(component or 0.0) for component in velocity) + if abs(altitude - target_alt_m) < SETTLE_ALT_BAND_M and speed < SETTLE_SPEED_M_S: + log(f"settled at {altitude:.2f} m (max axis speed {speed:.2f} m/s)") + return True + time.sleep(0.5) + log(f"did not settle within {SETTLE_TIMEOUT_SEC:.0f}s; continuing") + return False + + def yaw(self, heading_deg: float) -> dict: + """Yaw to an absolute compass heading and wait for it to arrive. + + The parent asks for an off-axis heading on purpose: at heading 0 an inverted yaw + convention produces the same answers as a correct one, so the run would prove nothing + about how yaw is applied. + """ + heading = heading_deg % 360.0 + message = self.vehicle.message_factory.command_long_encode( + 0, + 0, + mavutil.mavlink.MAV_CMD_CONDITION_YAW, + 0, + heading, # target angle, degrees + YAW_RATE_DEG_S, # angular speed + 1, # direction: 1 = clockwise (ignored for absolute angles) + 0, # 0 = absolute heading rather than relative offset + 0, + 0, + 0, + ) + self.vehicle.send_mavlink(message) + + deadline = time.monotonic() + YAW_TIMEOUT_SEC + reached = False + while time.monotonic() < deadline: + current = self.vehicle.heading + if current is not None: + # Shortest angular distance, so 359 -> 1 is 2 degrees rather than 358. + if abs((current - heading + 180.0) % 360.0 - 180.0) < YAW_TOLERANCE_DEG: + reached = True + break + time.sleep(0.5) + + if reached: + log(f"heading settled at {self.vehicle.heading} deg") + else: + log(f"heading did not reach {heading:.0f} deg; now {self.vehicle.heading}") + return {"heading": self.vehicle.heading, "reached": reached} + + def pose(self) -> dict: + """The pose the real flight code would be working from, in degrees.""" + location = self.vehicle.location.global_relative_frame + attitude = self.vehicle.attitude + return { + "lat": float(location.lat), + "lon": float(location.lon), + # Relative to the arming position, which is the ground plane the parent measured. + "alt_m": float(location.alt), + # DroneKit reports attitude in radians; the conversion under test wants degrees. + "yaw_deg": math.degrees(attitude.yaw), + "pitch_deg": math.degrees(attitude.pitch), + "roll_deg": math.degrees(attitude.roll), + "heading": self.vehicle.heading, + } + + def land(self) -> dict: + self.vehicle.mode = dronekit.VehicleMode("LAND") + return {} + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__.splitlines()[0]) + parser.add_argument("--address", default="tcp:127.0.0.1:5762") + parser.add_argument("--connect-timeout", type=float, default=120.0) + args = parser.parse_args() + + try: + flight = FlightSide(args.address, args.connect_timeout) + except Exception as err: # noqa: BLE001 -- must reach the parent as a reply, not a crash + reply(ok=False, error=f"could not connect to {args.address}: {err}") + return 1 + + reply(ok=True, event="ready") + + try: + for line in sys.stdin: + line = line.strip() + if not line: + continue + try: + request = json.loads(line) + command = request["cmd"] + except (ValueError, KeyError) as err: + reply(ok=False, error=f"malformed request {line!r}: {err}") + continue + + try: + if command == "takeoff": + reply(ok=True, **flight.takeoff(float(request["alt_m"]))) + elif command == "yaw": + reply(ok=True, **flight.yaw(float(request["heading_deg"]))) + elif command == "pose": + reply(ok=True, pose=flight.pose()) + elif command == "land": + reply(ok=True, **flight.land()) + elif command == "quit": + reply(ok=True) + break + else: + reply(ok=False, error=f"unknown command {command!r}") + except Exception as err: # noqa: BLE001 -- one bad command must not kill the run + reply(ok=False, error=f"{command} failed: {err}") + finally: + flight.close() + + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/tests/drone_coordinates_accuracy.py b/tests/drone_coordinates_accuracy.py new file mode 100644 index 0000000..007eae9 --- /dev/null +++ b/tests/drone_coordinates_accuracy.py @@ -0,0 +1,1163 @@ +"""End-to-end accuracy check for vision/common/drone_coordinates.py against Project AirSim. + +Flies one drone to a low hover, drops visible markers on the ground at positions the sim +knows exactly, photographs them with the nadir DownCamera, and asks +`pixel_to_geocoord_gimbal` where each marker's pixel points on the ground. The answer is +compared against the marker's true position, so the reported error is the real end-to-end +error of the flight pipeline -- GPS, attitude estimate and coordinate math together. + +Why markers rather than a closed-form expectation: computing the "right" answer from the +drone's pose and the camera intrinsics would just be a second implementation of the same +projection, and the two would agree on any shared misunderstanding of the frames. A cube +sitting at a known spot on the ground cannot be argued with. + +How a marker is located in the image without a detector: the marker is spawned *after* a +baseline frame has been captured, so the pixels that changed between the two frames are +exactly the marker. Segmentation frames are flat-shaded, so that difference is exact; the +scene frame is used as a fallback if segmentation capture is unavailable. Markers are +handled one at a time, which keeps the pixel-to-marker assignment unambiguous and lets each +one be measured against a freshly sampled pose. + +Which blob is the marker +------------------------ +"The largest thing that changed" is not enough. The drone drifts between the baseline and the +capture, so pixels change all over the frame, and a sprawling drift component can out-area the +marker; its centroid then lands near the principal point, which looks plausible for every +marker and is wrong for all of them -- and at a low hover the resulting miss is small enough +to sit inside a loose tolerance, so the run reports PASS having measured nothing. + +Instead the marker's expected pixel area is known ahead of the capture (it is spawned at a +known size, at a known height) and blobs are filtered on it, on a ceiling fraction of the +frame, and on distance from where nadir geometry says the marker should be. That predicted +pixel is a second implementation of the projection, so it is used only to reject blobs, never +to score one: the error that gets asserted on is still the spawned marker's position against +the converted position. Two markers detected at the same pixel invalidate each other, since +markers metres apart cannot share one. + +Pose in, truth out +------------------ +The pose handed to `pixel_to_geocoord_gimbal` is the one the real flight code uses -- +DroneKit's `location.global_relative_frame` and `attitude`. Expected values come from the +sim's ground truth. So a failure here is GPS/EKF error *or* a bug in the conversion. To tell +those apart, each row also reports what the conversion produces when fed the sim's exact +pose; that column is diagnostic only and is never asserted on. If the ground-truth column is +accurate and the DroneKit column is not, the math is fine and the autopilot's position +estimate is what moved. + +Why it hovers at 45 degrees +--------------------------- +The drone spawns pointing north, and at heading 0 any error in how the conversion applies +yaw vanishes -- a completely inverted yaw convention returns the same answers as a correct +one. Measuring nose-north would pass without testing yaw at all, so the run yaws off-axis +first. `--yaw-deg 0` gives the nose-north case for comparison; a run that passes at 0 and +fails at 45 has isolated the yaw handling. + +At the default 10 ft the camera only sees about 6.1 m x 3.4 m of ground, so a metre of GPS +error is a sizeable fraction of the frame. `--alt-ft` raises the hover if the numbers are +too noisy to be informative. + +Two processes, no environment changes +------------------------------------ +The env container installs projectairsim into the system Python and the flight dependencies +into the uv project venv, so no single interpreter can import both projectairsim and +dronekit. This script is the projectairsim half and never imports dronekit; it spawns +coordinate_flight_helper.py under `uv run` for the flying and drives it over a pipe. That is +the same split interfaces/iarc.py already uses when it shells out to `uv run run.py`, and it +means the test runs on a stock container with nothing installed. + +Usage (inside the `env` container, same handshake as interfaces/iarc.py): + + python simulation/tests/drone_coordinates_accuracy.py + +Start the Unreal sim first; the script loads an empty scene, then waits for you to launch +the SITL container before spawning the drone. Exits non-zero if any marker misses by more +than `--tolerance-m`. +""" + +from __future__ import annotations + +import argparse +import collections +import collections.abc +import json +import math +import os +import queue +import re +import subprocess +import sys +import threading +import time +from dataclasses import dataclass +from pathlib import Path + +# commentjson (a projectairsim dependency) still uses the pre-3.10 collections aliases. +# Must happen before projectairsim is imported, here and in iarc.py. +collections.MutableMapping = collections.abc.MutableMapping + +REPO_ROOT = Path(__file__).resolve().parents[2] +sys.path.insert(0, str(REPO_ROOT)) +sys.path.insert(0, str(REPO_ROOT / "simulation" / "interfaces")) + +import cv2 # noqa: E402 +import numpy as np # noqa: E402 + +import iarc # noqa: E402 -- ArduWorld plus the SITL/MAVLink handshake helpers +from projectairsim import Drone, ProjectAirSimClient, World # noqa: E402 +from projectairsim.types import ImageType, Pose, Quaternion, Vector3 # noqa: E402 +from projectairsim.utils import ( # noqa: E402 + geo_to_ned_coordinates, + projectairsim_log, + quaternion_to_rpy, + unpack_image, +) + +from vision.common.drone_coordinates import ( # noqa: E402 + DronePose, + GimbalPose, + pixel_to_geocoord_gimbal, +) + +FEET_TO_M = 0.3048 + +CAMERA_ID = "DownCamera" + +# Where to put the markers, as (label, forward, right) fractions of the half-footprint the +# camera covers in each body axis at the hover altitude. Fractions rather than metres so the +# pattern still fits the frame when --alt-ft changes; 0.6 keeps them clear of the edge, where +# a marker straddling the border would bias its own centroid. A centre-only run would miss +# exactly the errors that grow with distance from the principal point, which is most of them. +MARKER_PLACEMENTS: list[tuple[str, float, float]] = [ + ("centre", 0.0, 0.0), + ("forward", 0.6, 0.0), + ("aft", -0.6, 0.0), + ("right", 0.0, 0.6), + ("left", 0.0, -0.6), +] + +# A flat tile, not a cube: anything with height projects its top face rather than its +# footprint, and at a 3 m hover a 1 m tall object would throw the answer off by metres. +# These are scale factors on the asset's native size, so the preference list below favours +# the 1 m cubes, for which they are also metres. +# +# Width is sized as a fraction of the visible frame rather than fixed, so raising --alt-ft +# (the remedy when GPS noise swamps a 10 ft hover) does not shrink the marker below the +# detection floor. 0.08 puts it at roughly 32 px across on the 400 px-wide down camera. +MARKER_FRAME_FRACTION = 0.08 +MARKER_MIN_SIZE_M = 0.5 +MARKER_THICKNESS_M = 0.02 + +# Asset names to try for the marker, best first. Which of these exists depends on the +# packaged Unreal environment, so the list is probed against list_assets() at runtime. +MARKER_ASSET_PREFERENCES = [ + r"^1M_Cube_Chamfer$", + r"^1M_Cube$", + r".*Cube.*", + r".*Box.*", + r".*Cylinder$", + r".*Sphere$", +] + +# Pixels differing by more than this (0-255) count as changed when the scene image is used +# instead of segmentation. Segmentation frames are flat-shaded so they use a threshold of 0. +SCENE_DIFF_THRESHOLD = 25 + +# Blob plausibility. The marker's size in pixels is known in advance -- it is spawned at a +# known size at a known height -- so "the largest thing that changed" is a much weaker filter +# than the situation allows. Drift between the baseline and the capture changes pixels all +# over the frame, and a sprawling drift component can easily out-area the marker; taking it +# anyway yields a centroid near the principal point, which is the one answer that looks +# plausible for every marker and is wrong for all of them. +MARKER_AREA_MIN_RATIO = 0.25 +MARKER_AREA_MAX_RATIO = 4.0 +# No legitimate marker fills this much of the frame; anything that does is whole-frame drift. +MAX_BLOB_FRAME_FRACTION = 0.10 +# Two markers metres apart cannot land on the same pixel. If they do, the detector is +# tracking something that is not the markers. +DUPLICATE_PIXEL_RADIUS = 8.0 + +# How far a detection may sit from where nadir geometry says the marker should be before it +# is treated as a mis-detection, as a fraction of the frame diagonal. This is a *detection* +# gate, never the accuracy measure: it constrains where the marker appears in the image, which +# the sim knows exactly, and not where the conversion says that pixel points, which is the +# thing under test. Tightening it therefore cannot hide a conversion error -- that error is +# measured downstream, from the converted position against the spawned position. +# +# 0.10 is ~80 px on a 640x480 frame. The slack it needs to cover is the nadir approximation +# (roll and pitch are ignored; 5 deg of tilt at a 3 m hover moves the marker ~40 px) plus +# centroid noise. At 0.25 the gate was useless for the case it exists to catch: with markers +# at 0.6 of the half-footprint, a blob stuck on the image centre sits ~190 px from the +# prediction and slipped straight through. +DETECTION_GATE_DIAGONAL_FRACTION = 0.10 + +DEFAULT_TOLERANCE_M = 2.0 +# The tolerance has to be small next to what the camera can see, or it cannot fail: at a 10 ft +# hover the whole footprint is a few metres across, so a detector stuck on the image centre +# misses by less than the footprint's half-diagonal no matter what. A tolerance above this +# fraction of that half-diagonal is not measuring the conversion, so the run refuses to start. +MAX_TOLERANCE_FOOTPRINT_FRACTION = 1.0 / 3.0 + +# The flight half, run under `uv run` so it gets dronekit from the project venv. Arming, +# takeoff, yaw and settling all live over there; this side only sequences them. +HELPER_SCRIPT = Path(__file__).resolve().parent / "coordinate_flight_helper.py" +REPLY_PREFIX = "@@" +COMMAND_TIMEOUT_SEC = 60.0 +# Generous: a cold container makes `uv run` sync the project venv before the child starts, +# and the child then waits on a DroneKit connection that itself allows two minutes. +STARTUP_TIMEOUT_SEC = 600.0 +# Arming waits on is_armable, which needs the EKF to settle, then climbs and stabilises. +TAKEOFF_TIMEOUT_SEC = 300.0 +YAW_TIMEOUT_SEC = 120.0 + +# The drone spawns 15 m up and falls; these govern the wait for it to come to rest so the +# ground plane can be measured. +GROUNDING_TIMEOUT_SEC = 60.0 +GROUNDING_TOLERANCE_M = 0.02 +GROUNDING_STABLE_SAMPLES = 4 + +# Heading to measure at. Not 0: at heading 0 an inverted yaw convention returns the same +# answers as a correct one, so a nose-north run proves nothing about how yaw is applied. +DEFAULT_YAW_DEG = 45.0 + + +@dataclass +class CameraIntrinsics: + """What the conversion needs to know about the camera, read from the robot config.""" + + width: int + height: int + h_fov_rad: float + v_fov_rad: float + + def footprint(self, altitude_m: float) -> tuple[float, float]: + """Ground extent (forward, right) in metres visible from `altitude_m`, nadir.""" + forward = 2.0 * altitude_m * math.tan(self.v_fov_rad / 2.0) + right = 2.0 * altitude_m * math.tan(self.h_fov_rad / 2.0) + return forward, right + + +@dataclass +class GroundTruth: + """The sim's exact answer for where the drone is and how it is oriented.""" + + north: float + east: float + down: float + roll_deg: float + pitch_deg: float + yaw_deg: float + + +@dataclass +class MarkerResult: + """One marker's round trip from ground position to pixel and back.""" + + label: str + # Where the marker actually is. Every error below is measured against this. + marker_ned: tuple[float, float] + pixel: tuple[float, float] | None + blob_pixels: int + # Where the conversion says the pixel points, fed the DroneKit pose. + computed_ned: tuple[float, float] | None + computed_latlon: tuple[float, float] | None + # Same, fed the sim's exact pose. Diagnostic only. + gt_pose_ned: tuple[float, float] | None + # Where nadir geometry says the marker should have appeared. Diagnostic only -- it gates + # detection, it is never what the error is measured against. + predicted_pixel: tuple[float, float] | None = None + note: str = "" + + @property + def error_m(self) -> float | None: + """Horizontal miss, in metres, using the DroneKit pose. This is what is asserted.""" + return self._miss(self.computed_ned) + + @property + def gt_pose_error_m(self) -> float | None: + """Same miss recomputed from the sim's exact pose. Diagnostic only.""" + return self._miss(self.gt_pose_ned) + + def _miss(self, estimate: tuple[float, float] | None) -> float | None: + if estimate is None: + return None + return math.hypot(estimate[0] - self.marker_ned[0], estimate[1] - self.marker_ned[1]) + + +class CoordinateTestWorld(iarc.ArduWorld): + """ArduWorld that also turns on the down camera's segmentation capture. + + The shared robot config leaves segmentation off, since nothing in the mission needs it. + This test does: a segmentation frame is flat-shaded, so differencing two of them isolates + a newly spawned marker exactly, with none of the lighting noise a scene-image difference + picks up. Flipping it here rather than in robot_ardu_quadrotor.jsonc keeps the extra + render cost to this test. + """ + + def __init__(self, *args, camera_id: str = CAMERA_ID, **kwargs): + self._camera_id = camera_id + self.robot_config: dict = {} + super().__init__(*args, **kwargs) + + def _build_actors(self, actors: list, num_drones: int) -> list: + built = super()._build_actors(actors, num_drones) + for actor in built: + robot_config = actor.get("robot-config", {}) + for sensor in robot_config.get("sensors", []): + if sensor.get("id") != self._camera_id or sensor.get("type") != "camera": + continue + for capture in sensor.get("capture-settings", []): + if capture.get("image-type") == int(ImageType.SEGMENTATION): + capture["capture-enabled"] = True + # Every actor is a clone of the same template, so one copy describes them all. + self.robot_config = robot_config + return built + + +def read_camera_intrinsics( + robot_config: dict, camera_id: str, fov_axis: str = "horizontal" +) -> CameraIntrinsics: + """Pull resolution and field of view for `camera_id` out of the robot config. + + The config carries a single `fov-degrees`; the other axis is implied by the aspect ratio. + Passing the same number for both -- the obvious mistake, since it is the only angle in the + config -- stretches the narrow axis by nearly a third and walks every off-centre pixel + away from its true ground point. + + Which axis `fov-degrees` describes is not stated in the Project AirSim config docs. It is + taken as horizontal here, matching Unreal's `FOVAngle` and legacy AirSim's `FOV_Degrees`. + If that is wrong the error will be strongly axis-dependent -- forward/aft markers off + while left/right are fine, or the reverse -- which `--fov-axis vertical` then flips. + """ + for sensor in robot_config.get("sensors", []): + if sensor.get("id") != camera_id or sensor.get("type") != "camera": + continue + for capture in sensor.get("capture-settings", []): + if capture.get("image-type") != int(ImageType.SCENE): + continue + width = int(capture["width"]) + height = int(capture["height"]) + fov = math.radians(float(capture["fov-degrees"])) + if fov_axis == "vertical": + v_fov = fov + h_fov = 2.0 * math.atan(math.tan(fov / 2.0) * width / height) + else: + h_fov = fov + v_fov = 2.0 * math.atan(math.tan(fov / 2.0) * height / width) + return CameraIntrinsics(width, height, h_fov, v_fov) + raise RuntimeError( + f"camera '{camera_id}' has no scene capture-settings in the robot config. " + f"Cameras present: {[s.get('id') for s in robot_config.get('sensors', [])]}" + ) + + +def pick_marker_asset(world: World) -> str: + """Choose a marker asset that the running Unreal environment actually ships.""" + assets = world.list_assets(".*") + if not assets: + raise RuntimeError("the sim reported no spawnable assets, so no marker can be placed") + + for pattern in MARKER_ASSET_PREFERENCES: + for asset in assets: + if re.match(pattern, asset, flags=re.IGNORECASE): + projectairsim_log().info(f"Using '{asset}' as the ground marker asset.") + return asset + raise RuntimeError( + "none of the preferred marker assets are available in this environment. " + f"Add one of these to MARKER_ASSET_PREFERENCES: {sorted(assets)[:40]}" + ) + + +def pose_at(north: float, east: float, down: float) -> Pose: + """A Pose at a scene-NED point, unrotated.""" + return Pose( + { + "translation": Vector3({"x": north, "y": east, "z": down}), + "rotation": Quaternion({"w": 1.0, "x": 0.0, "y": 0.0, "z": 0.0}), + } + ) + + +def read_ground_truth(drone: Drone) -> GroundTruth: + """The drone's exact position and orientation, in one round trip. + + Position and orientation are read together because they are compared against a single + photograph; two separate calls would straddle a physics step and disagree. + """ + pose = drone.get_ground_truth_pose() + translation = pose["translation"] + rotation = pose["rotation"] + roll, pitch, yaw = quaternion_to_rpy( + float(rotation["w"]), float(rotation["x"]), float(rotation["y"]), float(rotation["z"]) + ) + return GroundTruth( + north=float(translation["x"]), + east=float(translation["y"]), + down=float(translation["z"]), + roll_deg=math.degrees(roll), + pitch_deg=math.degrees(pitch), + yaw_deg=math.degrees(yaw), + ) + + +def capture(drone: Drone, camera_id: str) -> tuple[np.ndarray | None, np.ndarray | None]: + """Grab a (scene, segmentation) pair. Either may be None if that type is not captured. + + Segmentation is only an aid to finding the markers, so a sim that refuses the request + outright must not take the scene image down with it -- ask again for scene alone. + """ + try: + images = drone.get_images(camera_id, [int(ImageType.SCENE), int(ImageType.SEGMENTATION)]) + except Exception as err: # noqa: BLE001 -- the fallback is the point, not the error type + projectairsim_log().warning( + f"Combined scene+segmentation capture failed ({err}); retrying scene only." + ) + images = drone.get_images(camera_id, [int(ImageType.SCENE)]) + + def unpack(image_type: ImageType) -> np.ndarray | None: + message = images.get(int(image_type)) + if not message or not message.get("data"): + return None + return unpack_image(message) + + return unpack(ImageType.SCENE), unpack(ImageType.SEGMENTATION) + + +@dataclass +class Blob: + """One connected region of pixels that changed between the baseline and the capture.""" + + centroid: tuple[float, float] + area: int + + +def find_blobs(baseline: np.ndarray, current: np.ndarray, threshold: int) -> list[Blob]: + """Every blob of changed pixels between two frames, largest first.""" + if baseline.shape != current.shape: + return [] + + difference = cv2.absdiff(baseline, current) + if difference.ndim == 3: + difference = difference.max(axis=2) + mask = (difference > threshold).astype(np.uint8) + + count, _, stats, centroids = cv2.connectedComponentsWithStats(mask, connectivity=8) + blobs = [ + Blob( + (float(centroids[label][0]), float(centroids[label][1])), + int(stats[label, cv2.CC_STAT_AREA]), + ) + for label in range(1, count) # label 0 is the unchanged background + ] + return sorted(blobs, key=lambda blob: blob.area, reverse=True) + + +def predict_pixel( + marker_ned: tuple[float, float], + truth: GroundTruth, + ground_plane_z: float, + intrinsics: CameraIntrinsics, +) -> tuple[float, float] | None: + """Where a nadir camera would see `marker_ned`, for gating detections only. + + Roll and pitch are ignored -- this is a hover, and the gate is loose enough that a couple + of degrees of tilt is irrelevant. It exists to answer "is this blob the marker?", never + "is the conversion right?": using it for the latter would be the second-implementation + trap the whole spawned-marker design avoids. + """ + altitude = ground_plane_z - truth.down + if altitude <= 0.0: + return None + + yaw = math.radians(truth.yaw_deg) + delta_north = marker_ned[0] - truth.north + delta_east = marker_ned[1] - truth.east + forward = delta_north * math.cos(yaw) + delta_east * math.sin(yaw) + right = -delta_north * math.sin(yaw) + delta_east * math.cos(yaw) + + forward_extent, right_extent = intrinsics.footprint(altitude) + px = intrinsics.width / 2.0 + right / right_extent * intrinsics.width + py = intrinsics.height / 2.0 - forward / forward_extent * intrinsics.height + return px, py + + +def select_marker_blob( + blobs: list[Blob], + expected_area_px: float, + frame_area_px: int, + predicted: tuple[float, float] | None, + gate_px: float, +) -> tuple[Blob | None, str]: + """Pick the blob that is actually the marker, or explain why none of them is. + + The marker's pixel area is known ahead of the capture, so plausibility is checked rather + than assumed. The rejected candidates are summarised in the returned note: a run that + finds nothing is only useful if it says what it did find instead. + """ + if not blobs: + return None, "no pixels changed between the baseline and the capture" + + candidates: list[Blob] = [] + for blob in blobs: + if blob.area > frame_area_px * MAX_BLOB_FRAME_FRACTION: + continue + if not ( + expected_area_px * MARKER_AREA_MIN_RATIO + <= blob.area + <= expected_area_px * MARKER_AREA_MAX_RATIO + ): + continue + if predicted is not None: + offset = math.hypot(blob.centroid[0] - predicted[0], blob.centroid[1] - predicted[1]) + if offset > gate_px: + continue + candidates.append(blob) + + if not candidates: + biggest = blobs[0] + return None, ( + f"no blob looked like the marker (expected ~{expected_area_px:.0f} px); " + f"largest of {len(blobs)} was {biggest.area} px at " + f"({biggest.centroid[0]:.0f},{biggest.centroid[1]:.0f})" + ) + + # Closest to the expected size, not simply the largest: the marker's area is the one thing + # known exactly, and drift blobs are what "largest" was picking up. + best = min(candidates, key=lambda blob: abs(blob.area - expected_area_px)) + return best, "" + + +def latlon_to_ned(home_geo_point: dict, lat: float, lon: float) -> tuple[float, float]: + """Horizontal scene-NED position of a lat/lon, for comparison in metres. + + Errors are judged in metres rather than degrees because a degree of longitude is not a + degree of latitude, so a raw lat/lon difference hides which direction the miss was in. + """ + north, east, _ = geo_to_ned_coordinates(home_geo_point, [lat, lon, home_geo_point["altitude"]]) + return float(north), float(east) + + +class FlightFailed(RuntimeError): + """The flight-side process refused a command or went away.""" + + +class FlightSide: + """The flight half of the test, driven over a pipe in its own interpreter. + + The env container puts projectairsim in the system Python and the flight dependencies in + the uv project venv, so no single interpreter can import both. Rather than requiring + anyone to modify their container, this follows the split the repo already uses: + interfaces/iarc.py runs on the system Python and spawns `uv run run.py` for flight. Here + the child is coordinate_flight_helper.py, launched the same way, and the parent keeps + control of sequencing so a marker is never photographed against a stale pose. + """ + + def __init__( + self, + repo_root: Path, + address: str, + startup_timeout: float, + command: list[str] | None = None, + ): + # `uv run` is what puts dronekit on the child's path, matching how iarc.py starts + # run.py. Overridable so the protocol can be exercised without a SITL. + command = command or ["uv", "run", "python", str(HELPER_SCRIPT), "--address", address] + projectairsim_log().info(f"Starting flight side: {' '.join(command)} (cwd {repo_root})") + try: + self._process = subprocess.Popen( + command, + cwd=str(repo_root), + stdin=subprocess.PIPE, + stdout=subprocess.PIPE, + # stderr is inherited so the child's progress lands on the console live. + text=True, + bufsize=1, + ) + except FileNotFoundError as err: + raise FlightFailed( + "'uv' is not on PATH. The flight half runs under `uv run` so it picks up " + "dronekit from the project venv, exactly as interfaces/iarc.py launches run.py." + ) from err + + self._replies: queue.Queue = queue.Queue() + self._reader = threading.Thread(target=self._pump_stdout, daemon=True) + self._reader.start() + + # `uv run` may sync the project venv before the child starts, which on a cold + # container is slow, so the first wait is the generous one. + ready = self._await_reply(startup_timeout) + if not ready.get("ok"): + raise FlightFailed(ready.get("error", "flight side failed to start")) + projectairsim_log().info("Flight side connected to the SITL.") + + def _pump_stdout(self) -> None: + """Move replies onto the queue; anything else on stdout is the child's own chatter.""" + for line in self._process.stdout: + line = line.strip() + if line.startswith(REPLY_PREFIX): + try: + self._replies.put(json.loads(line[len(REPLY_PREFIX) :])) + except ValueError: + print(f"[flight] unparseable reply: {line}", file=sys.stderr) + elif line: + print(f"[flight] {line}", file=sys.stderr) + # EOF: the child is gone. Unblock anyone waiting rather than hanging to the timeout. + self._replies.put({"ok": False, "error": "flight side exited"}) + + def _await_reply(self, timeout: float) -> dict: + try: + return self._replies.get(timeout=timeout) + except queue.Empty as err: + raise FlightFailed(f"flight side did not reply within {timeout:.0f}s") from err + + def request(self, cmd: str, timeout: float = COMMAND_TIMEOUT_SEC, **params) -> dict: + if self._process.poll() is not None: + raise FlightFailed(f"flight side exited with code {self._process.returncode}") + self._process.stdin.write(json.dumps({"cmd": cmd, **params}) + "\n") + self._process.stdin.flush() + + response = self._await_reply(timeout) + if not response.get("ok"): + raise FlightFailed(f"{cmd}: {response.get('error', 'refused')}") + return response + + def pose(self) -> DronePose: + """The pose the real flight code would be working from, ready for the conversion.""" + pose = self.request("pose")["pose"] + return DronePose( + lat=pose["lat"], + lon=pose["lon"], + altitude=pose["alt_m"], + yaw=pose["yaw_deg"], + pitch=pose["pitch_deg"], + roll=pose["roll_deg"], + ) + + def close(self, land: bool) -> None: + if self._process.poll() is not None: + return + try: + if land: + projectairsim_log().info("Landing...") + self.request("land") + time.sleep(2) + self.request("quit") + except (FlightFailed, OSError) as err: + projectairsim_log().warning(f"Flight side did not shut down cleanly: {err}") + finally: + try: + self._process.wait(timeout=15) + except subprocess.TimeoutExpired: + self._process.kill() + + +def wait_until_grounded(drone: Drone) -> float: + """Wait for the drone to come to rest on the ground, and return that scene-NED z. + + The scene spawns the drone 15 m up, so it is still falling when the SITL first comes + alive. Reading the ground plane mid-fall would place every marker at the wrong height, + and since the conversion intersects a ray with that plane, the whole run would be + measuring against fiction. So poll until the height stops changing. + """ + deadline = time.monotonic() + GROUNDING_TIMEOUT_SEC + previous = read_ground_truth(drone).down + stable_samples = 0 + + while time.monotonic() < deadline: + time.sleep(0.5) + current = read_ground_truth(drone).down + if abs(current - previous) < GROUNDING_TOLERANCE_M: + stable_samples += 1 + if stable_samples >= GROUNDING_STABLE_SAMPLES: + return current + else: + stable_samples = 0 + previous = current + + projectairsim_log().warning( + f"Drone height was still changing after {GROUNDING_TIMEOUT_SEC:.0f}s " + f"(scene NED z = {previous:.2f} m). Using it anyway; if the markers come out " + "floating or buried, this is why." + ) + return previous + + +def measure_marker( + *, + label: str, + drone: Drone, + world: World, + flight: FlightSide, + asset: str, + marker_ned: tuple[float, float, float], + marker_size_m: float, + baseline_scene: np.ndarray | None, + baseline_segmentation: np.ndarray | None, + intrinsics: CameraIntrinsics, + camera_id: str, + ground_plane_z: float, + home_geo_point: dict, + expected_area_px: float, +) -> MarkerResult: + """Spawn one marker, photograph it, and convert its pixel back to a ground position.""" + north, east, down = marker_ned + marker_position = (north, east) + object_name = f"coord_test_{label}" + spawned = "" + + try: + spawned = world.spawn_object( + object_name, + asset, + pose_at(north, east, down), + [marker_size_m, marker_size_m, MARKER_THICKNESS_M], + False, + ) + # The marker has to be rendered before it can be photographed. + time.sleep(0.5) + + scene, segmentation = capture(drone, camera_id) + + # Sample the pose next to the capture, not once at the start: the drone drifts + # between markers, and a stale pose would be charged to the conversion. + drone_pose = flight.pose() + truth = read_ground_truth(drone) + truth_geo = drone.get_ground_truth_geo_location() + + predicted = predict_pixel(marker_position, truth, ground_plane_z, intrinsics) + frame_area_px = intrinsics.width * intrinsics.height + gate_px = ( + math.hypot(intrinsics.width, intrinsics.height) * DETECTION_GATE_DIAGONAL_FRACTION + ) + + # Segmentation first (flat-shaded, so the difference is exact), scene as the fallback. + # Both are tried even when segmentation returns something implausible: a rejected + # segmentation blob says nothing about whether the scene image holds the marker. + found: Blob | None = None + note = "" + for baseline_frame, current_frame, threshold in ( + (baseline_segmentation, segmentation, 0), + (baseline_scene, scene, SCENE_DIFF_THRESHOLD), + ): + if baseline_frame is None or current_frame is None: + continue + blobs = find_blobs(baseline_frame, current_frame, threshold) + found, note = select_marker_blob( + blobs, expected_area_px, frame_area_px, predicted, gate_px + ) + if found is not None: + break + + if found is None: + return MarkerResult( + label=label, + marker_ned=marker_position, + pixel=None, + blob_pixels=0, + computed_ned=None, + computed_latlon=None, + gt_pose_ned=None, + predicted_pixel=predicted, + note=note or "marker not visible in frame", + ) + + (px, py), area = found.centroid, found.area + + # The down camera is bolted to the frame at a -90 deg pitch, which is the mounting + # that makes GimbalPose()'s zero rotation mean "straight down" in this model. The + # drone's own attitude is applied separately, which is what a rigid mount does. + gimbal_pose = GimbalPose() + + # h_fov/v_fov are consumed as radians here (tan(h_fov / 2), no conversion), unlike + # the copy in tests/flight_day_3-7 which converts from degrees. + computed_latlon = pixel_to_geocoord_gimbal( + px=px, + py=py, + image_width=intrinsics.width, + image_height=intrinsics.height, + h_fov=intrinsics.h_fov_rad, + v_fov=intrinsics.v_fov_rad, + drone=drone_pose, + gimbal=gimbal_pose, + ) + + # The same conversion again, but handed the pose the sim knows to be exact. If this + # column is tight and the DroneKit one is not, the maths is sound and the autopilot's + # position estimate is what moved. + gt_pose_latlon = None + if truth_geo: + gt_pose_latlon = pixel_to_geocoord_gimbal( + px=px, + py=py, + image_width=intrinsics.width, + image_height=intrinsics.height, + h_fov=intrinsics.h_fov_rad, + v_fov=intrinsics.v_fov_rad, + drone=DronePose( + lat=float(truth_geo["latitude"]), + lon=float(truth_geo["longitude"]), + # AGL measured to the plane the markers sit on, not the geoid. + altitude=ground_plane_z - truth.down, + yaw=truth.yaw_deg, + pitch=truth.pitch_deg, + roll=truth.roll_deg, + ), + gimbal=gimbal_pose, + ) + + return MarkerResult( + label=label, + marker_ned=marker_position, + pixel=(px, py), + blob_pixels=area, + computed_ned=( + latlon_to_ned(home_geo_point, *computed_latlon) if computed_latlon else None + ), + computed_latlon=computed_latlon, + gt_pose_ned=( + latlon_to_ned(home_geo_point, *gt_pose_latlon) if gt_pose_latlon else None + ), + predicted_pixel=predicted, + note="" if computed_latlon else "ray did not intersect the ground plane", + ) + finally: + if spawned: + world.destroy_object(spawned) + time.sleep(0.2) + + +def annotate(image: np.ndarray, results: list[MarkerResult], path: Path) -> None: + """Save the baseline frame with a crosshair where each marker was detected. + + The baseline is the marker-free capture, so the crosshairs show where the markers were + found rather than sitting on top of them -- which is what you want when checking whether + a detection landed on the tile's centre or drifted onto its shadow. + """ + canvas = image.copy() + if canvas.ndim == 2: + canvas = cv2.cvtColor(canvas, cv2.COLOR_GRAY2BGR) + + for result in results: + if result.pixel is None: + continue + x, y = int(round(result.pixel[0])), int(round(result.pixel[1])) + cv2.drawMarker(canvas, (x, y), (0, 0, 255), cv2.MARKER_CROSS, 12, 1) + cv2.putText( + canvas, result.label, (x + 6, y - 6), cv2.FONT_HERSHEY_SIMPLEX, 0.35, (0, 0, 255), 1 + ) + + path.parent.mkdir(parents=True, exist_ok=True) + cv2.imwrite(str(path), canvas) + projectairsim_log().info(f"Annotated capture written to {path}") + + +def report( + results: list[MarkerResult], + tolerance_m: float, + drone_gps_error_m: float, + heading_deg: float, +) -> bool: + """Print the per-marker table and return whether every marker met the tolerance.""" + header = ( + f"{'marker':<9} {'pixel':<13} {'expect px':<13} {'blob':>7} {'marker N,E':<16} " + f"{'computed N,E':<18} {'error':>7} {'gt-pose':>8}" + ) + print() + print(header) + print("-" * len(header)) + + passed = True + for result in results: + pixel = f"{result.pixel[0]:.0f},{result.pixel[1]:.0f}" if result.pixel else "-" + expected_pixel = ( + f"{result.predicted_pixel[0]:.0f},{result.predicted_pixel[1]:.0f}" + if result.predicted_pixel + else "-" + ) + blob = f"{result.blob_pixels:7d}" if result.blob_pixels else f"{'-':>7}" + actual = f"{result.marker_ned[0]:+.2f},{result.marker_ned[1]:+.2f}" + computed = ( + f"{result.computed_ned[0]:+.2f},{result.computed_ned[1]:+.2f}" + if result.computed_ned + else "-" + ) + error = result.error_m + gt_error = result.gt_pose_error_m + error_text = f"{error:7.2f}" if error is not None else f"{'-':>7}" + gt_text = f"{gt_error:8.2f}" if gt_error is not None else f"{'-':>8}" + print( + f"{result.label:<9} {pixel:<13} {expected_pixel:<13} {blob} {actual:<16} " + f"{computed:<18} {error_text} {gt_text}" + ) + if result.note: + print(f"{'':<9} -> {result.note}") + if error is None or error > tolerance_m: + passed = False + + print("-" * len(header)) + print( + f"measured at heading {heading_deg:.0f} deg; tolerance {tolerance_m:.2f} m; DroneKit " + f"position was {drone_gps_error_m:.2f} m from the sim's truth at capture time." + ) + print( + "'error' uses the DroneKit pose the flight code would have had; 'gt-pose' repeats the " + "conversion with the sim's exact pose and is diagnostic only." + ) + print( + "'expect px' is where nadir geometry says the marker should have appeared; it gates " + "detection only and is never what 'error' is measured against." + ) + if drone_gps_error_m > tolerance_m: + print( + "NOTE: the autopilot's own position error already exceeds the tolerance, so no " + "pixel can pass. Raise --tolerance-m or --alt-ft, or let the EKF settle longer." + ) + print("RESULT:", "PASS" if passed else "FAIL") + return passed + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description=__doc__.splitlines()[0]) + parser.add_argument( + "--alt-ft", + type=float, + default=10.0, + help="hover altitude in feet AGL (default: %(default)s). The camera footprint scales " + "with this, and so does the share of the frame that GPS error accounts for.", + ) + parser.add_argument( + "--tolerance-m", + type=float, + default=DEFAULT_TOLERANCE_M, + help="maximum acceptable horizontal miss per marker, in metres (default: %(default)s). " + "Must stay well under the camera footprint's half-diagonal or nothing in the frame can " + "fail it; the run refuses to start otherwise and prints the ceiling for the altitude.", + ) + parser.add_argument( + "--ground-offset-m", + type=float, + default=0.0, + help="added to the drone's resting height to get the plane markers sit on, positive " + "down (default: %(default)s). The resting height is the drone's origin, which stands " + "off the terrain by the height of its body and gear; raise this if markers spawn " + "floating, lower it if they spawn buried.", + ) + parser.add_argument( + "--yaw-deg", + type=float, + default=DEFAULT_YAW_DEG, + help="compass heading to hover at while measuring (default: %(default)s). Do not use " + "0 unless you specifically want the nose-north case: at heading 0 an inverted yaw " + "convention gives the same answers as a correct one, so the run proves nothing about " + "yaw. Compare a run at 0 against one at 45 to isolate yaw handling.", + ) + parser.add_argument( + "--camera-id", + default=CAMERA_ID, + help="camera sensor to photograph the markers with (default: %(default)s)", + ) + parser.add_argument( + "--fov-axis", + choices=("horizontal", "vertical"), + default="horizontal", + help="which axis the config's fov-degrees describes (default: %(default)s). Flip this " + "if the misses are large along one image axis and small along the other.", + ) + parser.add_argument( + "--out-dir", + type=Path, + default=REPO_ROOT / "Logs" / "coordinate_accuracy", + help="where to write the annotated capture (default: %(default)s)", + ) + parser.add_argument( + "--no-land", + action="store_true", + help="leave the drone hovering instead of landing, for repeated manual runs", + ) + return parser.parse_args() + + +def run(args: argparse.Namespace) -> bool: + client = ProjectAirSimClient(address=os.environ.get("PAS_HOST", "127.0.0.1")) + flight = None + altitude_m = args.alt_ft * FEET_TO_M + + try: + projectairsim_log().info("Connecting to Project AirSim...") + client.connect() + + # Same three-step handshake as interfaces/iarc.py: an empty scene gives the SITL + # something to pull scene data from, and a drone that spawns before its SITL exists + # softlocks with no way back. + World(client, iarc.EMPTY_SCENE, delay_after_load_sec=2) + iarc.wait_for_sitl_launch() + + world = CoordinateTestWorld( + client, + iarc.IARC_SCENE, + delay_after_load_sec=2, + num_drones=1, + camera_id=args.camera_id, + ) + drone = Drone(client, world, iarc.drone_name(1)) + iarc.wait_for_mavlink() + + intrinsics = read_camera_intrinsics(world.robot_config, args.camera_id, args.fov_axis) + forward_extent, right_extent = intrinsics.footprint(altitude_m) + projectairsim_log().info( + f"{args.camera_id}: {intrinsics.width}x{intrinsics.height}, " + f"h_fov {math.degrees(intrinsics.h_fov_rad):.1f} deg, " + f"v_fov {math.degrees(intrinsics.v_fov_rad):.1f} deg -> ground footprint at " + f"{altitude_m:.2f} m is {forward_extent:.2f} m fwd x {right_extent:.2f} m right" + ) + + # A tolerance that approaches the footprint's half-diagonal cannot be failed by any + # pixel in the frame, so the run would report PASS without measuring anything. + half_diagonal = math.hypot(forward_extent, right_extent) / 2.0 + tolerance_ceiling = half_diagonal * MAX_TOLERANCE_FOOTPRINT_FRACTION + if args.tolerance_m > tolerance_ceiling: + raise RuntimeError( + f"--tolerance-m {args.tolerance_m:.2f} is too loose to mean anything at this " + f"altitude: the camera only sees {forward_extent:.2f} x {right_extent:.2f} m, " + f"so no pixel in the frame can miss by more than {half_diagonal:.2f} m and " + f"every run would pass. Use --tolerance-m {tolerance_ceiling:.2f} or less, or " + "raise --alt-ft to widen the footprint." + ) + + home_geo_point = world.home_geo_point + asset = pick_marker_asset(world) + marker_size = max(MARKER_MIN_SIZE_M, MARKER_FRAME_FRACTION * right_extent) + marker_width_px = marker_size / right_extent * intrinsics.width + marker_height_px = marker_size / forward_extent * intrinsics.height + expected_area_px = marker_width_px * marker_height_px + projectairsim_log().info( + f"Markers will be {marker_size:.2f} m square " + f"(~{marker_width_px:.0f} px across, ~{expected_area_px:.0f} px in area)" + ) + + # The ground plane, taken as the drone's own resting height. DroneKit's relative + # altitude is measured from this same spot, so markers placed here sit at exactly + # the "AGL = 0" plane the conversion assumes -- no separate terrain lookup needed. + resting_z = wait_until_grounded(drone) + ground_plane_z = resting_z + args.ground_offset_m + projectairsim_log().info( + f"Ground plane taken as scene NED z = {ground_plane_z:.2f} m " + f"(drone rested at {resting_z:.2f} m, offset {args.ground_offset_m:+.2f} m). " + "This is the drone's own origin, which sits a little above the terrain by however " + "much its body and gear stand off the ground; --ground-offset-m corrects that if " + "the markers come out floating or buried." + ) + + flight = FlightSide( + REPO_ROOT, + f"tcp:127.0.0.1:{iarc.SITL_MAVLINK_PORT}", + startup_timeout=STARTUP_TIMEOUT_SEC, + ) + + projectairsim_log().info(f"Taking off to {altitude_m:.2f} m ({args.alt_ft} ft) AGL...") + flight.request("takeoff", timeout=TAKEOFF_TIMEOUT_SEC, alt_m=altitude_m) + + projectairsim_log().info(f"Yawing to {args.yaw_deg:.0f} deg to expose yaw handling...") + flight.request("yaw", timeout=YAW_TIMEOUT_SEC, heading_deg=args.yaw_deg) + + # Marker offsets are laid out in the body frame and rotated into NED by the drone's + # true heading, so they land in the frame whatever heading the hover settled on. + hover = read_ground_truth(drone) + yaw = math.radians(hover.yaw_deg) + + baseline_scene, baseline_segmentation = capture(drone, args.camera_id) + if baseline_segmentation is None: + projectairsim_log().warning( + "No segmentation frame available; falling back to differencing the scene " + "image, which is more sensitive to lighting and shadow." + ) + if baseline_scene is None and baseline_segmentation is None: + raise RuntimeError( + f"camera '{args.camera_id}' returned no images. Check that its scene capture " + "is enabled in the robot config." + ) + + results: list[MarkerResult] = [] + for label, forward_fraction, right_fraction in MARKER_PLACEMENTS: + forward = forward_fraction * forward_extent / 2.0 + right = right_fraction * right_extent / 2.0 + north = hover.north + forward * math.cos(yaw) - right * math.sin(yaw) + east = hover.east + forward * math.sin(yaw) + right * math.cos(yaw) + + projectairsim_log().info( + f"Marker '{label}' at scene NED ({north:.2f}, {east:.2f}) -- " + f"{forward:+.2f} m fwd, {right:+.2f} m right of the drone" + ) + result = measure_marker( + label=label, + drone=drone, + world=world, + flight=flight, + asset=asset, + marker_ned=(north, east, ground_plane_z), + marker_size_m=marker_size, + baseline_scene=baseline_scene, + baseline_segmentation=baseline_segmentation, + intrinsics=intrinsics, + camera_id=args.camera_id, + ground_plane_z=ground_plane_z, + home_geo_point=home_geo_point, + expected_area_px=expected_area_px, + ) + + # Markers metres apart cannot share a pixel. When they do, the detector is locked + # onto something that is not the markers -- the drone's own shadow, or a drift + # component whose centroid sits near the principal point -- and the conversion is + # being handed the same input every time. Discarding the reading keeps that from + # scoring as a near-miss instead of the failure it is. + twin = next( + ( + other + for other in results + if other.pixel is not None + and result.pixel is not None + and math.hypot( + other.pixel[0] - result.pixel[0], other.pixel[1] - result.pixel[1] + ) + < DUPLICATE_PIXEL_RADIUS + ), + None, + ) + if twin is not None: + result.computed_ned = None + result.computed_latlon = None + result.note = ( + f"detected at the same pixel as '{twin.label}', which is impossible for " + "markers this far apart -- the detection is not tracking the markers" + ) + + results.append(result) + + # How far the autopilot thought it was from where it actually was. This is the floor + # under every error in the table. + reported = flight.pose() + reported_north, reported_east = latlon_to_ned(home_geo_point, reported.lat, reported.lon) + actual = read_ground_truth(drone) + drone_gps_error = math.hypot(reported_north - actual.north, reported_east - actual.east) + + if baseline_scene is not None: + annotate(baseline_scene, results, args.out_dir / "markers_annotated.png") + + return report(results, args.tolerance_m, drone_gps_error, actual.yaw_deg) + + finally: + if flight is not None: + flight.close(land=not args.no_land) + client.disconnect() + + +def main() -> int: + args = parse_args() + try: + return 0 if run(args) else 1 + except Exception as err: # noqa: BLE001 -- a sim run should report, not traceback-dump + projectairsim_log().error(f"Exception occurred: {err}", exc_info=True) + return 2 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/tests/multidrone_world.py b/tests/multidrone_world.py new file mode 100644 index 0000000..f0c42ce --- /dev/null +++ b/tests/multidrone_world.py @@ -0,0 +1,83 @@ +import copy +import random + +from projectairsim import ProjectAirSimClient +from projectairsim.utils import load_scene_config_as_dict +from projectairsim.world import World + + +class MultidroneWorld(World): + + # ProjectAirsim has reading config from a file deeply integrated into it + # You can't even trick it using a buffer or something. + # Thus, the easiest strat (without creating a million temp files) is + # to essentially intercept the initialization process after the config is + # read to a dict and inject our generated settings there. + # Most of the below code is the same as the normal World class __init__ + def __init__( + self, + client: ProjectAirSimClient, + scene_config_name: str = "", + delay_after_load_sec: int = 0, + sim_config_path: str = "sim_config/", + sim_instance_idx: int = -1, + drone_grid: tuple[int, int] | None = None, + x_sep: float = 3.0, + y_sep: float = 3.0, + ): + """ProjectAirSim World Interface. + + Args: + client (ProjectAirSimClient): ProjectAirSim client object + scene_config (str): Name of the scene config JSON file to load in the sim + delay_after_load_sec (int): Time in seconds to wait after the scene is loaded + sim_config_path (string): Relative path to search for the scene_config + sim_instance_idx (int): the instance index of the simulation (for distributed sim only) + """ + self.client = client + self.sim_config_path = sim_config_path + self.sim_instance_idx = sim_instance_idx + self.parent_topic = "/Sim/SceneBasicDrone" # default-scene's ID + + self.sim_config = None + self.home_geo_point = None + if scene_config_name: + config_loaded, config_paths = load_scene_config_as_dict( + scene_config_name, + sim_config_path, + sim_instance_idx, + ) + config_dict = config_loaded + + if drone_grid is not None: + row, col = drone_grid + template = config_dict["actors"][0] + template["name"] = "Drone_0_0" + start_x, start_y, z = map(float, template["origin"]["xyz"].split()) + + for r in range(row): + for c in range(col): + # don't remake existing drone (i.e., the template) + if r == 0 and c == 0: + continue + + new_drone = copy.deepcopy(template) + new_drone["name"] = f"Drone_{r}_{c}" + + new_drone["origin"]["xyz"] = " ".join(map(str, [start_x + c * x_sep, start_y + r * y_sep, z])) + + drone_num = col * r + c + ardu_settings = new_drone["robot-config"]["controller"]["ardupilot-settings"] + ardu_settings["ardupilot-udp-port"] += 10 * drone_num + ardu_settings["local-host-udp-port"] += 10 * drone_num + + config_dict["actors"].append(new_drone) + + self.scene_config_path = config_paths[0] + self.robot_config_paths = config_paths[1] + self.envactor_config_paths = config_paths[2] + self.load_scene(config_dict, delay_after_load_sec=delay_after_load_sec) + random.seed() + self.import_ned_trajectory( + "null_trajectory", [0, 1], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0] + ) \ No newline at end of file diff --git a/update_airsim_settings.ps1 b/update_airsim_settings.ps1 new file mode 100644 index 0000000..28d7f9f --- /dev/null +++ b/update_airsim_settings.ps1 @@ -0,0 +1,167 @@ +<# +.SYNOPSIS + This script simplifies modifying AirSim settings. Fundamentally, it + copies a given file into a `settings.json` file where AirSim expects it + (or into a directory specified by the user). +.DESCRIPTION + There are two main usages of this script. The first is copying a + settings file to AirSim's anticipated global settings location. The + second is automatically configuring multi-drone scenarios, using a + given file as a template. +.PARAMETER File + The file to copy to AirSim Settings +.PARAMETER OutDir + The directory to copy the given file to, under the name "settings.json". + By default, this is the directory AirSim looks for settigns globally. + If using a compiled version of the simulation, you may want to change this + to the directory of the executable, since AirSim looks there first. +.PARAMETER NumDrones + This is part of a ease-of-use functionality that automatically creates + drones compatible with what our simulation Docker container expects. The + settings of each drone is copied from the drone settings provided by the given + File, but the properties "ControlPort", "UdpPort", "X", and "Y" are adjusted + automatically and may be overridden. + + This parameter expects a grid size input, or ROW,COL. +.PARAMETER XSep + Given that NumDrones is provided, determines the x-separation between drones + in meters. This parameter is 3 by default. +.PARAMETER YSep + Given that NumDrones is provided, determines the y-separation between drones + in meters. This parameter is 3 by default. +.PARAMETER ZOffset + Given the NumDrones is provided, sets the Z offset for all drones. All drones + have the same offset. Negative values correspond to higher altitudes. + This parameter is 0 by default. +.PARAMETER StartControlPort + Given that NumDrones is provided, determines the control port of the first drone. + Each successive drone has a port ten higher than the previous. It is 9002 by default. + + e.g., drone ports will be {9002, 9012, 9022, 9032, ...} if this value is + 9002 +.PARAMETER StartUdpPort + Given that NumDrones is provided, determines the UDP port of the first drone. + Each successive drone has a port ten higher than the previous. It is 9003 by default. + + e.g., drone ports will be {9003, 9013, 9023, 9033, ...} if this value is + 9003 +.EXAMPLE + C:\SUAS-2025> ./update_airsim_settings.ps1 ./my_settings.json + Simplest case copying the contents of ./my_settings.json to the default AirSim settigns.json. + + C:\SUAS-2025> ./update_airsim_settings.ps1 ./my_settings.json -NumDrones 5,5 + Example of automatically creating a 5 by 5 grid of drones. The settigns in my_settings.json + will be used as a template. +#> + +# This script copies the content of the provided file into AirSim's settings +# Simulation only works on Windows, hence a powershell script + +param( + [Parameter(Mandatory, Position=0)] + [string]$File, # this is positional (e.g., ./update_airsim_settings.ps1 file.json) + + [string]$OutDir = [System.IO.Path]::Combine([Environment]::GetFolderPath('Personal'), "AirSim"), + + [Parameter(ParameterSetName="ndrones")] + [int[]]$NumDrones, + [Parameter(ParameterSetName="ndrones")] + [float]$XSep = 3, + [Parameter(ParameterSetName="ndrones")] + [float]$YSep = 3, + [Parameter(ParameterSetName="ndrones")] + [float]$ZOffset = 0, + [Parameter(ParameterSetName="ndrones")] + [int]$StartControlPort = 9002, + [Parameter(ParameterSetName="ndrones")] + [int]$StartUdpPort = 9003 + +) + +# returns the value of an object's property or a specifed default value if the property is null +function Get-ObjectPropertyOrDefault { + param( + [Parameter(Mandatory=$true)] + $Object, + + [Parameter(Mandatory=$true)] + [string]$PropertyName, + + [Parameter(Mandatory=$true)] + $DefaultValue + ) + + # Check if the property actually exists on the object + if ($Object.$PropertyName) { + # Return the actual value of the property + return $Object.$PropertyName + } + else { + # Return the default value + return $DefaultValue + } +} + +# make sure the file we trying to copy is real (like the One Piece) +# not doing this will result in the contents of settings.json being empty +if (-not (Test-Path -Path $File -PathType Leaf)) { + Write-Error "Provided input file does not exist or is not a file." + exit 1 +} + +$settings = Get-Content "$File" + +# code for automatically creating multiple drones +if ($null -ne $NumDrones) { + + # validate NumDrones + if ($NumDrones.Count -ne 2) { + Write-Error -Message "NumDrones must be an array of size 2! NumDrones should be given in NumRows,NumCols format." -Category InvalidArgument + exit + } + if ($NumDrones[0] -lt 1 -or $NumDrones[1] -lt 1) { + Write-Error -Message "Both values in NumDrones must be integers greater than 0!" -Category InvalidArgument + exit + + } + + # CREATE DRONES + + $settingsJSON = $settings | ConvertFrom-Json # convert the settings to a PowerShell object + + # copy the first vehicle in Vehicles to $templateDrone + $templateDrone = @{} + $($settingsJSON.Vehicles.PsObject.Properties.Value | Select-Object -First 1).PsObject.Properties | ForEach-Object { $templateDrone[$_.Name] = $_.Value } + + # find base X and Y offsets + $baseX = Get-ObjectPropertyOrDefault $templateDrone X 0 + $baseY = Get-ObjectPropertyOrDefault $templateDrone Y 0 + + $settingsJSON.Vehicles = @{} + + # create drone grid + for ($row = 0; $row -lt $NumDrones[0]; $row++) { + for ($col = 0; $col -lt $NumDrones[1]; $col++) { + $droneId = $NumDrones[1] * $row + $col + $newDrone = $templateDrone.Clone() + + $newDrone["X"] = $baseX + ($XSep * $row) + $newDrone["Y"] = $baseY + ($YSep * $col) + $newDrone["Z"] = $ZOffset + $newDrone["UdpPort"] = $StartUdpPort + (10 * $droneId) + $newDrone["ControlPort"] = $StartControlPort + (10 * $droneId) + + $settingsJSON.Vehicles["Copter$($droneId + 1)"] = $newDrone + } + } + $settings = $settingsJSON | ConvertTo-Json -Depth 100 + + Write-Output "Generated settings for $($NumDrones[0] * $NumDrones[1]) total drones." +} + +# write the data +Write-Output "$settings" > "$outdir\settings.json" + +# log messages +Write-Output "Updated $outdir\settings.json" +Write-Output "Settings updated!"