From 771de2a8a95ca01cbe92be07cfc24813fc5b0d1a Mon Sep 17 00:00:00 2001 From: Samardeep Singh Date: Sat, 25 Apr 2026 12:31:18 -0600 Subject: [PATCH 1/2] working and resolving issue #70 - add MJPEG video stream endpoint --- repomix-output.xml | 6149 ++++++++++++++++++++++ samples/aruco_stream.py | 49 + src/modules/emu/emu.py | 53 +- src/modules/imaging/.aruco_stream.py.swp | Bin 0 -> 12288 bytes src/modules/imaging/analysis.py | 8 +- src/modules/imaging/aruco_stream.py | 33 + src/modules/imaging/detector.py | 57 +- src/modules/imaging/video_emu_stream.py | 88 + test/test_analysis.py | 9 +- 9 files changed, 6406 insertions(+), 40 deletions(-) create mode 100644 repomix-output.xml create mode 100644 samples/aruco_stream.py create mode 100644 src/modules/imaging/.aruco_stream.py.swp create mode 100644 src/modules/imaging/aruco_stream.py create mode 100644 src/modules/imaging/video_emu_stream.py diff --git a/repomix-output.xml b/repomix-output.xml new file mode 100644 index 0000000..bc32bc0 --- /dev/null +++ b/repomix-output.xml @@ -0,0 +1,6149 @@ +This file is a merged representation of the entire codebase, combined into a single document by Repomix. + + +This section contains a summary of this file. + + +This file contains a packed representation of the entire repository's contents. +It is designed to be easily consumable by AI systems for analysis, code review, +or other automated processes. + + + +The content is organized as follows: +1. This summary section +2. Repository information +3. Directory structure +4. Repository files (if enabled) +5. Multiple file entries, each consisting of: + - File path as an attribute + - Full contents of the file + + + +- This file should be treated as read-only. Any changes should be made to the + original repository files, not this packed version. +- When processing this file, use the file path to distinguish + between different files in the repository. +- Be aware that this file may contain sensitive information. Handle it with + the same level of security as you would the original repository. + + + +- Some files may have been excluded based on .gitignore rules and Repomix's configuration +- Binary files are not included in this packed representation. Please refer to the Repository Structure section for a complete list of file paths, including binary files +- Files matching patterns in .gitignore are excluded +- Files matching default ignore patterns are excluded +- Files are sorted by Git change count (files with more changes are at the bottom) + + + + + +.github/workflows/lint.yml +.github/workflows/tests.yml +.github/workflows/type.yml +.gitignore +.gitmodules +bin/run-nav.sh +dep/README.md +doc/README.md +install/install.sh +install/mavlink-router.sh +install/setup-ssh-keys.py +install/setup-ssh-keys.sh +install/shepard.service +install/shepard.sh +README.md +requirements.txt +res/README.md +res/test-image.jpeg +samples/analysis.py +samples/aruco_stream.py +samples/battery_status.py +samples/bucket_test_simple.py +samples/bucket_test.py +samples/camera.py +samples/emu_connection.py +samples/geofence_test.py +samples/geofence.json +samples/image_process.py +samples/img_debug.py +samples/img_test.py +samples/kml_mock.py +samples/models/best.pt +samples/models/n416.pt +samples/models/n512.pt +samples/models/n640.pt +samples/models/s14.pt +samples/models/s512.pt +samples/models/s640.pt +samples/oakd.py +samples/README.md +samples/run.sh +samples/test_nn.py +samples/xm125_polling.py +samples/xm125_tui.py +scripts/fmt.sh +scripts/lint.sh +scripts/test.sh +scripts/type.sh +src/__init__.py +src/flight_tests/altimeter_forwarding_2025_02_27.py +src/flight_tests/altimeter_log_2025_03_01.py +src/flight_tests/balloon_search_2025_03_08.py +src/flight_tests/ft_camera_log.py +src/flight_tests/geofence/geofence.json +src/flight_tests/ir.py +src/flight_tests/task2_2025_ir.py +src/flight_tests/task2_2025.py +src/modules/__init__.py +src/modules/autopilot/__init__.py +src/modules/autopilot/altimeter_mavlink.py +src/modules/autopilot/altimeter_xm125.py +src/modules/autopilot/altimeter.py +src/modules/autopilot/lander.py +src/modules/autopilot/messenger.py +src/modules/autopilot/navigator.py +src/modules/autopilot/simulator.py +src/modules/emu/__init__.py +src/modules/emu/emu.py +src/modules/georeference/__init__.py +src/modules/georeference/inference_georeference.py +src/modules/imaging/__init__.py +src/modules/imaging/analysis_view.py +src/modules/imaging/analysis.py +src/modules/imaging/aruco_stream.py +src/modules/imaging/battery.py +src/modules/imaging/bucket_detector.py +src/modules/imaging/camera.py +src/modules/imaging/debug_analysis.py +src/modules/imaging/debug.py +src/modules/imaging/detector.py +src/modules/imaging/LED.py +src/modules/imaging/location.py +src/modules/imaging/mavlink.py +src/modules/imaging/video_stream.py +src/modules/landingspot/landingspot.py +src/modules/README.md +src/README.md +src/sim_scripts/gazebo_camera_test.py +system/boot/config.txt +test/README.md +test/test_analysis.py +test/test_battery.py +test/test_camera.py +test/test_location.py +tools/README.md + + + +This section contains the contents of the repository's files. + + +name: Lint + +on: [push] + +jobs: + build: + runs-on: ubuntu-latest + strategy: + matrix: + python-version: ["3.10"] + steps: + - uses: actions/checkout@v4 + - name: Set up Python ${{ matrix.python-version }} + uses: actions/setup-python@v3 + with: + python-version: ${{ matrix.python-version }} + - uses: actions/setup-python@v4 + with: + python-version: '3.10' + cache: 'pip' + - name: Install dependencies + run: pip install -r requirements.txt + - name: Run linter + run: ./scripts/lint.sh + + + +name: Tests + +on: + push: + branches: [ "main" ] + pull_request: + branches: [ "main" ] + +permissions: + contents: read + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v4 + with: + submodules: 'recursive' + - name: Set up Python 3.10 + uses: actions/setup-python@v3 + with: + python-version: "3.10" + - uses: actions/setup-python@v4 + with: + python-version: '3.10' + cache: 'pip' + - name: Install dependencies + run: pip install -r requirements.txt + - name: Run tests + run: ./scripts/test.sh + + + +name: Typecheck + +on: [push] + +jobs: + build: + runs-on: ubuntu-latest + strategy: + matrix: + python-version: ["3.10"] + steps: + - uses: actions/checkout@v4 + - name: Set up Python ${{ matrix.python-version }} + uses: actions/setup-python@v3 + with: + python-version: ${{ matrix.python-version }} + - uses: actions/setup-python@v4 + with: + python-version: '3.10' + cache: 'pip' + - name: Install dependencies + run: pip install -r requirements.txt + - name: Run mypy + run: ./scripts/type.sh + + + +# These are some examples of commonly ignored file patterns. +# You should customize this list as applicable to your project. +# Learn more about .gitignore: +# https://www.atlassian.com/git/tutorials/saving-changes/gitignore + +# Node artifact files +node_modules/ +dist/ + +# Compiled Java class files +*.class + +# Compiled Python bytecode +*.py[cod] + +# Log files +*.log + +# Package files +*.jar + +# Python Virtual Enviroments +venv + +# Maven +target/ +dist/ + +# JetBrains IDE +.idea/ + +# Unit test reports +TEST*.xml + +# Generated by MacOS +.DS_Store + +# Generated by Windows +Thumbs.db + +# Applications +*.app +*.exe +*.war + +# Large media files +*.mp4 +*.tiff +*.avi +*.flv +*.mov +*.wmv + +.python-version + +# Credentials +credentials.json +id_rsa + + +mavlink-router/ +log + + + +# Dependencies + +This directory contains the dependencies required by Shepard. + + + +# Documentation + +This directory contains files related to documentation. + + + +#!/usr/bin/bash + +set -o errexit + +sudo apt update +sudo apt upgrade + +sudo apt install python3-pyqrcode + +echo "Setting up ssh keys" +./install/setup-ssh-keys.sh + +echo "Installing mavlink router" +./install/mavlink-router.sh + +echo "Installing shepard" +./install/shepard.sh + + + +#!/usr/bin/env bash + +set -o errexit +set -x + +echo "Installing dependencies" +sudo apt install -y git meson ninja-build pkg-config gcc g++ systemd + +echo "Building mavlink-router" +if test -d mavlink-router; then + cd mavlink-router + git pull + git submodule update --init --recursive +else + git clone --recursive https://github.com/mavlink-router/mavlink-router + cd mavlink-router/ +fi +meson setup build . +ninja -C build + +echo "Installing mavlink-router" +sudo ninja -C build install +cd .. + +echo "Setting up system files" +sudo cp -r system/etc/mavlink-router /etc +sudo cp system/boot/config.txt /boot/config.txt + +sudo systemctl enable mavlink-router.service + +echo "Mavlink router install complete" +echo "Please restart with 'sudo reboot' so the changes can take effect" + + + +import pyqrcode + +with open("/home/uaarg/.ssh/id_rsa.pub", "r") as f: + key = f.read() + qr = pyqrcode.create(key) + print(qr.terminal()) + + + +#!/usr/bin/env bash + +ssh-keygen +python3 install/setup-ssh-keys.py + +echo "Add that key to github" +echo "Press enter when done" +read + + + +[Unit] +Description=Shepard +After=login.target + +[Service] +ExecStart=/home/uaarg/Documents/shepard/bin/run-nav.sh +WorkingDirectory=/home/uaarg/Documents/shepard + +[Install] +WantedBy=multi-user.target + + + +#!/usr/bin/bash + +set -o errexit +set -x + +echo "Installing system dependencies" +sudo apt install python3 python3-pip python3-venv + +echo "Installing submodules" +git submodule update --init --recursive + +echo "Installing pip dependencies" +python3 -m venv venv +source venv/bin/activate +pip install -r requirements.txt + +echo "Running tests (sanity check)" +./scripts/test.sh + +echo "Installing shepard.service" +sudo ln install/shepard.service /etc/systemd/system +sudo systemctl enable mavlink-router.service + +echo "Installation complete. You will have to reboot for the installation to take effect" + + + +# UAARG Onboard Script # + +This repository holds the scripts for: + +* Recording and logging images, positional information and messages +* Making pathing decisions based on current data and mission objectives +* Making inferences on images for object identification and location +* Communications between PixHawk controller board + +### How do I get set up? ### + +This script is designed to run on a system with a USB or UART connection to a PixHawk Board, and a camera system. Some functions may not work if the above requirements are not met, but our systems should be designed in a robust way to allow most functions to still work. + +## For X86 or arm64 Systems (ie Desktop + Laptop Computers + RPi) + +* Install Python3 +* Optional Step: It's highly recommended, you create a virtual env before installing dependencies. Activate the virtual environment and proceed. OS specific steps are available in the docs [https://packaging.python.org/guides/installing-using-pip-and-virtual-environments/] +* Initialize all submodules ```git submodule update --init --recursive``` +* Install the required dependancies via ```pip install -r requirements.txt``` +* You should now be able to run testing python scripts and launch the application + +## For armv7l Systems (ie ODroid XU4) + +* Install Python3 +* Initialize all submodules ```git submodule update --init --recursive``` +* Install via apt ```sudo apt install opencv-python``` +* Download and install the pip wheels for torch and torchvision on armv7l +* Install the required dependancies via ```pip install -r requirements.txt``` +* You should now be able to run testing python scripts and launch the application + +## How to setup for auto-running on a rpi + +To setup a systemd service so that shepard will automatically start and +restart. + +```sh +uaarg@raspberrypi:~/shepard $ sudo ln shepard.service /etc/systemd/system +uaarg@raspberrypi:~/shepard $ sudo systemctl enable shepard.service +Created symlink /etc/systemd/system/multi-user.target.wants/shepard.service → /etc/systemd/system/shepard.service. +uaarg@raspberrypi:~/shepard $ sudo systemctl start shepard.service +``` + +Then you can confirm that everything is working by running: + +```sh +uaarg@raspberrypi:~/shepard $ sudo systemctl status shepard.service +● shepard.service - Shepard + Loaded: loaded (/etc/systemd/system/shepard.service; enabled; vendor preset: enabled) + Active: active (running) since Mon 2023-04-03 18:40:55 MDT; 8min ago + Main PID: 2210 (python) + Tasks: 9 (limit: 3933) + CPU: 15min 59.449s + CGroup: /system.slice/shepard.service + ├─2210 python /home/uaarg/shepard/main.py + ├─2220 python /home/uaarg/shepard/main.py + ├─2221 python /home/uaarg/shepard/main.py + └─2222 python /home/uaarg/shepard/main.py +``` + +## How to Solve Permission Errors on Linux + +* If you have permission errors accessing ports, you may need to add your user to the 'tty' and 'dialout' groups +* ```sudo usermod -a -G tty {your username}``` +* ```sudo usermod -a -G dialout {your username}``` +* You will then need to logout and login completely to update your permissions + +### Contribution guidelines ### + +* Write tests so that functionality can be easily reviewed at a later date +* Create a new branch if working on implementing a new feature that will need review or multiple commits +* Add new required packages to requirements.txt + +We have several tools setup, please use them. These are: + +- `./scripts/lint.sh` Run a linter over all python files +- `./scripts/fmt.sh` Run a formatter over all python files + +Also, keep in mind that the tests and linter is run on every commit and/or PR in our CI. + +### Who do I talk to? ### + +* UAARG Imaging Leads, Autopilot Leads, and VP Technical should be able to help you with any questions + + + +# Resources + +This directory contains all static resources, such as images, data files, etc. + + + +from src.modules.imaging import analysis +from src.modules.imaging import camera +from src.modules.imaging import debug +from src.modules.imaging import location +from dep.labeller.benchmarks import yolo + +mavlink_delegate = location.MAVLinkDelegate() +location_provider = location.MAVLinkLocationProvider(mavlink_delegate) +location_provider = location.DebugLocationProvider() +location_provider.debug_change_location(altitude=1) + +detector = yolo.YoloDetector(weights="landing_nano.pt") +cam = camera.RPiCamera() +debugger = debug.ImageAnalysisDebugger() +debugger = None +img_analysis = analysis.ImageAnalysisDelegate(detector, cam, location_provider, + debugger) + +#img_analysis.subscribe(lambda _, __, ___: debugger.show()) +img_analysis.subscribe(lambda _, lon, lat: print(lon, lat)) +img_analysis.start() + +mavlink_delegate.run() + + + +import time + +from src.modules.emu import Emu +from src.modules.imaging.detector import ArucoDetector +from src.modules.imaging.camera import DebugCamera +from src.modules.imaging.location import DebugLocationProvider +from src.modules.imaging.analysis import ImageAnalysisDelegate +from src.modules.imaging.aruco_stream import ArucoEmuStreamer + +def main(): + emu = Emu("tmp") + emu.start_comms() + time.sleep(2) + + detector = ArucoDetector() + camera = DebugCamera("res/test-image.jpeg") + location_provider = DebugLocationProvider() + + analysis = ImageAnalysisDelegate(detector, camera, location_provider) + streamer = ArucoEmuStreamer(emu, "tmp") + + analysis.subscribe(streamer.on_detection) + analysis.start() + + time.sleep(60) + analysis.stop() + +if __name__ == "__main__": + main() + + + +from src.modules.imaging.mavlink import MAVLinkDelegate +from src.modules.imaging.battery import MAVLinkBatteryStatusProvider +import time +import threading + +mavlink = MAVLinkDelegate() +battery = MAVLinkBatteryStatusProvider(mavlink) + + +def wait_for_voltage(): + while True: + try: + voltage = battery.voltage() + print("voltage: ", voltage) + except ValueError: + pass + #print("no data yet") + time.sleep(0.5) + + +threading.Thread(target=wait_for_voltage).start() +mavlink.run() + + + +# Test the WebcamCamera class + +import time +import sys + +from src.modules.imaging.camera import RPiCamera + +cam = RPiCamera() + +#cam.set_size((1000, 1000)) + +print("Taking a picture in 3s.") + +for i in reversed(range(3)): + print(f"{i + 1} ", end="") + # stdout is linebuffered, so nothing will print until a \n otherwise + sys.stdout.flush() + + time.sleep(1) + +print("\rSay cheese!") +time.sleep(1) + +cam.caputure_to("tmp/you.png") +print("Image saved to tmp/you.png") + + + +import json +from src.modules.autopilot.lander import Lander +from src.modules.georeference import inference_georeference + +with open('./samples/geofence.json', 'r') as f: + geofence = json.load(f)['features'][0]['geometry']['coordinates'][0] + + +origin = [-113.54815575690603, 53.495546666117804] + +geofence = inference_georeference.Geofence_to_XY(origin, geofence) +print(geofence) + +lander = Lander(0, 0, geofence) +''' +point1 = [53.4958, -113.5477] +point2 = [53.4956, -113.5481] +point3 = [53.4956, -113.5475] +point4 = [53.4955, -113.5488] +point5 = [53.4955, -113.5488] +''' +point1 = [0, 0] +point2 = [5, 5] +point3 = [10, 10] +point4 = [-5, -5] +point5 = [-10, 10] +point6 = [30.4, + 30.4] +point7 = [-113.54827877970348, + 53.49573842746909] + + +print(lander.geofence_check(point1)) +print(lander.geofence_check(point2)) +print(lander.geofence_check(point3)) +print(lander.geofence_check(point4)) +print(lander.geofence_check(point5)) +print(lander.geofence_check(point6)) +print(lander.geofence_check(point7)) + + + +{ + "type": "FeatureCollection", + "features": [ + { + "type": "Feature", + "properties": {}, + "geometry": { + "coordinates": [ + [ + [ + -113.54797355858594, + 53.49538269557871 + ], + [ + -113.54769792522994, + 53.49557167851731 + ], + [ + -113.5479829020893, + 53.495734721944046 + ], + [ + -113.5484064742526, + 53.495672654349875 + ], + [ + -113.5486322755893, + 53.495509610684024 + ], + [ + -113.54837844374161, + 53.495381769189436 + ], + [ + -113.54797355858594, + 53.49538269557871 + ] + ] + ], + "type": "Polygon" + } + }, + { + "type": "Feature", + "properties": {}, + "geometry": { + "coordinates": [ + -113.54815575690603, + 53.495546666117804 + ], + "type": "Point" + } + } + ] +} + + + +# This can be run with `samples/run.sh image_process.py` +# Or, if you don't have bash installed, run the equivalent of: +# PYTHONPATH=".:dep/labeller" python3 samples/image_process.py + +import time + +from src.modules import imaging +from dep.labeller.benchmarks.colorfilter import ColorFilterDetector + +detector = ColorFilterDetector() +camera = imaging.camera.DebugCamera("./res/test-image.jpeg") +debugger = imaging.debug.ImageAnalysisDebugger() +analysis = imaging.analysis.ImageAnalysisDelegate(detector, camera, debugger) + +analysis.subscribe(print) # Will print all results to stdout +analysis.start() + +print("Analysis is running in the background.") +print("This should have printed very soon after the program started.") + +while True: + # Dummy loop. In a real scenario, we would be running autopilot control + # code here. + time.sleep(100) + + + +from PIL import Image, ImageOps +import time + +from src.modules.imaging.debug import ImageAnalysisDebugger +from benchmarks.detector import BoundingBox, Vec2 + +im = Image.open("res/test-image.jpeg") +bb = BoundingBox(Vec2(200, 120), Vec2(100, 100)) + +dbg = ImageAnalysisDebugger() +print("The debugger should not be visible right now.") + +dbg.set_image(im) +dbg.set_bounding_box(bb) + +dbg.show() +print("The debugger should now be visible") + +print("Because the debugger is not blocking, we can continue to run code.") +print("Close the window to stop") +for i in range(10): + print(f"Running {10 - i} more times") + print("Flipping image") + im = ImageOps.flip(im.copy()) + bb.position = Vec2(bb.position.x, im.height - bb.position.y) + + dbg.set_image(im) + + print("Waiting 1s to display bounding box") + time.sleep(1) + + dbg.set_bounding_box(bb) + dbg.show() + + print("Waiting 2s to display new image") + time.sleep(2) + +print("Before you go! Look at the image for another 3s") +dbg.show() +time.sleep(3) + +print("Bye! (Should exit the program now)") + + + +from src.modules.imaging.kml import KMLGenerator, LatLong + + +kml = KMLGenerator() + +hotspot_1 = LatLong(1,1) +hotspot_2 = LatLong(1,2) +hotspot_3 = LatLong(0,0) + +kml.push(hotspot_1) +kml.push(hotspot_2) +kml.push(hotspot_3) + +print(kml.read(-1).latitude, kml.read(-1).longitude) +kml.pop() + +print(kml.read(-1).latitude, kml.read(-1).longitude) + +kml.set_source("Crashed Drone", LatLong(24, 24)) + +kml.generate("out.kml") + + + +# Samples + +This directory contains samples to help get set up, and code that supports +understanding of the documentation. + +Samples can be run with the helper script. This set's up a `PYTHONPATH` +variable so that you can import modules with, ie. `import +src.modules.imaging.camera`. Here is the usage: + +```sh +samples/run.sh # Where is the name of the samples +samples/run.sh # Leaving blank will print all samples to run +``` + + + +#!/usr/bin/env bash + +set -o errexit + +sample="$1" + +if ! git status 2>&1 >/dev/null; then + echo "Please run this script from inside the shepard repository" + exit 1 +fi + +#cd "$(git worktree list | awk '{ print $1 }')" + +if ! test -f "samples/$sample.py"; then + echo "ERROR: Please pass a sample which is one of:" + ls samples/*.py | sed 's/^samples\// - /' | sed 's/\.py$//' + exit 1 +fi + +PYTHONPATH=".:dep/labeller:dep/labeller/third_party/yolov5" python3 "samples/$sample.py" + + + +from time import sleep + +import src.modules.autopilot.altimeter_xm125 as altimeter_module + +altimeter_module = altimeter_module.XM125(average_window=5) +altimeter_module.begin() + +while True: + result = altimeter_module.measure() + if result: + average = result[0]['averaged'] + + # Average distance will not be available until at least `average_window` measurements have been taken + if average: + average_distance = average[0] + + # `average_distance` can now be used in your autopilot logic + print("Average dist: ", average_distance) + + sleep(0.5) + + + +# xm125_tui.py +import curses +import time +import statistics +from datetime import datetime +from src.modules.autopilot.altimeter_xm125 import XM125, SensorError + + +def format_measurement_table(screen, start_y, peaks, width): + """Format measurements in a clean table layout""" + # Table headers + screen.addstr(start_y, 0, "Raw Measurements:", curses.A_BOLD) + screen.addstr(start_y, width // 2, "Averaged Measurements:", curses.A_BOLD) + + # Column headers for raw data + screen.addstr(start_y + 1, 2, "Peak │ Distance (mm) │ Strength", curses.A_DIM) + screen.addstr(start_y + 2, 2, "─────┼──────────────┼──────────", curses.A_DIM) + + # Column headers for averaged data + screen.addstr(start_y + 1, width // 2 + 2, "Peak │ Distance (mm) │ Strength", curses.A_DIM) + screen.addstr(start_y + 2, width // 2 + 2, "─────┼──────────────┼──────────", curses.A_DIM) + + if peaks: + for i, peak_data in enumerate(peaks): + raw_distance, raw_strength = peak_data['raw'] + + # Raw data + raw_line = f" {i:3d} │ {raw_distance:>12d} │ {raw_strength:>8d}" + screen.addstr(start_y + 3 + i, 2, raw_line) + + # Averaged data if available + if peak_data['averaged']: + avg_distance, avg_strength = peak_data['averaged'] + avg_line = f" {i:3d} │ {avg_distance:>12.1f} │ {avg_strength:>8.1f}" + screen.addstr(start_y + 3 + i, width // 2 + 2, avg_line) + else: + screen.addstr(start_y + 3, 2, "No peaks detected", curses.color_pair(2)) + + return start_y + 5 + (len(peaks) if peaks else 1) + + +def tui_main(): + def setup_curses(): + screen = curses.initscr() + curses.start_color() + curses.use_default_colors() + curses.init_pair(1, curses.COLOR_GREEN, -1) + curses.init_pair(2, curses.COLOR_YELLOW, -1) + curses.init_pair(3, curses.COLOR_RED, -1) + curses.noecho() + curses.cbreak() + screen.keypad(True) + screen.nodelay(1) + return screen + + def cleanup_curses(screen): + screen.keypad(False) + curses.nocbreak() + curses.echo() + curses.endwin() + + try: + screen = setup_curses() + sensor = XM125(average_window=10) + + # Statistics tracking + distance_history = [] + MAX_HISTORY = 100 + error_count = 0 + measurement_count = 0 + start_time = datetime.now() + + if not sensor.begin(): + cleanup_curses(screen) + print("Failed to initialize sensor") + return + + while True: + try: + screen.clear() + height, width = screen.getmaxyx() + + # Header + runtime = datetime.now() - start_time + header = f"XM125 Radar Sensor Monitor - Runtime: {runtime}" + screen.addstr(0, 0, header, curses.A_BOLD) + screen.addstr(1, 0, "=" * width) + + # Get measurement + peaks = sensor.measure() + measurement_count += 1 + + # Status section + status_y = 3 + screen.addstr(status_y, 0, "Status:", curses.A_BOLD) + status_info = [ + f"Measurements: {measurement_count}", + f"Errors: {error_count}", + f"Sample Rate: {1000 / 100:.1f} Hz" # Assuming 100ms sleep + ] + for i, info in enumerate(status_info): + screen.addstr(status_y, 15 + i * 25, info) + + # Measurements section + readings_y = 5 + next_y = format_measurement_table(screen, readings_y, peaks, width) + + # Update history and calculate statistics + if peaks and peaks[0]['raw'][0]: # Use first peak for statistics + distance_history.append(peaks[0]['raw'][0]) + if len(distance_history) > MAX_HISTORY: + distance_history.pop(0) + + # Statistics section + stats_y = next_y + 1 + screen.addstr(stats_y, 0, "Statistics:", curses.A_BOLD) + + if distance_history: + mean_dist = statistics.mean(distance_history) + if len(distance_history) > 1: + stdev_dist = statistics.stdev(distance_history) + else: + stdev_dist = 0 + min_dist = min(distance_history) + max_dist = max(distance_history) + + stats_data = [ + f"Mean: {mean_dist:.1f}mm", + f"Std Dev: {stdev_dist:.1f}mm", + f"Min: {min_dist}mm", + f"Max: {max_dist}mm", + f"Samples: {len(distance_history)}" + ] + + for i, stat in enumerate(stats_data): + screen.addstr(stats_y + 1, 2 + i * 20, stat) + + # Help section + help_y = height - 2 + help_text = [ + ("q", "Quit"), + ("r", "Reset Statistics") + ] + screen.addstr(help_y, 0, "Commands:", curses.A_DIM) + for i, (key, desc) in enumerate(help_text): + screen.addstr(help_y, 12 + i * 20, f"{key}: {desc}", curses.A_DIM) + + screen.refresh() + + # Check for quit command + c = screen.getch() + if c == ord('q'): + break + elif c == ord('r'): + distance_history.clear() + + time.sleep(0.1) + + except SensorError as e: + error_count += 1 + screen.addstr(height - 3, 0, f"Sensor error: {e}", curses.color_pair(3)) + screen.refresh() + time.sleep(2.0) + continue + + except KeyboardInterrupt: + pass + except Exception as e: + cleanup_curses(screen) + print(f"Fatal error: {e}") + finally: + cleanup_curses(screen) + sensor.bus.close() + + +if __name__ == "__main__": + tui_main() + + + +#!/usr/bin/env sh + +yapf --parallel --in-place pylint $(git ls-files '*.py') + + + +#!/usr/bin/env sh + +flake8 \ + --max-line-length 140 \ + --ignore E265,F541,F811,W504,E402,E126,E125,E251,W503 \ + $(git ls-files '*.py') + + + +#!/usr/bin/env sh + +PYTHONPATH=".:dep/labeller" pytest + + + +#!/usr/bin/env sh + +export PYTHONPATH=".:dep/labeller:dep/labeller/third_party/yolov5" + +echo "Checking imaging modules" +mypy --check-untyped-defs --ignore-missing-imports src/modules/imaging/*.py + +echo "Checking tests" +mypy --ignore-missing-imports test/*.py + + + + + + + + + + + + + + + +import threading +import time +from typing import Optional + +from pymavlink import mavutil + +from src.modules.autopilot.altimeter import Altimeter + + +class MavlinkAltimeterProvider: + """ + A class to handle altitude measurements from any Altimeter sensor + and send them to the Pixhawk via MAVLink. + """ + + def __init__(self, sensor: Altimeter, connection_string: str, update_rate_hz: float = 10.0): + """ + Initialize the MavlinkAltimeterProvider. + + Args: + sensor: Altimeter sensor instance + connection_string: MAVLink connection string (e.g., 'udp:127.0.0.1:14550') + update_rate_hz: Rate at which to send altitude updates (Hz) + """ + self.sensor = sensor + self.connection_string = connection_string + self.update_interval = 1.0 / update_rate_hz + self._latest_altitude_mm = None + self._latest_altitude_timestamp = 0 + self._running = False + self._thread = None + self._lock = threading.Lock() + self._mavlink_connection = None + self._tstart = None + + def start(self): + """Start the altimeter thread.""" + if self._running: + return + + try: + self._tstart = time.time() + + # Initialize MAVLink connection + self._mavlink_connection = mavutil.mavlink_connection(self.connection_string) + # Wait for a heartbeat to ensure connection is established + self._mavlink_connection.wait_heartbeat() + print(f"MAVLink connection established on {self.connection_string}") + + # Start the measurement thread + self._running = True + self._thread = threading.Thread(target=self._measurement_loop, daemon=True) + self._thread.start() + print("Altimeter measurement thread started") + + except Exception as e: + print(f"Failed to start MavlinkAltimeterProvider: {e}") + self._running = False + if self._mavlink_connection: + self._mavlink_connection.close() + + def stop(self): + """Stop the altimeter thread.""" + self._running = False + if self._thread: + self._thread.join(timeout=2.0) + if self._mavlink_connection: + self._mavlink_connection.close() + print("Altimeter measurement thread stopped") + + def get_latest_altitude(self) -> Optional[float]: + """ + Get the latest altitude measurement in millimeters. + + Returns: + The latest altitude in mm, or None if no valid measurement is available + """ + with self._lock: + return self._latest_altitude_mm + + def get_latest_altitude_meters(self) -> Optional[float]: + """ + Get the latest altitude measurement in meters. + + Returns: + The latest altitude in meters, or None if no valid measurement is available + """ + with self._lock: + if self._latest_altitude_mm is not None: + return self._latest_altitude_mm / 1000.0 + return None + + def _measurement_loop(self): + """Continuous measurement loop that runs in a separate thread.""" + while self._running: + try: + start_time = time.time() + + # Get measurement from sensor + self.sensor.measure() + + # Get the distance + distance_mm = self.sensor.get_distance_mm() + + if distance_mm is not None: + # Update the latest altitude + with self._lock: + self._latest_altitude_mm = distance_mm + self._latest_altitude_timestamp = time.time() + + # Send to Pixhawk via MAVLink + self._send_distance_sensor_message(distance_mm) + + # Sleep to maintain the desired update rate + elapsed = time.time() - start_time + sleep_time = max(0, self.update_interval - elapsed) + time.sleep(sleep_time) + + except Exception as e: + print(f"Error in measurement loop: {e}") + time.sleep(0.5) + + def _send_distance_sensor_message(self, distance_mm: float): + """ + Send a DISTANCE_SENSOR MAVLink message to the Pixhawk. + + Args: + distance_mm: Distance measurement in millimeters + """ + if not self._mavlink_connection: + return + + try: + # Convert to centimeters for MAVLink message + distance_cm = int(distance_mm / 10.0) + + # Create and send the DISTANCE_SENSOR message + # Documentation: https://mavlink.io/en/messages/common.html#DISTANCE_SENSOR + self._mavlink_connection.mav.distance_sensor_send( + int((time.time() - self._tstart) * 1000), # time_boot_ms + self.sensor.min_distance_cm, # min_distance (cm) + self.sensor.max_distance_cm, # max_distance (cm) + distance_cm, # current_distance (cm) + self.sensor.mavlink_sensor_type, # type (from sensor) + self.sensor.sensor_id, # id (unique ID for this sensor) + mavutil.mavlink.MAV_SENSOR_ROTATION_PITCH_270, # orientation (downward facing) + 0, # covariance + ) + + except Exception as e: + print(f"Error sending MAVLink message: {e}") + + + +import smbus2 +import time +from enum import Enum +from typing import Optional, List, Dict, Any + +from src.modules.autopilot.altimeter import Altimeter, MovingAverage + +DEBUG = False + + +class SensorError(Exception): + """Base exception class for sensor errors""" + pass + + +class SensorState(Enum): + """Enum to track sensor state""" + UNINITIALIZED = 0 + INITIALIZED = 1 + ERROR = 2 + NEEDS_CALIBRATION = 3 + + +class SensorReflectorShape(Enum): + """Enum to define reflector shape""" + GENERIC = 1 + PLANAR = 2 + + +class XM125(Altimeter): + # Register addresses + REG_VERSION = 0x0000 + REG_PROTOCOL_STATUS = 0x0001 + REG_MEASURE_COUNTER = 0x0002 + REG_DETECTOR_STATUS = 0x0003 + REG_DISTANCE_RESULT = 0x0010 + REG_PEAK0_DISTANCE = 0x0011 + REG_PEAK0_STRENGTH = 0x001b + REG_START = 0x0040 + REG_END = 0x0041 + REG_COMMAND = 0x0100 + REG_REFLECTOR_SHAPE = 0x004b + + # Command values + CMD_APPLY_CONFIG_AND_CALIBRATE = 1 + CMD_MEASURE_DISTANCE = 2 + CMD_RECALIBRATE = 5 + CMD_RESET_MODULE = 1381192737 + + # Config values + REFLECTOR_SHAPE = SensorReflectorShape.PLANAR.value + + # Status masks + DETECTOR_STATUS_BUSY_MASK = 0x80000000 + DISTANCE_RESULT_NUM_DISTANCES_MASK = 0x0000000f + DISTANCE_RESULT_NEAR_START_EDGE = 0x00000100 + DISTANCE_RESULT_CALIBRATION_NEEDED = 0x00000200 + DISTANCE_RESULT_MEASURE_DISTANCE_ERROR = 0x00000400 + + # Error recovery constants + MAX_RETRIES = 3 + RETRY_DELAY = 0.5 # seconds + ERROR_TIMEOUT = 5.0 # seconds + + def __init__(self, sensor_id=0, min_distance=250, max_distance=10000, bus=1, address=0x52, average_window=5): + super().__init__(sensor_id, min_distance, max_distance) + self.bus = smbus2.SMBus(bus) + self.address = address + self.distance_avg = MovingAverage(average_window) + self.strength_avg = MovingAverage(average_window) + self.last_error_time = 0 + self.error_count = 0 + self.consecutive_errors = 0 + self._latest_measurement = None + + @property + def mavlink_sensor_type(self) -> int: + """Get the MAVLink sensor type (RADAR)""" + return self.SENSOR_TYPE_RADAR + + def get_distance_mm(self) -> Optional[float]: + """Get the latest distance measurement in millimeters""" + if self._latest_measurement and self._latest_measurement[0]['averaged']: + return self._latest_measurement[0]['averaged'][0] + return None + + def _read_register(self, reg_addr) -> Optional[int]: + """Read a 32-bit register value with error handling.""" + try: + write = smbus2.i2c_msg.write(self.address, [(reg_addr >> 8) & 0xFF, reg_addr & 0xFF]) + read = smbus2.i2c_msg.read(self.address, 4) + + if DEBUG: + print(f"\nREAD OPERATION:") + print(f" Register: 0x{reg_addr:04x}") + print(f" Sending address bytes: MSB=0x{(reg_addr >> 8) & 0xFF:02x}, LSB=0x{reg_addr & 0xFF:02x}") + + self.bus.i2c_rdwr(write, read) + data = list(read) + value = (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | data[3] + + if DEBUG: + print(f" Received bytes: [0x{data[0]:02x}, 0x{data[1]:02x}, 0x{data[2]:02x}, 0x{data[3]:02x}]") + print(f" Decoded value: 0x{value:08x} ({value})") + + return value + except IOError as e: + self._handle_error(f"I/O error reading register 0x{reg_addr:04x}: {e}") + return None + + def _write_register(self, reg_addr: int, value: int) -> bool: + """Write a 32-bit register value with error handling.""" + try: + data = [ + (reg_addr >> 8) & 0xFF, # Address to slave [15:8] + reg_addr & 0xFF, # Address to slave [7:0] + (value >> 24) & 0xFF, # Data to slave [31:24] + (value >> 16) & 0xFF, # Data to slave [23:16] + (value >> 8) & 0xFF, # Data to slave [15:8] + value & 0xFF # Data to slave [7:0] + ] + + if DEBUG: + print(f"\nWRITE OPERATION:") + print(f" Register: 0x{reg_addr:04x}") + print(f" Value to write: 0x{value:08x} ({value})") + print(f" Sending bytes: {[f'0x{b:02x}' for b in data]}") + + self.bus.write_i2c_block_data(self.address, data[0], data[1:]) + return True + except IOError as e: + self._handle_error(f"I/O error writing register 0x{reg_addr:04x}: {e}") + return False + + def _handle_error(self, error_msg: str): + """Handle errors and implement recovery logic.""" + current_time = time.time() + self.consecutive_errors += 1 + self.error_count += 1 + + print(f"Error: {error_msg}") + print(f"Consecutive errors: {self.consecutive_errors}") + print(f"Total errors: {self.error_count}") + + # If we've hit max retries and enough time has passed since last reset + if self.consecutive_errors >= self.MAX_RETRIES: + if current_time - self.last_error_time > self.ERROR_TIMEOUT: + print("Attempting sensor reset and recalibration...") + try: + self.reset_and_recalibrate() + self.consecutive_errors = 0 # Only reset if successful + self.last_error_time = current_time + return + except Exception as e: + print(f"Reset failed: {e}") + + # If we get here, either the timeout hasn't passed or reset failed + raise SensorError(f"Too many consecutive errors: {error_msg}") + + time.sleep(self.RETRY_DELAY) + + def _wait_not_busy(self): + """Wait until the detector is not busy.""" + if DEBUG: + print("\nWaiting for detector not busy...") + + attempts = 0 + while True: + status = self._read_register(self.REG_DETECTOR_STATUS) + if status is None: + if DEBUG: + print(" Failed to read status") + break + + busy = bool(status & self.DETECTOR_STATUS_BUSY_MASK) + if DEBUG: + print(f" Status: 0x{status:08x}, Busy: {busy}") + + if not busy: + break + + time.sleep(0.01) + attempts += 1 + if attempts >= 100: # Timeout after 1 second + if DEBUG: + print(" Timeout waiting for not busy") + break + + def begin(self) -> bool: + """Initialize the sensor with given start and end distances in mm.""" + if DEBUG: + print(f"\nInitializing sensor:") + print(f" Start distance: {self.min_distance_mm}mm") + print(f" End distance: {self.max_distance_mm}mm") + print(f" Reflector shape: {self.REFLECTOR_SHAPE}") + + # Configure start and end distances + if not self._write_register(self.REG_START, self.min_distance_mm): + if DEBUG: + print("Failed to set start distance") + return False + + if not self._write_register(self.REG_END, self.max_distance_mm): + if DEBUG: + print("Failed to set end distance") + return False + + if not self._write_register(self.REG_REFLECTOR_SHAPE, self.REFLECTOR_SHAPE): + if DEBUG: + print("Failed to set reflector shape") + return False + + if DEBUG: + print("Applying configuration and calibrating...") + + # Apply configuration and calibrate + if not self._write_register(self.REG_COMMAND, self.CMD_APPLY_CONFIG_AND_CALIBRATE): + if DEBUG: + print("Failed to apply configuration and calibrate") + return False + + self._wait_not_busy() + + # Check status + status = self._read_register(self.REG_DETECTOR_STATUS) + if DEBUG: + print(f"Final status: 0x{status:08x}" if status is not None else "Failed to read final status") + + success = status is not None and status >= 0 + if success: + self.state = SensorState.INITIALIZED + + return success + + def reset_and_recalibrate(self): + """Reset the sensor and recalibrate.""" + try: + # Reset the module + self._write_register(self.REG_COMMAND, self.CMD_RESET_MODULE) + time.sleep(1.0) # Wait for reset + + # Clear averaging buffers + self.distance_avg.reset() + self.strength_avg.reset() + + # Reinitialize + if not self.begin(): + raise SensorError("Failed to reinitialize sensor after reset") + + print("Sensor reset and recalibration successful") + self.state = SensorState.INITIALIZED + + except Exception as e: + self.state = SensorState.ERROR + raise SensorError(f"Reset and recalibration failed: {str(e)}") + + def check_calibration(self) -> bool: + """Check if sensor needs calibration and perform if needed.""" + result = self._read_register(self.REG_DISTANCE_RESULT) + if result is None: + return False + + if result & self.DISTANCE_RESULT_CALIBRATION_NEEDED: + print("Calibration needed, recalibrating...") + self._write_register(self.REG_COMMAND, self.CMD_RECALIBRATE) + time.sleep(0.5) + self.state = SensorState.INITIALIZED + return True + + return True + + def measure(self) -> List[Dict[str, Any]]: + """Perform a distance measurement with error checking and recovery.""" + try: + if self.state == SensorState.ERROR: + self.reset_and_recalibrate() + + if not self.check_calibration(): + return [] + + # Start measurement + if not self._write_register(self.REG_COMMAND, self.CMD_MEASURE_DISTANCE): + return [] + + self._wait_not_busy() + + # Read result + result = self._read_register(self.REG_DISTANCE_RESULT) + if result is None: + return [] + + if result & self.DISTANCE_RESULT_MEASURE_DISTANCE_ERROR: + self._handle_error("Measurement error") + return [] + + num_distances = (result & self.DISTANCE_RESULT_NUM_DISTANCES_MASK) + peaks_with_average = [] + + for i in range(num_distances): + distance = self._read_register(self.REG_PEAK0_DISTANCE + i) + strength = self._read_register(self.REG_PEAK0_STRENGTH + i) + + if distance is not None and strength is not None: + # Convert strength from 32-bit unsigned to signed int + if strength > 0x7FFFFFFF: + strength = int(0x100000000 - strength) + + self.distance_avg.add(distance) + self.strength_avg.add(strength) + + peak_data = { + 'raw': (distance, strength), + 'averaged': (self.distance_avg.get_average(), + self.strength_avg.get_average()) if self.distance_avg.is_valid() else None + } + peaks_with_average.append(peak_data) + + # Only reset consecutive errors if we got valid data + if peaks_with_average: + self.consecutive_errors = 0 + self._latest_measurement = peaks_with_average + + return peaks_with_average + + except Exception as e: + self._handle_error(f"Measurement error: {str(e)}") + return [] + + +def main(): + sensor = XM125(average_window=5) + + try: + if not sensor.begin(): + print("Failed to initialize sensor") + return + + print("Sensor initialized successfully") + + while True: + try: + peaks = sensor.measure() + + if peaks: + for i, peak_data in enumerate(peaks): + raw_distance, raw_strength = peak_data['raw'] + print(f"\nPeak {i}:") + print(f" Raw: Distance = {raw_distance}mm, Strength = {raw_strength}") + + if peak_data['averaged']: + avg_distance, avg_strength = peak_data['averaged'] + print(f" Avg: Distance = {avg_distance:.1f}mm, Strength = {avg_strength:.1f}") + else: + print("No peaks detected") + + time.sleep(0.5) + + except SensorError as e: + print(f"Sensor error: {e}") + print("Waiting before retry...") + time.sleep(2.0) # Longer delay on serious errors + + # Try to reset the sensor on serious errors + try: + sensor.reset_and_recalibrate() + except Exception as reset_error: + print(f"Reset failed: {reset_error}") + time.sleep(5.0) # Even longer delay if reset fails + + except KeyboardInterrupt: + print("\nStopping measurements") + sensor.bus.close() + except Exception as e: + print(f"Fatal error: {e}") + + +if __name__ == "__main__": + main() + + + +from abc import ABC, abstractmethod +from typing import Optional, List, Dict, Any + + +class MovingAverage: + def __init__(self, window_size=5): + self.window_size = window_size + self.values = [] + + def add(self, value): + self.values.append(value) + if len(self.values) > self.window_size: + self.values.pop(0) + + def get_average(self): + if not self.values: + return None + return sum(self.values) / len(self.values) + + def is_valid(self): + return len(self.values) == self.window_size + + def reset(self): + """Clear all values""" + self.values = [] + + +class Altimeter(ABC): + """ + Abstract base class for altimeter sensors. + All altimeter implementations should inherit from this class. + """ + + # MAVLink sensor type constants + SENSOR_TYPE_LASER = 0 + SENSOR_TYPE_ULTRASOUND = 1 + SENSOR_TYPE_INFRARED = 2 + SENSOR_TYPE_RADAR = 3 + SENSOR_TYPE_UNKNOWN = 4 + + def __init__(self, sensor_id: int = 0, min_distance_mm: int = 0, max_distance_mm: int = 0): + """ + Initialize the altimeter. + + Args: + sensor_id: Unique ID for this sensor (used in MAVLink messages) + min_distance_mm: Minimum measurable distance in millimeters + max_distance_mm: Maximum measurable distance in millimeters + """ + self.sensor_id = sensor_id + self.min_distance_mm = min_distance_mm + self.max_distance_mm = max_distance_mm + self.state = 0 # Generic value, interpretation depends on sensor type + + @abstractmethod + def begin(self) -> bool: + """ + Initialize the sensor hardware. + + Returns: + True if initialization was successful, False otherwise + """ + pass + + @abstractmethod + def measure(self) -> List[Dict[str, Any]]: + """ + Perform a measurement. + + Returns: + A list of measurement data (format depends on sensor type) + """ + pass + + @abstractmethod + def get_distance_mm(self) -> Optional[float]: + """ + Get the latest distance measurement in millimeters. + + Returns: + The distance in millimeters, or None if no valid measurement is available + """ + pass + + def get_distance_m(self) -> Optional[float]: + """ + Get the latest distance measurement in meters. + + Returns: + The distance in meters, or None if no valid measurement is available + """ + distance_mm = self.get_distance_mm() + if distance_mm is not None: + return distance_mm / 1000.0 + return None + + @property + def min_distance_cm(self) -> int: + """Get minimum distance in centimeters for MAVLink messages""" + return int(self.min_distance_mm / 10) + + @property + def max_distance_cm(self) -> int: + """Get maximum distance in centimeters for MAVLink messages""" + return int(self.max_distance_mm / 10) + + @property + @abstractmethod + def mavlink_sensor_type(self) -> int: + """ + Get the MAVLink sensor type for this altimeter. + + Returns: + An integer representing the MAVLink sensor type + """ + pass + + + +from pymavlink import mavutil +import pymavlink.dialects.v20.all as dialect + + +class Messenger: + """ + Messenger class for sending messages from the autopilot to the ground station. + + Source: + https://github.com/mustafa-gokce/ardupilot-software-development/blob/main/pymavlink/send-status-text.py + """ + + def __init__(self, port): + self.__master = mavutil.mavlink_connection( + device=f"udp:127.0.0.1:{port}", + source_system=1, + source_component=1) + + def send(self, message, prefix="SHEPARD"): + """ + Sends a message to the ground station. + + :param message: The message to send. + :param prefix: The prefix to add to the message. + :return: None + """ + + message = f"{prefix}: {message}" + mav_message = dialect.MAVLink_statustext_message( + severity=dialect.MAV_SEVERITY_INFO, text=message.encode("utf-8")) + self.__master.mav.send(mav_message) + + + +import dronekit_sitl + + +class Simulator: + """ + A class to create and handle a simulated drone. + """ + + __sitl = None + __conn_str = None + __gcs_conn_str = None + + @property + def sitl(self): + return self.__sitl + + @property + def conn_str(self): + return self.__conn_str + + @property + def gcs_conn_str(self): + return self.__gcs_conn_str + + def __init__(self): + self.__sitl = dronekit_sitl.start_default() + self.__conn_str = self.__sitl.connection_string() + # TODO: Find a way to set gcs_conn_str programmatically + self.__gcs_conn_str = "127.0.0.1:5763" + + def __del__(self): + self.__sitl.stop() + + + + + + + + + + + +from PIL import Image, ImageDraw +import os +from typing import Optional, Tuple + +from src.modules.emu import Emu +from src.modules.imaging.detector import BoundingBox + + +class ArucoEmuStreamer: + def __init__(self, emu: Emu, output_dir: str = "tmp"): + self.emu = emu + self.output_dir = output_dir + self.counter = 0 + + if not os.path.exists(self.output_dir): + os.makedirs(self.output_dir) + + def on_detection(self, image: Image.Image, bounding_box: Optional[BoundingBox], position: Optional[Tuple[float, float]]): + im = image.copy() + + if bounding_box is not None: + draw = ImageDraw.Draw(im) + bb = (bounding_box.position.x, bounding_box.position.y, + bounding_box.size.x, bounding_box.size.y) + draw.rectangle(bb, outline="red") + + filename = f"{self.counter}.jpeg" + filepath = os.path.join(self.output_dir, filename) + im.save(filepath) + + self.emu.send_image(filename) + self.counter += 1 + + + +from typing import Optional +from src.modules.imaging.mavlink import MAVLinkDelegate +import pymavlink.dialects.v20.all as dialect + + +class BatteryStatusProvider: + """ + Provides the current status of the drone's battery / power usage. + """ + + def voltage(self) -> float: + """ + Get the latest voltage of the drone batter in Volts. + """ + raise NotImplementedError() + + +class DebugBatteryStatusProvider(BatteryStatusProvider): + """ + For testing / debugging + """ + + def __init__(self) -> None: + self._current_voltage = 0 + + def voltage(self) -> float: + return self._current_voltage + + def set_voltage(self, new_voltage): + self._current_voltage = new_voltage + + +class MAVLinkBatteryStatusProvider: + """ + For use in production when a MAVLink connection is available. + """ + + def __init__(self, mavlink_delegate: MAVLinkDelegate): + self.mavlink_delegate = mavlink_delegate + self._voltage: Optional[float] = None + + # Subscribe to the delegate's messages + self.mavlink_delegate.subscribe(self._process_message) + self.mavlink_delegate.send( + dialect.MAVLink_command_long_message( + target_system=1, + target_component=1, + command=dialect.MAV_CMD_SET_MESSAGE_INTERVAL, + confirmation=0, + param1=1, # param1: send SYS_STATUS message + param2=500000, # param2: send every 5e5 us + param3=0, + param4=0, + param5=0, + param6=0, + param7=1)) # param7: send messaged to requester + + def _process_message(self, message): + # This callback processes incoming MAVLink messages and updates the internal state + if message.get_type() == 'SYS_STATUS': + # message.voltage_battery is an int in mV + self._voltage = float(message.voltage_battery) / 1e3 + + def voltage(self) -> float: + if self._voltage is not None: + return self._voltage + else: + raise ValueError("No valid voltage data available") + + + +import pymavlink.dialects.v20.all as dialect +from mavlink import MAVLinkDelegate + + +class StatusLeds: + "stores the state of LED lights" + + def __init__(self, mavlink: MAVLinkDelegate, RGB=[0, 0, 0]): + self.RGB = RGB + self.mavlinkDelegate = mavlink + + def change_color(self, RGB: list): + self.RGB = RGB + #print current color of LED + if (self.RGB[0] == 255 and self.RGB[1] == 0 and self.RGB[2] == 0): + print("RED COLOR") + elif (self.RGB[0] == 0 and self.RGB[1] == 255 and self.RGB[2] == 0): + print("GREEN COLOR") + elif (self.RGB[0] == 0 and self.RGB[1] == 0 and self.RGB[2] == 255): + print("BLUE COLOR") + else: + print(f"RGB values: {self.RGB}") + self.send_message() + + def send_message(self): + message = dialect.MAVLink_debug_vect_message(name=bytes( + "LED_STATUS", 'utf-8'), + time_usec=0, + x=self.RGB[0], + y=self.RGB[1], + z=self.RGB[2]) + self.mavlinkDelegate.send(message) + + + +from typing import Optional +from dataclasses import dataclass +import math +from src.modules.imaging.mavlink import MAVLinkDelegate +import pymavlink.dialects.v20.all as dialect +import json + + +@dataclass +class LatLng: + """ + Latitude and longitude in WGS84 coordinates. + """ + + lat: float + lng: float + + def to_json(self): + return {"lat": self.lat, "lng": self.lng} + + +@dataclass +class Heading: + """ + Heading, as defined in an aeronautical sense, with respect to + north. Measured in degrees. + """ + + heading: float + + def to_json(self): + return self.heading + + +@dataclass +class Rotation: + """ + Orientation of the drone using euler angles in degrees. + """ + + pitch: float + roll: float + yaw: float + + def to_json(self): + return { + "pitch": self.pitch, + "roll": self.roll, + "yaw": self.yaw, + } + + +class LocationProvider: + """ + Provides the drone's current location and orientation as read from + telemetry or a debug-source. + """ + + def location(self) -> LatLng: + """ + Get the latest lat/lng location of the drone. + """ + raise NotImplementedError() + + def heading(self) -> Heading: + """ + Get the latest heading of the drone (with respect to north.) + """ + raise NotImplementedError() + + def altitude(self) -> float: + """ + Get the latest altitude of the drone in meters. + """ + raise NotImplementedError() + + def orientation(self) -> Rotation: + """ + Get the latest 3D orientation of the drone in it's inertial frame. + """ + raise NotImplementedError() + + def dump_to(self, path: str): + try: + dump = { + "location": self.location().to_json(), + "heading": self.heading().to_json(), + "altitude": self.altitude(), + "orientation": self.orientation().to_json(), + } + + with open(path, "w") as f: + json.dump(dump, f) + except ValueError: + pass # Raised if we cannot get any location data + + +class DebugLocationProvider(LocationProvider): + """ + Will return a series of given locations and orientations. + """ + + def __init__(self) -> None: + self._current_location = LatLng(0.0, 0.0) + self._current_heading = Heading(0.0) + self._current_altitude = 10.0 + self._current_orientation = Rotation(0.0, 0.0, 0.0) + + def location(self) -> LatLng: + return self._current_location + + def heading(self) -> Heading: + return self._current_heading + + def altitude(self) -> float: + return self._current_altitude + + def orientation(self) -> Rotation: + return self._current_orientation + + def set_location(self, new_location): + self._current_location = new_location + + def set_heading(self, new_heading): + self._current_heading = new_heading + + def set_altitude(self, new_altitude): + self._current_altitude = new_altitude + + def set_orientation(self, new_orientation): + self._current_orientation = new_orientation + + def debug_change_location(self, **kwargs): + """ + Change the location reported by this DebugLocationProvider. + + Keyword Arguments: + lat -- Latitude + lng -- Longitude + heading -- Heading direction + altitude -- Altitude + pitch -- Pitch angle + roll -- Roll angle + yaw -- Yaw angle + """ + if 'lat' in kwargs or 'lng' in kwargs: + lat = kwargs.get('lat', self._current_location.lat) + lng = kwargs.get('lng', self._current_location.lng) + + self._current_location = LatLng(lat, lng) + + if 'heading' in kwargs: + self._current_heading = Heading(kwargs['heading']) + + if 'altitude' in kwargs: + self._current_altitude = kwargs['altitude'] + + if any(k in kwargs for k in ['pitch', 'roll', 'yaw']): + pitch = kwargs.get('pitch', self._current_orientation.pitch) + roll = kwargs.get('roll', self._current_orientation.roll) + yaw = kwargs.get('yaw', self._current_orientation.yaw) + + self._current_orientation = Rotation(pitch, roll, yaw) + + +class MAVLinkLocationProvider(LocationProvider): + """ + Will provide location information based on information received as MAVLink messages. + """ + + def __init__(self, mavlink_delegate: MAVLinkDelegate): + self.mavlink_delegate = mavlink_delegate + self._location: Optional[LatLng] = None + self._heading: Optional[Heading] = None + self._altitude: Optional[float] = None + self._orientation: Optional[Rotation] = None + + # Subscribe to the delegate's messages + self.mavlink_delegate.subscribe(self._process_message) + self.mavlink_delegate.send( + dialect.MAVLink_command_long_message( + target_system=1, + target_component=1, + command=dialect.MAV_CMD_SET_MESSAGE_INTERVAL, + confirmation=0, + param1=33, # param1: send GLOBAL_POSITION_INT message + param2=500000, # param2: send every 5e5 us + param3=0, + param4=0, + param5=0, + param6=0, + param7=1)) # param7: send messaged to requester + self.mavlink_delegate.send( + dialect.MAVLink_command_long_message( + target_system=1, + target_component=1, + command=dialect.MAV_CMD_SET_MESSAGE_INTERVAL, + confirmation=0, + param1=30, # param1: send ATTITUDE message + param2=500000, # param2: send every 5e5 us + param3=0, + param4=0, + param5=0, + param6=0, + param7=1)) # param7: send messaged to requester + + def _process_message(self, message): + # This callback processes incoming MAVLink messages and updates the internal state + if message.get_type() == 'GLOBAL_POSITION_INT': + self._location = LatLng(message.lat / 1e7, message.lon / 1e7) + self._altitude = message.alt / 1000.0 # Altitude in meters + self._heading = Heading(message.hdg / 1e7) # Heading in degrees + elif message.get_type() == 'ATTITUDE': + self._orientation = Rotation(pitch=math.degrees(message.pitch), + roll=math.degrees(message.roll), + yaw=math.degrees(message.yaw)) + + def location(self) -> LatLng: + if self._location is not None: + return self._location + else: + raise ValueError("No valid location data available") + + def heading(self) -> Heading: + if self._heading is not None: + return self._heading + else: + raise ValueError("No valid heading data available") + + def altitude(self) -> float: + if self._altitude is not None: + return self._altitude + else: + raise ValueError("No valid altitude data available") + + def orientation(self) -> Rotation: + if self._orientation is not None: + return self._orientation + else: + raise ValueError("No valid orientation data available") + + + +# let the present location be (0,0) +# we pass in the length of the side of the square cam area +# we pass in the number of loops around the origin, and has a default value of 20 + + +def landing_spot(a, number_of_loops=20): + route = [] + x, y = 0, 0 + for i in range(2, number_of_loops, 2): + + for j in range(4): + if j == 0: + y = y - a + route.append([x, y]) + for k in range(i - 1): + x = x + a + route.append([x, y]) + if j == 1: + for k in range(i): + y = y + a + route.append([x, y]) + if j == 2: + for k in range(i): + x = x - a + route.append([x, y]) + if j == 3: + for k in range(i): + y = y - a + route.append([x, y]) + print(route) + + +landing_spot(1, 15) + + + +# src/modules + +The different modules used by the Shepard software. + + + +# Source + +This directory contains the main scripts that are used when running Shepard. + + + +# For more options and information see +# http://rptl.io/configtxt +# Some settings may impact device functionality. See link above for details + +# Uncomment some or all of these to enable the optional hardware interfaces +#dtparam=i2c_arm=on +#dtparam=i2s=on +#dtparam=spi=on + +# Enable audio (loads snd_bcm2835) +dtparam=audio=on + +# Additional overlays and parameters are documented +# /boot/firmware/overlays/README + +# Automatically load overlays for detected cameras +camera_auto_detect=1 + +# Automatically load overlays for detected DSI displays +display_auto_detect=1 + +# Automatically load initramfs files, if found +auto_initramfs=1 + +# Enable DRM VC4 V3D driver +dtoverlay=vc4-kms-v3d +max_framebuffers=2 + +# Don't have the firmware create an initial video= setting in cmdline.txt. +# Use the kernel's default instead. +disable_fw_kms_setup=1 + +# Run in 64-bit mode +arm_64bit=1 + +# Disable compensation for displays with overscan +disable_overscan=1 + +# Run as fast as firmware / board allows +arm_boost=1 + +[cm4] +# Enable host mode on the 2711 built-in XHCI USB controller. +# This line should be removed if the legacy DWC2 controller is required +# (e.g. for USB device mode) or if USB support is not required. +otg_mode=1 + +[all] +enable_uart=1 + + + +# Test + +This directory contains automated test scripts (unit tests, etc.). + + + +# Tests can be run with ./scripts/test.sh +# They will run all files in the test/ directory starting with 'test_'. +# Then, all functions starting with 'test_' will be run in that file. If the +# function raises an error, the test fails. Otherwise, the test passes. +# See test/test_camera.py for an example. + +from typing import Optional + +from PIL import Image + +from src.modules.imaging.analysis import ImageAnalysisDelegate +from src.modules.imaging.camera import DebugCamera +from src.modules.imaging.location import DebugLocationProvider +from src.modules.imaging.debug import ImageAnalysisDebugger +from dep.labeller.benchmarks.detector import LandingPadDetector, BoundingBox +from dep.labeller.loader.label import Vec2 + + +class DebugLandingPadDetector(LandingPadDetector): + + def __init__(self, + vector: Optional[Vec2] = None, + bb: Optional[BoundingBox] = None): + self.bounding_box = bb + + def predict(self, _image: Image.Image) -> Optional[BoundingBox]: + return self.bounding_box + + +def test_analysis_subscriber(): + camera = DebugCamera("res/test-image.jpeg") + detector = DebugLandingPadDetector() + location_provider = DebugLocationProvider() + location_provider.set_altitude(1.0) + analysis = ImageAnalysisDelegate(detector, camera, location_provider) + + global detected + detected = None + + def _callback(_image, bounding_box, pos): + global detected + if pos is not None: + detected = Vec2(pos[0], pos[1]) + else: + detected = None + + analysis.subscribe(_callback) + + detector.bounding_box = None + analysis._analyze_image() + assert detected is None + detector.bounding_box = BoundingBox(Vec2(20, 20), Vec2(50, 50)) + analysis._analyze_image() + assert (detected - + Vec2(-115.48873916832288, 5.483286467459389e-06)).norm < 0.01 + + +class MockImageAnlaysisDebugger(ImageAnalysisDebugger): + + def __init__(self): + self.image: Optional[Image.Image] = None + self.bounding_box: Optional[BoundingBox] = None + self.is_visible = False + + def show(self): + #if not self.image: + #raise RuntimeError("No image set. Cannot show without an image") + self.is_visible = True + + def hide(self): + self.is_visible = False + + def visible(self) -> bool: + return self.is_visible + + def set_image(self, image: Image.Image): + self.image = image.copy() + + def set_bounding_box(self, bb: BoundingBox): + if not self.image: + return # return no image set error + + self.bounding_box = bb + + +def test_analysis_debugger(): + camera = DebugCamera("res/test-image.jpeg") + detector = DebugLandingPadDetector() + debug = MockImageAnlaysisDebugger() + location_provider = DebugLocationProvider() + location_provider.set_altitude(1.0) + analysis = ImageAnalysisDelegate(detector, camera, location_provider, + debug) + + def run_analysis(): + detector.bounding_box = BoundingBox(Vec2(0, 0), Vec2(100, 100)) + analysis._analyze_image() + assert debug.image is not None + assert debug.bounding_box == detector.bounding_box + + detector.bounding_box = None + analysis._analyze_image() + assert debug.image is not None + + # ImageAnalysisDelegate will not hide the debugger + debug.show() + run_analysis() + assert debug.is_visible + + # ImageAnalysisDelegate will not show the debugger + debug.hide() + run_analysis() + assert not debug.is_visible + + + +import threading +import time + +from dronekit import connect + +from src.modules.autopilot import navigator +from src.modules.autopilot import lander +from src.modules.imaging.mavlink import MAVLinkDelegate +from src.modules.imaging.battery import MAVLinkBatteryStatusProvider + +CONN_STR = "udp:127.0.0.1:14551" +MESSENGER_PORT = 14552 + +drone = connect(CONN_STR, wait_ready=False) + +nav = navigator.Navigator(drone, MESSENGER_PORT) +lander = lander.Lander() + +nav.send_status_message("Shepard is online") + +mavlink = MAVLinkDelegate() +battery = MAVLinkBatteryStatusProvider(mavlink) + + +def wait_for_voltage(): + while True: + try: + voltage = battery.voltage() + print("voltage: ", voltage) + except ValueError: + pass + # print("no data yet") + time.sleep(0.5) + + +threading.Thread(daemon=True, target=mavlink.run).start() + +while True: + nav.send_status_message(nav.sufficient_battery(battery)) + time.sleep(1) + + + +from PIL import Image +import hashlib +import os + +from src.modules.imaging.camera import DebugCamera + + +def md5sum(path: str | os.PathLike) -> bytes: + with open(path, "rb") as f: + contents = f.read() + return hashlib.md5(contents).digest() + + +def test_debug_camera(tmp_path): + cam = DebugCamera("res/test-image.jpeg") + + # Simple image capture + im = cam.capture() + assert im.width == 600 + assert im.height == 400 + + # Can capture the image as an ndarray + im_np = cam.caputure_as_ndarry() + assert im_np.shape == (400, 600, 3) + assert im_np.sum() == 121089435 # Stupid simple checksum + + # Can save the image to a path + im_path = tmp_path / "copy.jpeg" + cam.caputure_to(im_path) + im_md5 = md5sum(im_path) + + # Manually save a copy of 'test-image.jpeg'. + # This is required as Image.save() may differ slightly from + # 'test-image.jpeg' due to changes from re-compression. + og_im_path = tmp_path / "og.jpeg" + og_im = Image.open("res/test-image.jpeg") + og_im.save(og_im_path) + og_im_md5 = md5sum(og_im_path) + + assert im_md5 == og_im_md5 + + # Can shrink the size + cam.set_size((100, 100)) + im = cam.capture() + assert im.width == 100 + assert im.height == 100 + + # Can increase the size + cam.set_size((1000, 1000)) + im = cam.capture() + assert im.width == 1000 + assert im.height == 1000 + + # Can round-trip even after changing size + cam.set_size((600, 400)) + im = cam.capture() + assert im.tobytes() == og_im.tobytes() + + + +import math +from src.modules.imaging.location import (DebugLocationProvider, + MAVLinkLocationProvider, LatLng, + Heading, Rotation) +from src.modules.imaging.mavlink import MAVLinkDelegateMock +from pymavlink.dialects.v20 import all as dialect + + +def test_get_location(): + loc = DebugLocationProvider() + loc.debug_change_location(lat=10.0, lng=20.0) + + assert loc.location() == LatLng( + 10.0, 20.0), "Location did not match expected value" + + +def test_get_location_partial(): + loc = DebugLocationProvider() + loc.debug_change_location(lat=10.0, lng=20.0) + loc.debug_change_location(lat=15.0) # Missing lng + + assert loc.location() == LatLng( + 15.0, 20.0), "Location did not match expected value" + + +def test_get_heading(): + loc = DebugLocationProvider() + loc.debug_change_location(heading=45.0) + + assert loc.heading() == Heading( + 45.0), "Heading did not match expected value" + + +def test_get_altitude(): + loc = DebugLocationProvider() + loc.debug_change_location(altitude=100.0) + + assert loc.altitude() == 100.0, "Altitude did not match expected value" + + +def test_get_orientation(): + loc = DebugLocationProvider() + loc.debug_change_location(pitch=10.0, roll=20.0, yaw=30.0) + + assert loc.orientation() == Rotation( + 10.0, 20.0, 30.0), "Orientation did not match expected value" + + +def test_get_orientation_partial(): + loc = DebugLocationProvider() + loc.debug_change_location(pitch=10.0, roll=20.0, yaw=30.0) + loc.debug_change_location(pitch=12.0) + + assert loc.orientation() == Rotation( + 12.0, 20.0, 30.0), "Orientation did not match expected value" + + +def test_position_changes(): + loc = DebugLocationProvider() + + # Initial change + loc.debug_change_location(lat=10.0, lng=20.0) + assert loc.location() == LatLng( + 10.0, 20.0), "Initial location did not match expected value" + + # Change to new location + loc.debug_change_location(lat=40.0, lng=50.0) + assert loc.location() == LatLng( + 40.0, 50.0), "New location did not match expected value" + + +def test_get_MAVLink_location(): + mavlink = MAVLinkDelegateMock() + loc_mavlink = MAVLinkLocationProvider(mavlink) + + # Create a mock GLOBAL_POSITION_INT message for the initial location + initial_message = dialect.MAVLink_global_position_int_message( + time_boot_ms=0, # Assuming a timestamp of 0 for the test + lat=int(10.0 * 1e7), # Latitude in degE7 + lon=int(20.0 * 1e7), # Longitude in degE7 + alt=1000, # Altitude in mm above MSL + relative_alt=500, # Altitude in mm above the ground + vx=0, + vy=0, + vz=0, # Ground X, Y, Z Speed + hdg=0 # Heading + ) + mavlink.send(initial_message) + assert loc_mavlink.location() == LatLng( + 10.0, 20.0), "Initial location did not match expected value" + + # Create a mock GLOBAL_POSITION_INT message for the new location + new_message = dialect.MAVLink_global_position_int_message( + time_boot_ms=0, # Assuming a timestamp of 0 for the test + lat=int(40.0 * 1e7), # Latitude in degE7 + lon=int(50.0 * 1e7), # Longitude in degE7 + alt=2000, # Altitude in mm above MSL + relative_alt=1000, # Altitude in mm above the ground + vx=0, + vy=0, + vz=0, # Ground X, Y, Z Speed + hdg=0 # Heading + ) + mavlink.send(new_message) + assert loc_mavlink.location() == LatLng( + 40.0, 50.0), "New location did not match expected value" + + +def test_get_MAVLink_heading(): + mavlink = MAVLinkDelegateMock() + loc_mavlink = MAVLinkLocationProvider(mavlink) + + # Create a mock VFR_HUD message for heading + heading_message = dialect.MAVLink_global_position_int_message( + time_boot_ms=0, + lat=0, + lon=0, + alt=2000, # Altitude in mm above MSL + relative_alt= + 0, # Relative altitude in mm above the ground (not used in this test) + vx=0, + vy=0, + vz=0, # Ground X, Y, Z Speed (not used in this test) + hdg=150 * 1e7 # Heading in degE7 + ) + mavlink.send(heading_message) + + # Check if the MAVLinkLocationProvider reports the heading correctly + assert loc_mavlink.heading() == Heading( + 150), "Heading did not match expected value" + + +def test_get_MAVLink_altitude(): + mavlink = MAVLinkDelegateMock() + loc_mavlink = MAVLinkLocationProvider(mavlink) + + # Create a mock GLOBAL_POSITION_INT message for altitude + altitude_message = dialect.MAVLink_global_position_int_message( + time_boot_ms=0, # Timestamp for the test + lat=0, # Dummy latitude + lon=0, # Dummy longitude + alt=15000, # Altitude in mm above MSL + relative_alt=0, # Relative altitude in mm above the ground + vx=0, + vy=0, + vz=0, # Ground X, Y, Z Speed + hdg=0 # Heading + ) + mavlink.send(altitude_message) + + # Check if the MAVLinkLocationProvider reports the altitude correctly + assert loc_mavlink.altitude( + ) == 15.0, "Altitude did not match expected value in meters" + + +def test_get_MAVLink_orientation(): + mavlink = MAVLinkDelegateMock() + loc_mavlink = MAVLinkLocationProvider(mavlink) + + # Create a mock ATTITUDE message for orientation + orientation_message = dialect.MAVLink_attitude_message( + time_boot_ms=0, # Timestamp for the test + pitch=0.1, # Pitch in radians + roll=0.2, # Roll in radians + yaw=0.3, # Yaw in radians + rollspeed=0, # Roll speed (not used in this test) + pitchspeed=0, # Pitch speed (not used in this test) + yawspeed=0 # Yaw speed (not used in this test) + ) + mavlink.send(orientation_message) + + # Check if the MAVLinkLocationProvider reports the orientation correctly + expected_orientation = Rotation(math.degrees(0.1), math.degrees(0.2), + math.degrees(0.3)) + actual_orientation = loc_mavlink.orientation() + assert actual_orientation == expected_orientation, "Orientation did not match expected value" + + +def test_get_MAVLink_position_changes(): + mavlink = MAVLinkDelegateMock() + loc_mavlink = MAVLinkLocationProvider(mavlink) + + # Send a mock GLOBAL_POSITION_INT message for initial position + initial_position_message = dialect.MAVLink_global_position_int_message( + time_boot_ms=0, + lat=int(10.0 * 1e7), + lon=int(20.0 * 1e7), + alt=1000, + relative_alt=500, + vx=0, + vy=0, + vz=0, + hdg=0.0) + mavlink.send(initial_position_message) + + # Verify initial position + assert loc_mavlink.location() == LatLng( + 10.0, 20.0), "Initial location did not match expected value" + + # Send another mock GLOBAL_POSITION_INT message for new position + new_position_message = dialect.MAVLink_global_position_int_message( + time_boot_ms=1000, # Simulate a time after the initial message + lat=int(40.0 * 1e7), + lon=int(50.0 * 1e7), + alt=2000, + relative_alt=1000, + vx=0, + vy=0, + vz=0, + hdg=45.0 * 1e7) + mavlink.send(new_position_message) + + # Verify new position + assert loc_mavlink.location() == LatLng( + 40.0, 50.0), "New location did not match expected value" + assert loc_mavlink.heading() == Heading( + 45.0), "New heading did not match expected value" + assert loc_mavlink.altitude( + ) == 2.0, "New altitude did not match expected value in meters" + + +def test_get_MAVLink_position_not_init(): + mavlink = MAVLinkDelegateMock() + loc_mavlink = MAVLinkLocationProvider(mavlink) + + def assert_raises(f): + try: + f() + except ValueError: + pass + else: + assert False, "Did not raise when accessing an uninitialized value" + + methods = [ + loc_mavlink.altitude, + loc_mavlink.location, + loc_mavlink.heading, + loc_mavlink.orientation, + ] + for method in methods: + assert_raises(method) + + + +# Tools + +This directory contains convenience scripts, such as setup scripts, conversion tools, etc. + + + +from src.modules.imaging.detector import IrDetector +from src.modules.imaging.analysis import ImageAnalysisDelegate +from src.modules.imaging.camera import RPiCamera +from src.modules.imaging.location import DebugLocationProvider + + +camera = RPiCamera() +detector = IrDetector() +location = DebugLocationProvider() + + +def test(img, _): + print("Image taken") + + + +analysis = ImageAnalysisDelegate(detector, camera, location) +analysis.subscribe(test) + +analysis.start() + + + +""" +benchmark the speed of different trained yolo models +""" + +from ultralytics import YOLO +import os +import time +import math +import json + + +class ModelStats: + def __init__(self): + self.n = 0 + self.mean = 0.0 + self.M2 = 0.0 + def update(self, t): + self.n += 1 + delta = t-self.mean + self.mean += delta / self.n + delta2 = t - self.mean + self.M2 += delta * delta2 + def get_mean(self): + return self.mean + + def get_stddev(self): + return math.sqrt(self.M2 / self.n) if self.n > 1 else 0.0 + + +models = os.listdir("models") + +images = os.listdir("images") +images = [ os.path.join("images", file_name) for file_name in images ] + +stats = {} + +for model_name in models: + print(model_name) + model = YOLO(os.path.join("models", model_name)) + + model_stats = ModelStats() + + for img_name in images: + start = time.time() + results = model(img_name) + end = time.time() + model_stats.update(end - start) + print(f"\tmean: {model_stats.get_mean()}") + stats[model_name] = {"mean": model_stats.get_mean(), "stddev": model_stats.get_stddev()} + + +with open("results.json", "w") as f: + json.dump(stats, f) + + + +import time + +from dronekit import connect + +from src.modules.autopilot import navigator +from src.modules.autopilot.altimeter_xm125 import XM125 +from src.modules.autopilot.altimeter_mavlink import MavlinkAltimeterProvider + +# Connection settings +CONN_STR = "tcp:127.0.0.1:14550" +MESSENGER_PORT = 14550 +MAVLINK_ALTITUDE_CONN_STR = "tcp:127.0.0.1:14550" + +AltimeterData = [] +PixHawkData = [] +Delta = [] + +# Test settings +STATUS_INTERVAL = 2.0 # seconds +TEST_DURATION = 600 # 10 minutes + +# Connect to the drone +drone = connect(CONN_STR, wait_ready=False) + +# Initialize navigator +nav = navigator.Navigator(drone, MESSENGER_PORT) +nav.send_status_message("Altimeter test initializing") + +# Initialize the XM125 radar altimeter +radar_sensor = XM125( + sensor_id=0, + min_distance=250, + max_distance=10000, + average_window=5 +) + + +if not radar_sensor.begin(): + nav.send_status_message("ERROR: Failed to initialize radar sensor") + drone.close() + exit(1) + +# Create and start the MAVLink altimeter provider +mavlink_altimeter = MavlinkAltimeterProvider(radar_sensor, MAVLINK_ALTITUDE_CONN_STR) +mavlink_altimeter.start() + +nav.send_status_message("XM125 Altimeter test starting") + +try: + nav.send_status_message("Forwarding altitude data to Pixhawk") + + # Main loop - forward altitude data and print status + last_status_time = time.time() + end_time = time.time() + TEST_DURATION + + while time.time() < end_time: + # Check if it's time to send a status message + current_time = time.time() + if current_time - last_status_time >= STATUS_INTERVAL: + + # Get the latest altimeter reading and log + result = radar_sensor.measure() + if result: + average = result[0]['averaged'] + + if average: + AltimeterData.append(average[0]) + + # Get the latest altitude reading and log + altitude_m = mavlink_altimeter.get_latest_altitude_meters() + if altitude_m is not None: + status_msg = f"Radar Altitude: {altitude_m:.2f} m" + nav.send_status_message(status_msg) + PixHawkData.append(altitude_m) + Delta.append(float(altitude_m) - float(average[0])) + + else: + nav.send_status_message("No valid altitude reading") + + last_status_time = current_time + + # Sleep to avoid consuming too much CPU + time.sleep(0.5) + +except KeyboardInterrupt: + nav.send_status_message("Test interrupted by user") +except Exception as e: + nav.send_status_message(f"ERROR: {str(e)}") +finally: + # Clean up + nav.send_status_message("Stopping altitude provider") + mavlink_altimeter.stop() + + nav.send_status_message("Altimeter test completed") + drone.close() + + + +import time + +from dronekit import connect + +from src.modules.autopilot import navigator +from src.modules.autopilot.altimeter_xm125 import XM125 +# from src.modules.autopilot.altimeter_mavlink import MavlinkAltimeterProvider + +import json + +# Connection settings +CONN_STR = "tcp:127.0.0.1:14550" +MESSENGER_PORT = 14550 +MAVLINK_ALTITUDE_CONN_STR = "tcp:127.0.0.1:14550" + +AltimeterData = [] +PixHawkData = [] +Delta = [] + +# Test settings +STATUS_INTERVAL = 2.0 # seconds +TEST_DURATION = 600 # 10 minutes + +# Connect to the drone +drone = connect(CONN_STR, wait_ready=False) + +# Initialize navigator +nav = navigator.Navigator(drone, MESSENGER_PORT) +nav.send_status_message("SHEPARD: Altimeter test initializing") + +# Initialize the XM125 radar altimeter +radar_sensor = XM125( + sensor_id=0, + min_distance=250, + max_distance=10000, + average_window=5 +) + + +if not radar_sensor.begin(): + nav.send_status_message("ERROR: Failed to initialize radar sensor") + drone.close() + exit(1) + + +nav.send_status_message("SHEPARD: XM125 Altimeter test starting") + +try: + nav.send_status_message("Forwarding altitude data to Pixhawk") + + # Main loop - forward altitude data and print status + last_status_time = time.time() + end_time = time.time() + TEST_DURATION + + while time.time() < end_time: + # Check if it's time to send a status message + current_time = time.time() + if current_time - last_status_time >= STATUS_INTERVAL: + + # Get the latest altimeter reading and log + result = radar_sensor.measure() + if result: + average = result[0]['averaged'] + + if average: + AltimeterData.append(average[0]) + + # Get the latest altitude reading and log + altitude_m = float(drone.location.global_relative_frame.alt) + if altitude_m is not None: + status_msg = f"Radar Altitude: {altitude_m:.2f} m" + nav.send_status_message(status_msg) + PixHawkData.append(altitude_m) + Delta.append(float(altitude_m) - float(average[0])) + + else: + nav.send_status_message("No valid altitude reading") + + last_status_time = current_time + + # Sleep to avoid consuming too much CPU + time.sleep(0.5) + +except KeyboardInterrupt: + nav.send_status_message("Test interrupted by user") +except Exception as e: + nav.send_status_message(f"ERROR: {str(e)}") +finally: + # Clean up + nav.send_status_message("Stopping altitude provider") + + # Log altimeter data to json file + with open("./logs/log1.json", "w", encoding="utf-8") as file: + data = { + "AltimeterData": AltimeterData, + "PixHawkData": PixHawkData, + "Delta": Delta + } + data = json.dumps(data) + + file.write(data) + + nav.send_status_message("SHEPARD: Altimeter test completed") + drone.close() + + + +import math +import os +# import threading +import time + +from dronekit import connect, VehicleMode + +from src.modules.autopilot import navigator +from src.modules.autopilot.altimeter_xm125 import XM125 +from src.modules.autopilot.altimeter_mavlink import MavlinkAltimeterProvider + +# from src.modules.imaging.camera import RPiCamera +from src.modules.imaging.detector import Detector + +# Connection settings +CONN_STR = "tcp:127.0.0.1:14550" +MESSENGER_PORT = 14550 +MAVLINK_ALTITUDE_CONN_STR = "tcp:127.0.0.1:14550" + +# Connect to the drone +drone = connect(CONN_STR, wait_ready=False) + +# Imaging setup +detector = Detector() + +# Initialize navigator +nav = navigator.Navigator(drone, MESSENGER_PORT) +nav.send_status_message("Altimeter test initializing") + +# Initialize the XM125 radar altimeter +radar_sensor = XM125( + sensor_id=0, + min_distance=250, + max_distance=10000, + average_window=5 +) + +if not radar_sensor.begin(): + nav.send_status_message("ERROR: Failed to initialize radar sensor") + drone.close() + exit(1) + +# Create and start the MAVLink altimeter provider +mavlink_altimeter = MavlinkAltimeterProvider(radar_sensor, MAVLINK_ALTITUDE_CONN_STR) +mavlink_altimeter.start() + +nav.send_status_message("SHEPARD is online") + +try: + while not (drone.armed and drone.mode == VehicleMode("GUIDED")): + pass + + nav.send_status_message("Executing mission") + time.sleep(2) + + nav.takeoff(10) + time.sleep(2) + + nav.send_status_message("Starting balloon search") + + current_pic = 0 + + dirs = os.listdir("tmp/log") + ft_num = len(dirs) + + while True: + step_size = 1 # meters + last_pic = current_pic + distance, direction, current_pic = detector.process_image_directory(directory_path=f"tmp/log/{ft_num}") + + if current_pic == last_pic: + continue + + if direction is not None: + nav.send_status_message(f"Balloon detected: Move {direction}, Distance: {distance:.2f}") + + if direction == "center": + # Move in that direction, need to calculate the N and E offsets based on heading + current_heading = drone.heading + metres_north_relative = step_size * math.sin(math.radians(current_heading)) + metres_east_relative = step_size * math.cos(math.radians(current_heading)) + nav.set_position_relative(metres_north_relative, metres_east_relative) + elif direction == "right": + nav.set_heading_relative(10) + elif direction == "left": + nav.set_heading_relative(-10) + + else: + nav.send_status_message("No balloons detected") + nav.set_heading_relative(10) + +except KeyboardInterrupt: + nav.send_status_message("Script interrupted by user") +except Exception as e: + nav.send_status_message(f"ERROR: {str(e)}") +finally: + nav.send_status_message("Ending mission") + + # Ensure drone RTLs + nav.return_to_launch() + + # Clean up + nav.send_status_message("Stopping altitude provider") + mavlink_altimeter.stop() + + nav.send_status_message("Altimeter test completed") + drone.close() + + + +{ + "type": "FeatureCollection", + "features": [ + { + "type": "Feature", + "properties": {}, + "geometry": { + "coordinates": [ + [ + [ + -113.54797355858594, + 53.49538269557871 + ], + [ + -113.54769792522994, + 53.49557167851731 + ], + [ + -113.5479829020893, + 53.495734721944046 + ], + [ + -113.5484064742526, + 53.495672654349875 + ], + [ + -113.5486322755893, + 53.495509610684024 + ], + [ + -113.54837844374161, + 53.495381769189436 + ], + [ + -113.54797355858594, + 53.49538269557871 + ] + ] + ], + "type": "Polygon" + } + } + { + ] +} + + + +import math +import time +from datetime import datetime + +import dronekit +from pymavlink import mavutil + +from src.modules.autopilot.messenger import Messenger + + +class Navigator: + """ + A class to handle navigation. + """ + + vehicle: dronekit.Vehicle | None = None + POSITION_TOLERANCE = 1 + + def __init__(self, vehicle, messenger_port): + self.vehicle = vehicle + self.mavlink_messenger = Messenger(messenger_port) + + def send_status_message(self, message): + self.__message(message) + + def __message(self, msg): + """ + Prints a message to the console. + :param msg: The message to print. + :return: None + """ + + print(f"SHEPARD_NAV: {msg}") + self.mavlink_messenger.send(msg) + + def takeoff(self, target_alt): + """ + Takes off to a given altitude. + + :param target_alt: The target altitude in meters. + :return: True if successful, False otherwise. + """ + + self.__message(f"Taking off to {target_alt} m") + + self.vehicle.simple_takeoff(target_alt) + + while self.vehicle.location.global_relative_frame.alt < target_alt - 1: + time.sleep(1) + + return True + + def set_position(self, lat, lon): + """ + Moves the vehicle to a given position. + + :param lat: The latitude of the target position. + :param lon: The longitude of the target position. + :return: None + """ + + self.__message(f"Moving to {lat}, {lon}") + + target_location = dronekit.LocationGlobalRelative( + lat, lon, self.vehicle.location.global_relative_frame.alt) + self.vehicle.simple_goto(target_location) + + while self.vehicle.mode.name == "GUIDED": + remaining_distance = self.__get_distance_metres( + self.vehicle.location.global_relative_frame, target_location) + self.__message(f"Distance to target: {remaining_distance} m") + if remaining_distance <= self.POSITION_TOLERANCE: + self.__message("Reached target") + break + time.sleep(2) + + def set_position_relative(self, d_north, d_east): + """ + Moves the vehicle to a given position relative to its current position. + + :param d_north: The distance to move north in meters. + :param d_east: The distance to move east in meters. + :return: None + """ + + self.__message(f"Moving {d_north} m north and {d_east} m east") + + current_location = self.vehicle.location.global_relative_frame + target_location = self.__get_location_metres(current_location, d_north, + d_east) + + self.vehicle.simple_goto(target_location) + + while self.vehicle.mode.name == "GUIDED": + remaining_distance = self.__get_distance_metres( + self.vehicle.location.global_relative_frame, target_location) + self.__message(f"Distance to target: {remaining_distance} m") + if remaining_distance <= self.POSITION_TOLERANCE: + self.__message("Reached target") + break + time.sleep(2) + + def get_local_position_ned(self): + # Gets the current location of the drone in the local NED frame + location = self.vehicle.location.local_frame + return (location.north, location.east, location.down) + + def get_global_position(self): + location = self.vehicle.location.global_frame + return (location.lat, location.lon, location.alt) + + def set_heading(self, heading): + """ + Sets the heading of the vehicle. + + :param heading: The heading in degrees. + :return: None + """ + + self.__message(f"Changing heading to {heading} degrees") + + # self.vehicle.commands.condition_yaw(heading) + self.__condition_yaw(heading) + + def set_heading_relative(self, heading): + """ + Sets the heading of the vehicle relative to its current heading. + + :param heading: The heading in degrees. + :return: None + """ + + self.__message( + f"Changing heading to {heading} degrees relative to current heading" + ) + + # self.vehicle.commands.condition_yaw(heading, relative=True) + self.__condition_yaw(heading, relative=True) + + def set_altitude(self, altitude): + """ + Sets the altitude of the vehicle. + + :param altitude: The altitude in meters. + :return: None + """ + + self.__message(f"Setting altitude to {altitude} m") + + target_altitude = dronekit.LocationGlobalRelative( + self.vehicle.location.global_relative_frame.lat, + self.vehicle.location.global_relative_frame.lon, + altitude, + ) + self.vehicle.simple_goto(target_altitude) + + while self.vehicle.mode.name == "GUIDED": + remaining_distance = abs( + self.vehicle.location.global_relative_frame.alt - + target_altitude.alt) + self.__message(f"Distance to target: {remaining_distance} m") + if remaining_distance <= self.POSITION_TOLERANCE: + self.__message("Reached target") + break + time.sleep(2) + + def set_altitude_relative(self, altitude): + """ + Sets the altitude of the vehicle relative to its current altitude. + + :param altitude: The altitude relative to current altitude in meters, positive value to ascend and negative to descend. + :return: None + """ + + self.__message( + f"Setting altitude to {altitude} m relative to current altitude") + + target_altitude = dronekit.LocationGlobalRelative( + self.vehicle.location.global_relative_frame.lat, + self.vehicle.location.global_relative_frame.lon, + self.vehicle.location.global_relative_frame.alt + altitude, + ) + self.vehicle.simple_goto(target_altitude) + + while self.vehicle.mode.name == "GUIDED": + remaining_distance = self.__get_distance_metres( + self.vehicle.location.global_relative_frame, target_altitude) + self.__message(f"Distance to target: {remaining_distance} m") + if remaining_distance <= self.POSITION_TOLERANCE: + self.__message("Reached target") + break + time.sleep(2) + + def set_altitude_position(self, + lat, + lon, + alt, + battery=None, + voltage_hard_cutoff=22.4, + hard_cutoff_enable=False): + """ + Sets the altitude and the position in absolute terms + + :param lat: The latitude of the target position. + :param lon: The longitude of the target position. + :param alt: The target altitude in metres + :param battery: MAVLinkBatteryStatusProvider object + :return: None + """ + self.__message(f"Moving to lat: {lat} lon: {lon} alt: {alt}") + + target_altitude_position = dronekit.LocationGlobalRelative( + lat, lon, alt) + + if hard_cutoff_enable: + if self.sufficient_battery(battery, voltage_hard_cutoff): + self.__message("--------Hard Cutoff reached----------") + self.__message("--------Returning to Launch----------") + self.return_to_launch() + return + + self.vehicle.simple_goto(target_altitude_position) + + while self.vehicle.mode.name == "GUIDED": + remaining_distance = self.__get_distance_metres( + self.vehicle.location.global_relative_frame, + target_altitude_position) + self.__message(f"Distance to target: {remaining_distance} m") + if remaining_distance <= self.POSITION_TOLERANCE: + self.__message("Reached target") + break + time.sleep(2) + + def set_altitude_position_relative(self, d_north, d_east, alt): + """ + Sets the altitude and the position relative to current position + + :param d_north: The distance to be moved north. + :param d_east: The distance to be moved east. + :param alt: Altitude to be moved in metres. + :return: None + """ + + self.__message( + f"Moving {d_north} m north and {d_east} m east and {alt} m in altitude" + ) + + current_location = self.vehicle.location.global_relative_frame + target_location = self.__get_location_metres(current_location, d_north, + d_east) + target_location.alt += alt + + self.vehicle.simple_goto(target_location) + + while self.vehicle.mode.name == "GUIDED": + remaining_distance = self.__get_distance_metres( + self.vehicle.location.global_relative_frame, target_location) + self.__message(f"Distance to target: {remaining_distance} m") + if remaining_distance <= self.POSITION_TOLERANCE: + self.__message("Reached target") + break + time.sleep(2) + + def land(self): + """ + Lands the vehicle. + + :return: None + """ + + self.__message("Landing") + self.vehicle.mode = dronekit.VehicleMode("LAND") + + def precision_landing(self, lat, long, alt): + """ + Experimenting with precision landing and utilizing the PyMavlink precision landing function. + Creates and sends a precision landing message to the mavlink + Requires landing target data + """ + + abort_land_alt = 5 + + msg = self.vehicle.message_factory.mav_cmd_nav_land( + abort_land_alt, # minimum altitude if landing is aborted + 2, # Enable precision landing mode + 0, # blank + 0, # Desired Yaw Angle + lat, # Latitude of target + long, # Longitude of the target + alt # Altitude of the ground in the current reference frame + ) + self.vehicle.send_mavlink(msg) + + def return_to_launch(self): + """ + Returns the vehicle to its launch position. + + :return: None + """ + + self.__message("Returning to launch") + self.vehicle.mode = dronekit.VehicleMode("RTL") + + def set_speed(self, speed): + """ + Set the vehicle speed. + + :param speed: The target speed in m/s. + :return: None + """ + + self.__message(f"Setting speed to {speed} m/s") + msg = self.vehicle.message_factory.command_long_encode( + 0, + 0, # target system, target component + mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # command + 0, # confirmation + 0, # speed type, ignored in Copter + speed, # speed + 0, + 0, + 0, + 0, + 0, # ignore other parameters + ) + + self.vehicle.send_mavlink(msg) + self.vehicle.flush() + + def __get_location_metres(self, original_location, d_north, d_east): + """ + Returns a `LocationGlobalRelative` object containing the latitude/longitude `d_north` and `d_east` metres from the + specified `original_location`. The returned `LocationGlobalRelative` has the same `alt` value as `original_location`. + + :param original_location: The reference `LocationGlobal`. + :param d_north: The distance to the north in meters. + :param d_east: The distance to the east in meters. + :return: A `LocationGlobalRelative` object. + """ + + earth_radius = 6378137.0 # Radius of "spherical" earth + # Coordinate offsets in radians + d_lat = d_north / earth_radius + d_lon = d_east / (earth_radius * + math.cos(math.pi * original_location.lat / 180)) + + # New position in decimal degrees + new_lat = original_location.lat + (d_lat * 180 / math.pi) + new_lon = original_location.lon + (d_lon * 180 / math.pi) + if type(original_location) is dronekit.LocationGlobal: + target_location = dronekit.LocationGlobal(new_lat, new_lon, + original_location.alt) + elif type(original_location) is dronekit.LocationGlobalRelative: + target_location = dronekit.LocationGlobalRelative( + new_lat, new_lon, original_location.alt) + else: + raise Exception("Invalid Location object passed") + + return target_location + + def __get_distance_metres(self, location_1, location_2): + """ + Returns the ground distance in metres between two `LocationGlobal` or `LocationGlobalRelative` objects. + + This method is an approximation, and will not be accurate over large distances and close to the earth's poles. + + :param location_1: The first `LocationGlobal` or `LocationGlobalRelative` object. + :param location_2: The second `LocationGlobal` or `LocationGlobalRelative` object. + :return: The ground distance in metres. + """ + + d_lat = location_2.lat - location_1.lat + d_lon = location_2.lon - location_1.lon + + return math.sqrt((d_lat * d_lat) + (d_lon * d_lon)) * 1.113195e5 + + def __condition_yaw(self, heading, relative=False): + if relative: + is_relative = 1 # yaw relative to direction of travel + else: + is_relative = 0 # yaw is an absolute angle + # create the CONDITION_YAW command using command_long_encode() + msg = self.vehicle.message_factory.command_long_encode( + 0, + 0, # target system, target component + mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command + 0, # confirmation + heading, # param 1, yaw in degrees + 0, # param 2, yaw speed deg/s + 1, # param 3, direction -1 ccw, 1 cw + is_relative, # param 4, relative offset 1, absolute angle 0 + 0, + 0, + 0, + ) # param 5 ~ 7 not used + # send command to vehicle + self.vehicle.send_mavlink(msg) + + @staticmethod + def time_left(string_land_time): + """ + Calculates the time between now and the time passed as a string arguement. + + :param string_land_time: the target landing time as a string in the format "HH:MM:SS" + + :return: time left in seconds + """ + time_now = datetime.now().time() + + land_time = datetime.strptime(string_land_time, "%H:%M:%S") + + seconds_now = (time_now.hour * 360) + (time_now.minute * + 60) + (time_now.second) + + seconds_land = (land_time.hour * 360) + (land_time.minute * + 60) + (land_time.second) + + return seconds_land - seconds_now + + def optimum_speed(self, time_left, waypoints): + """ + Finds the optimum horizontal speed required to go from current position to all waypoints and land within the given time + + :param time_left: The time left to land in [seconds], initially the land time minus the takeoff time. + :param waypoints: List of all remaining way points to go to. + :return: required ground speed in [m/s]. + """ + + self.__message("Calculating optimum horizontal speed") + + total_distance = self.__get_distance_metres( + self.vehicle.location.global_relative_frame, waypoints[0]) + for i in range(1, len(waypoints)): + total_distance += self.__get_distance_metres( + waypoints[i - 1], waypoints[i]) + + speed_required = total_distance / time_left + self.__message( + f"Speed required to travel {total_distance} m in {time_left} s is {speed_required} m/s" + ) + + return speed_required + + def sufficient_battery(self, battery, voltage_required=10.0): + """ + Returns True or False based on whether or not the drone has enough battery to do another lap. + Each lap is when drone starts at Alpha, goes to Bravo and comes back to Alpha. + So we check if drone has little more battery than what is required for twice the distance between Alpha and Bravo. + + :param voltage_required: value of voltage required to travel the lap distance + (Distance between Alpha and Bravo in Alma is 3km, so hard code this to voltage required for 6km of flight) + :param battery: instance of the MAVLinkBatteryStatusProvider + + :return: a Boolean value, True indicating the drone has sufficient battery for another lap. + """ + + self.__message("Attempting to read voltage reading") + while True: + try: + voltage = battery.voltage() + break + except ValueError: + pass + time.sleep(0.1) + + self.__message(f"Current battery voltage is {voltage} V") + + if voltage < voltage_required: + return False + + return True + + def generate_typemask(self, keeps): + # Generates typemask based on which values to be included + mask = 0 + for bit in keeps: + mask |= (0 << bit) + return mask + + def set_position_target_local_ned( + self, + time_boot_ms=0, + coordinate_frame=mavutil.mavlink.MAV_FRAME_LOCAL_NED, + type_mask=0x07FF, + x=0, y=0, z=0, + vx=0, vy=0, vz=0, + afx=0, afy=0, afz=0, + yaw=0, yaw_rate=0): + msg = self.vehicle.message_factory.set_position_target_local_ned_encode( + time_boot_ms, # Time since system boot + 0, # Target System ID + 0, # Target Component ID + coordinate_frame, # Coordinate Frame + type_mask, # Typemask of POSITION_TARGET_TYPEMASK + x, + y, + z, + vx, + vy, + vz, + afx, + afy, + afz, + yaw, + yaw_rate + ) + + self.vehicle.send_mavlink(msg) + + ''' + def cancel_command(self, command_id=mavutil.mavlink.SET_POSITION_TARGET_LOCAL_NED): + msg = self.vehicle.message_factory.command_long_encode( + 0, + 0, + mavutil.mavlink.COMMAND_CANCEL, + 0, + 0, + command_id + ) + + self.vehicle.send_mavlink(msg) + ''' + + + +from .emu import * + + + +from mavlink import MAVLinkDelegate +import pymavlink.dialects.v20.all as dialect +from .detector import BoundingBox + + +class AnalysisView: + """ + Sends debugging messages to ground station through mavlink protocol. + """ + + def __init__(self, mavlinkDelegate: MAVLinkDelegate) -> None: + self.mavlink = mavlinkDelegate + + def send_BoundingBox(self, bbox: BoundingBox) -> None: + position = bbox.position + size = bbox.size + values = [position.x, position.y, size.x, size.y] + for i in range(4, 58): + values[i] = 0.0 + + dbg_box = dialect.MAVLink_debug_float_array_message( + name=bytes("dbg_box", 'utf-8'), + time_usec=0, # timestamp + array_id=0, + data=values) + self.mavlink.send(dbg_box) + + + +from typing import Optional, Tuple +from PIL import Image, ImageDraw, ImageTk +import tkinter as tk +from .detector import BoundingBox + + +class ImageAnalysisDebugger: + """ + Helper class to display a debug window containing image analysis results. + + Note, all methods are non-blocking. This means, ie. calling show() will not + block the program until the window is closed. This debugger will run in the + background and should not interfere in any way. + """ + + def __init__(self): + self.image: Optional[Image.Image] = None + self.root = tk.Tk() + self.is_visible = False + + def show(self): + """ + Start displaying the debugger window. + """ + if not self.image: + raise RuntimeError("No image set. Cannot show without an image") + + self.root.deiconify() + self.is_visible = True + img = ImageTk.PhotoImage(self.image) + self.root.geometry('%dx%d' % (self.image.size[0], self.image.size[1])) + label_image = tk.Label(self.root, image=img) + label_image.place(x=0, + y=0, + width=self.image.size[0], + height=self.image.size[1]) + self.root.update() + + def hide(self): + """ + Stop displaying the debugger window. + """ + self.root.withdraw() + self.is_visible = False + + def visible(self) -> bool: + """ + Returns True if the debug window is visible. + False if it is no longer visible because: + - show() has not been called + - hide() was called + - the user closed the window + """ + return self.is_visible + + def set_image(self, image: Image.Image): + """ + Update the image currently being analysed. Will remove any old bounding boxes. + """ + self.image = image.copy() + + def set_bounding_box(self, bb: BoundingBox): + """ + Update the bounding box currently displaying the image being debugged. + """ + if not self.image: + return # return no image set error + + image = self.image + top_left_corner: Tuple[float, float] = (bb.position.x, bb.position.y) + bottom_right_corner: Tuple[float, float] = (bb.position.x + bb.size.x, + bb.position.y + bb.size.y) + + draw = ImageDraw.Draw(image) + draw.rectangle((top_left_corner, bottom_right_corner)) + + + +from typing import List, Callable + +from pymavlink import mavutil +import pymavlink.dialects.v20.all as dialect +import time + + +class MAVLinkDelegate: + """ + MAVLink connection delegate which forwards messages to subscribers. + """ + + def __init__(self, conn_str: str = "tcp:127.0.0.1:14550"): + self._conn = mavutil.mavlink_connection(device=conn_str, + source_system=255, + source_component=42) + + self._listeners: List[Callable] = [] + + def subscribe(self, listener: Callable): + """ + Subscribe to messages received from the mavlink connection. + """ + self._listeners.append(listener) + + def send(self, mav_message: dialect.MAVLink_message): + """ + Sends a mavlink message. + """ + self._conn.mav.send(mav_message) + + def run(self): + """ + Start the mavlink delegate. Will never return. + """ + last_heartbeat = time.time() + while True: + msg = self._conn.recv_match(blocking=False) + if msg: + for listener in self._listeners: + listener(msg) + + continue # Check for more messages immediately + + now = time.time() + if now - last_heartbeat > 1: + last_heartbeat = now + self.send(dialect.MAVLink_heartbeat_message(0, 0, 0, 0, 0, 0)) + + time.sleep(0.0001) # 100 us + + +class MAVLinkDelegateMock(MAVLinkDelegate): + """ + Mock MAVLink connection delegate which forwards messages to subscribers. + """ + + def __init__(self): + self._listeners = [] + + def send(self, mav_message: dialect.MAVLink_message): + """ + Sends a mavlink message. + """ + for listener in self._listeners: + listener(mav_message) + + def run(self): + """ + Not implemented + """ + raise AssertionError("Not implemented") + + +class MessagePrinter: + + def __init__(self, mavlink_delegate: MAVLinkDelegate): + self.mavlink_delegate = mavlink_delegate + self.mavlink_delegate.subscribe(self._process_message) + + def _process_message(self, message: dialect.MAVLink_message): + print(message) + + +if __name__ == "__main__": + mavlink_delegate = MAVLinkDelegate() + message_printer = MessagePrinter(mavlink_delegate) + print("Starting mavlink_delegate.run()") + mavlink_delegate.run() + + + +[submodule "src/modules/mavctl"] + path = src/modules/mavctl + url = git@github.com:uaarg/mavctl-python.git + + + +#!/usr/bin/env bash + +source venv/bin/activate +logfile="log/$(date +'%x-%X').log" +imgfile="log/$(date +'%x-%X').imglog" +mkdir -p "$(dirname "$logfile")" +mkdir -p "$(dirname "$imgfile")" + +PYTHONUNBUFFERED=1 PYTHONPATH=".:dep/labeller:dep/labeller/third_party/yolov5" python3 src/aeac2025/task2.py 2>&1 | tee "$logfile" & +PYTHONUNBUFFERED=1 PYTHONPATH=".:dep/labeller:dep/labeller/third_party/yolov5" python3 src/video_server/server.py 2>&1 | tee "$imgfile" + + + +dronekit==2.9.2 +dronekit-sitl==3.3.0 +exceptiongroup==1.2.0 +flake8==6.1.0 +future==0.18.3 +importlib-metadata==7.0.1 +iniconfig==2.0.0 +lxml==5.1.0 +mccabe==0.7.0 +monotonic==1.6 +mypy==1.8.0 +mypy-extensions==1.0.0 +numpy==1.26.3 +opencv-python==4.9.0.80 +packaging==23.2 +pillow==10.2.0 +platformdirs==4.1.0 +pluggy==1.3.0 +psutil==5.9.7 +pycodestyle==2.11.1 +pyflakes==3.1.0 +pymavlink==2.4.41 +pyproj==3.6.1 +pytest==7.4.4 +six==1.16.0 +tomli==2.0.1 +types-Pillow==10.2.0.20240125 +typing_extensions==4.9.0 +yapf==0.40.1 +zipp==3.17.0 +ultralytics==8.3.128 +websockets==15.0.1 + + + +from PIL import Image +from src.modules.emu import Emu +import time +import json +import threading + +from src.modules.imaging.camera import DepthCapture, OakdCamera + +emu = Emu("tmp") +i = 0 + +def print_conn(): + print("connecton made") + +emu.set_on_connect(print_conn) +latest_capture: DepthCapture | None = None + +emu.start_comms() +time.sleep(2) + +camera = OakdCamera() + +camera_thread = threading.Thread(target=camera.start(), daemon=True) +camera_thread.start() + +def send_img(message): + global latest_capture, i + + msg = json.loads(message) + + if msg["type"] == "image" and msg["message"] == "capture": + print("sending image") + latest_capture = camera.capture_with_depth() + im = Image.fromarray(latest_capture.rgb) + + im.save(f"./tmp/{i}.jpeg") + emu.send_image(f"{i}.jpeg") + + i += 1 + +def measure(message): + global latest_capture + + msg = json.loads(message) + + if msg["type"] == "getDistance": + print("getting distance") + points = msg["message"] + + p1 = points["p1"] + p2 = points["p2"] + + if latest_capture is not None: + distance = latest_capture.distance_between_points(p1["x"], p1["y"], p2["x"], p2["y"]) + send = { + "type": "distance", + "message": distance + } + emu.send_msg(json.dumps(send)) + print(f"distance sent: {send}") + + +emu.subscribe(send_img) +emu.subscribe(measure) +time.sleep(500) + + + +from src.modules.imaging.detector import IrDetector, BoundingBox +import time +from typing import Tuple +from src.modules.imaging.analysis import ImageAnalysisDelegate +from src.modules.imaging.camera import RPiCamera +from src.modules.imaging.location import DebugLocationProvider +from PIL import Image, ImageDraw + + +def func(image: Image.Image, bb: BoundingBox): + top_left_corner: Tuple[float, float] = (bb.position.x, bb.position.y) + bottom_right_corner: Tuple[float, float] = (bb.position.x + bb.size.x, + bb.position.y + bb.size.y) + + draw = ImageDraw.Draw(image) + draw.rectangle((top_left_corner, bottom_right_corner), outline="red", width=3) + image.show() + time.sleep(3) + + +camera = RPiCamera() +detector = IrDetector() +location = DebugLocationProvider() + +analysis = ImageAnalysisDelegate(detector, camera, location) +analysis.subscribe(func) + +analysis.start() + + + +from src.modules.imaging.camera import GazeboCamera +import os + + +cam = GazeboCamera() + +os.makedirs("tmp/log", exist_ok=True) +dirs = os.listdir("tmp/log") +ft_num = len(dirs) +os.makedirs(f"tmp/log/{ft_num}") # no exist_ok bc. this folder should be new + + +cam.capture_to(f"tmp/log/{ft_num}/gazebocamera.png") + + + +from src.modules.imaging.bucket_detector import BucketDetector +from src.modules.imaging.camera import DebugCameraFromDir, RPiCamera, DebugCamera +from src.modules.imaging.location import DebugLocationProvider +from src.modules.imaging.analysis import ImageAnalysisDelegate + +from ultralytics import YOLO +import os + +import cv2 +from PIL import Image +import time + +cam = RPiCamera(0) +model_path = 'samples/models' +models = os.listdir(model_path) +models = ["best.pt"] +for file in models: + model = YOLO(os.path.join(model_path, file)) + i = len(os.listdir("photos")) + while True: + image = cam.capture() + image.save(f"photos/{i}.png") + results = model(image) + + result = results[0] # because one image + a = result.plot() + # b = Image.fromarray(a) + # b.save(f"results/{i}.png") + # + cv2.imshow(f'Result for model: {file}', a) + cv2.waitKey(0) + cv2.destroyAllWindows() + time.sleep(0.5) + i += 1 + # boxes = result.boxes + + # if boxes is not None and len(boxes) > 0: + # best_box = boxes[boxes.conf.argmax()] + # (x1, y1, x2, y2) = best_box.xyxy[0].tolist() # box [x1, y1, x2, y2] + # conf = best_box.conf.item() # confidence threshold + # if conf < 0.5: + # print("not confident") + # else: + # print("confident") + # else: + # print("no bounding box") + + + +import time +import os +import threading + +from dronekit import connect +import json + +from src.modules.imaging.camera import RPiCamera +# from src.modules.imaging.mavlink import MAVLinkDelegate +# from src.modules.imaging.location import MAVLinkLocationProvider + +CONN_STR = "tcp:127.0.0.1:14550" +MESSENGER_PORT = 14552 + +cam = RPiCamera() +#mavlink = MAVLinkDelegate() + +drone = connect(CONN_STR, wait_read=False) + +os.makedirs("tmp/log", exist_ok=True) +dirs = os.listdir("tmp/log") +ft_num = len(dirs) +os.makedirs(f"tmp/log/{ft_num}") # no exist_ok bc. this folder should be new + +i = 0 +last_picture = time.time() + + +def location_dump_to(path: str): + with open(path, 'w') as file: + location = {"location": str(drone.location.global_relative_frame)} + json.dump(location, file) + + +def take_picture(_): + global i + global last_picture + + cam.caputure_to(f"tmp/log/{ft_num}/{i}.png") + location_dump_to(f"tmp/log/{ft_num}/{i}.json") + print(i) + i += 1 + + +def picture_loop(sleep): + while True: + take_picture(None) + time.sleep(sleep) + + +thread_1 = threading.thread(picture_loop, 3) + +thread_1.start() + +#mavlink.run() + + + +from typing import Callable, Optional, List, Callable, Any + +import threading +# from multiprocessing import Process +from .detector import BaseDetector, BoundingBox +from .camera import CameraProvider +from .debug import ImageAnalysisDebugger +from ..georeference.inference_georeference import get_object_location +from .location import LocationProvider +from PIL import Image, ImageDraw + +import os + +from .analysis import CameraAttributes, Inference + + +class DebugImageAnalysisDelegate: + """ + Implements an imaging inference loops and provides several methods which + can be used to query the latest image analysis results. + + Responsible for capturing pictures regularly, detecting any landing pads in + those pictures and then providing the most recent estimate of the landing + pad location from the camera's perspective. + + Pass an `ImageAnalysisDebugger` when constructing to see a window with live + results. + + TODO: geolocate the landing pad using the drone's location. + """ + + def __init__(self, + detector: BaseDetector, + camera: CameraProvider, + location_provider: LocationProvider, + debugger: Optional[ImageAnalysisDebugger] = None, + ): + import os + self.detector = detector + self.camera = camera + self.debugger = debugger + self.location_provider = location_provider + self.subscribers: List[Callable[[Image.Image, float, float], Any]] = [] + self.camera_attributes = CameraAttributes() + + # log pictures taken + os.makedirs("tmp/log", exist_ok=True) + dirs = os.listdir("tmp/log") + current_path = f"tmp/log/{len(dirs)}" + os.makedirs(current_path) + + # path to store images taken during flight + self.img_path = f"{current_path}/images" + os.makedirs(self.img_path) + + # annotated bounding boxes + self.bb_img_path = f"{current_path}/bb" + os.makedirs(self.bb_img_path) + + # image number + self.i = 0 + + def get_inference(self, bounding_box: BoundingBox) -> Inference: + inference = Inference(bounding_box, self.location_provider.altitude()) + return inference + + def start(self): + """ + Will start the image analysis process in another thread. + """ + thread = threading.Thread(target=self._analysis_loop) + # process = Process(target=self._analysis_loop) + thread.start() + # process.start() + # Use `threading` to start `self._analysis_loop` in another thread. + + def _analyze_image(self): + """ + Actually performs the image analysis once. Only useful for testing, + should otherwise we run by `start()` which then starts + `_analysis_loop()` in another thread. + """ + im = self.camera.capture() + im.save(os.path.join(self.img_path, f"{self.i}.png")) + + bounding_box = self.detector.predict(im) + + if bounding_box: + draw = ImageDraw.Draw(im) + bb = (bounding_box.position.x, bounding_box.position.y, + bounding_box.size.x, bounding_box.size.y) + draw.rectangle(bb) + + im.save(os.path.join(self.bb_img_path, f"{self.i}.png")) + + self.i += 1 + + if self.debugger is not None: + self.debugger.set_image(im) + if bounding_box is not None: + self.debugger.set_bounding_box(bounding_box) + + for subscriber in self.subscribers: + if bounding_box: + inference = self.get_inference(bounding_box) + if inference: + x, y = get_object_location(self.camera_attributes, + inference) + subscriber(im, (x, y)) + else: + subscriber(im, None) + + def _analysis_loop(self): + """ + Indefinitely run image analysis. This should be run in another thread; + use `start()` to do so. + """ + while True: + self._analyze_image() + + def subscribe(self, callback: Callable): + """ + Subscribe to image analysis updates. For example: + + def myhandler(image: Image.Image, bounding_box: BoundingBox): + if bounding_box is None: + print("No bounding box detected") + else: + print("Bounding box detected") + + imaging_process.subscribe(myhandler) + """ + self.subscribers.append(callback) + + + +from src.modules.imaging.camera import WebcamCamera +# from src.modules.imaging.camera import RPiCamera +import time +import struct +import pickle +import socket + + +# interval in seconds +interval = 1 + +cam = WebcamCamera() +# cam = RPiCamera(0) + +client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) +client_socket.connect(('127.0.0.1', 8080)) + +while True: + img = cam.capture() + data = pickle.dumps(img) + message = struct.pack("Q", len(data)) + data + + client_socket.sendall(message) + + time.sleep(interval) + + + +from src.modules.imaging.bucket_detector import BucketDetector +from src.modules.imaging.camera import DebugCameraFromDir +from src.modules.imaging.location import DebugLocationProvider +# from src.modules.imaging.analysis import ImageAnalysisDelegate +from src.modules.imaging.debug_analysis import DebugImageAnalysisDelegate + + + +cam = DebugCameraFromDir("images") +det = BucketDetector("samples/models/n640.pt") +location = DebugLocationProvider() +analysis = DebugImageAnalysisDelegate(det, cam, location) +# analysis = ImageAnalysisDelegate(det, cam, location) + +def test(a1, a2): + if a1 != None and a2 != None: + print(a1) + print(a2) + +analysis.subscribe(test) + +analysis.start() + + + +from src.modules.imaging.detector import IrDetector +import time +from src.modules.imaging.analysis import ImageAnalysisDelegate +from src.modules.imaging.camera import RPiCamera +from src.modules.imaging.location import DebugLocationProvider +from src.modules.imaging.mavlink import MAVLinkDelegate +from pymavlink import mavutil + +from src.modules.autopilot import navigator + +from dronekit import connect, VehicleMode + + +CONN_STR = "udp:127.0.0.1:14551" +MESSENGER_PORT = 14552 + +drone = connect(CONN_STR, wait_ready=False) + +nav = navigator.Navigator(drone, MESSENGER_PORT) +mavlink = MAVLinkDelegate(conn_str = CONN_STR) + +time.sleep(2) + +bucket_avg = [[], []] +target_height = 0.9 + + +def moving_bucket_avg(_, pos): + if pos: + if len(bucket_avg[0]) > 0 and len(bucket_avg[1]) > 0: + num_x = len(bucket_avg[0]) + num_y = len(bucket_avg[1]) + + avg_x = sum(bucket_avg[0]) / num_x + avg_y = sum(bucket_avg[1]) / num_y + + new_x_avg = avg_x + pos[0] / (num_x + 1) + new_y_avg = avg_y + pos[1] / (num_y + 1) + + bucket_avg[0].append(new_x_avg) + bucket_avg[1].append(new_y_avg) + else: + bucket_avg[0].append(pos[0]) + bucket_avg[1].append(pos[1]) + + print(f"X: {bucket_avg[0]}") + print(f"Y: {bucket_avg[1]}") + time.sleep(1) + + +camera = RPiCamera(0) +detector = IrDetector() +location = DebugLocationProvider() + +analysis = ImageAnalysisDelegate(detector, camera, location) +analysis.subscribe(moving_bucket_avg) + +nav.send_status_message("Shepard is online") + +while not (drone.armed and drone.mode == VehicleMode("GUIDED")): + pass + +time.sleep(5) +analysis.start() + +nav.takeoff(30) + +type_mask = nav.generate_typemask([0, 1, 2]) + +nav.send_status_message("Executing") +current_alt = nav.get_local_position_ned()[2] + +delta = 0.5 +sleep_time = 2 + + +def bucket_descent(): + # VERIFY ALL OF THESE COORINDATE SYSTEMS BEFORE FLYING!!!!!!!!!!!!!!!!!!!!!!!!!! + + coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED + + nav.set_position_target_local_ned(x = bucket_avg[0][-1], + y = bucket_avg[1][-1], + z = 0, + type_mask = type_mask, + coordinate_frame = coordinate_frame) + + time.sleep(5) + + nav.set_position_target_local_ned(x = (bucket_avg[0][-1] - bucket_avg[0][0]), + y = (bucket_avg[1][-1] - bucket_avg[1][0]), + z = 10, + type_mask = type_mask, + coordinate_frame = coordinate_frame) + + time.sleep(5) + + nav.set_position_target_local_ned(x = (bucket_avg[0][-1] - bucket_avg[0][0]), + y = (bucket_avg[1][-1] - bucket_avg[1][0]), + z = 5, + type_mask = type_mask, + coordinate_frame = coordinate_frame) + + time.sleep(5) + + nav.set_position_target_local_ned(x = (bucket_avg[0][-1] - bucket_avg[0][0]), + y = (bucket_avg[1][-1] - bucket_avg[1][0]), + z = 5, + type_mask = type_mask, + coordinate_frame = coordinate_frame) + + time.sleep(5) + + nav.set_position_target_local_ned(x = (bucket_avg[0][-1] - bucket_avg[0][0]), + y = (bucket_avg[1][-1] - bucket_avg[1][0]), + z = 5, + type_mask = type_mask, + coordinate_frame = coordinate_frame) + + time.sleep(5) + + #Full commit + + nav.set_position_target_local_ned(x = 0, + y = 0, + z = 5 - target_height, + type_mask = type_mask, + coordinate_frame = coordinate_frame) + + time.sleep(5) + + +if len(bucket_avg[0]) > 0 and len(bucket_avg[1]) > 0: + bucket_descent() +else: + time.sleep(5) + bucket_descent() + +nav.return_to_launch() + +drone.close() + +nav.send_status_message("Flight test script execution terminated") + + + +from src.modules.emu import Emu +import time +import json + +emu = Emu("res") + +# def onConnect(): +# loadCurrent = { +# "type": "load", +# "uavStatus": { +# "connection": "no", +# "mode": "test", +# "imageCount": "2", +# "timeSinceMessage": "3" +# }, +# "imageName": "res/sample1.jpg" +# } +# emu.send_msg(json.dumps(loadCurrent)) + + +# emu.set_on_connect(onConnect) +emu.start_comms() +time.sleep(2) + +# test different logs +for i in range(6): + print(f"sending log {i}") + if i % 3 == 0: severity = "normal" + elif i % 3 == 1: severity = "warning" + else: severity = "error" + emu.send_log(f"log text {i}", severity) + time.sleep(1) + +while True: + print("sending image") + emu.send_log("sending image") + emu.send_image("test-image.jpeg") + time.sleep(2) + + + +""" +Functions for getting the location of objects we find in our images + +This is done by assuming that we are looking a flat field +We can find the angle from our camera lens based on the pixel location +We can find the angle of the camera based on the Autopilot Roll, Yaw and Pitch + +Note for this analysis, we use UTM coordinates. +This maps traditional Latitude and Longitude into an X Y coordinate grid +where X, Y are in meters +""" + +# TODO: Requires a circular-import... but we only need these for type annotations +import numpy as np +from math import cos, tan, atan, radians, sqrt +import pyproj +from pyproj import Geod + +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from ..imaging.analysis import CameraAttributes, Inference + + +def LonLat_To_XY(lon, lat, zone=12): + """ + Returns the Easting and Northing of given Lon, Lat on the WGS84 + Projection Model + + Input is in degrees, return is in meters + """ + P = pyproj.Proj(proj='utm', zone=zone, ellps='WGS84', preserve_units=True) + + return P(lon, lat) + + +def XY_To_LonLat(x, y, zone=12): + """ + Returns the Lon, Lat of given Easting, Northing on the WGS84 + Projection Model + + Input is in meters, return is in degrees + """ + P = pyproj.Proj(proj='utm', zone=zone, ellps='WGS84', preserve_units=True) + + return P(x, y, inverse=True) + + +def Geofence_to_XY(origin, geofence): + # Returns a new geofence which consists of position vectors in meters + # Guys the Earth is like mostly flat right?? + + try: + R = 6378137 # Radius of the Earth in meters + + new_fence = [] + + origin_lon = origin[0] + origin_lat = origin[1] + + origin_lon_rad = radians(origin_lon) + origin_lat_rad = radians(origin_lat) + + for point in geofence: + + point_lon_rad = radians(point[0]) + point_lat_rad = radians(point[1]) + + delta_lat = point_lat_rad - origin_lat_rad + delta_lon = point_lon_rad - origin_lon_rad + + x = delta_lat * R + y = delta_lon * cos((origin_lat_rad + point_lat_rad) / 2) * R + + new_fence.append((x, y)) + + return new_fence + + except TypeError: + return None + + +''' +def meters_to_LonLat(origin, points): + + R = 635900 + + new_points = [] + + origin_lon = origin[0] + origin_lat = origin[1] + + origin_lon_rad = radians(origin_lon) + origin_lat_rad = radians(origin_lat) + + for point in points: + + point_x = point[0] + point_y = point[1] + + delta_lat = point_x / R + delta_lon = point_y / (cos(origin_lat_rad + delta_lat / 2) * R) + + delta_lat = degrees(delta_lat) + delta_lon = degrees(delta_lon) + + new_points.append((delta_lat + origin_lat, delta_lon + origin_lon)) + + return new_points +''' + + +def meters_to_LonLat(origin, points): + g = Geod(ellps='clrk66') + new_points = [] + for point in points: + az = atan(point[1] / point[0]) + dist = sqrt((point[0] ** 2 + point[1] ** 2)) + + point_lon, point_lat, backaz = g.fwd(origin[0], origin[1], az, dist) + new_points.append((point_lat, point_lon)) + + return new_points + + +def pixel_to_rel_position(camera_attributes: 'CameraAttributes', + inference: 'Inference', fovh, fovv) -> np.array: + """ + Calculates the unit vector from an angled camera to an object at x, y pixel coordinates + x and y are the normalized pixel coordinates between 0 and 1 + both fovs' are in radians. + """ + + direction_vector = np.zeros(2) + + #calculating image height and width in meters + height = 2 * camera_attributes.focal_length * tan(fovv / 2) + width = 2 * camera_attributes.focal_length * tan(fovh / 2) + + #pixels to meters conversion + y_m = inference.y * height + x_m = inference.x * width + + if (y_m > height / 2): + y_m = height - y_m + theta_y = camera_attributes.angle - atan( + (height / 2 - y_m) / camera_attributes.focal_length) + elif (y_m <= height / 2): + theta_y = atan( + (height / 2 - y_m) / + camera_attributes.focal_length) + camera_attributes.angle + + if (x_m > width / 2): + x_m = width - x_m + theta_x = -1 * (atan( + (width / 2 - x_m) / camera_attributes.focal_length)) + elif (x_m <= width / 2): + theta_x = atan((width / 2 - x_m) / camera_attributes.focal_length) + + y_comp = inference.relative_alt * tan(theta_y) + x_comp = (inference.relative_alt / + cos(camera_attributes.angle)) * tan(theta_x) + + offset = calculate_object_offsets() + + # NOTE: This correction was made because of the camera being upside down + # This corrects the issues regarding the autopilot navigating to the wrong direction + + direction_vector[1] = -1 * (x_comp + offset[0]) + direction_vector[0] = y_comp + offset[1] + + return direction_vector + + +#TODO get measurements to calculate offset due to shifted position of camera from the gps +def calculate_object_offsets() -> np.array: + return np.array([0, 0]) + + +def get_object_location(camera_attributes: 'CameraAttributes', + inference: 'Inference') -> tuple: + """ + This calculates the location of the inference provided + and returns the longitude, latitude in degrees + """ + + H_FOV = radians(62.2) + V_FOV = radians(48.8) + + dir_vector = pixel_to_rel_position( + camera_attributes, + inference, + H_FOV, + V_FOV, + ) + print("dir_vector", dir_vector) + + #lon, lat = XY_To_LonLat(dir_vector[0], dir_vector[1]) + + return (dir_vector[0], dir_vector[1]) + + + +from typing import Tuple + +import pathlib +from PIL import Image +import numpy as np +import cv2 +import depthai as dai +from dataclasses import dataclass + + +class CameraProvider: + """ + Manage a camera source. This could be the raspberry pi camera, a web cam, + or a series of images. + """ + + def set_size(self, size: Tuple[int, int]): + """ + Set the pixel width and height of all images taken by this camera. + """ + # Should be implemented by deriving classes. + raise NotImplementedError() + + def capture(self) -> Image.Image: + """ + Captures a single image from the camera. This image will be of the size + set by `set_size`. + """ + # Should be implemented by deriving classes. + raise NotImplementedError() + + def capture_to(self, path: str | pathlib.Path): + """ + Captures a single image and saves it to `path`. + """ + self.capture().save(path) + + def caputure_as_ndarry(self) -> np.ndarray: + """ + Captures a single image returns it's numpy.ndarray representation. Will + have shape (height, width, colors). + """ + return np.array(self.capture()) + +@dataclass +class DepthCapture: + rgb: np.ndarray + point_cloud: np.ndarray + width: int + height: int + + def get_point(self, x: int, y: int) -> np.ndarray: + """Get the 3D coordinates relative to the camera frame (in mm) of a pixel). + + (x, y) are in pixel coordinates in the self.rgb frame.""" + idx = y * self.width + x + p = self.point_cloud[idx] + + # I am not sure if this is possible... + assert not np.any(np.isnan(p)), "Invalid depth at this point" + + return p + + def distance_between_points(self, x1: int, y1: int, x2: int, y2: int): + """Get the physical distance between points from pixels in the rgb + frame (x1, y1) and (x2, y2). + + Resulting distance is in mm.""" + p1 = self.get_point(x1, y1) + p2 = self.get_point(x2, y2) + + dist = np.linalg.norm(p1 - p2) + + return dist + + +class OakdCamera(CameraProvider): + """ + Manages an OAK-D device with on-demand capturing of 3D pictures (see DepthCapture). + """ + + def __init__(self, fps: int = 30): + self._init_pipeline(fps) + + def _init_pipeline(self, fps: int): + """Initialize the Depth AI pipeline (will be run on the OAK-D)""" + pipeline = dai.Pipeline() + + camRgb = pipeline.create(dai.node.ColorCamera) + monoLeft = pipeline.create(dai.node.MonoCamera) + monoRight = pipeline.create(dai.node.MonoCamera) + depth = pipeline.create(dai.node.StereoDepth) + pointcloud = pipeline.create(dai.node.PointCloud) + sync = pipeline.create(dai.node.Sync) + xOut = pipeline.create(dai.node.XLinkOut) + + camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P) + camRgb.setBoardSocket(dai.CameraBoardSocket.CAM_A) + camRgb.setIspScale(1, 3) + camRgb.setFps(fps) + + monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P) + monoLeft.setCamera("left") + monoLeft.setFps(fps) + + monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P) + monoRight.setCamera("right") + monoRight.setFps(fps) + + depth.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY) + depth.setLeftRightCheck(True) + depth.setSubpixel(True) + depth.setDepthAlign(dai.CameraBoardSocket.CAM_A) + + monoLeft.out.link(depth.left) + monoRight.out.link(depth.right) + depth.depth.link(pointcloud.inputDepth) + + camRgb.isp.link(sync.inputs["rgb"]) + pointcloud.outputPointCloud.link(sync.inputs["pcl"]) + + sync.out.link(xOut.input) + xOut.setStreamName("out") + + self.pipeline = pipeline + + def capture_with_depth(self) -> DepthCapture: + """Capture a current 3D frame on the OAK-D. + + NOTE: .start() must have been called first. If it has not, this will raise Exception.""" + if not self.device or self.device.isClosed(): + raise Exception("No oakD connection, perhaps you forgot to call the .start() function") + + msg = self.queue.get() + rgbFrame = msg["rgb"] + cv_frame = rgbFrame.getCvFrame() + rgb_frame = cv2.cvtColor(cv_frame, cv2.COLOR_BGR2RGB) + rgb = np.array(rgb_frame) + pcl = msg["pcl"] + + point_cloud = pcl.getPoints().astype(np.float64) + height, width, _ = rgb.shape + + capture = DepthCapture(rgb, point_cloud, width, height) + return capture + + def capture(self) -> Image.Image: + capture = self.capture_with_depth + img = Image.fromarray(capture.rgb, "RGB") + return img + + + def start(self): + """Start the depth-perception process on the OAK-D""" + print("Starting OAK-D Connection") + self.device = dai.Device(self.pipeline) + self.queue = self.device.getOutputQueue("out", maxSize=1, blocking=False) + + def stop(self): + """Stop the depth-perception process""" + self.device.close() + self.queue = None + +class DebugCamera(CameraProvider): + """ + Debug camera source which always returns the same image loaded from + `dummy_image_path`. + """ + + def __init__(self, dummy_image_path: str | pathlib.Path): + self.og_im = Image.open(dummy_image_path) + self.im = self.og_im # Keep a copy of the original image for resizing. + self.size = (self.im.width, self.im.height) + + def set_size(self, size: Tuple[int, int]): + # Always resize from the original "dummy" image + self.im = self.og_im.resize(size) + self.size = size + + def capture(self) -> Image.Image: + return self.im + + +class DebugCameraFromDir(CameraProvider): + """ + Debug camera that returns an image from directory 'image_dir' + containing only images + """ + def __init__(self, image_dir: str | pathlib.Path): + import os # used to get images in folder + self.image_dir = image_dir + self.imgs = os.listdir(image_dir) + self.imgs = [os.path.join(image_dir, file) for file in self.imgs] + if len(self.imgs) == 0: + raise ValueError('no files in directory') + self.index = 0 + + # set size at first based on first image + im = Image.open(self.imgs[self.index]) + self.size = (im.width, im.height) + + def set_size(self, size: Tuple[int, int]): + # set size as each image is resized on load + self.size = size + + def capture(self) -> Image.Image: + # return the next image in the directory + filename = self.imgs[self.index] + print(filename) + self.index = (self.index + 1) % len(self.imgs) + + return Image.open(filename).resize(self.size) + + +class GazeboCamera(CameraProvider): + """ + Video sourced from the Gazebo Simulator over UDP + """ + + def __init__(self): + self.port = 5600 + + gst_pipeline = ( + "udpsrc address=127.0.0.1 port=5600 ! " + "application/x-rtp, encoding-name=H264 ! " + "rtph264depay ! " + "avdec_h264 ! " + "videoconvert ! " + "appsink" + ) + self.size = (640, 480) + self.cap = cv2.VideoCapture(gst_pipeline, cv2.CAP_GSTREAMER) + + if not self.cap.isOpened(): + print("Failed to open UDP Stream") + exit() + + def set_size(self, size: Tuple[int, int]): + self.size = size + self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, size[0]) + self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, size[1]) + + def capture(self) -> Image.Image: + ret, frame = self.cap.read() + if ret: + frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) + return Image.fromarray(frame).resize(self.size) + else: + raise RuntimeError("Failed to capture image from webcam") + + def show_images(self): + while True: + ret, frame = self.cap.read() + if not ret: + print("Frame not received") + break + + cv2.imshow("Gazebo Video Stream", frame) + if cv2.waitKey(1) & 0xFF == ord('q'): + break + self.cap.release() + cv2.destroyAllWindows() + + +class WebcamCamera(CameraProvider): + """ + Debug camera source which uses the computer's webcam as the image source. + """ + + def __init__(self): + self.cap = cv2.VideoCapture(0) # 0 is typically the default webcam + self.size = (640, 480) + self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.size[0]) + self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.size[1]) + + def set_size(self, size: Tuple[int, int]): + self.size = size + self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, size[0]) + self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, size[1]) + + def capture(self) -> Image.Image: + ret, frame = self.cap.read() + if ret: + frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) + return Image.fromarray(frame).resize(self.size) + else: + raise RuntimeError("Failed to capture image from webcam") + + +class RPiCamera(CameraProvider): + """ + Note: Need picamera2 installed on the raspberry pi for this to work. + Production camera source which uses the raspberry pi camera as the image + source. + """ + + def __init__(self, cam_num: int): + from picamera2 import Picamera2 + self.camera = Picamera2(cam_num) + self.size = (640, 480) + self.configure_camera() + self.camera.start() + print(self.camera.capture_metadata()['ScalerCrop']) + print(self.camera.camera_controls['ScalerCrop']) + + def configure_camera(self): + # Configuring camera properties + config = self.camera.create_preview_configuration( + main={"size": self.size}) + self.camera.configure(config) + + def set_size(self, size: Tuple[int, int]): + self.size = size + self.configure_camera() + + def capture(self) -> Image.Image: + # Capture an image + self.camera.start() + capture_result = self.camera.capture_array() + image = Image.fromarray(capture_result) + return image + + + +from typing import Optional + +from PIL import Image + +from .detector import Vec2, BoundingBox, BaseDetector + +from ultralytics import YOLO + + +class BucketDetector(BaseDetector): + + def __init__(self, model_path): + print(f"model: {model_path}") + self.model = YOLO(model_path) + + def predict(self, image: Image.Image) -> Optional[BoundingBox]: + results = self.model(image, verbose = False) + + result = results[0] # because one image + + boxes = result.boxes + + if boxes is not None and len(boxes) > 0: + best_box = boxes[boxes.conf.argmax()] + (x1, y1, x2, y2) = best_box.xyxy[0].tolist() # box [x1, y1, x2, y2] + conf = best_box.conf.item() # confidence threshold + if conf < 0.75: + return None + else: + return BoundingBox(Vec2(x1, y1), Vec2(x2, y2)) + else: + return None + + + +from functools import lru_cache +from typing import Optional + +from PIL import Image +import numpy as np +import cv2 +from cv2 import aruco + + +@dataclass +class Vec2: + """2-component vector with float elements.""" + x: float + y: float + + def __add__(self, other: 'Vec2') -> 'Vec2': + return Vec2(self.x + other.x, self.y + other.y) + + def __sub__(self, other: 'Vec2') -> 'Vec2': + return Vec2(self.x - other.x, self.y - other.y) + + def __rmul__(self, scalar: float) -> 'Vec2': + return Vec2(self.x * scalar, self.y * scalar) + + def __rtruediv__(self, scalar: float) -> 'Vec2': + return Vec2(self.x / scalar, self.y / scalar) + + @cached_property + def norm(self) -> float: + """Return the euclidean norm of the vector""" + return math.sqrt(self.x**2 + self.y**2) + + def normalize(self) -> 'Vec2': + """Reduce the norm to 1 while preserving direction.""" + magnitude = self.norm + if magnitude != 0: + return Vec2(self.x / magnitude, self.y / magnitude) + else: + return Vec2(0, 0) + + @staticmethod + def dot(v1: 'Vec2', v2: 'Vec2') -> float: + """Compute the standard inner product between v1 and v2.""" + return v1.x * v2.x + v1.y * v2.y + + @staticmethod + def min(v1: 'Vec2', v2: 'Vec2') -> 'Vec2': + """Compute component-wise min of v1 and v2""" + return Vec2(min(v1.x, v2.x), min(v1.y, v2.y)) + + @staticmethod + def max(v1: 'Vec2', v2: 'Vec2') -> 'Vec2': + """Compute component-wise max of v1 and v2""" + return Vec2(max(v1.x, v2.x), max(v1.y, v2.y)) + + +class BoundingBox: + +def __init__(self, position: Vec2, size: Vec2): + self.position = position + self.size = size + +@lru_cache(maxsize=2) +def intersection(self, other: 'BoundingBox') -> float: + top_left = Vec2.max(self.position, other.position) + bottom_right = Vec2.min(self.position + self.size, + other.position + other.size) + + size = bottom_right - top_left + + intersection = size.x * size.y + return max(intersection, 0) + +def union(self, other: 'BoundingBox') -> float: + intersection = self.intersection(other) + if intersection == 0: + return 0 + + union = self.size.x * self.size.y + other.size.x * other.size.y - intersection + return union + +def intersection_over_union(self, pred: 'BoundingBox') -> Optional[float]: + intersection = self.intersection(pred) + if intersection == 0: + return 0 + iou = intersection / self.union(pred) + return iou + + +class BaseDetector: + def predict(self, image: Image.Image) -> Optional[BoundingBox]: + raise NotImplementedError() + + +class IrDetector(BaseDetector): + + def predict(self, image: Image.Image) -> Optional[BoundingBox]: + img = np.array(image) + + gray_img = cv2.cvtColor(img, cv2.COLOR_RGBA2GRAY) + max_val = np.max(gray_img) # returns maximum value of brightness + if max_val < 200: + return None # lower threshold for intensity + _, thresh = cv2.threshold(gray_img, max_val - 10, 255, cv2.THRESH_BINARY) + contours, _ = cv2.findContours(thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + + # cv2.drawContours(thresh, contours, -1, (0, 255, 0), 2) + + if len(contours) == 0: + return None + + for cnt in contours: + x, y, w, h = cv2.boundingRect(cnt) + + return BoundingBox(Vec2(x, y), Vec2(w, h)) + + +class ArucoDetector(): + + def predict(self, image: Image.Image) -> Optional[BoundingBox]: + img = cv2.cvtColor(np.array(image), cv2.COLOR_RGB2BGR) + + aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) + + params = cv2.aruco.DetectorParemeters() + + corners, ids, rejected = cv2.aruco.detectMarkers(img, aruco_dict, parameters=params) + + if ids: + for c in zip(corners, ids): + + pts = c[0] + + x_min = pts[:, 0].min() + x_max = pts[:, 0].max() + y_min = pts[:, 1].min() + y_max = pts[:, 1].max() + + x = (x_min + x_max) / 2 + y = (y_min + y_max) / 2 + w = (x_max - x_min) + h = (y_max - y_min) + + return BoundingBox(Vec2(x, y), Vec2(w, h)) + + + +import time + +from pymavlink import mavutil + +from src.modules.autopilot.navigator import Navigator +from src.modules import imaging +import math + + +class Lander: + """ + A class to handle everything regarding landing that is not already handled by ardupilot + """ + + HORIZONTAL_ANGLE = math.radians(30) + VERTICAL_ANGLE = math.radians(24) + + def __init__(self, nav, max_velocity, geofence): + self.__spiral_route = [] # Private attribute + self.__bounding_box_pos = [] + self.__buffer = [[], + []] + # self.landing_spot = landing_spot + self.nav = nav + self.i = 10 + self.max_velocity = max_velocity + self.bounding_box_detected = False + self.bounding_box_pos = [] + self.bounding_box_log = [] + self.bounding_box_log_og = [] + self.leave_frame = False + self.geofence = geofence + + self.null_radius = 10 # Radius in METERS of bounding box detection being ignored + + @property + def route(self): + return self.__spiral_route + + def generateSpiralSearch(self, numberOfLoops=10): + """ + Generate a landing route in a square spiral pattern. The generated route is to be multiplied by the current height in metres + to get the relative distance traveled in metres. + + :param numberOfLoops: The number of loops to be made, with a default value of 10 + :return: None + """ + + self.__spiral_route = [] # Clear the existing route + self.y, self.x = 0, 0 + + # verticalScanRatio = math.tan(Lander.VERTICAL_ANGLE) + # horizontalScanRatio = math.tan(Lander.HORIZONTAL_ANGLE) + + step_size = 5 + + steps_per_side = 2 + curr_side_iter = 0 + loops_completed = 0 + + axis = "x" + + dir_x = 1 + dir_y = 1 + + x, y = 0, 0 + + while loops_completed != numberOfLoops: + if axis == "x": + x += dir_x * step_size + for i in range(steps_per_side): + if len(self.__spiral_route) > 0: + last_x = self.__spiral_route[-1][0] + last_y = self.__spiral_route[-1][1] + else: + last_x = 0 + last_y = 0 + + self.__spiral_route.append((x + last_x, y + last_y)) + + axis = "y" + dir_x *= -1 + elif axis == "y": + y += dir_y * step_size + + for i in range(steps_per_side): + if len(self.__spiral_route) > 0: + last_x = self.__spiral_route[-1][0] + last_y = self.__spiral_route[-1][1] + else: + last_x = 0 + last_y = 0 + + self.__spiral_route.append((x + last_x, y + last_y)) + + axis = "x" + dir_y *= -1 + + curr_side_iter += 1 + + if curr_side_iter % 2 == 0: + steps_per_side += 1 + + if curr_side_iter % 5 == 0: + loops_completed += 1 + + self.x = x + self.y = y + x = 0 + y = 0 + + ''' + def executeSearch(self, altitude): + """ + Move the drone to the next position in the landing route. + + :param Navigator: An instance of the Navigator class. + :param route: A list of 2 elements representing the relative distance ratio to be specified as [north, east]. + :param altitude: The altitude in metres. + :return: None + """ + + type_mask = self.nav.generate_typemask([0, 1]) + i = 0 + while i <= len(self.__spiral_route) - 1: + + current_local_pos = self.nav.get_local_position_ned() + print(self.geofence_check((self.__spiral_route[i][0], self.__spiral_route[i][1]))) + print(self.__spiral_route[i]) + if self.bounding_box_detected: + print(self.bounding_box_pos) + new_x, new_y = self.bounding_box_pos[0], self.bounding_box_pos[1] + if len(self.bounding_box_log) == 0: + self.bounding_box_log.append((new_x, new_y)) + print(self.bounding_box_log) + for bounding_box in self.bounding_box_log: + delta_x = bounding_box[0] - new_x - current_local_pos[0] + delta_y = bounding_box[1] - new_y - current_local_pos[1] + + if math.sqrt((delta_x ** 2) + (delta_y ** 2)) >= self.null_radius: + self.boundingBoxAction() + time.sleep(1/(self.max_velocity)) + break + self.bounding_box_detected = False + else: + + if self.geofence_check((self.__spiral_route[i][0], self.__spiral_route[i][1])): + + self.nav.set_position_target_local_ned(x = self.__spiral_route[i][0], + y = self.__spiral_route[i][1], + type_mask=type_mask, + coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED) + + i += 1 + time.sleep(1 / (self.max_velocity)) + + return self.bounding_box_log + + #self.nav.set_position_relative(route[0], route[1]) + ''' + # Find the reason why double detection with one having high errors + # Make task 1 script ready for comp + # Make spiral take pictures as it goes around the IR emitter to revise accuracy + # Fix heading so that the camera can face towards the inside of the spiral + # Make it so that it is plug and play for this morning + + def executeSearch(self, altitude): + ''' + (location.north, location.east, location.down) + to always face inwards, always face the origin + ''' + type_mask = self.nav.generate_typemask([0, 1]) + i = 0 + origin = self.nav.get_local_position_ned() + try: + while i <= len(self.__spiral_route) - 1: + current_local_pos = self.nav.get_local_position_ned() + if self.bounding_box_detected: + print(self.bounding_box_pos) + new_x, new_y = self.bounding_box_pos[0], self.bounding_box_pos[1] + if len(self.bounding_box_log) == 0: + self.bounding_box_log.append((new_x, new_y)) + self.bounding_box_log_og.append((new_x + current_local_pos[0], new_y + current_local_pos[1])) + print(self.bounding_box_log) + for bounding_box in self.bounding_box_log: + delta_x = bounding_box[0] - new_x - current_local_pos[0] + delta_y = bounding_box[1] - new_y - current_local_pos[1] + + if math.sqrt((delta_x ** 2) + (delta_y ** 2)) >= self.null_radius: + self.boundingBoxAction() + time.sleep(2 / (self.max_velocity)) + break + self.bounding_box_detected = False + else: + # NOTE: THESE ARE ABSOLUTE COORDINATES + # Spiral search is in absolute coordinates in which it adds the offset to the origin + self.nav.set_position_target_local_ned(x = self.__spiral_route[i][0] + origin[0] - (self.__spiral_route[4][0] / 2), + y = self.__spiral_route[i][1] + origin[1] - (self.__spiral_route[9][1] / 2), + z = -altitude, + type_mask=type_mask, + coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_NED) + + i += 1 + + # Increase if the drone is overriding itself or moving too quickly + # Decrease if there is more "stop, start" than desirable + time.sleep(1.5 / (self.max_velocity)) + + print(self.bounding_box_log_og) + finally: + return self.bounding_box_log_og + + def boundingBoxAction(self): + # go to bounding box and go around it in a square + type_mask = self.nav.generate_typemask([0, 1]) + + current_local_pos = self.nav.get_local_position_ned() + time.sleep(1) + + x, y = self.bounding_box_pos[0], self.bounding_box_pos[1] + if self.geofence_check((x + 2, y)) and self.geofence_check((x, y + 2)) and self.geofence_check((x + 2, y + 2)): + self.nav.set_position_target_local_ned(x = x, + y = y, + type_mask=type_mask, + coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED) + time.sleep((math.sqrt(x ** 2 + y ** 2) / (self.max_velocity))) + self.nav.set_position_target_local_ned(x = 2, + y = 0, + type_mask=type_mask, + coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED) + time.sleep((3 / (self.max_velocity))) + self.nav.set_position_target_local_ned(x = 0, + y = 2, + type_mask=type_mask, + coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED) + time.sleep(3 / (self.max_velocity)) + self.nav.set_position_target_local_ned(x = -2, + y = 0, + type_mask=type_mask, + coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED) + time.sleep((3 / (self.max_velocity))) + self.nav.set_position_target_local_ned(x = 0, + y = -2, + type_mask=type_mask, + coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED) + time.sleep((3 / (self.max_velocity))) + + self.nav.set_position_target_local_ned(x = -x, + y = -y, + type_mask=type_mask, + coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED) + time.sleep((math.sqrt(x ** 2 + y ** 2) / (self.max_velocity))) + + else: + self.nav.set_position_target_local_ned(x = x, + y = y, + type_mask=type_mask, + coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED) + time.sleep((math.sqrt(x ** 2 + y ** 2) / (self.max_velocity))) + + self.bounding_box_detected = False + self.bounding_box_log.append((x, y)) + self.bounding_box_log_og.append((x + current_local_pos[0], y + current_local_pos[1])) + + def detectBoundingBox(self, _, bounding_box_pos): + if bounding_box_pos: + if not self.leave_frame: + self.bounding_box_detected = True + self.bounding_box_pos = bounding_box_pos + else: + + if self.leave_frame: + self.leave_frame = False + + def geofence_check(self, point): + # Adapted from code provided by Aiden + #lat, lon, alt = self.nav.get_global_position() + lat, lon = point[1], point[0] + hitcount = 0 + for i in range(len(self.geofence) - 1): + # Model geofence lines as y = mx + b + A = self.geofence[i] + B = self.geofence[i + 1] + + m = (B[1] - A[1]) / (B[0] - A[0]) + + # Handling the case of delta_lon == 0 + if abs(m) == float('inf'): + m = (B[1] - A[1]) / (B[0] - A[0] + 0.00000001) + + b = A[0] - m * A[0] + + x = (lat - b) / m + + if x > lon and lat > min(A[1], B[1]) and lat < max(A[1], B[1]): + hitcount += 1 + return bool(hitcount & 1) + + def enable_precision_land(self, Navigator): + + # NOTE: CHANGE THE CAMERA TYPE DURING ACTUAL USE + camera = imaging.camera.DebugCamera("./res/test-image.jpeg") + + analysis = imaging.analysis.ImageAnalysisDetector(camera = camera, nav = Navigator) + + analysis.subscribe(self._precision_land) + analysis.run() + + def _precision_land(self, im, lon, lat, x, y): + + # Append new values for position to the buffer and compute the moving average, taking the new values into account. + # Adjust buffer size depending on the refresh rate of the imaging script + + buffer_size = 5 + + self.__buffer[0].append(x) + self.__buffer[1].append(y) + + x = sum(self.__buffer[0]) / len(self._buffer[0]) + y = sum(self.__buffer[1] / len(self.__buffer[1])) + + if len(self.__buffer[0]) >= buffer_size and len(self.__buffer[1]) >= buffer_size: + type_mask = self.nav.generate_typemask([0, 1, 2]) + + self.nav.set_postion_target_local_NED(x = self.__buffer[0][-1], y = self.__buffer[1][-1], z = -(self.i), type_mask = type_mask) + self.i -= 1 + + # Maintain the size of the buffer + self.__buffer[0].pop(0) + self.__buffer[1].pop(0) + + +def main(): + LandingSpotFinder1 = Lander() + Navigator1 = Navigator() + + LandingSpotFinder1.generateRoute() # Call the method to generate the route + + for i in LandingSpotFinder1.route: + # add code to break the loop when landing spot is found + LandingSpotFinder1.goNext(Navigator1, i) + time.sleep(5) + ''' + if i == 0: + self.nav.set_heading(0) + time.sleep(5) + elif ((i - 1) % 5 == 0) and i != 1: + self.nav.set_heading_relative(90) + else: + self.nav.set_heading_relative(0) + + ''' + + +if __name__ == "__main__": + main() + + + +from typing import Callable, Optional, List, Any, Tuple + +import threading +# from multiprocessing import Process +from .detector import BaseDetector, BoundingBox +from .camera import CameraProvider +from .debug import ImageAnalysisDebugger +from ..georeference.inference_georeference import get_object_location +from .location import LocationProvider +from ..autopilot.navigator import Navigator +from PIL import Image + + +class CameraAttributes: + def __init__(self): + self.focal_length = 0.0034 + self.angle = 0 # in radians + self.resolution = (1920, 1080) + + +class Inference: + def __init__(self, bounding_box: BoundingBox, relative_alt): + camera_attributes = CameraAttributes() + position = bounding_box.position + size = bounding_box.size + self.x = (position.x + size.x / 2) / camera_attributes.resolution[0] + self.y = (position.y + size.y / 2) / camera_attributes.resolution[1] + self.relative_alt = relative_alt + + +class ImageAnalysisDelegate: + """ + Implements an imaging inference loops and provides several methods which + can be used to query the latest image analysis results. + + Responsible for capturing pictures regularly, detecting any landing pads in + those pictures and then providing the most recent estimate of the landing + pad location from the camera's perspective. + + Pass an `ImageAnalysisDebugger` when constructing to see a window with live + results. + + TODO: geolocate the landing pad using the drone's location. + """ + + def __init__(self, + detector: BaseDetector, + camera: CameraProvider, + location_provider: LocationProvider = None, + navigation_provider: Navigator = None, + debugger: Optional[ImageAnalysisDebugger] = None): + self.detector = detector + self.camera = camera + self.debugger = debugger + + if location_provider is None and navigation_provider is None: + raise ValueError("Either location_provider or navigation_provider must be provided.") + + self.location_provider = location_provider + self.navigation_provider = navigation_provider + + self.subscribers: List[Callable[[Image.Image, Optional[BoundingBox], Optional[Tuple[float, float]]], Any]] = [] + self.camera_attributes = CameraAttributes() + self.thread = None + self.loop = True + + def get_inference(self, bounding_box: BoundingBox) -> Inference: + if self.location_provider is not None: + altitude = self.location_provider.altitude() + elif self.navigation_provider is not None: + altitude = -1 * self.navigation_provider.get_local_position_ned()[2] + else: + raise ValueError("No altitude information provider available.") + + inference = Inference(bounding_box, altitude) + return inference + + def start(self): + """ + Will start the image analysis process in another thread. + """ + self.loop = True + self.thread = threading.Thread(target=self._analysis_loop) + # process = Process(target=self._analysis_loop) + self.thread.start() + # process.start() + # Use `threading` to start `self._analysis_loop` in another thread. + + def stop(self): + self.loop = False + self.thread.join() + + def _analyze_image(self): + """ + Actually performs the image analysis once. Only useful for testing, + should otherwise we run by `start()` which then starts + `_analysis_loop()` in another thread. + """ + im = self.camera.capture() + bounding_box = self.detector.predict(im) + + if self.debugger is not None: + self.debugger.set_image(im) + if bounding_box is not None: + self.debugger.set_bounding_box(bounding_box) + + for subscriber in self.subscribers: + if bounding_box: + inference = self.get_inference(bounding_box) + if inference: + x, y = get_object_location(self.camera_attributes, + inference) + subscriber(im, bounding_box, (x, y)) + else: + subscriber(im, None, None) + + def _analysis_loop(self): + """ + Indefinitely run image analysis. This should be run in another thread; + use `start()` to do so. + """ + while self.loop: + self._analyze_image() + + def subscribe(self, callback: Callable): + """ + Subscribe to image analysis updates. For example: + + def myhandler(image: Image.Image, bounding_box: BoundingBox): + if bounding_box is None: + print("No bounding box detected") + else: + print("Bounding box detected") + + imaging_process.subscribe(myhandler) + """ + self.subscribers.append(callback) + + + +from src.modules.imaging.bucket_detector import BucketDetector +import time +from src.modules.imaging.analysis import ImageAnalysisDelegate +from src.modules.imaging.camera import DebugCamera +from src.modules.imaging.mavlink import MAVLinkDelegate +from pymavlink import mavutil + +from src.modules.autopilot import navigator +from dronekit import connect, VehicleMode +import os +import RPi.GPIO as GPIO + + +CONN_STR = "udp:127.0.0.1:14551" +MESSENGER_PORT = 14552 +mavlink_str = "udp:127.0.0.1:14553" +GPIO_PIN = 23 + +drone = connect(CONN_STR, wait_ready=False) + +nav = navigator.Navigator(drone, MESSENGER_PORT) +mavlink = MAVLinkDelegate(conn_str = mavlink_str) + + +GPIO.setmode(GPIO.BCM) +GPIO.setup(GPIO_PIN, GPIO.OUT) +GPIO.output(23, GPIO.HIGH) + +time.sleep(2) + +GPIO.output(23, GPIO.LOW) + + +bucket_avg = [[], []] +big_bucket_height = 1 + +os.makedirs("tmp/log", exist_ok=True) +dirs = os.listdir("tmp/log") +ft_num = len(dirs) +os.makedirs(f"tmp/log/{ft_num}") + + +def moving_bucket_avg(_, pos): + + if pos: + if len(bucket_avg[0]) > 0 and len(bucket_avg[1]) > 0: + + num_x = len(bucket_avg[0]) + num_y = len(bucket_avg[1]) + + avg_x = sum(bucket_avg[0]) / num_x + avg_y = sum(bucket_avg[1]) / num_y + + new_x_avg = avg_x + pos[0] / (num_x + 1) + new_y_avg = avg_y + pos[1] / (num_y + 1) + + bucket_avg[0].append(new_x_avg) + bucket_avg[1].append(new_y_avg) + else: + bucket_avg[0].append(pos[0]) + bucket_avg[1].append(pos[1]) + + time.sleep(1) + + +camera = DebugCamera("photos/375.png") + +model_file = "best.pt" + +detector = BucketDetector(f"samples/models/{model_file}") + +analysis = ImageAnalysisDelegate(detector, camera, navigation_provider = nav) +analysis.subscribe(moving_bucket_avg) + +nav.send_status_message("Shepard is online") + +while not (drone.armed and drone.mode == VehicleMode("GUIDED")): + pass + +time.sleep(5) + +nav.takeoff(30) + +type_mask = nav.generate_typemask([0, 1, 2]) + +nav.send_status_message("Executing") +current_alt = nav.get_local_position_ned()[2] + +delta = 0.5 +sleep_time = 2 + + +def bucket_descent(target_height): + # VERIFY ALL OF THESE COORINDATE SYSTEMS BEFORE FLYING!!!!!!!!!!!!!!!!!!!!!!!!!! + + coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED + + nav.set_position_target_local_ned(x = bucket_avg[0][-1], + y = bucket_avg[1][-1], + z = 0, + type_mask = type_mask, + coordinate_frame = coordinate_frame) + + time.sleep(5) + alt = nav.get_local_position_ned()[2] + i = 0 + while -(alt) >= 11: + print(i) + + alt = nav.get_local_position_ned()[2] + + print(alt) + nav.set_position_target_local_ned(x = float(bucket_avg[0][-1] - bucket_avg[0][0]), + y = float(bucket_avg[1][-1] - bucket_avg[1][0]), + z = 5, + type_mask = type_mask, + coordinate_frame = coordinate_frame) + i += 1 + time.sleep(5) + + # Full Commit, drone is set to hover at the target height + # NOTE: COORDINATE SYSTEM HAS CHANGED. IT IS IN NED, DOWN IS POSITIVE + + nav.send_status_message("Comitting to bucket") + coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_NED + current_local_pos = nav.get_local_position_ned() + + nav.set_position_target_local_ned(x = current_local_pos[0], + y = current_local_pos[1], + z = -target_height, + type_mask = type_mask, + coordinate_frame = coordinate_frame) + + +def ActivatePump(duration): + coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_NED + current_local_pos = nav.get_local_position_ned() + # Runs the pump that is connected to GPIO 23 for a specified amount of time + + nav.set_position_target_local_ned(x = current_local_pos[0], + y = current_local_pos[1], + z = current_local_pos[2], + type_mask = type_mask, + coordinate_frame = coordinate_frame) + + GPIO.output(GPIO_PIN, GPIO.HIGH) + + # Make sure that the drone maintains its location above the bucket, hopefully avoiding drift due to wind or other sources + # Adjusts the drones position every half second, and the duration is equivalent to the duration passed to ActivatePump() + for _ in range(duration - 1): + + nav.set_position_target_local_ned(x = current_local_pos[0], + y = current_local_pos[1], + z = current_local_pos[2], + type_mask = type_mask, + coordinate_frame = coordinate_frame) + time.sleep(0.5) + + nav.set_position_target_local_ned(x = current_local_pos[0], + y = current_local_pos[1], + z = current_local_pos[2], + type_mask = type_mask, + coordinate_frame = coordinate_frame) + time.sleep(0.5) + + GPIO.output(GPIO_PIN, GPIO.LOW) + + +nav.send_status_message("Descending to bucket") +analysis.start() +time.sleep(5) +if len(bucket_avg[0]) > 0 and len(bucket_avg[1]) > 0: + bucket_descent(big_bucket_height) +else: + time.sleep(5) + bucket_descent(big_bucket_height) +analysis.stop() +nav.send_status_message("Stopped the analysis") + +nav.send_status_message("Activating Pump") + +ActivatePump(5) + +nav.return_to_launch() + +drone.close() + +nav.send_status_message("Flight test script execution terminated") + + + +import threading +import queue + +import asyncio +from typing import Callable, List + +from aiohttp import web +from aiohttp import web +import aiohttp + +import json + + +class Emu(): + """ + class representation of a connection to Emu + """ + def __init__(self, img_dir: str): + self.img_dir = img_dir + + self._send_queue = queue.Queue() + self._recv_queue = queue.Queue() + + # All messages in ._recv_queue are advertised to subscribers + self._subscribers: List[Callable] = [] + + # set default onConnect to be a no-op + self._on_connect = lambda: None + self._is_connected = False + + def start_comms(self): + self._comms_thread = threading.Thread(target=self._start_comms_loop, daemon=True) + self._comms_thread.start() + + def send_image(self, path: str): + """ + sends the image at specified path to Emu + the path sent should be accessable from within self.img_dir so it can be accessed through + /images/{filename} + """ + print(path) + img_url = "/images/" + path + print(img_url) + content = { + "type": "img", + "value": img_url + } + self._send_queue.put(json.dumps(content)) + + def send_log(self, message: str, severity: str="normal"): + """ + sends a log message to Emu + message: string of flog + severity: "normal", "warning", "error" + """ + content = { + "type": "log", + "message": message, + "severity": severity + } + self._send_queue.put(json.dumps(content)) + + def send_msg(self, message: str): + """ + sends message as it is, follow the proper JSON API messages + """ + self._send_queue.put(message) + + def set_on_connect(self, func: Callable): + self._on_connect = func + + def _start_comms_loop(self): + """ + starts connection loop with asyncio + """ + print("start_comms loop") + self.app = web.Application() + self.app.add_routes([web.static('/images', self.img_dir), + web.get('/ws', self.handle_websocket)]) + + web.run_app(self.app, handle_signals=False) + + def subscribe(self, subscriber: Callable): + self._subscribers.append(subscriber) + + async def producer_handler(self, ws): + """ + handles sending messages to the client + """ + event_loop = asyncio.get_running_loop() + while not ws.closed: + message = await asyncio.to_thread(self._send_queue.get) + + await ws.send_str(message) + + async def consumer_handler(self, ws): + async for msg in ws: + if msg.type == aiohttp.WSMsgType.TEXT: + for subscriber in self._subscribers: + subscriber(msg.data) + + elif msg.type == aiohttp.WSMsgType.ERROR: + print("WebSocket error:", ws.exception()) + + async def handle_websocket(self, request): + ws = web.WebSocketResponse() + await ws.prepare(request) + + self._is_connected = True + self._on_connect() + + consumer_task = asyncio.create_task(self.consumer_handler(ws)) + producer_task = asyncio.create_task(self.producer_handler(ws)) + + _, pending = await asyncio.wait( + [producer_task, consumer_task], + return_when=asyncio.FIRST_COMPLETED, + ) + + for task in pending: + task.cancel() + + print('websocket connection closed') + self._is_connected = False + + return ws + + + diff --git a/samples/aruco_stream.py b/samples/aruco_stream.py new file mode 100644 index 0000000..90778e1 --- /dev/null +++ b/samples/aruco_stream.py @@ -0,0 +1,49 @@ +import time + +from src.modules.emu import Emu +from src.modules.imaging.detector import ArucoDetector +from src.modules.imaging.camera import DebugCamera +from src.modules.imaging.location import DebugLocationProvider +from src.modules.imaging.analysis import ImageAnalysisDelegate +from src.modules.imaging.aruco_stream import ArucoEmuStreamer +from src.modules.imaging.video_emu_stream import SharedFrameCamera, VideoEmuStreamer + + +def main(): + emu = Emu("tmp") + emu.start_comms() + time.sleep(1) + + # Base camera — swap DebugCamera for RPiCamera/OakdCamera on real hardware + base_camera = DebugCamera("res/test-image.jpeg") + + # SharedFrameCamera captures at 15fps; both video stream and analysis read from it + shared_cam = SharedFrameCamera(base_camera, fps=15) + shared_cam.start() + + # Video stream → EMU /video endpoint (browser: ) + video_streamer = VideoEmuStreamer(emu, shared_cam, fps=15, quality=70) + video_streamer.start() + + # ArUco detection pipeline reads latest frame from shared camera + detector = ArucoDetector() + location_provider = DebugLocationProvider() + + analysis = ImageAnalysisDelegate(detector, shared_cam, location_provider) + aruco_streamer = ArucoEmuStreamer(emu, "tmp") + analysis.subscribe(aruco_streamer.on_detection) + analysis.start() + + try: + while True: + time.sleep(1) + except KeyboardInterrupt: + pass + finally: + analysis.stop() + video_streamer.stop() + shared_cam.stop() + + +if __name__ == "__main__": + main() diff --git a/src/modules/emu/emu.py b/src/modules/emu/emu.py index b808e72..6531a8d 100644 --- a/src/modules/emu/emu.py +++ b/src/modules/emu/emu.py @@ -2,9 +2,8 @@ import queue import asyncio -from typing import Callable, List +from typing import Callable, List, Optional -from aiohttp import web from aiohttp import web import aiohttp @@ -28,6 +27,9 @@ def __init__(self, img_dir: str): self._on_connect = lambda: None self._is_connected = False + self._latest_video_frame: Optional[bytes] = None + self._video_lock = threading.Lock() + def start_comms(self): self._comms_thread = threading.Thread(target=self._start_comms_loop, daemon=True) self._comms_thread.start() @@ -38,9 +40,7 @@ def send_image(self, path: str): the path sent should be accessable from within self.img_dir so it can be accessed through /images/{filename} """ - print(path) img_url = "/images/" + path - print(img_url) content = { "type": "img", "value": img_url @@ -66,6 +66,14 @@ def send_msg(self, message: str): """ self._send_queue.put(message) + def send_video_frame(self, jpeg_bytes: bytes): + """ + Update the latest video frame served at /video (MJPEG stream). + Call this from a background thread with raw JPEG bytes. + """ + with self._video_lock: + self._latest_video_frame = jpeg_bytes + def set_on_connect(self, func: Callable): self._on_connect = func @@ -75,8 +83,11 @@ def _start_comms_loop(self): """ print("start_comms loop") self.app = web.Application() - self.app.add_routes([web.static('/images', self.img_dir), - web.get('/ws', self.handle_websocket)]) + self.app.add_routes([ + web.static('/images', self.img_dir), + web.get('/ws', self.handle_websocket), + web.get('/video', self.handle_video_stream), + ]) web.run_app(self.app, handle_signals=False) @@ -102,6 +113,36 @@ async def consumer_handler(self, ws): elif msg.type == aiohttp.WSMsgType.ERROR: print("WebSocket error:", ws.exception()) + async def handle_video_stream(self, request): + """ + MJPEG stream endpoint. Connect with: + No frontend JS needed — the browser handles multipart natively. + """ + response = web.StreamResponse(headers={ + 'Content-Type': 'multipart/x-mixed-replace; boundary=frame', + 'Cache-Control': 'no-cache', + }) + await response.prepare(request) + + try: + while True: + with self._video_lock: + frame = self._latest_video_frame + + if frame is not None: + await response.write( + b'--frame\r\n' + b'Content-Type: image/jpeg\r\n\r\n' + + frame + + b'\r\n' + ) + + await asyncio.sleep(1 / 15) + except (ConnectionResetError, asyncio.CancelledError): + pass + + return response + async def handle_websocket(self, request): ws = web.WebSocketResponse() await ws.prepare(request) diff --git a/src/modules/imaging/.aruco_stream.py.swp b/src/modules/imaging/.aruco_stream.py.swp new file mode 100644 index 0000000000000000000000000000000000000000..c618dce061bcd398e7cc6f7a5110e23255e57e25 GIT binary patch literal 12288 zcmeI2zi-n(6vwYDAfSL?N7pbAMC!C4A&At0szp?kAEJN|r84LEl3uZW#y*z>gb*7m z8xjlu00SNPCm0bD0}|pNK}mH#LbAp%eH2=#VCe01?<3fy{I!&yL?b zeQJ8jE;v@3;71PM+}T>PA_7E!2oM1xKm>>Y5g-CY;GZC1;xYCVC)rh;W}@iFe(Q@@ z>WBalAOb{y2oM1xKm>>Y5g-CYfCvzQzmS0N8T+}Hv6uT0JpTVLfB%0u$k<0{19}U+ zfbK%e&>d(N+6{d_z}S0e19}TRgKk4h(8qDc-a&7m=a2^-g+A|R>=hJ3_nDGa6K5@!-P&+_8+GNyPsBqfk!>TQZXoNP*WlPIP?4OeCDN~r;xG? z7ld6;_ncU#$|xNWP?I{lqH=xytu*Qf^Uc{pXgve7UE*ls`3Z?H0W+uK9K z%YseToCRP%>M9(?syY=Zqq@c1=%s3eZx3ei znGaiHuB#$tT(_WwsTQ9Pv%EHo??H>>Vmg-9Oc>X2+;$++l%Ka()?vAnUs{(50%e^< zhaC6Yo#KAGV>5;Fbp}b%39=FE#PCg|w=O~qr5*eiUQIuSqjJ1VS%c>(VTD_r4!m7k OSS&4zjOU%LJ@yNXTA^+L literal 0 HcmV?d00001 diff --git a/src/modules/imaging/analysis.py b/src/modules/imaging/analysis.py index 94ad15a..1c90d54 100644 --- a/src/modules/imaging/analysis.py +++ b/src/modules/imaging/analysis.py @@ -1,4 +1,4 @@ -from typing import Callable, Optional, List, Callable, Any +from typing import Callable, Optional, List, Any, Tuple import threading # from multiprocessing import Process @@ -59,7 +59,7 @@ def __init__(self, self.location_provider = location_provider self.navigation_provider = navigation_provider - self.subscribers: List[Callable[[Image.Image, float, float], Any]] = [] + self.subscribers: List[Callable[[Image.Image, Optional[BoundingBox], Optional[Tuple[float, float]]], Any]] = [] self.camera_attributes = CameraAttributes() self.thread = None self.loop = True @@ -110,9 +110,9 @@ def _analyze_image(self): if inference: x, y = get_object_location(self.camera_attributes, inference) - subscriber(im, (x, y)) + subscriber(im, bounding_box, (x, y)) else: - subscriber(im, None) + subscriber(im, None, None) def _analysis_loop(self): """ diff --git a/src/modules/imaging/aruco_stream.py b/src/modules/imaging/aruco_stream.py new file mode 100644 index 0000000..45daed0 --- /dev/null +++ b/src/modules/imaging/aruco_stream.py @@ -0,0 +1,33 @@ +from PIL import Image, ImageDraw +import os +from typing import Optional, Tuple + +from src.modules.emu import Emu +from src.modules.imaging.detector import BoundingBox + + +class ArucoEmuStreamer: + def __init__(self, emu: Emu, output_dir: str = "tmp"): + self.emu = emu + self.output_dir = output_dir + self.counter = 0 + + if not os.path.exists(self.output_dir): + os.makedirs(self.output_dir) + + def on_detection(self, image: Image.Image, bounding_box: Optional[BoundingBox], position: Optional[Tuple[float, float]]): + im = image.copy() + + if bounding_box is not None: + draw = ImageDraw.Draw(im) + bb = (bounding_box.position.x, bounding_box.position.y, + bounding_box.position.x + bounding_box.size.x, + bounding_box.position.y + bounding_box.size.y) + draw.rectangle(bb, outline="red") + + filename = f"{self.counter}.jpeg" + filepath = os.path.join(self.output_dir, filename) + im.save(filepath) + + self.emu.send_image(filename) + self.counter += 1 diff --git a/src/modules/imaging/detector.py b/src/modules/imaging/detector.py index 65dd8fc..0a7b381 100644 --- a/src/modules/imaging/detector.py +++ b/src/modules/imaging/detector.py @@ -1,5 +1,7 @@ -from functools import lru_cache +from functools import lru_cache, cached_property from typing import Optional +from dataclasses import dataclass +import math from PIL import Image import numpy as np @@ -56,35 +58,35 @@ def max(v1: 'Vec2', v2: 'Vec2') -> 'Vec2': class BoundingBox: -def __init__(self, position: Vec2, size: Vec2): - self.position = position - self.size = size + def __init__(self, position: Vec2, size: Vec2): + self.position = position + self.size = size -@lru_cache(maxsize=2) -def intersection(self, other: 'BoundingBox') -> float: - top_left = Vec2.max(self.position, other.position) - bottom_right = Vec2.min(self.position + self.size, - other.position + other.size) + @lru_cache(maxsize=2) + def intersection(self, other: 'BoundingBox') -> float: + top_left = Vec2.max(self.position, other.position) + bottom_right = Vec2.min(self.position + self.size, + other.position + other.size) - size = bottom_right - top_left + size = bottom_right - top_left - intersection = size.x * size.y - return max(intersection, 0) + intersection = size.x * size.y + return max(intersection, 0) -def union(self, other: 'BoundingBox') -> float: - intersection = self.intersection(other) - if intersection == 0: - return 0 + def union(self, other: 'BoundingBox') -> float: + intersection = self.intersection(other) + if intersection == 0: + return 0 - union = self.size.x * self.size.y + other.size.x * other.size.y - intersection - return union + union = self.size.x * self.size.y + other.size.x * other.size.y - intersection + return union -def intersection_over_union(self, pred: 'BoundingBox') -> Optional[float]: - intersection = self.intersection(pred) - if intersection == 0: - return 0 - iou = intersection / self.union(pred) - return iou + def intersection_over_union(self, pred: 'BoundingBox') -> Optional[float]: + intersection = self.intersection(pred) + if intersection == 0: + return 0 + iou = intersection / self.union(pred) + return iou class BaseDetector: @@ -115,16 +117,17 @@ def predict(self, image: Image.Image) -> Optional[BoundingBox]: return BoundingBox(Vec2(x, y), Vec2(w, h)) -class ArucoDetector(): +class ArucoDetector(BaseDetector): def predict(self, image: Image.Image) -> Optional[BoundingBox]: img = cv2.cvtColor(np.array(image), cv2.COLOR_RGB2BGR) aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) - params = cv2.aruco.DetectorParemeters() + params = cv2.aruco.DetectorParameters() - corners, ids, rejected = cv2.aruco.detectMarkers(img, aruco_dict, parameters=params) + detector = cv2.aruco.ArucoDetector(aruco_dict, params) + corners, ids, rejected = detector.detectMarkers(img) if ids: for c in zip(corners, ids): diff --git a/src/modules/imaging/video_emu_stream.py b/src/modules/imaging/video_emu_stream.py new file mode 100644 index 0000000..20aac33 --- /dev/null +++ b/src/modules/imaging/video_emu_stream.py @@ -0,0 +1,88 @@ +import io +import threading +import time + +from PIL import Image + +from src.modules.emu import Emu +from src.modules.imaging.camera import CameraProvider + + +class SharedFrameCamera(CameraProvider): + """ + Wraps a CameraProvider and shares the latest captured frame across + multiple consumers (e.g. video streamer + analysis pipeline) without + both threads calling camera.capture() simultaneously. + """ + + def __init__(self, camera: CameraProvider, fps: int = 15): + self._camera = camera + self._fps = fps + self._latest: Image.Image | None = None + self._lock = threading.Lock() + self._running = False + self._thread: threading.Thread | None = None + + def start(self): + self._running = True + self._thread = threading.Thread(target=self._capture_loop, daemon=True) + self._thread.start() + + def stop(self): + self._running = False + if self._thread: + self._thread.join() + + def get_latest(self) -> Image.Image | None: + with self._lock: + return self._latest + + def capture(self) -> Image.Image: + while True: + with self._lock: + if self._latest is not None: + return self._latest + time.sleep(0.01) + + def _capture_loop(self): + interval = 1 / self._fps + while self._running: + frame = self._camera.capture() + with self._lock: + self._latest = frame + time.sleep(interval) + + +class VideoEmuStreamer: + """ + Continuously grabs frames from a SharedFrameCamera and pushes them + to EMU's MJPEG /video endpoint at the given fps. + """ + + def __init__(self, emu: Emu, shared_cam: SharedFrameCamera, fps: int = 15, quality: int = 70): + self.emu = emu + self.shared_cam = shared_cam + self.fps = fps + self.quality = quality + self._running = False + self._thread: threading.Thread | None = None + + def start(self): + self._running = True + self._thread = threading.Thread(target=self._stream_loop, daemon=True) + self._thread.start() + + def stop(self): + self._running = False + if self._thread: + self._thread.join() + + def _stream_loop(self): + interval = 1 / self.fps + while self._running: + frame = self.shared_cam.get_latest() + if frame is not None: + buf = io.BytesIO() + frame.save(buf, format="JPEG", quality=self.quality) + self.emu.send_video_frame(buf.getvalue()) + time.sleep(interval) diff --git a/test/test_analysis.py b/test/test_analysis.py index 080f1b8..42245dd 100644 --- a/test/test_analysis.py +++ b/test/test_analysis.py @@ -37,9 +37,12 @@ def test_analysis_subscriber(): global detected detected = None - def _callback(_image, lon, lat): + def _callback(_image, bounding_box, pos): global detected - detected = Vec2(lon, lat) + if pos is not None: + detected = Vec2(pos[0], pos[1]) + else: + detected = None analysis.subscribe(_callback) @@ -87,7 +90,7 @@ def test_analysis_debugger(): location_provider = DebugLocationProvider() location_provider.set_altitude(1.0) analysis = ImageAnalysisDelegate(detector, camera, location_provider, - debug) + debugger=debug) def run_analysis(): detector.bounding_box = BoundingBox(Vec2(0, 0), Vec2(100, 100)) From 8d66b611adc4b3199d51d9b70a7779eb94295045 Mon Sep 17 00:00:00 2001 From: Samardeep Singh Date: Sat, 25 Apr 2026 13:47:50 -0600 Subject: [PATCH 2/2] deleted and updated the unneccessary files --- repomix-output.xml | 6149 -------------------------------------------- 1 file changed, 6149 deletions(-) delete mode 100644 repomix-output.xml diff --git a/repomix-output.xml b/repomix-output.xml deleted file mode 100644 index bc32bc0..0000000 --- a/repomix-output.xml +++ /dev/null @@ -1,6149 +0,0 @@ -This file is a merged representation of the entire codebase, combined into a single document by Repomix. - - -This section contains a summary of this file. - - -This file contains a packed representation of the entire repository's contents. -It is designed to be easily consumable by AI systems for analysis, code review, -or other automated processes. - - - -The content is organized as follows: -1. This summary section -2. Repository information -3. Directory structure -4. Repository files (if enabled) -5. Multiple file entries, each consisting of: - - File path as an attribute - - Full contents of the file - - - -- This file should be treated as read-only. Any changes should be made to the - original repository files, not this packed version. -- When processing this file, use the file path to distinguish - between different files in the repository. -- Be aware that this file may contain sensitive information. Handle it with - the same level of security as you would the original repository. - - - -- Some files may have been excluded based on .gitignore rules and Repomix's configuration -- Binary files are not included in this packed representation. Please refer to the Repository Structure section for a complete list of file paths, including binary files -- Files matching patterns in .gitignore are excluded -- Files matching default ignore patterns are excluded -- Files are sorted by Git change count (files with more changes are at the bottom) - - - - - -.github/workflows/lint.yml -.github/workflows/tests.yml -.github/workflows/type.yml -.gitignore -.gitmodules -bin/run-nav.sh -dep/README.md -doc/README.md -install/install.sh -install/mavlink-router.sh -install/setup-ssh-keys.py -install/setup-ssh-keys.sh -install/shepard.service -install/shepard.sh -README.md -requirements.txt -res/README.md -res/test-image.jpeg -samples/analysis.py -samples/aruco_stream.py -samples/battery_status.py -samples/bucket_test_simple.py -samples/bucket_test.py -samples/camera.py -samples/emu_connection.py -samples/geofence_test.py -samples/geofence.json -samples/image_process.py -samples/img_debug.py -samples/img_test.py -samples/kml_mock.py -samples/models/best.pt -samples/models/n416.pt -samples/models/n512.pt -samples/models/n640.pt -samples/models/s14.pt -samples/models/s512.pt -samples/models/s640.pt -samples/oakd.py -samples/README.md -samples/run.sh -samples/test_nn.py -samples/xm125_polling.py -samples/xm125_tui.py -scripts/fmt.sh -scripts/lint.sh -scripts/test.sh -scripts/type.sh -src/__init__.py -src/flight_tests/altimeter_forwarding_2025_02_27.py -src/flight_tests/altimeter_log_2025_03_01.py -src/flight_tests/balloon_search_2025_03_08.py -src/flight_tests/ft_camera_log.py -src/flight_tests/geofence/geofence.json -src/flight_tests/ir.py -src/flight_tests/task2_2025_ir.py -src/flight_tests/task2_2025.py -src/modules/__init__.py -src/modules/autopilot/__init__.py -src/modules/autopilot/altimeter_mavlink.py -src/modules/autopilot/altimeter_xm125.py -src/modules/autopilot/altimeter.py -src/modules/autopilot/lander.py -src/modules/autopilot/messenger.py -src/modules/autopilot/navigator.py -src/modules/autopilot/simulator.py -src/modules/emu/__init__.py -src/modules/emu/emu.py -src/modules/georeference/__init__.py -src/modules/georeference/inference_georeference.py -src/modules/imaging/__init__.py -src/modules/imaging/analysis_view.py -src/modules/imaging/analysis.py -src/modules/imaging/aruco_stream.py -src/modules/imaging/battery.py -src/modules/imaging/bucket_detector.py -src/modules/imaging/camera.py -src/modules/imaging/debug_analysis.py -src/modules/imaging/debug.py -src/modules/imaging/detector.py -src/modules/imaging/LED.py -src/modules/imaging/location.py -src/modules/imaging/mavlink.py -src/modules/imaging/video_stream.py -src/modules/landingspot/landingspot.py -src/modules/README.md -src/README.md -src/sim_scripts/gazebo_camera_test.py -system/boot/config.txt -test/README.md -test/test_analysis.py -test/test_battery.py -test/test_camera.py -test/test_location.py -tools/README.md - - - -This section contains the contents of the repository's files. - - -name: Lint - -on: [push] - -jobs: - build: - runs-on: ubuntu-latest - strategy: - matrix: - python-version: ["3.10"] - steps: - - uses: actions/checkout@v4 - - name: Set up Python ${{ matrix.python-version }} - uses: actions/setup-python@v3 - with: - python-version: ${{ matrix.python-version }} - - uses: actions/setup-python@v4 - with: - python-version: '3.10' - cache: 'pip' - - name: Install dependencies - run: pip install -r requirements.txt - - name: Run linter - run: ./scripts/lint.sh - - - -name: Tests - -on: - push: - branches: [ "main" ] - pull_request: - branches: [ "main" ] - -permissions: - contents: read - -jobs: - build: - - runs-on: ubuntu-latest - - steps: - - uses: actions/checkout@v4 - with: - submodules: 'recursive' - - name: Set up Python 3.10 - uses: actions/setup-python@v3 - with: - python-version: "3.10" - - uses: actions/setup-python@v4 - with: - python-version: '3.10' - cache: 'pip' - - name: Install dependencies - run: pip install -r requirements.txt - - name: Run tests - run: ./scripts/test.sh - - - -name: Typecheck - -on: [push] - -jobs: - build: - runs-on: ubuntu-latest - strategy: - matrix: - python-version: ["3.10"] - steps: - - uses: actions/checkout@v4 - - name: Set up Python ${{ matrix.python-version }} - uses: actions/setup-python@v3 - with: - python-version: ${{ matrix.python-version }} - - uses: actions/setup-python@v4 - with: - python-version: '3.10' - cache: 'pip' - - name: Install dependencies - run: pip install -r requirements.txt - - name: Run mypy - run: ./scripts/type.sh - - - -# These are some examples of commonly ignored file patterns. -# You should customize this list as applicable to your project. -# Learn more about .gitignore: -# https://www.atlassian.com/git/tutorials/saving-changes/gitignore - -# Node artifact files -node_modules/ -dist/ - -# Compiled Java class files -*.class - -# Compiled Python bytecode -*.py[cod] - -# Log files -*.log - -# Package files -*.jar - -# Python Virtual Enviroments -venv - -# Maven -target/ -dist/ - -# JetBrains IDE -.idea/ - -# Unit test reports -TEST*.xml - -# Generated by MacOS -.DS_Store - -# Generated by Windows -Thumbs.db - -# Applications -*.app -*.exe -*.war - -# Large media files -*.mp4 -*.tiff -*.avi -*.flv -*.mov -*.wmv - -.python-version - -# Credentials -credentials.json -id_rsa - - -mavlink-router/ -log - - - -# Dependencies - -This directory contains the dependencies required by Shepard. - - - -# Documentation - -This directory contains files related to documentation. - - - -#!/usr/bin/bash - -set -o errexit - -sudo apt update -sudo apt upgrade - -sudo apt install python3-pyqrcode - -echo "Setting up ssh keys" -./install/setup-ssh-keys.sh - -echo "Installing mavlink router" -./install/mavlink-router.sh - -echo "Installing shepard" -./install/shepard.sh - - - -#!/usr/bin/env bash - -set -o errexit -set -x - -echo "Installing dependencies" -sudo apt install -y git meson ninja-build pkg-config gcc g++ systemd - -echo "Building mavlink-router" -if test -d mavlink-router; then - cd mavlink-router - git pull - git submodule update --init --recursive -else - git clone --recursive https://github.com/mavlink-router/mavlink-router - cd mavlink-router/ -fi -meson setup build . -ninja -C build - -echo "Installing mavlink-router" -sudo ninja -C build install -cd .. - -echo "Setting up system files" -sudo cp -r system/etc/mavlink-router /etc -sudo cp system/boot/config.txt /boot/config.txt - -sudo systemctl enable mavlink-router.service - -echo "Mavlink router install complete" -echo "Please restart with 'sudo reboot' so the changes can take effect" - - - -import pyqrcode - -with open("/home/uaarg/.ssh/id_rsa.pub", "r") as f: - key = f.read() - qr = pyqrcode.create(key) - print(qr.terminal()) - - - -#!/usr/bin/env bash - -ssh-keygen -python3 install/setup-ssh-keys.py - -echo "Add that key to github" -echo "Press enter when done" -read - - - -[Unit] -Description=Shepard -After=login.target - -[Service] -ExecStart=/home/uaarg/Documents/shepard/bin/run-nav.sh -WorkingDirectory=/home/uaarg/Documents/shepard - -[Install] -WantedBy=multi-user.target - - - -#!/usr/bin/bash - -set -o errexit -set -x - -echo "Installing system dependencies" -sudo apt install python3 python3-pip python3-venv - -echo "Installing submodules" -git submodule update --init --recursive - -echo "Installing pip dependencies" -python3 -m venv venv -source venv/bin/activate -pip install -r requirements.txt - -echo "Running tests (sanity check)" -./scripts/test.sh - -echo "Installing shepard.service" -sudo ln install/shepard.service /etc/systemd/system -sudo systemctl enable mavlink-router.service - -echo "Installation complete. You will have to reboot for the installation to take effect" - - - -# UAARG Onboard Script # - -This repository holds the scripts for: - -* Recording and logging images, positional information and messages -* Making pathing decisions based on current data and mission objectives -* Making inferences on images for object identification and location -* Communications between PixHawk controller board - -### How do I get set up? ### - -This script is designed to run on a system with a USB or UART connection to a PixHawk Board, and a camera system. Some functions may not work if the above requirements are not met, but our systems should be designed in a robust way to allow most functions to still work. - -## For X86 or arm64 Systems (ie Desktop + Laptop Computers + RPi) - -* Install Python3 -* Optional Step: It's highly recommended, you create a virtual env before installing dependencies. Activate the virtual environment and proceed. OS specific steps are available in the docs [https://packaging.python.org/guides/installing-using-pip-and-virtual-environments/] -* Initialize all submodules ```git submodule update --init --recursive``` -* Install the required dependancies via ```pip install -r requirements.txt``` -* You should now be able to run testing python scripts and launch the application - -## For armv7l Systems (ie ODroid XU4) - -* Install Python3 -* Initialize all submodules ```git submodule update --init --recursive``` -* Install via apt ```sudo apt install opencv-python``` -* Download and install the pip wheels for torch and torchvision on armv7l -* Install the required dependancies via ```pip install -r requirements.txt``` -* You should now be able to run testing python scripts and launch the application - -## How to setup for auto-running on a rpi - -To setup a systemd service so that shepard will automatically start and -restart. - -```sh -uaarg@raspberrypi:~/shepard $ sudo ln shepard.service /etc/systemd/system -uaarg@raspberrypi:~/shepard $ sudo systemctl enable shepard.service -Created symlink /etc/systemd/system/multi-user.target.wants/shepard.service → /etc/systemd/system/shepard.service. -uaarg@raspberrypi:~/shepard $ sudo systemctl start shepard.service -``` - -Then you can confirm that everything is working by running: - -```sh -uaarg@raspberrypi:~/shepard $ sudo systemctl status shepard.service -● shepard.service - Shepard - Loaded: loaded (/etc/systemd/system/shepard.service; enabled; vendor preset: enabled) - Active: active (running) since Mon 2023-04-03 18:40:55 MDT; 8min ago - Main PID: 2210 (python) - Tasks: 9 (limit: 3933) - CPU: 15min 59.449s - CGroup: /system.slice/shepard.service - ├─2210 python /home/uaarg/shepard/main.py - ├─2220 python /home/uaarg/shepard/main.py - ├─2221 python /home/uaarg/shepard/main.py - └─2222 python /home/uaarg/shepard/main.py -``` - -## How to Solve Permission Errors on Linux - -* If you have permission errors accessing ports, you may need to add your user to the 'tty' and 'dialout' groups -* ```sudo usermod -a -G tty {your username}``` -* ```sudo usermod -a -G dialout {your username}``` -* You will then need to logout and login completely to update your permissions - -### Contribution guidelines ### - -* Write tests so that functionality can be easily reviewed at a later date -* Create a new branch if working on implementing a new feature that will need review or multiple commits -* Add new required packages to requirements.txt - -We have several tools setup, please use them. These are: - -- `./scripts/lint.sh` Run a linter over all python files -- `./scripts/fmt.sh` Run a formatter over all python files - -Also, keep in mind that the tests and linter is run on every commit and/or PR in our CI. - -### Who do I talk to? ### - -* UAARG Imaging Leads, Autopilot Leads, and VP Technical should be able to help you with any questions - - - -# Resources - -This directory contains all static resources, such as images, data files, etc. - - - -from src.modules.imaging import analysis -from src.modules.imaging import camera -from src.modules.imaging import debug -from src.modules.imaging import location -from dep.labeller.benchmarks import yolo - -mavlink_delegate = location.MAVLinkDelegate() -location_provider = location.MAVLinkLocationProvider(mavlink_delegate) -location_provider = location.DebugLocationProvider() -location_provider.debug_change_location(altitude=1) - -detector = yolo.YoloDetector(weights="landing_nano.pt") -cam = camera.RPiCamera() -debugger = debug.ImageAnalysisDebugger() -debugger = None -img_analysis = analysis.ImageAnalysisDelegate(detector, cam, location_provider, - debugger) - -#img_analysis.subscribe(lambda _, __, ___: debugger.show()) -img_analysis.subscribe(lambda _, lon, lat: print(lon, lat)) -img_analysis.start() - -mavlink_delegate.run() - - - -import time - -from src.modules.emu import Emu -from src.modules.imaging.detector import ArucoDetector -from src.modules.imaging.camera import DebugCamera -from src.modules.imaging.location import DebugLocationProvider -from src.modules.imaging.analysis import ImageAnalysisDelegate -from src.modules.imaging.aruco_stream import ArucoEmuStreamer - -def main(): - emu = Emu("tmp") - emu.start_comms() - time.sleep(2) - - detector = ArucoDetector() - camera = DebugCamera("res/test-image.jpeg") - location_provider = DebugLocationProvider() - - analysis = ImageAnalysisDelegate(detector, camera, location_provider) - streamer = ArucoEmuStreamer(emu, "tmp") - - analysis.subscribe(streamer.on_detection) - analysis.start() - - time.sleep(60) - analysis.stop() - -if __name__ == "__main__": - main() - - - -from src.modules.imaging.mavlink import MAVLinkDelegate -from src.modules.imaging.battery import MAVLinkBatteryStatusProvider -import time -import threading - -mavlink = MAVLinkDelegate() -battery = MAVLinkBatteryStatusProvider(mavlink) - - -def wait_for_voltage(): - while True: - try: - voltage = battery.voltage() - print("voltage: ", voltage) - except ValueError: - pass - #print("no data yet") - time.sleep(0.5) - - -threading.Thread(target=wait_for_voltage).start() -mavlink.run() - - - -# Test the WebcamCamera class - -import time -import sys - -from src.modules.imaging.camera import RPiCamera - -cam = RPiCamera() - -#cam.set_size((1000, 1000)) - -print("Taking a picture in 3s.") - -for i in reversed(range(3)): - print(f"{i + 1} ", end="") - # stdout is linebuffered, so nothing will print until a \n otherwise - sys.stdout.flush() - - time.sleep(1) - -print("\rSay cheese!") -time.sleep(1) - -cam.caputure_to("tmp/you.png") -print("Image saved to tmp/you.png") - - - -import json -from src.modules.autopilot.lander import Lander -from src.modules.georeference import inference_georeference - -with open('./samples/geofence.json', 'r') as f: - geofence = json.load(f)['features'][0]['geometry']['coordinates'][0] - - -origin = [-113.54815575690603, 53.495546666117804] - -geofence = inference_georeference.Geofence_to_XY(origin, geofence) -print(geofence) - -lander = Lander(0, 0, geofence) -''' -point1 = [53.4958, -113.5477] -point2 = [53.4956, -113.5481] -point3 = [53.4956, -113.5475] -point4 = [53.4955, -113.5488] -point5 = [53.4955, -113.5488] -''' -point1 = [0, 0] -point2 = [5, 5] -point3 = [10, 10] -point4 = [-5, -5] -point5 = [-10, 10] -point6 = [30.4, - 30.4] -point7 = [-113.54827877970348, - 53.49573842746909] - - -print(lander.geofence_check(point1)) -print(lander.geofence_check(point2)) -print(lander.geofence_check(point3)) -print(lander.geofence_check(point4)) -print(lander.geofence_check(point5)) -print(lander.geofence_check(point6)) -print(lander.geofence_check(point7)) - - - -{ - "type": "FeatureCollection", - "features": [ - { - "type": "Feature", - "properties": {}, - "geometry": { - "coordinates": [ - [ - [ - -113.54797355858594, - 53.49538269557871 - ], - [ - -113.54769792522994, - 53.49557167851731 - ], - [ - -113.5479829020893, - 53.495734721944046 - ], - [ - -113.5484064742526, - 53.495672654349875 - ], - [ - -113.5486322755893, - 53.495509610684024 - ], - [ - -113.54837844374161, - 53.495381769189436 - ], - [ - -113.54797355858594, - 53.49538269557871 - ] - ] - ], - "type": "Polygon" - } - }, - { - "type": "Feature", - "properties": {}, - "geometry": { - "coordinates": [ - -113.54815575690603, - 53.495546666117804 - ], - "type": "Point" - } - } - ] -} - - - -# This can be run with `samples/run.sh image_process.py` -# Or, if you don't have bash installed, run the equivalent of: -# PYTHONPATH=".:dep/labeller" python3 samples/image_process.py - -import time - -from src.modules import imaging -from dep.labeller.benchmarks.colorfilter import ColorFilterDetector - -detector = ColorFilterDetector() -camera = imaging.camera.DebugCamera("./res/test-image.jpeg") -debugger = imaging.debug.ImageAnalysisDebugger() -analysis = imaging.analysis.ImageAnalysisDelegate(detector, camera, debugger) - -analysis.subscribe(print) # Will print all results to stdout -analysis.start() - -print("Analysis is running in the background.") -print("This should have printed very soon after the program started.") - -while True: - # Dummy loop. In a real scenario, we would be running autopilot control - # code here. - time.sleep(100) - - - -from PIL import Image, ImageOps -import time - -from src.modules.imaging.debug import ImageAnalysisDebugger -from benchmarks.detector import BoundingBox, Vec2 - -im = Image.open("res/test-image.jpeg") -bb = BoundingBox(Vec2(200, 120), Vec2(100, 100)) - -dbg = ImageAnalysisDebugger() -print("The debugger should not be visible right now.") - -dbg.set_image(im) -dbg.set_bounding_box(bb) - -dbg.show() -print("The debugger should now be visible") - -print("Because the debugger is not blocking, we can continue to run code.") -print("Close the window to stop") -for i in range(10): - print(f"Running {10 - i} more times") - print("Flipping image") - im = ImageOps.flip(im.copy()) - bb.position = Vec2(bb.position.x, im.height - bb.position.y) - - dbg.set_image(im) - - print("Waiting 1s to display bounding box") - time.sleep(1) - - dbg.set_bounding_box(bb) - dbg.show() - - print("Waiting 2s to display new image") - time.sleep(2) - -print("Before you go! Look at the image for another 3s") -dbg.show() -time.sleep(3) - -print("Bye! (Should exit the program now)") - - - -from src.modules.imaging.kml import KMLGenerator, LatLong - - -kml = KMLGenerator() - -hotspot_1 = LatLong(1,1) -hotspot_2 = LatLong(1,2) -hotspot_3 = LatLong(0,0) - -kml.push(hotspot_1) -kml.push(hotspot_2) -kml.push(hotspot_3) - -print(kml.read(-1).latitude, kml.read(-1).longitude) -kml.pop() - -print(kml.read(-1).latitude, kml.read(-1).longitude) - -kml.set_source("Crashed Drone", LatLong(24, 24)) - -kml.generate("out.kml") - - - -# Samples - -This directory contains samples to help get set up, and code that supports -understanding of the documentation. - -Samples can be run with the helper script. This set's up a `PYTHONPATH` -variable so that you can import modules with, ie. `import -src.modules.imaging.camera`. Here is the usage: - -```sh -samples/run.sh # Where is the name of the samples -samples/run.sh # Leaving blank will print all samples to run -``` - - - -#!/usr/bin/env bash - -set -o errexit - -sample="$1" - -if ! git status 2>&1 >/dev/null; then - echo "Please run this script from inside the shepard repository" - exit 1 -fi - -#cd "$(git worktree list | awk '{ print $1 }')" - -if ! test -f "samples/$sample.py"; then - echo "ERROR: Please pass a sample which is one of:" - ls samples/*.py | sed 's/^samples\// - /' | sed 's/\.py$//' - exit 1 -fi - -PYTHONPATH=".:dep/labeller:dep/labeller/third_party/yolov5" python3 "samples/$sample.py" - - - -from time import sleep - -import src.modules.autopilot.altimeter_xm125 as altimeter_module - -altimeter_module = altimeter_module.XM125(average_window=5) -altimeter_module.begin() - -while True: - result = altimeter_module.measure() - if result: - average = result[0]['averaged'] - - # Average distance will not be available until at least `average_window` measurements have been taken - if average: - average_distance = average[0] - - # `average_distance` can now be used in your autopilot logic - print("Average dist: ", average_distance) - - sleep(0.5) - - - -# xm125_tui.py -import curses -import time -import statistics -from datetime import datetime -from src.modules.autopilot.altimeter_xm125 import XM125, SensorError - - -def format_measurement_table(screen, start_y, peaks, width): - """Format measurements in a clean table layout""" - # Table headers - screen.addstr(start_y, 0, "Raw Measurements:", curses.A_BOLD) - screen.addstr(start_y, width // 2, "Averaged Measurements:", curses.A_BOLD) - - # Column headers for raw data - screen.addstr(start_y + 1, 2, "Peak │ Distance (mm) │ Strength", curses.A_DIM) - screen.addstr(start_y + 2, 2, "─────┼──────────────┼──────────", curses.A_DIM) - - # Column headers for averaged data - screen.addstr(start_y + 1, width // 2 + 2, "Peak │ Distance (mm) │ Strength", curses.A_DIM) - screen.addstr(start_y + 2, width // 2 + 2, "─────┼──────────────┼──────────", curses.A_DIM) - - if peaks: - for i, peak_data in enumerate(peaks): - raw_distance, raw_strength = peak_data['raw'] - - # Raw data - raw_line = f" {i:3d} │ {raw_distance:>12d} │ {raw_strength:>8d}" - screen.addstr(start_y + 3 + i, 2, raw_line) - - # Averaged data if available - if peak_data['averaged']: - avg_distance, avg_strength = peak_data['averaged'] - avg_line = f" {i:3d} │ {avg_distance:>12.1f} │ {avg_strength:>8.1f}" - screen.addstr(start_y + 3 + i, width // 2 + 2, avg_line) - else: - screen.addstr(start_y + 3, 2, "No peaks detected", curses.color_pair(2)) - - return start_y + 5 + (len(peaks) if peaks else 1) - - -def tui_main(): - def setup_curses(): - screen = curses.initscr() - curses.start_color() - curses.use_default_colors() - curses.init_pair(1, curses.COLOR_GREEN, -1) - curses.init_pair(2, curses.COLOR_YELLOW, -1) - curses.init_pair(3, curses.COLOR_RED, -1) - curses.noecho() - curses.cbreak() - screen.keypad(True) - screen.nodelay(1) - return screen - - def cleanup_curses(screen): - screen.keypad(False) - curses.nocbreak() - curses.echo() - curses.endwin() - - try: - screen = setup_curses() - sensor = XM125(average_window=10) - - # Statistics tracking - distance_history = [] - MAX_HISTORY = 100 - error_count = 0 - measurement_count = 0 - start_time = datetime.now() - - if not sensor.begin(): - cleanup_curses(screen) - print("Failed to initialize sensor") - return - - while True: - try: - screen.clear() - height, width = screen.getmaxyx() - - # Header - runtime = datetime.now() - start_time - header = f"XM125 Radar Sensor Monitor - Runtime: {runtime}" - screen.addstr(0, 0, header, curses.A_BOLD) - screen.addstr(1, 0, "=" * width) - - # Get measurement - peaks = sensor.measure() - measurement_count += 1 - - # Status section - status_y = 3 - screen.addstr(status_y, 0, "Status:", curses.A_BOLD) - status_info = [ - f"Measurements: {measurement_count}", - f"Errors: {error_count}", - f"Sample Rate: {1000 / 100:.1f} Hz" # Assuming 100ms sleep - ] - for i, info in enumerate(status_info): - screen.addstr(status_y, 15 + i * 25, info) - - # Measurements section - readings_y = 5 - next_y = format_measurement_table(screen, readings_y, peaks, width) - - # Update history and calculate statistics - if peaks and peaks[0]['raw'][0]: # Use first peak for statistics - distance_history.append(peaks[0]['raw'][0]) - if len(distance_history) > MAX_HISTORY: - distance_history.pop(0) - - # Statistics section - stats_y = next_y + 1 - screen.addstr(stats_y, 0, "Statistics:", curses.A_BOLD) - - if distance_history: - mean_dist = statistics.mean(distance_history) - if len(distance_history) > 1: - stdev_dist = statistics.stdev(distance_history) - else: - stdev_dist = 0 - min_dist = min(distance_history) - max_dist = max(distance_history) - - stats_data = [ - f"Mean: {mean_dist:.1f}mm", - f"Std Dev: {stdev_dist:.1f}mm", - f"Min: {min_dist}mm", - f"Max: {max_dist}mm", - f"Samples: {len(distance_history)}" - ] - - for i, stat in enumerate(stats_data): - screen.addstr(stats_y + 1, 2 + i * 20, stat) - - # Help section - help_y = height - 2 - help_text = [ - ("q", "Quit"), - ("r", "Reset Statistics") - ] - screen.addstr(help_y, 0, "Commands:", curses.A_DIM) - for i, (key, desc) in enumerate(help_text): - screen.addstr(help_y, 12 + i * 20, f"{key}: {desc}", curses.A_DIM) - - screen.refresh() - - # Check for quit command - c = screen.getch() - if c == ord('q'): - break - elif c == ord('r'): - distance_history.clear() - - time.sleep(0.1) - - except SensorError as e: - error_count += 1 - screen.addstr(height - 3, 0, f"Sensor error: {e}", curses.color_pair(3)) - screen.refresh() - time.sleep(2.0) - continue - - except KeyboardInterrupt: - pass - except Exception as e: - cleanup_curses(screen) - print(f"Fatal error: {e}") - finally: - cleanup_curses(screen) - sensor.bus.close() - - -if __name__ == "__main__": - tui_main() - - - -#!/usr/bin/env sh - -yapf --parallel --in-place pylint $(git ls-files '*.py') - - - -#!/usr/bin/env sh - -flake8 \ - --max-line-length 140 \ - --ignore E265,F541,F811,W504,E402,E126,E125,E251,W503 \ - $(git ls-files '*.py') - - - -#!/usr/bin/env sh - -PYTHONPATH=".:dep/labeller" pytest - - - -#!/usr/bin/env sh - -export PYTHONPATH=".:dep/labeller:dep/labeller/third_party/yolov5" - -echo "Checking imaging modules" -mypy --check-untyped-defs --ignore-missing-imports src/modules/imaging/*.py - -echo "Checking tests" -mypy --ignore-missing-imports test/*.py - - - - - - - - - - - - - - - -import threading -import time -from typing import Optional - -from pymavlink import mavutil - -from src.modules.autopilot.altimeter import Altimeter - - -class MavlinkAltimeterProvider: - """ - A class to handle altitude measurements from any Altimeter sensor - and send them to the Pixhawk via MAVLink. - """ - - def __init__(self, sensor: Altimeter, connection_string: str, update_rate_hz: float = 10.0): - """ - Initialize the MavlinkAltimeterProvider. - - Args: - sensor: Altimeter sensor instance - connection_string: MAVLink connection string (e.g., 'udp:127.0.0.1:14550') - update_rate_hz: Rate at which to send altitude updates (Hz) - """ - self.sensor = sensor - self.connection_string = connection_string - self.update_interval = 1.0 / update_rate_hz - self._latest_altitude_mm = None - self._latest_altitude_timestamp = 0 - self._running = False - self._thread = None - self._lock = threading.Lock() - self._mavlink_connection = None - self._tstart = None - - def start(self): - """Start the altimeter thread.""" - if self._running: - return - - try: - self._tstart = time.time() - - # Initialize MAVLink connection - self._mavlink_connection = mavutil.mavlink_connection(self.connection_string) - # Wait for a heartbeat to ensure connection is established - self._mavlink_connection.wait_heartbeat() - print(f"MAVLink connection established on {self.connection_string}") - - # Start the measurement thread - self._running = True - self._thread = threading.Thread(target=self._measurement_loop, daemon=True) - self._thread.start() - print("Altimeter measurement thread started") - - except Exception as e: - print(f"Failed to start MavlinkAltimeterProvider: {e}") - self._running = False - if self._mavlink_connection: - self._mavlink_connection.close() - - def stop(self): - """Stop the altimeter thread.""" - self._running = False - if self._thread: - self._thread.join(timeout=2.0) - if self._mavlink_connection: - self._mavlink_connection.close() - print("Altimeter measurement thread stopped") - - def get_latest_altitude(self) -> Optional[float]: - """ - Get the latest altitude measurement in millimeters. - - Returns: - The latest altitude in mm, or None if no valid measurement is available - """ - with self._lock: - return self._latest_altitude_mm - - def get_latest_altitude_meters(self) -> Optional[float]: - """ - Get the latest altitude measurement in meters. - - Returns: - The latest altitude in meters, or None if no valid measurement is available - """ - with self._lock: - if self._latest_altitude_mm is not None: - return self._latest_altitude_mm / 1000.0 - return None - - def _measurement_loop(self): - """Continuous measurement loop that runs in a separate thread.""" - while self._running: - try: - start_time = time.time() - - # Get measurement from sensor - self.sensor.measure() - - # Get the distance - distance_mm = self.sensor.get_distance_mm() - - if distance_mm is not None: - # Update the latest altitude - with self._lock: - self._latest_altitude_mm = distance_mm - self._latest_altitude_timestamp = time.time() - - # Send to Pixhawk via MAVLink - self._send_distance_sensor_message(distance_mm) - - # Sleep to maintain the desired update rate - elapsed = time.time() - start_time - sleep_time = max(0, self.update_interval - elapsed) - time.sleep(sleep_time) - - except Exception as e: - print(f"Error in measurement loop: {e}") - time.sleep(0.5) - - def _send_distance_sensor_message(self, distance_mm: float): - """ - Send a DISTANCE_SENSOR MAVLink message to the Pixhawk. - - Args: - distance_mm: Distance measurement in millimeters - """ - if not self._mavlink_connection: - return - - try: - # Convert to centimeters for MAVLink message - distance_cm = int(distance_mm / 10.0) - - # Create and send the DISTANCE_SENSOR message - # Documentation: https://mavlink.io/en/messages/common.html#DISTANCE_SENSOR - self._mavlink_connection.mav.distance_sensor_send( - int((time.time() - self._tstart) * 1000), # time_boot_ms - self.sensor.min_distance_cm, # min_distance (cm) - self.sensor.max_distance_cm, # max_distance (cm) - distance_cm, # current_distance (cm) - self.sensor.mavlink_sensor_type, # type (from sensor) - self.sensor.sensor_id, # id (unique ID for this sensor) - mavutil.mavlink.MAV_SENSOR_ROTATION_PITCH_270, # orientation (downward facing) - 0, # covariance - ) - - except Exception as e: - print(f"Error sending MAVLink message: {e}") - - - -import smbus2 -import time -from enum import Enum -from typing import Optional, List, Dict, Any - -from src.modules.autopilot.altimeter import Altimeter, MovingAverage - -DEBUG = False - - -class SensorError(Exception): - """Base exception class for sensor errors""" - pass - - -class SensorState(Enum): - """Enum to track sensor state""" - UNINITIALIZED = 0 - INITIALIZED = 1 - ERROR = 2 - NEEDS_CALIBRATION = 3 - - -class SensorReflectorShape(Enum): - """Enum to define reflector shape""" - GENERIC = 1 - PLANAR = 2 - - -class XM125(Altimeter): - # Register addresses - REG_VERSION = 0x0000 - REG_PROTOCOL_STATUS = 0x0001 - REG_MEASURE_COUNTER = 0x0002 - REG_DETECTOR_STATUS = 0x0003 - REG_DISTANCE_RESULT = 0x0010 - REG_PEAK0_DISTANCE = 0x0011 - REG_PEAK0_STRENGTH = 0x001b - REG_START = 0x0040 - REG_END = 0x0041 - REG_COMMAND = 0x0100 - REG_REFLECTOR_SHAPE = 0x004b - - # Command values - CMD_APPLY_CONFIG_AND_CALIBRATE = 1 - CMD_MEASURE_DISTANCE = 2 - CMD_RECALIBRATE = 5 - CMD_RESET_MODULE = 1381192737 - - # Config values - REFLECTOR_SHAPE = SensorReflectorShape.PLANAR.value - - # Status masks - DETECTOR_STATUS_BUSY_MASK = 0x80000000 - DISTANCE_RESULT_NUM_DISTANCES_MASK = 0x0000000f - DISTANCE_RESULT_NEAR_START_EDGE = 0x00000100 - DISTANCE_RESULT_CALIBRATION_NEEDED = 0x00000200 - DISTANCE_RESULT_MEASURE_DISTANCE_ERROR = 0x00000400 - - # Error recovery constants - MAX_RETRIES = 3 - RETRY_DELAY = 0.5 # seconds - ERROR_TIMEOUT = 5.0 # seconds - - def __init__(self, sensor_id=0, min_distance=250, max_distance=10000, bus=1, address=0x52, average_window=5): - super().__init__(sensor_id, min_distance, max_distance) - self.bus = smbus2.SMBus(bus) - self.address = address - self.distance_avg = MovingAverage(average_window) - self.strength_avg = MovingAverage(average_window) - self.last_error_time = 0 - self.error_count = 0 - self.consecutive_errors = 0 - self._latest_measurement = None - - @property - def mavlink_sensor_type(self) -> int: - """Get the MAVLink sensor type (RADAR)""" - return self.SENSOR_TYPE_RADAR - - def get_distance_mm(self) -> Optional[float]: - """Get the latest distance measurement in millimeters""" - if self._latest_measurement and self._latest_measurement[0]['averaged']: - return self._latest_measurement[0]['averaged'][0] - return None - - def _read_register(self, reg_addr) -> Optional[int]: - """Read a 32-bit register value with error handling.""" - try: - write = smbus2.i2c_msg.write(self.address, [(reg_addr >> 8) & 0xFF, reg_addr & 0xFF]) - read = smbus2.i2c_msg.read(self.address, 4) - - if DEBUG: - print(f"\nREAD OPERATION:") - print(f" Register: 0x{reg_addr:04x}") - print(f" Sending address bytes: MSB=0x{(reg_addr >> 8) & 0xFF:02x}, LSB=0x{reg_addr & 0xFF:02x}") - - self.bus.i2c_rdwr(write, read) - data = list(read) - value = (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | data[3] - - if DEBUG: - print(f" Received bytes: [0x{data[0]:02x}, 0x{data[1]:02x}, 0x{data[2]:02x}, 0x{data[3]:02x}]") - print(f" Decoded value: 0x{value:08x} ({value})") - - return value - except IOError as e: - self._handle_error(f"I/O error reading register 0x{reg_addr:04x}: {e}") - return None - - def _write_register(self, reg_addr: int, value: int) -> bool: - """Write a 32-bit register value with error handling.""" - try: - data = [ - (reg_addr >> 8) & 0xFF, # Address to slave [15:8] - reg_addr & 0xFF, # Address to slave [7:0] - (value >> 24) & 0xFF, # Data to slave [31:24] - (value >> 16) & 0xFF, # Data to slave [23:16] - (value >> 8) & 0xFF, # Data to slave [15:8] - value & 0xFF # Data to slave [7:0] - ] - - if DEBUG: - print(f"\nWRITE OPERATION:") - print(f" Register: 0x{reg_addr:04x}") - print(f" Value to write: 0x{value:08x} ({value})") - print(f" Sending bytes: {[f'0x{b:02x}' for b in data]}") - - self.bus.write_i2c_block_data(self.address, data[0], data[1:]) - return True - except IOError as e: - self._handle_error(f"I/O error writing register 0x{reg_addr:04x}: {e}") - return False - - def _handle_error(self, error_msg: str): - """Handle errors and implement recovery logic.""" - current_time = time.time() - self.consecutive_errors += 1 - self.error_count += 1 - - print(f"Error: {error_msg}") - print(f"Consecutive errors: {self.consecutive_errors}") - print(f"Total errors: {self.error_count}") - - # If we've hit max retries and enough time has passed since last reset - if self.consecutive_errors >= self.MAX_RETRIES: - if current_time - self.last_error_time > self.ERROR_TIMEOUT: - print("Attempting sensor reset and recalibration...") - try: - self.reset_and_recalibrate() - self.consecutive_errors = 0 # Only reset if successful - self.last_error_time = current_time - return - except Exception as e: - print(f"Reset failed: {e}") - - # If we get here, either the timeout hasn't passed or reset failed - raise SensorError(f"Too many consecutive errors: {error_msg}") - - time.sleep(self.RETRY_DELAY) - - def _wait_not_busy(self): - """Wait until the detector is not busy.""" - if DEBUG: - print("\nWaiting for detector not busy...") - - attempts = 0 - while True: - status = self._read_register(self.REG_DETECTOR_STATUS) - if status is None: - if DEBUG: - print(" Failed to read status") - break - - busy = bool(status & self.DETECTOR_STATUS_BUSY_MASK) - if DEBUG: - print(f" Status: 0x{status:08x}, Busy: {busy}") - - if not busy: - break - - time.sleep(0.01) - attempts += 1 - if attempts >= 100: # Timeout after 1 second - if DEBUG: - print(" Timeout waiting for not busy") - break - - def begin(self) -> bool: - """Initialize the sensor with given start and end distances in mm.""" - if DEBUG: - print(f"\nInitializing sensor:") - print(f" Start distance: {self.min_distance_mm}mm") - print(f" End distance: {self.max_distance_mm}mm") - print(f" Reflector shape: {self.REFLECTOR_SHAPE}") - - # Configure start and end distances - if not self._write_register(self.REG_START, self.min_distance_mm): - if DEBUG: - print("Failed to set start distance") - return False - - if not self._write_register(self.REG_END, self.max_distance_mm): - if DEBUG: - print("Failed to set end distance") - return False - - if not self._write_register(self.REG_REFLECTOR_SHAPE, self.REFLECTOR_SHAPE): - if DEBUG: - print("Failed to set reflector shape") - return False - - if DEBUG: - print("Applying configuration and calibrating...") - - # Apply configuration and calibrate - if not self._write_register(self.REG_COMMAND, self.CMD_APPLY_CONFIG_AND_CALIBRATE): - if DEBUG: - print("Failed to apply configuration and calibrate") - return False - - self._wait_not_busy() - - # Check status - status = self._read_register(self.REG_DETECTOR_STATUS) - if DEBUG: - print(f"Final status: 0x{status:08x}" if status is not None else "Failed to read final status") - - success = status is not None and status >= 0 - if success: - self.state = SensorState.INITIALIZED - - return success - - def reset_and_recalibrate(self): - """Reset the sensor and recalibrate.""" - try: - # Reset the module - self._write_register(self.REG_COMMAND, self.CMD_RESET_MODULE) - time.sleep(1.0) # Wait for reset - - # Clear averaging buffers - self.distance_avg.reset() - self.strength_avg.reset() - - # Reinitialize - if not self.begin(): - raise SensorError("Failed to reinitialize sensor after reset") - - print("Sensor reset and recalibration successful") - self.state = SensorState.INITIALIZED - - except Exception as e: - self.state = SensorState.ERROR - raise SensorError(f"Reset and recalibration failed: {str(e)}") - - def check_calibration(self) -> bool: - """Check if sensor needs calibration and perform if needed.""" - result = self._read_register(self.REG_DISTANCE_RESULT) - if result is None: - return False - - if result & self.DISTANCE_RESULT_CALIBRATION_NEEDED: - print("Calibration needed, recalibrating...") - self._write_register(self.REG_COMMAND, self.CMD_RECALIBRATE) - time.sleep(0.5) - self.state = SensorState.INITIALIZED - return True - - return True - - def measure(self) -> List[Dict[str, Any]]: - """Perform a distance measurement with error checking and recovery.""" - try: - if self.state == SensorState.ERROR: - self.reset_and_recalibrate() - - if not self.check_calibration(): - return [] - - # Start measurement - if not self._write_register(self.REG_COMMAND, self.CMD_MEASURE_DISTANCE): - return [] - - self._wait_not_busy() - - # Read result - result = self._read_register(self.REG_DISTANCE_RESULT) - if result is None: - return [] - - if result & self.DISTANCE_RESULT_MEASURE_DISTANCE_ERROR: - self._handle_error("Measurement error") - return [] - - num_distances = (result & self.DISTANCE_RESULT_NUM_DISTANCES_MASK) - peaks_with_average = [] - - for i in range(num_distances): - distance = self._read_register(self.REG_PEAK0_DISTANCE + i) - strength = self._read_register(self.REG_PEAK0_STRENGTH + i) - - if distance is not None and strength is not None: - # Convert strength from 32-bit unsigned to signed int - if strength > 0x7FFFFFFF: - strength = int(0x100000000 - strength) - - self.distance_avg.add(distance) - self.strength_avg.add(strength) - - peak_data = { - 'raw': (distance, strength), - 'averaged': (self.distance_avg.get_average(), - self.strength_avg.get_average()) if self.distance_avg.is_valid() else None - } - peaks_with_average.append(peak_data) - - # Only reset consecutive errors if we got valid data - if peaks_with_average: - self.consecutive_errors = 0 - self._latest_measurement = peaks_with_average - - return peaks_with_average - - except Exception as e: - self._handle_error(f"Measurement error: {str(e)}") - return [] - - -def main(): - sensor = XM125(average_window=5) - - try: - if not sensor.begin(): - print("Failed to initialize sensor") - return - - print("Sensor initialized successfully") - - while True: - try: - peaks = sensor.measure() - - if peaks: - for i, peak_data in enumerate(peaks): - raw_distance, raw_strength = peak_data['raw'] - print(f"\nPeak {i}:") - print(f" Raw: Distance = {raw_distance}mm, Strength = {raw_strength}") - - if peak_data['averaged']: - avg_distance, avg_strength = peak_data['averaged'] - print(f" Avg: Distance = {avg_distance:.1f}mm, Strength = {avg_strength:.1f}") - else: - print("No peaks detected") - - time.sleep(0.5) - - except SensorError as e: - print(f"Sensor error: {e}") - print("Waiting before retry...") - time.sleep(2.0) # Longer delay on serious errors - - # Try to reset the sensor on serious errors - try: - sensor.reset_and_recalibrate() - except Exception as reset_error: - print(f"Reset failed: {reset_error}") - time.sleep(5.0) # Even longer delay if reset fails - - except KeyboardInterrupt: - print("\nStopping measurements") - sensor.bus.close() - except Exception as e: - print(f"Fatal error: {e}") - - -if __name__ == "__main__": - main() - - - -from abc import ABC, abstractmethod -from typing import Optional, List, Dict, Any - - -class MovingAverage: - def __init__(self, window_size=5): - self.window_size = window_size - self.values = [] - - def add(self, value): - self.values.append(value) - if len(self.values) > self.window_size: - self.values.pop(0) - - def get_average(self): - if not self.values: - return None - return sum(self.values) / len(self.values) - - def is_valid(self): - return len(self.values) == self.window_size - - def reset(self): - """Clear all values""" - self.values = [] - - -class Altimeter(ABC): - """ - Abstract base class for altimeter sensors. - All altimeter implementations should inherit from this class. - """ - - # MAVLink sensor type constants - SENSOR_TYPE_LASER = 0 - SENSOR_TYPE_ULTRASOUND = 1 - SENSOR_TYPE_INFRARED = 2 - SENSOR_TYPE_RADAR = 3 - SENSOR_TYPE_UNKNOWN = 4 - - def __init__(self, sensor_id: int = 0, min_distance_mm: int = 0, max_distance_mm: int = 0): - """ - Initialize the altimeter. - - Args: - sensor_id: Unique ID for this sensor (used in MAVLink messages) - min_distance_mm: Minimum measurable distance in millimeters - max_distance_mm: Maximum measurable distance in millimeters - """ - self.sensor_id = sensor_id - self.min_distance_mm = min_distance_mm - self.max_distance_mm = max_distance_mm - self.state = 0 # Generic value, interpretation depends on sensor type - - @abstractmethod - def begin(self) -> bool: - """ - Initialize the sensor hardware. - - Returns: - True if initialization was successful, False otherwise - """ - pass - - @abstractmethod - def measure(self) -> List[Dict[str, Any]]: - """ - Perform a measurement. - - Returns: - A list of measurement data (format depends on sensor type) - """ - pass - - @abstractmethod - def get_distance_mm(self) -> Optional[float]: - """ - Get the latest distance measurement in millimeters. - - Returns: - The distance in millimeters, or None if no valid measurement is available - """ - pass - - def get_distance_m(self) -> Optional[float]: - """ - Get the latest distance measurement in meters. - - Returns: - The distance in meters, or None if no valid measurement is available - """ - distance_mm = self.get_distance_mm() - if distance_mm is not None: - return distance_mm / 1000.0 - return None - - @property - def min_distance_cm(self) -> int: - """Get minimum distance in centimeters for MAVLink messages""" - return int(self.min_distance_mm / 10) - - @property - def max_distance_cm(self) -> int: - """Get maximum distance in centimeters for MAVLink messages""" - return int(self.max_distance_mm / 10) - - @property - @abstractmethod - def mavlink_sensor_type(self) -> int: - """ - Get the MAVLink sensor type for this altimeter. - - Returns: - An integer representing the MAVLink sensor type - """ - pass - - - -from pymavlink import mavutil -import pymavlink.dialects.v20.all as dialect - - -class Messenger: - """ - Messenger class for sending messages from the autopilot to the ground station. - - Source: - https://github.com/mustafa-gokce/ardupilot-software-development/blob/main/pymavlink/send-status-text.py - """ - - def __init__(self, port): - self.__master = mavutil.mavlink_connection( - device=f"udp:127.0.0.1:{port}", - source_system=1, - source_component=1) - - def send(self, message, prefix="SHEPARD"): - """ - Sends a message to the ground station. - - :param message: The message to send. - :param prefix: The prefix to add to the message. - :return: None - """ - - message = f"{prefix}: {message}" - mav_message = dialect.MAVLink_statustext_message( - severity=dialect.MAV_SEVERITY_INFO, text=message.encode("utf-8")) - self.__master.mav.send(mav_message) - - - -import dronekit_sitl - - -class Simulator: - """ - A class to create and handle a simulated drone. - """ - - __sitl = None - __conn_str = None - __gcs_conn_str = None - - @property - def sitl(self): - return self.__sitl - - @property - def conn_str(self): - return self.__conn_str - - @property - def gcs_conn_str(self): - return self.__gcs_conn_str - - def __init__(self): - self.__sitl = dronekit_sitl.start_default() - self.__conn_str = self.__sitl.connection_string() - # TODO: Find a way to set gcs_conn_str programmatically - self.__gcs_conn_str = "127.0.0.1:5763" - - def __del__(self): - self.__sitl.stop() - - - - - - - - - - - -from PIL import Image, ImageDraw -import os -from typing import Optional, Tuple - -from src.modules.emu import Emu -from src.modules.imaging.detector import BoundingBox - - -class ArucoEmuStreamer: - def __init__(self, emu: Emu, output_dir: str = "tmp"): - self.emu = emu - self.output_dir = output_dir - self.counter = 0 - - if not os.path.exists(self.output_dir): - os.makedirs(self.output_dir) - - def on_detection(self, image: Image.Image, bounding_box: Optional[BoundingBox], position: Optional[Tuple[float, float]]): - im = image.copy() - - if bounding_box is not None: - draw = ImageDraw.Draw(im) - bb = (bounding_box.position.x, bounding_box.position.y, - bounding_box.size.x, bounding_box.size.y) - draw.rectangle(bb, outline="red") - - filename = f"{self.counter}.jpeg" - filepath = os.path.join(self.output_dir, filename) - im.save(filepath) - - self.emu.send_image(filename) - self.counter += 1 - - - -from typing import Optional -from src.modules.imaging.mavlink import MAVLinkDelegate -import pymavlink.dialects.v20.all as dialect - - -class BatteryStatusProvider: - """ - Provides the current status of the drone's battery / power usage. - """ - - def voltage(self) -> float: - """ - Get the latest voltage of the drone batter in Volts. - """ - raise NotImplementedError() - - -class DebugBatteryStatusProvider(BatteryStatusProvider): - """ - For testing / debugging - """ - - def __init__(self) -> None: - self._current_voltage = 0 - - def voltage(self) -> float: - return self._current_voltage - - def set_voltage(self, new_voltage): - self._current_voltage = new_voltage - - -class MAVLinkBatteryStatusProvider: - """ - For use in production when a MAVLink connection is available. - """ - - def __init__(self, mavlink_delegate: MAVLinkDelegate): - self.mavlink_delegate = mavlink_delegate - self._voltage: Optional[float] = None - - # Subscribe to the delegate's messages - self.mavlink_delegate.subscribe(self._process_message) - self.mavlink_delegate.send( - dialect.MAVLink_command_long_message( - target_system=1, - target_component=1, - command=dialect.MAV_CMD_SET_MESSAGE_INTERVAL, - confirmation=0, - param1=1, # param1: send SYS_STATUS message - param2=500000, # param2: send every 5e5 us - param3=0, - param4=0, - param5=0, - param6=0, - param7=1)) # param7: send messaged to requester - - def _process_message(self, message): - # This callback processes incoming MAVLink messages and updates the internal state - if message.get_type() == 'SYS_STATUS': - # message.voltage_battery is an int in mV - self._voltage = float(message.voltage_battery) / 1e3 - - def voltage(self) -> float: - if self._voltage is not None: - return self._voltage - else: - raise ValueError("No valid voltage data available") - - - -import pymavlink.dialects.v20.all as dialect -from mavlink import MAVLinkDelegate - - -class StatusLeds: - "stores the state of LED lights" - - def __init__(self, mavlink: MAVLinkDelegate, RGB=[0, 0, 0]): - self.RGB = RGB - self.mavlinkDelegate = mavlink - - def change_color(self, RGB: list): - self.RGB = RGB - #print current color of LED - if (self.RGB[0] == 255 and self.RGB[1] == 0 and self.RGB[2] == 0): - print("RED COLOR") - elif (self.RGB[0] == 0 and self.RGB[1] == 255 and self.RGB[2] == 0): - print("GREEN COLOR") - elif (self.RGB[0] == 0 and self.RGB[1] == 0 and self.RGB[2] == 255): - print("BLUE COLOR") - else: - print(f"RGB values: {self.RGB}") - self.send_message() - - def send_message(self): - message = dialect.MAVLink_debug_vect_message(name=bytes( - "LED_STATUS", 'utf-8'), - time_usec=0, - x=self.RGB[0], - y=self.RGB[1], - z=self.RGB[2]) - self.mavlinkDelegate.send(message) - - - -from typing import Optional -from dataclasses import dataclass -import math -from src.modules.imaging.mavlink import MAVLinkDelegate -import pymavlink.dialects.v20.all as dialect -import json - - -@dataclass -class LatLng: - """ - Latitude and longitude in WGS84 coordinates. - """ - - lat: float - lng: float - - def to_json(self): - return {"lat": self.lat, "lng": self.lng} - - -@dataclass -class Heading: - """ - Heading, as defined in an aeronautical sense, with respect to - north. Measured in degrees. - """ - - heading: float - - def to_json(self): - return self.heading - - -@dataclass -class Rotation: - """ - Orientation of the drone using euler angles in degrees. - """ - - pitch: float - roll: float - yaw: float - - def to_json(self): - return { - "pitch": self.pitch, - "roll": self.roll, - "yaw": self.yaw, - } - - -class LocationProvider: - """ - Provides the drone's current location and orientation as read from - telemetry or a debug-source. - """ - - def location(self) -> LatLng: - """ - Get the latest lat/lng location of the drone. - """ - raise NotImplementedError() - - def heading(self) -> Heading: - """ - Get the latest heading of the drone (with respect to north.) - """ - raise NotImplementedError() - - def altitude(self) -> float: - """ - Get the latest altitude of the drone in meters. - """ - raise NotImplementedError() - - def orientation(self) -> Rotation: - """ - Get the latest 3D orientation of the drone in it's inertial frame. - """ - raise NotImplementedError() - - def dump_to(self, path: str): - try: - dump = { - "location": self.location().to_json(), - "heading": self.heading().to_json(), - "altitude": self.altitude(), - "orientation": self.orientation().to_json(), - } - - with open(path, "w") as f: - json.dump(dump, f) - except ValueError: - pass # Raised if we cannot get any location data - - -class DebugLocationProvider(LocationProvider): - """ - Will return a series of given locations and orientations. - """ - - def __init__(self) -> None: - self._current_location = LatLng(0.0, 0.0) - self._current_heading = Heading(0.0) - self._current_altitude = 10.0 - self._current_orientation = Rotation(0.0, 0.0, 0.0) - - def location(self) -> LatLng: - return self._current_location - - def heading(self) -> Heading: - return self._current_heading - - def altitude(self) -> float: - return self._current_altitude - - def orientation(self) -> Rotation: - return self._current_orientation - - def set_location(self, new_location): - self._current_location = new_location - - def set_heading(self, new_heading): - self._current_heading = new_heading - - def set_altitude(self, new_altitude): - self._current_altitude = new_altitude - - def set_orientation(self, new_orientation): - self._current_orientation = new_orientation - - def debug_change_location(self, **kwargs): - """ - Change the location reported by this DebugLocationProvider. - - Keyword Arguments: - lat -- Latitude - lng -- Longitude - heading -- Heading direction - altitude -- Altitude - pitch -- Pitch angle - roll -- Roll angle - yaw -- Yaw angle - """ - if 'lat' in kwargs or 'lng' in kwargs: - lat = kwargs.get('lat', self._current_location.lat) - lng = kwargs.get('lng', self._current_location.lng) - - self._current_location = LatLng(lat, lng) - - if 'heading' in kwargs: - self._current_heading = Heading(kwargs['heading']) - - if 'altitude' in kwargs: - self._current_altitude = kwargs['altitude'] - - if any(k in kwargs for k in ['pitch', 'roll', 'yaw']): - pitch = kwargs.get('pitch', self._current_orientation.pitch) - roll = kwargs.get('roll', self._current_orientation.roll) - yaw = kwargs.get('yaw', self._current_orientation.yaw) - - self._current_orientation = Rotation(pitch, roll, yaw) - - -class MAVLinkLocationProvider(LocationProvider): - """ - Will provide location information based on information received as MAVLink messages. - """ - - def __init__(self, mavlink_delegate: MAVLinkDelegate): - self.mavlink_delegate = mavlink_delegate - self._location: Optional[LatLng] = None - self._heading: Optional[Heading] = None - self._altitude: Optional[float] = None - self._orientation: Optional[Rotation] = None - - # Subscribe to the delegate's messages - self.mavlink_delegate.subscribe(self._process_message) - self.mavlink_delegate.send( - dialect.MAVLink_command_long_message( - target_system=1, - target_component=1, - command=dialect.MAV_CMD_SET_MESSAGE_INTERVAL, - confirmation=0, - param1=33, # param1: send GLOBAL_POSITION_INT message - param2=500000, # param2: send every 5e5 us - param3=0, - param4=0, - param5=0, - param6=0, - param7=1)) # param7: send messaged to requester - self.mavlink_delegate.send( - dialect.MAVLink_command_long_message( - target_system=1, - target_component=1, - command=dialect.MAV_CMD_SET_MESSAGE_INTERVAL, - confirmation=0, - param1=30, # param1: send ATTITUDE message - param2=500000, # param2: send every 5e5 us - param3=0, - param4=0, - param5=0, - param6=0, - param7=1)) # param7: send messaged to requester - - def _process_message(self, message): - # This callback processes incoming MAVLink messages and updates the internal state - if message.get_type() == 'GLOBAL_POSITION_INT': - self._location = LatLng(message.lat / 1e7, message.lon / 1e7) - self._altitude = message.alt / 1000.0 # Altitude in meters - self._heading = Heading(message.hdg / 1e7) # Heading in degrees - elif message.get_type() == 'ATTITUDE': - self._orientation = Rotation(pitch=math.degrees(message.pitch), - roll=math.degrees(message.roll), - yaw=math.degrees(message.yaw)) - - def location(self) -> LatLng: - if self._location is not None: - return self._location - else: - raise ValueError("No valid location data available") - - def heading(self) -> Heading: - if self._heading is not None: - return self._heading - else: - raise ValueError("No valid heading data available") - - def altitude(self) -> float: - if self._altitude is not None: - return self._altitude - else: - raise ValueError("No valid altitude data available") - - def orientation(self) -> Rotation: - if self._orientation is not None: - return self._orientation - else: - raise ValueError("No valid orientation data available") - - - -# let the present location be (0,0) -# we pass in the length of the side of the square cam area -# we pass in the number of loops around the origin, and has a default value of 20 - - -def landing_spot(a, number_of_loops=20): - route = [] - x, y = 0, 0 - for i in range(2, number_of_loops, 2): - - for j in range(4): - if j == 0: - y = y - a - route.append([x, y]) - for k in range(i - 1): - x = x + a - route.append([x, y]) - if j == 1: - for k in range(i): - y = y + a - route.append([x, y]) - if j == 2: - for k in range(i): - x = x - a - route.append([x, y]) - if j == 3: - for k in range(i): - y = y - a - route.append([x, y]) - print(route) - - -landing_spot(1, 15) - - - -# src/modules - -The different modules used by the Shepard software. - - - -# Source - -This directory contains the main scripts that are used when running Shepard. - - - -# For more options and information see -# http://rptl.io/configtxt -# Some settings may impact device functionality. See link above for details - -# Uncomment some or all of these to enable the optional hardware interfaces -#dtparam=i2c_arm=on -#dtparam=i2s=on -#dtparam=spi=on - -# Enable audio (loads snd_bcm2835) -dtparam=audio=on - -# Additional overlays and parameters are documented -# /boot/firmware/overlays/README - -# Automatically load overlays for detected cameras -camera_auto_detect=1 - -# Automatically load overlays for detected DSI displays -display_auto_detect=1 - -# Automatically load initramfs files, if found -auto_initramfs=1 - -# Enable DRM VC4 V3D driver -dtoverlay=vc4-kms-v3d -max_framebuffers=2 - -# Don't have the firmware create an initial video= setting in cmdline.txt. -# Use the kernel's default instead. -disable_fw_kms_setup=1 - -# Run in 64-bit mode -arm_64bit=1 - -# Disable compensation for displays with overscan -disable_overscan=1 - -# Run as fast as firmware / board allows -arm_boost=1 - -[cm4] -# Enable host mode on the 2711 built-in XHCI USB controller. -# This line should be removed if the legacy DWC2 controller is required -# (e.g. for USB device mode) or if USB support is not required. -otg_mode=1 - -[all] -enable_uart=1 - - - -# Test - -This directory contains automated test scripts (unit tests, etc.). - - - -# Tests can be run with ./scripts/test.sh -# They will run all files in the test/ directory starting with 'test_'. -# Then, all functions starting with 'test_' will be run in that file. If the -# function raises an error, the test fails. Otherwise, the test passes. -# See test/test_camera.py for an example. - -from typing import Optional - -from PIL import Image - -from src.modules.imaging.analysis import ImageAnalysisDelegate -from src.modules.imaging.camera import DebugCamera -from src.modules.imaging.location import DebugLocationProvider -from src.modules.imaging.debug import ImageAnalysisDebugger -from dep.labeller.benchmarks.detector import LandingPadDetector, BoundingBox -from dep.labeller.loader.label import Vec2 - - -class DebugLandingPadDetector(LandingPadDetector): - - def __init__(self, - vector: Optional[Vec2] = None, - bb: Optional[BoundingBox] = None): - self.bounding_box = bb - - def predict(self, _image: Image.Image) -> Optional[BoundingBox]: - return self.bounding_box - - -def test_analysis_subscriber(): - camera = DebugCamera("res/test-image.jpeg") - detector = DebugLandingPadDetector() - location_provider = DebugLocationProvider() - location_provider.set_altitude(1.0) - analysis = ImageAnalysisDelegate(detector, camera, location_provider) - - global detected - detected = None - - def _callback(_image, bounding_box, pos): - global detected - if pos is not None: - detected = Vec2(pos[0], pos[1]) - else: - detected = None - - analysis.subscribe(_callback) - - detector.bounding_box = None - analysis._analyze_image() - assert detected is None - detector.bounding_box = BoundingBox(Vec2(20, 20), Vec2(50, 50)) - analysis._analyze_image() - assert (detected - - Vec2(-115.48873916832288, 5.483286467459389e-06)).norm < 0.01 - - -class MockImageAnlaysisDebugger(ImageAnalysisDebugger): - - def __init__(self): - self.image: Optional[Image.Image] = None - self.bounding_box: Optional[BoundingBox] = None - self.is_visible = False - - def show(self): - #if not self.image: - #raise RuntimeError("No image set. Cannot show without an image") - self.is_visible = True - - def hide(self): - self.is_visible = False - - def visible(self) -> bool: - return self.is_visible - - def set_image(self, image: Image.Image): - self.image = image.copy() - - def set_bounding_box(self, bb: BoundingBox): - if not self.image: - return # return no image set error - - self.bounding_box = bb - - -def test_analysis_debugger(): - camera = DebugCamera("res/test-image.jpeg") - detector = DebugLandingPadDetector() - debug = MockImageAnlaysisDebugger() - location_provider = DebugLocationProvider() - location_provider.set_altitude(1.0) - analysis = ImageAnalysisDelegate(detector, camera, location_provider, - debug) - - def run_analysis(): - detector.bounding_box = BoundingBox(Vec2(0, 0), Vec2(100, 100)) - analysis._analyze_image() - assert debug.image is not None - assert debug.bounding_box == detector.bounding_box - - detector.bounding_box = None - analysis._analyze_image() - assert debug.image is not None - - # ImageAnalysisDelegate will not hide the debugger - debug.show() - run_analysis() - assert debug.is_visible - - # ImageAnalysisDelegate will not show the debugger - debug.hide() - run_analysis() - assert not debug.is_visible - - - -import threading -import time - -from dronekit import connect - -from src.modules.autopilot import navigator -from src.modules.autopilot import lander -from src.modules.imaging.mavlink import MAVLinkDelegate -from src.modules.imaging.battery import MAVLinkBatteryStatusProvider - -CONN_STR = "udp:127.0.0.1:14551" -MESSENGER_PORT = 14552 - -drone = connect(CONN_STR, wait_ready=False) - -nav = navigator.Navigator(drone, MESSENGER_PORT) -lander = lander.Lander() - -nav.send_status_message("Shepard is online") - -mavlink = MAVLinkDelegate() -battery = MAVLinkBatteryStatusProvider(mavlink) - - -def wait_for_voltage(): - while True: - try: - voltage = battery.voltage() - print("voltage: ", voltage) - except ValueError: - pass - # print("no data yet") - time.sleep(0.5) - - -threading.Thread(daemon=True, target=mavlink.run).start() - -while True: - nav.send_status_message(nav.sufficient_battery(battery)) - time.sleep(1) - - - -from PIL import Image -import hashlib -import os - -from src.modules.imaging.camera import DebugCamera - - -def md5sum(path: str | os.PathLike) -> bytes: - with open(path, "rb") as f: - contents = f.read() - return hashlib.md5(contents).digest() - - -def test_debug_camera(tmp_path): - cam = DebugCamera("res/test-image.jpeg") - - # Simple image capture - im = cam.capture() - assert im.width == 600 - assert im.height == 400 - - # Can capture the image as an ndarray - im_np = cam.caputure_as_ndarry() - assert im_np.shape == (400, 600, 3) - assert im_np.sum() == 121089435 # Stupid simple checksum - - # Can save the image to a path - im_path = tmp_path / "copy.jpeg" - cam.caputure_to(im_path) - im_md5 = md5sum(im_path) - - # Manually save a copy of 'test-image.jpeg'. - # This is required as Image.save() may differ slightly from - # 'test-image.jpeg' due to changes from re-compression. - og_im_path = tmp_path / "og.jpeg" - og_im = Image.open("res/test-image.jpeg") - og_im.save(og_im_path) - og_im_md5 = md5sum(og_im_path) - - assert im_md5 == og_im_md5 - - # Can shrink the size - cam.set_size((100, 100)) - im = cam.capture() - assert im.width == 100 - assert im.height == 100 - - # Can increase the size - cam.set_size((1000, 1000)) - im = cam.capture() - assert im.width == 1000 - assert im.height == 1000 - - # Can round-trip even after changing size - cam.set_size((600, 400)) - im = cam.capture() - assert im.tobytes() == og_im.tobytes() - - - -import math -from src.modules.imaging.location import (DebugLocationProvider, - MAVLinkLocationProvider, LatLng, - Heading, Rotation) -from src.modules.imaging.mavlink import MAVLinkDelegateMock -from pymavlink.dialects.v20 import all as dialect - - -def test_get_location(): - loc = DebugLocationProvider() - loc.debug_change_location(lat=10.0, lng=20.0) - - assert loc.location() == LatLng( - 10.0, 20.0), "Location did not match expected value" - - -def test_get_location_partial(): - loc = DebugLocationProvider() - loc.debug_change_location(lat=10.0, lng=20.0) - loc.debug_change_location(lat=15.0) # Missing lng - - assert loc.location() == LatLng( - 15.0, 20.0), "Location did not match expected value" - - -def test_get_heading(): - loc = DebugLocationProvider() - loc.debug_change_location(heading=45.0) - - assert loc.heading() == Heading( - 45.0), "Heading did not match expected value" - - -def test_get_altitude(): - loc = DebugLocationProvider() - loc.debug_change_location(altitude=100.0) - - assert loc.altitude() == 100.0, "Altitude did not match expected value" - - -def test_get_orientation(): - loc = DebugLocationProvider() - loc.debug_change_location(pitch=10.0, roll=20.0, yaw=30.0) - - assert loc.orientation() == Rotation( - 10.0, 20.0, 30.0), "Orientation did not match expected value" - - -def test_get_orientation_partial(): - loc = DebugLocationProvider() - loc.debug_change_location(pitch=10.0, roll=20.0, yaw=30.0) - loc.debug_change_location(pitch=12.0) - - assert loc.orientation() == Rotation( - 12.0, 20.0, 30.0), "Orientation did not match expected value" - - -def test_position_changes(): - loc = DebugLocationProvider() - - # Initial change - loc.debug_change_location(lat=10.0, lng=20.0) - assert loc.location() == LatLng( - 10.0, 20.0), "Initial location did not match expected value" - - # Change to new location - loc.debug_change_location(lat=40.0, lng=50.0) - assert loc.location() == LatLng( - 40.0, 50.0), "New location did not match expected value" - - -def test_get_MAVLink_location(): - mavlink = MAVLinkDelegateMock() - loc_mavlink = MAVLinkLocationProvider(mavlink) - - # Create a mock GLOBAL_POSITION_INT message for the initial location - initial_message = dialect.MAVLink_global_position_int_message( - time_boot_ms=0, # Assuming a timestamp of 0 for the test - lat=int(10.0 * 1e7), # Latitude in degE7 - lon=int(20.0 * 1e7), # Longitude in degE7 - alt=1000, # Altitude in mm above MSL - relative_alt=500, # Altitude in mm above the ground - vx=0, - vy=0, - vz=0, # Ground X, Y, Z Speed - hdg=0 # Heading - ) - mavlink.send(initial_message) - assert loc_mavlink.location() == LatLng( - 10.0, 20.0), "Initial location did not match expected value" - - # Create a mock GLOBAL_POSITION_INT message for the new location - new_message = dialect.MAVLink_global_position_int_message( - time_boot_ms=0, # Assuming a timestamp of 0 for the test - lat=int(40.0 * 1e7), # Latitude in degE7 - lon=int(50.0 * 1e7), # Longitude in degE7 - alt=2000, # Altitude in mm above MSL - relative_alt=1000, # Altitude in mm above the ground - vx=0, - vy=0, - vz=0, # Ground X, Y, Z Speed - hdg=0 # Heading - ) - mavlink.send(new_message) - assert loc_mavlink.location() == LatLng( - 40.0, 50.0), "New location did not match expected value" - - -def test_get_MAVLink_heading(): - mavlink = MAVLinkDelegateMock() - loc_mavlink = MAVLinkLocationProvider(mavlink) - - # Create a mock VFR_HUD message for heading - heading_message = dialect.MAVLink_global_position_int_message( - time_boot_ms=0, - lat=0, - lon=0, - alt=2000, # Altitude in mm above MSL - relative_alt= - 0, # Relative altitude in mm above the ground (not used in this test) - vx=0, - vy=0, - vz=0, # Ground X, Y, Z Speed (not used in this test) - hdg=150 * 1e7 # Heading in degE7 - ) - mavlink.send(heading_message) - - # Check if the MAVLinkLocationProvider reports the heading correctly - assert loc_mavlink.heading() == Heading( - 150), "Heading did not match expected value" - - -def test_get_MAVLink_altitude(): - mavlink = MAVLinkDelegateMock() - loc_mavlink = MAVLinkLocationProvider(mavlink) - - # Create a mock GLOBAL_POSITION_INT message for altitude - altitude_message = dialect.MAVLink_global_position_int_message( - time_boot_ms=0, # Timestamp for the test - lat=0, # Dummy latitude - lon=0, # Dummy longitude - alt=15000, # Altitude in mm above MSL - relative_alt=0, # Relative altitude in mm above the ground - vx=0, - vy=0, - vz=0, # Ground X, Y, Z Speed - hdg=0 # Heading - ) - mavlink.send(altitude_message) - - # Check if the MAVLinkLocationProvider reports the altitude correctly - assert loc_mavlink.altitude( - ) == 15.0, "Altitude did not match expected value in meters" - - -def test_get_MAVLink_orientation(): - mavlink = MAVLinkDelegateMock() - loc_mavlink = MAVLinkLocationProvider(mavlink) - - # Create a mock ATTITUDE message for orientation - orientation_message = dialect.MAVLink_attitude_message( - time_boot_ms=0, # Timestamp for the test - pitch=0.1, # Pitch in radians - roll=0.2, # Roll in radians - yaw=0.3, # Yaw in radians - rollspeed=0, # Roll speed (not used in this test) - pitchspeed=0, # Pitch speed (not used in this test) - yawspeed=0 # Yaw speed (not used in this test) - ) - mavlink.send(orientation_message) - - # Check if the MAVLinkLocationProvider reports the orientation correctly - expected_orientation = Rotation(math.degrees(0.1), math.degrees(0.2), - math.degrees(0.3)) - actual_orientation = loc_mavlink.orientation() - assert actual_orientation == expected_orientation, "Orientation did not match expected value" - - -def test_get_MAVLink_position_changes(): - mavlink = MAVLinkDelegateMock() - loc_mavlink = MAVLinkLocationProvider(mavlink) - - # Send a mock GLOBAL_POSITION_INT message for initial position - initial_position_message = dialect.MAVLink_global_position_int_message( - time_boot_ms=0, - lat=int(10.0 * 1e7), - lon=int(20.0 * 1e7), - alt=1000, - relative_alt=500, - vx=0, - vy=0, - vz=0, - hdg=0.0) - mavlink.send(initial_position_message) - - # Verify initial position - assert loc_mavlink.location() == LatLng( - 10.0, 20.0), "Initial location did not match expected value" - - # Send another mock GLOBAL_POSITION_INT message for new position - new_position_message = dialect.MAVLink_global_position_int_message( - time_boot_ms=1000, # Simulate a time after the initial message - lat=int(40.0 * 1e7), - lon=int(50.0 * 1e7), - alt=2000, - relative_alt=1000, - vx=0, - vy=0, - vz=0, - hdg=45.0 * 1e7) - mavlink.send(new_position_message) - - # Verify new position - assert loc_mavlink.location() == LatLng( - 40.0, 50.0), "New location did not match expected value" - assert loc_mavlink.heading() == Heading( - 45.0), "New heading did not match expected value" - assert loc_mavlink.altitude( - ) == 2.0, "New altitude did not match expected value in meters" - - -def test_get_MAVLink_position_not_init(): - mavlink = MAVLinkDelegateMock() - loc_mavlink = MAVLinkLocationProvider(mavlink) - - def assert_raises(f): - try: - f() - except ValueError: - pass - else: - assert False, "Did not raise when accessing an uninitialized value" - - methods = [ - loc_mavlink.altitude, - loc_mavlink.location, - loc_mavlink.heading, - loc_mavlink.orientation, - ] - for method in methods: - assert_raises(method) - - - -# Tools - -This directory contains convenience scripts, such as setup scripts, conversion tools, etc. - - - -from src.modules.imaging.detector import IrDetector -from src.modules.imaging.analysis import ImageAnalysisDelegate -from src.modules.imaging.camera import RPiCamera -from src.modules.imaging.location import DebugLocationProvider - - -camera = RPiCamera() -detector = IrDetector() -location = DebugLocationProvider() - - -def test(img, _): - print("Image taken") - - - -analysis = ImageAnalysisDelegate(detector, camera, location) -analysis.subscribe(test) - -analysis.start() - - - -""" -benchmark the speed of different trained yolo models -""" - -from ultralytics import YOLO -import os -import time -import math -import json - - -class ModelStats: - def __init__(self): - self.n = 0 - self.mean = 0.0 - self.M2 = 0.0 - def update(self, t): - self.n += 1 - delta = t-self.mean - self.mean += delta / self.n - delta2 = t - self.mean - self.M2 += delta * delta2 - def get_mean(self): - return self.mean - - def get_stddev(self): - return math.sqrt(self.M2 / self.n) if self.n > 1 else 0.0 - - -models = os.listdir("models") - -images = os.listdir("images") -images = [ os.path.join("images", file_name) for file_name in images ] - -stats = {} - -for model_name in models: - print(model_name) - model = YOLO(os.path.join("models", model_name)) - - model_stats = ModelStats() - - for img_name in images: - start = time.time() - results = model(img_name) - end = time.time() - model_stats.update(end - start) - print(f"\tmean: {model_stats.get_mean()}") - stats[model_name] = {"mean": model_stats.get_mean(), "stddev": model_stats.get_stddev()} - - -with open("results.json", "w") as f: - json.dump(stats, f) - - - -import time - -from dronekit import connect - -from src.modules.autopilot import navigator -from src.modules.autopilot.altimeter_xm125 import XM125 -from src.modules.autopilot.altimeter_mavlink import MavlinkAltimeterProvider - -# Connection settings -CONN_STR = "tcp:127.0.0.1:14550" -MESSENGER_PORT = 14550 -MAVLINK_ALTITUDE_CONN_STR = "tcp:127.0.0.1:14550" - -AltimeterData = [] -PixHawkData = [] -Delta = [] - -# Test settings -STATUS_INTERVAL = 2.0 # seconds -TEST_DURATION = 600 # 10 minutes - -# Connect to the drone -drone = connect(CONN_STR, wait_ready=False) - -# Initialize navigator -nav = navigator.Navigator(drone, MESSENGER_PORT) -nav.send_status_message("Altimeter test initializing") - -# Initialize the XM125 radar altimeter -radar_sensor = XM125( - sensor_id=0, - min_distance=250, - max_distance=10000, - average_window=5 -) - - -if not radar_sensor.begin(): - nav.send_status_message("ERROR: Failed to initialize radar sensor") - drone.close() - exit(1) - -# Create and start the MAVLink altimeter provider -mavlink_altimeter = MavlinkAltimeterProvider(radar_sensor, MAVLINK_ALTITUDE_CONN_STR) -mavlink_altimeter.start() - -nav.send_status_message("XM125 Altimeter test starting") - -try: - nav.send_status_message("Forwarding altitude data to Pixhawk") - - # Main loop - forward altitude data and print status - last_status_time = time.time() - end_time = time.time() + TEST_DURATION - - while time.time() < end_time: - # Check if it's time to send a status message - current_time = time.time() - if current_time - last_status_time >= STATUS_INTERVAL: - - # Get the latest altimeter reading and log - result = radar_sensor.measure() - if result: - average = result[0]['averaged'] - - if average: - AltimeterData.append(average[0]) - - # Get the latest altitude reading and log - altitude_m = mavlink_altimeter.get_latest_altitude_meters() - if altitude_m is not None: - status_msg = f"Radar Altitude: {altitude_m:.2f} m" - nav.send_status_message(status_msg) - PixHawkData.append(altitude_m) - Delta.append(float(altitude_m) - float(average[0])) - - else: - nav.send_status_message("No valid altitude reading") - - last_status_time = current_time - - # Sleep to avoid consuming too much CPU - time.sleep(0.5) - -except KeyboardInterrupt: - nav.send_status_message("Test interrupted by user") -except Exception as e: - nav.send_status_message(f"ERROR: {str(e)}") -finally: - # Clean up - nav.send_status_message("Stopping altitude provider") - mavlink_altimeter.stop() - - nav.send_status_message("Altimeter test completed") - drone.close() - - - -import time - -from dronekit import connect - -from src.modules.autopilot import navigator -from src.modules.autopilot.altimeter_xm125 import XM125 -# from src.modules.autopilot.altimeter_mavlink import MavlinkAltimeterProvider - -import json - -# Connection settings -CONN_STR = "tcp:127.0.0.1:14550" -MESSENGER_PORT = 14550 -MAVLINK_ALTITUDE_CONN_STR = "tcp:127.0.0.1:14550" - -AltimeterData = [] -PixHawkData = [] -Delta = [] - -# Test settings -STATUS_INTERVAL = 2.0 # seconds -TEST_DURATION = 600 # 10 minutes - -# Connect to the drone -drone = connect(CONN_STR, wait_ready=False) - -# Initialize navigator -nav = navigator.Navigator(drone, MESSENGER_PORT) -nav.send_status_message("SHEPARD: Altimeter test initializing") - -# Initialize the XM125 radar altimeter -radar_sensor = XM125( - sensor_id=0, - min_distance=250, - max_distance=10000, - average_window=5 -) - - -if not radar_sensor.begin(): - nav.send_status_message("ERROR: Failed to initialize radar sensor") - drone.close() - exit(1) - - -nav.send_status_message("SHEPARD: XM125 Altimeter test starting") - -try: - nav.send_status_message("Forwarding altitude data to Pixhawk") - - # Main loop - forward altitude data and print status - last_status_time = time.time() - end_time = time.time() + TEST_DURATION - - while time.time() < end_time: - # Check if it's time to send a status message - current_time = time.time() - if current_time - last_status_time >= STATUS_INTERVAL: - - # Get the latest altimeter reading and log - result = radar_sensor.measure() - if result: - average = result[0]['averaged'] - - if average: - AltimeterData.append(average[0]) - - # Get the latest altitude reading and log - altitude_m = float(drone.location.global_relative_frame.alt) - if altitude_m is not None: - status_msg = f"Radar Altitude: {altitude_m:.2f} m" - nav.send_status_message(status_msg) - PixHawkData.append(altitude_m) - Delta.append(float(altitude_m) - float(average[0])) - - else: - nav.send_status_message("No valid altitude reading") - - last_status_time = current_time - - # Sleep to avoid consuming too much CPU - time.sleep(0.5) - -except KeyboardInterrupt: - nav.send_status_message("Test interrupted by user") -except Exception as e: - nav.send_status_message(f"ERROR: {str(e)}") -finally: - # Clean up - nav.send_status_message("Stopping altitude provider") - - # Log altimeter data to json file - with open("./logs/log1.json", "w", encoding="utf-8") as file: - data = { - "AltimeterData": AltimeterData, - "PixHawkData": PixHawkData, - "Delta": Delta - } - data = json.dumps(data) - - file.write(data) - - nav.send_status_message("SHEPARD: Altimeter test completed") - drone.close() - - - -import math -import os -# import threading -import time - -from dronekit import connect, VehicleMode - -from src.modules.autopilot import navigator -from src.modules.autopilot.altimeter_xm125 import XM125 -from src.modules.autopilot.altimeter_mavlink import MavlinkAltimeterProvider - -# from src.modules.imaging.camera import RPiCamera -from src.modules.imaging.detector import Detector - -# Connection settings -CONN_STR = "tcp:127.0.0.1:14550" -MESSENGER_PORT = 14550 -MAVLINK_ALTITUDE_CONN_STR = "tcp:127.0.0.1:14550" - -# Connect to the drone -drone = connect(CONN_STR, wait_ready=False) - -# Imaging setup -detector = Detector() - -# Initialize navigator -nav = navigator.Navigator(drone, MESSENGER_PORT) -nav.send_status_message("Altimeter test initializing") - -# Initialize the XM125 radar altimeter -radar_sensor = XM125( - sensor_id=0, - min_distance=250, - max_distance=10000, - average_window=5 -) - -if not radar_sensor.begin(): - nav.send_status_message("ERROR: Failed to initialize radar sensor") - drone.close() - exit(1) - -# Create and start the MAVLink altimeter provider -mavlink_altimeter = MavlinkAltimeterProvider(radar_sensor, MAVLINK_ALTITUDE_CONN_STR) -mavlink_altimeter.start() - -nav.send_status_message("SHEPARD is online") - -try: - while not (drone.armed and drone.mode == VehicleMode("GUIDED")): - pass - - nav.send_status_message("Executing mission") - time.sleep(2) - - nav.takeoff(10) - time.sleep(2) - - nav.send_status_message("Starting balloon search") - - current_pic = 0 - - dirs = os.listdir("tmp/log") - ft_num = len(dirs) - - while True: - step_size = 1 # meters - last_pic = current_pic - distance, direction, current_pic = detector.process_image_directory(directory_path=f"tmp/log/{ft_num}") - - if current_pic == last_pic: - continue - - if direction is not None: - nav.send_status_message(f"Balloon detected: Move {direction}, Distance: {distance:.2f}") - - if direction == "center": - # Move in that direction, need to calculate the N and E offsets based on heading - current_heading = drone.heading - metres_north_relative = step_size * math.sin(math.radians(current_heading)) - metres_east_relative = step_size * math.cos(math.radians(current_heading)) - nav.set_position_relative(metres_north_relative, metres_east_relative) - elif direction == "right": - nav.set_heading_relative(10) - elif direction == "left": - nav.set_heading_relative(-10) - - else: - nav.send_status_message("No balloons detected") - nav.set_heading_relative(10) - -except KeyboardInterrupt: - nav.send_status_message("Script interrupted by user") -except Exception as e: - nav.send_status_message(f"ERROR: {str(e)}") -finally: - nav.send_status_message("Ending mission") - - # Ensure drone RTLs - nav.return_to_launch() - - # Clean up - nav.send_status_message("Stopping altitude provider") - mavlink_altimeter.stop() - - nav.send_status_message("Altimeter test completed") - drone.close() - - - -{ - "type": "FeatureCollection", - "features": [ - { - "type": "Feature", - "properties": {}, - "geometry": { - "coordinates": [ - [ - [ - -113.54797355858594, - 53.49538269557871 - ], - [ - -113.54769792522994, - 53.49557167851731 - ], - [ - -113.5479829020893, - 53.495734721944046 - ], - [ - -113.5484064742526, - 53.495672654349875 - ], - [ - -113.5486322755893, - 53.495509610684024 - ], - [ - -113.54837844374161, - 53.495381769189436 - ], - [ - -113.54797355858594, - 53.49538269557871 - ] - ] - ], - "type": "Polygon" - } - } - { - ] -} - - - -import math -import time -from datetime import datetime - -import dronekit -from pymavlink import mavutil - -from src.modules.autopilot.messenger import Messenger - - -class Navigator: - """ - A class to handle navigation. - """ - - vehicle: dronekit.Vehicle | None = None - POSITION_TOLERANCE = 1 - - def __init__(self, vehicle, messenger_port): - self.vehicle = vehicle - self.mavlink_messenger = Messenger(messenger_port) - - def send_status_message(self, message): - self.__message(message) - - def __message(self, msg): - """ - Prints a message to the console. - :param msg: The message to print. - :return: None - """ - - print(f"SHEPARD_NAV: {msg}") - self.mavlink_messenger.send(msg) - - def takeoff(self, target_alt): - """ - Takes off to a given altitude. - - :param target_alt: The target altitude in meters. - :return: True if successful, False otherwise. - """ - - self.__message(f"Taking off to {target_alt} m") - - self.vehicle.simple_takeoff(target_alt) - - while self.vehicle.location.global_relative_frame.alt < target_alt - 1: - time.sleep(1) - - return True - - def set_position(self, lat, lon): - """ - Moves the vehicle to a given position. - - :param lat: The latitude of the target position. - :param lon: The longitude of the target position. - :return: None - """ - - self.__message(f"Moving to {lat}, {lon}") - - target_location = dronekit.LocationGlobalRelative( - lat, lon, self.vehicle.location.global_relative_frame.alt) - self.vehicle.simple_goto(target_location) - - while self.vehicle.mode.name == "GUIDED": - remaining_distance = self.__get_distance_metres( - self.vehicle.location.global_relative_frame, target_location) - self.__message(f"Distance to target: {remaining_distance} m") - if remaining_distance <= self.POSITION_TOLERANCE: - self.__message("Reached target") - break - time.sleep(2) - - def set_position_relative(self, d_north, d_east): - """ - Moves the vehicle to a given position relative to its current position. - - :param d_north: The distance to move north in meters. - :param d_east: The distance to move east in meters. - :return: None - """ - - self.__message(f"Moving {d_north} m north and {d_east} m east") - - current_location = self.vehicle.location.global_relative_frame - target_location = self.__get_location_metres(current_location, d_north, - d_east) - - self.vehicle.simple_goto(target_location) - - while self.vehicle.mode.name == "GUIDED": - remaining_distance = self.__get_distance_metres( - self.vehicle.location.global_relative_frame, target_location) - self.__message(f"Distance to target: {remaining_distance} m") - if remaining_distance <= self.POSITION_TOLERANCE: - self.__message("Reached target") - break - time.sleep(2) - - def get_local_position_ned(self): - # Gets the current location of the drone in the local NED frame - location = self.vehicle.location.local_frame - return (location.north, location.east, location.down) - - def get_global_position(self): - location = self.vehicle.location.global_frame - return (location.lat, location.lon, location.alt) - - def set_heading(self, heading): - """ - Sets the heading of the vehicle. - - :param heading: The heading in degrees. - :return: None - """ - - self.__message(f"Changing heading to {heading} degrees") - - # self.vehicle.commands.condition_yaw(heading) - self.__condition_yaw(heading) - - def set_heading_relative(self, heading): - """ - Sets the heading of the vehicle relative to its current heading. - - :param heading: The heading in degrees. - :return: None - """ - - self.__message( - f"Changing heading to {heading} degrees relative to current heading" - ) - - # self.vehicle.commands.condition_yaw(heading, relative=True) - self.__condition_yaw(heading, relative=True) - - def set_altitude(self, altitude): - """ - Sets the altitude of the vehicle. - - :param altitude: The altitude in meters. - :return: None - """ - - self.__message(f"Setting altitude to {altitude} m") - - target_altitude = dronekit.LocationGlobalRelative( - self.vehicle.location.global_relative_frame.lat, - self.vehicle.location.global_relative_frame.lon, - altitude, - ) - self.vehicle.simple_goto(target_altitude) - - while self.vehicle.mode.name == "GUIDED": - remaining_distance = abs( - self.vehicle.location.global_relative_frame.alt - - target_altitude.alt) - self.__message(f"Distance to target: {remaining_distance} m") - if remaining_distance <= self.POSITION_TOLERANCE: - self.__message("Reached target") - break - time.sleep(2) - - def set_altitude_relative(self, altitude): - """ - Sets the altitude of the vehicle relative to its current altitude. - - :param altitude: The altitude relative to current altitude in meters, positive value to ascend and negative to descend. - :return: None - """ - - self.__message( - f"Setting altitude to {altitude} m relative to current altitude") - - target_altitude = dronekit.LocationGlobalRelative( - self.vehicle.location.global_relative_frame.lat, - self.vehicle.location.global_relative_frame.lon, - self.vehicle.location.global_relative_frame.alt + altitude, - ) - self.vehicle.simple_goto(target_altitude) - - while self.vehicle.mode.name == "GUIDED": - remaining_distance = self.__get_distance_metres( - self.vehicle.location.global_relative_frame, target_altitude) - self.__message(f"Distance to target: {remaining_distance} m") - if remaining_distance <= self.POSITION_TOLERANCE: - self.__message("Reached target") - break - time.sleep(2) - - def set_altitude_position(self, - lat, - lon, - alt, - battery=None, - voltage_hard_cutoff=22.4, - hard_cutoff_enable=False): - """ - Sets the altitude and the position in absolute terms - - :param lat: The latitude of the target position. - :param lon: The longitude of the target position. - :param alt: The target altitude in metres - :param battery: MAVLinkBatteryStatusProvider object - :return: None - """ - self.__message(f"Moving to lat: {lat} lon: {lon} alt: {alt}") - - target_altitude_position = dronekit.LocationGlobalRelative( - lat, lon, alt) - - if hard_cutoff_enable: - if self.sufficient_battery(battery, voltage_hard_cutoff): - self.__message("--------Hard Cutoff reached----------") - self.__message("--------Returning to Launch----------") - self.return_to_launch() - return - - self.vehicle.simple_goto(target_altitude_position) - - while self.vehicle.mode.name == "GUIDED": - remaining_distance = self.__get_distance_metres( - self.vehicle.location.global_relative_frame, - target_altitude_position) - self.__message(f"Distance to target: {remaining_distance} m") - if remaining_distance <= self.POSITION_TOLERANCE: - self.__message("Reached target") - break - time.sleep(2) - - def set_altitude_position_relative(self, d_north, d_east, alt): - """ - Sets the altitude and the position relative to current position - - :param d_north: The distance to be moved north. - :param d_east: The distance to be moved east. - :param alt: Altitude to be moved in metres. - :return: None - """ - - self.__message( - f"Moving {d_north} m north and {d_east} m east and {alt} m in altitude" - ) - - current_location = self.vehicle.location.global_relative_frame - target_location = self.__get_location_metres(current_location, d_north, - d_east) - target_location.alt += alt - - self.vehicle.simple_goto(target_location) - - while self.vehicle.mode.name == "GUIDED": - remaining_distance = self.__get_distance_metres( - self.vehicle.location.global_relative_frame, target_location) - self.__message(f"Distance to target: {remaining_distance} m") - if remaining_distance <= self.POSITION_TOLERANCE: - self.__message("Reached target") - break - time.sleep(2) - - def land(self): - """ - Lands the vehicle. - - :return: None - """ - - self.__message("Landing") - self.vehicle.mode = dronekit.VehicleMode("LAND") - - def precision_landing(self, lat, long, alt): - """ - Experimenting with precision landing and utilizing the PyMavlink precision landing function. - Creates and sends a precision landing message to the mavlink - Requires landing target data - """ - - abort_land_alt = 5 - - msg = self.vehicle.message_factory.mav_cmd_nav_land( - abort_land_alt, # minimum altitude if landing is aborted - 2, # Enable precision landing mode - 0, # blank - 0, # Desired Yaw Angle - lat, # Latitude of target - long, # Longitude of the target - alt # Altitude of the ground in the current reference frame - ) - self.vehicle.send_mavlink(msg) - - def return_to_launch(self): - """ - Returns the vehicle to its launch position. - - :return: None - """ - - self.__message("Returning to launch") - self.vehicle.mode = dronekit.VehicleMode("RTL") - - def set_speed(self, speed): - """ - Set the vehicle speed. - - :param speed: The target speed in m/s. - :return: None - """ - - self.__message(f"Setting speed to {speed} m/s") - msg = self.vehicle.message_factory.command_long_encode( - 0, - 0, # target system, target component - mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # command - 0, # confirmation - 0, # speed type, ignored in Copter - speed, # speed - 0, - 0, - 0, - 0, - 0, # ignore other parameters - ) - - self.vehicle.send_mavlink(msg) - self.vehicle.flush() - - def __get_location_metres(self, original_location, d_north, d_east): - """ - Returns a `LocationGlobalRelative` object containing the latitude/longitude `d_north` and `d_east` metres from the - specified `original_location`. The returned `LocationGlobalRelative` has the same `alt` value as `original_location`. - - :param original_location: The reference `LocationGlobal`. - :param d_north: The distance to the north in meters. - :param d_east: The distance to the east in meters. - :return: A `LocationGlobalRelative` object. - """ - - earth_radius = 6378137.0 # Radius of "spherical" earth - # Coordinate offsets in radians - d_lat = d_north / earth_radius - d_lon = d_east / (earth_radius * - math.cos(math.pi * original_location.lat / 180)) - - # New position in decimal degrees - new_lat = original_location.lat + (d_lat * 180 / math.pi) - new_lon = original_location.lon + (d_lon * 180 / math.pi) - if type(original_location) is dronekit.LocationGlobal: - target_location = dronekit.LocationGlobal(new_lat, new_lon, - original_location.alt) - elif type(original_location) is dronekit.LocationGlobalRelative: - target_location = dronekit.LocationGlobalRelative( - new_lat, new_lon, original_location.alt) - else: - raise Exception("Invalid Location object passed") - - return target_location - - def __get_distance_metres(self, location_1, location_2): - """ - Returns the ground distance in metres between two `LocationGlobal` or `LocationGlobalRelative` objects. - - This method is an approximation, and will not be accurate over large distances and close to the earth's poles. - - :param location_1: The first `LocationGlobal` or `LocationGlobalRelative` object. - :param location_2: The second `LocationGlobal` or `LocationGlobalRelative` object. - :return: The ground distance in metres. - """ - - d_lat = location_2.lat - location_1.lat - d_lon = location_2.lon - location_1.lon - - return math.sqrt((d_lat * d_lat) + (d_lon * d_lon)) * 1.113195e5 - - def __condition_yaw(self, heading, relative=False): - if relative: - is_relative = 1 # yaw relative to direction of travel - else: - is_relative = 0 # yaw is an absolute angle - # create the CONDITION_YAW command using command_long_encode() - msg = self.vehicle.message_factory.command_long_encode( - 0, - 0, # target system, target component - mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command - 0, # confirmation - heading, # param 1, yaw in degrees - 0, # param 2, yaw speed deg/s - 1, # param 3, direction -1 ccw, 1 cw - is_relative, # param 4, relative offset 1, absolute angle 0 - 0, - 0, - 0, - ) # param 5 ~ 7 not used - # send command to vehicle - self.vehicle.send_mavlink(msg) - - @staticmethod - def time_left(string_land_time): - """ - Calculates the time between now and the time passed as a string arguement. - - :param string_land_time: the target landing time as a string in the format "HH:MM:SS" - - :return: time left in seconds - """ - time_now = datetime.now().time() - - land_time = datetime.strptime(string_land_time, "%H:%M:%S") - - seconds_now = (time_now.hour * 360) + (time_now.minute * - 60) + (time_now.second) - - seconds_land = (land_time.hour * 360) + (land_time.minute * - 60) + (land_time.second) - - return seconds_land - seconds_now - - def optimum_speed(self, time_left, waypoints): - """ - Finds the optimum horizontal speed required to go from current position to all waypoints and land within the given time - - :param time_left: The time left to land in [seconds], initially the land time minus the takeoff time. - :param waypoints: List of all remaining way points to go to. - :return: required ground speed in [m/s]. - """ - - self.__message("Calculating optimum horizontal speed") - - total_distance = self.__get_distance_metres( - self.vehicle.location.global_relative_frame, waypoints[0]) - for i in range(1, len(waypoints)): - total_distance += self.__get_distance_metres( - waypoints[i - 1], waypoints[i]) - - speed_required = total_distance / time_left - self.__message( - f"Speed required to travel {total_distance} m in {time_left} s is {speed_required} m/s" - ) - - return speed_required - - def sufficient_battery(self, battery, voltage_required=10.0): - """ - Returns True or False based on whether or not the drone has enough battery to do another lap. - Each lap is when drone starts at Alpha, goes to Bravo and comes back to Alpha. - So we check if drone has little more battery than what is required for twice the distance between Alpha and Bravo. - - :param voltage_required: value of voltage required to travel the lap distance - (Distance between Alpha and Bravo in Alma is 3km, so hard code this to voltage required for 6km of flight) - :param battery: instance of the MAVLinkBatteryStatusProvider - - :return: a Boolean value, True indicating the drone has sufficient battery for another lap. - """ - - self.__message("Attempting to read voltage reading") - while True: - try: - voltage = battery.voltage() - break - except ValueError: - pass - time.sleep(0.1) - - self.__message(f"Current battery voltage is {voltage} V") - - if voltage < voltage_required: - return False - - return True - - def generate_typemask(self, keeps): - # Generates typemask based on which values to be included - mask = 0 - for bit in keeps: - mask |= (0 << bit) - return mask - - def set_position_target_local_ned( - self, - time_boot_ms=0, - coordinate_frame=mavutil.mavlink.MAV_FRAME_LOCAL_NED, - type_mask=0x07FF, - x=0, y=0, z=0, - vx=0, vy=0, vz=0, - afx=0, afy=0, afz=0, - yaw=0, yaw_rate=0): - msg = self.vehicle.message_factory.set_position_target_local_ned_encode( - time_boot_ms, # Time since system boot - 0, # Target System ID - 0, # Target Component ID - coordinate_frame, # Coordinate Frame - type_mask, # Typemask of POSITION_TARGET_TYPEMASK - x, - y, - z, - vx, - vy, - vz, - afx, - afy, - afz, - yaw, - yaw_rate - ) - - self.vehicle.send_mavlink(msg) - - ''' - def cancel_command(self, command_id=mavutil.mavlink.SET_POSITION_TARGET_LOCAL_NED): - msg = self.vehicle.message_factory.command_long_encode( - 0, - 0, - mavutil.mavlink.COMMAND_CANCEL, - 0, - 0, - command_id - ) - - self.vehicle.send_mavlink(msg) - ''' - - - -from .emu import * - - - -from mavlink import MAVLinkDelegate -import pymavlink.dialects.v20.all as dialect -from .detector import BoundingBox - - -class AnalysisView: - """ - Sends debugging messages to ground station through mavlink protocol. - """ - - def __init__(self, mavlinkDelegate: MAVLinkDelegate) -> None: - self.mavlink = mavlinkDelegate - - def send_BoundingBox(self, bbox: BoundingBox) -> None: - position = bbox.position - size = bbox.size - values = [position.x, position.y, size.x, size.y] - for i in range(4, 58): - values[i] = 0.0 - - dbg_box = dialect.MAVLink_debug_float_array_message( - name=bytes("dbg_box", 'utf-8'), - time_usec=0, # timestamp - array_id=0, - data=values) - self.mavlink.send(dbg_box) - - - -from typing import Optional, Tuple -from PIL import Image, ImageDraw, ImageTk -import tkinter as tk -from .detector import BoundingBox - - -class ImageAnalysisDebugger: - """ - Helper class to display a debug window containing image analysis results. - - Note, all methods are non-blocking. This means, ie. calling show() will not - block the program until the window is closed. This debugger will run in the - background and should not interfere in any way. - """ - - def __init__(self): - self.image: Optional[Image.Image] = None - self.root = tk.Tk() - self.is_visible = False - - def show(self): - """ - Start displaying the debugger window. - """ - if not self.image: - raise RuntimeError("No image set. Cannot show without an image") - - self.root.deiconify() - self.is_visible = True - img = ImageTk.PhotoImage(self.image) - self.root.geometry('%dx%d' % (self.image.size[0], self.image.size[1])) - label_image = tk.Label(self.root, image=img) - label_image.place(x=0, - y=0, - width=self.image.size[0], - height=self.image.size[1]) - self.root.update() - - def hide(self): - """ - Stop displaying the debugger window. - """ - self.root.withdraw() - self.is_visible = False - - def visible(self) -> bool: - """ - Returns True if the debug window is visible. - False if it is no longer visible because: - - show() has not been called - - hide() was called - - the user closed the window - """ - return self.is_visible - - def set_image(self, image: Image.Image): - """ - Update the image currently being analysed. Will remove any old bounding boxes. - """ - self.image = image.copy() - - def set_bounding_box(self, bb: BoundingBox): - """ - Update the bounding box currently displaying the image being debugged. - """ - if not self.image: - return # return no image set error - - image = self.image - top_left_corner: Tuple[float, float] = (bb.position.x, bb.position.y) - bottom_right_corner: Tuple[float, float] = (bb.position.x + bb.size.x, - bb.position.y + bb.size.y) - - draw = ImageDraw.Draw(image) - draw.rectangle((top_left_corner, bottom_right_corner)) - - - -from typing import List, Callable - -from pymavlink import mavutil -import pymavlink.dialects.v20.all as dialect -import time - - -class MAVLinkDelegate: - """ - MAVLink connection delegate which forwards messages to subscribers. - """ - - def __init__(self, conn_str: str = "tcp:127.0.0.1:14550"): - self._conn = mavutil.mavlink_connection(device=conn_str, - source_system=255, - source_component=42) - - self._listeners: List[Callable] = [] - - def subscribe(self, listener: Callable): - """ - Subscribe to messages received from the mavlink connection. - """ - self._listeners.append(listener) - - def send(self, mav_message: dialect.MAVLink_message): - """ - Sends a mavlink message. - """ - self._conn.mav.send(mav_message) - - def run(self): - """ - Start the mavlink delegate. Will never return. - """ - last_heartbeat = time.time() - while True: - msg = self._conn.recv_match(blocking=False) - if msg: - for listener in self._listeners: - listener(msg) - - continue # Check for more messages immediately - - now = time.time() - if now - last_heartbeat > 1: - last_heartbeat = now - self.send(dialect.MAVLink_heartbeat_message(0, 0, 0, 0, 0, 0)) - - time.sleep(0.0001) # 100 us - - -class MAVLinkDelegateMock(MAVLinkDelegate): - """ - Mock MAVLink connection delegate which forwards messages to subscribers. - """ - - def __init__(self): - self._listeners = [] - - def send(self, mav_message: dialect.MAVLink_message): - """ - Sends a mavlink message. - """ - for listener in self._listeners: - listener(mav_message) - - def run(self): - """ - Not implemented - """ - raise AssertionError("Not implemented") - - -class MessagePrinter: - - def __init__(self, mavlink_delegate: MAVLinkDelegate): - self.mavlink_delegate = mavlink_delegate - self.mavlink_delegate.subscribe(self._process_message) - - def _process_message(self, message: dialect.MAVLink_message): - print(message) - - -if __name__ == "__main__": - mavlink_delegate = MAVLinkDelegate() - message_printer = MessagePrinter(mavlink_delegate) - print("Starting mavlink_delegate.run()") - mavlink_delegate.run() - - - -[submodule "src/modules/mavctl"] - path = src/modules/mavctl - url = git@github.com:uaarg/mavctl-python.git - - - -#!/usr/bin/env bash - -source venv/bin/activate -logfile="log/$(date +'%x-%X').log" -imgfile="log/$(date +'%x-%X').imglog" -mkdir -p "$(dirname "$logfile")" -mkdir -p "$(dirname "$imgfile")" - -PYTHONUNBUFFERED=1 PYTHONPATH=".:dep/labeller:dep/labeller/third_party/yolov5" python3 src/aeac2025/task2.py 2>&1 | tee "$logfile" & -PYTHONUNBUFFERED=1 PYTHONPATH=".:dep/labeller:dep/labeller/third_party/yolov5" python3 src/video_server/server.py 2>&1 | tee "$imgfile" - - - -dronekit==2.9.2 -dronekit-sitl==3.3.0 -exceptiongroup==1.2.0 -flake8==6.1.0 -future==0.18.3 -importlib-metadata==7.0.1 -iniconfig==2.0.0 -lxml==5.1.0 -mccabe==0.7.0 -monotonic==1.6 -mypy==1.8.0 -mypy-extensions==1.0.0 -numpy==1.26.3 -opencv-python==4.9.0.80 -packaging==23.2 -pillow==10.2.0 -platformdirs==4.1.0 -pluggy==1.3.0 -psutil==5.9.7 -pycodestyle==2.11.1 -pyflakes==3.1.0 -pymavlink==2.4.41 -pyproj==3.6.1 -pytest==7.4.4 -six==1.16.0 -tomli==2.0.1 -types-Pillow==10.2.0.20240125 -typing_extensions==4.9.0 -yapf==0.40.1 -zipp==3.17.0 -ultralytics==8.3.128 -websockets==15.0.1 - - - -from PIL import Image -from src.modules.emu import Emu -import time -import json -import threading - -from src.modules.imaging.camera import DepthCapture, OakdCamera - -emu = Emu("tmp") -i = 0 - -def print_conn(): - print("connecton made") - -emu.set_on_connect(print_conn) -latest_capture: DepthCapture | None = None - -emu.start_comms() -time.sleep(2) - -camera = OakdCamera() - -camera_thread = threading.Thread(target=camera.start(), daemon=True) -camera_thread.start() - -def send_img(message): - global latest_capture, i - - msg = json.loads(message) - - if msg["type"] == "image" and msg["message"] == "capture": - print("sending image") - latest_capture = camera.capture_with_depth() - im = Image.fromarray(latest_capture.rgb) - - im.save(f"./tmp/{i}.jpeg") - emu.send_image(f"{i}.jpeg") - - i += 1 - -def measure(message): - global latest_capture - - msg = json.loads(message) - - if msg["type"] == "getDistance": - print("getting distance") - points = msg["message"] - - p1 = points["p1"] - p2 = points["p2"] - - if latest_capture is not None: - distance = latest_capture.distance_between_points(p1["x"], p1["y"], p2["x"], p2["y"]) - send = { - "type": "distance", - "message": distance - } - emu.send_msg(json.dumps(send)) - print(f"distance sent: {send}") - - -emu.subscribe(send_img) -emu.subscribe(measure) -time.sleep(500) - - - -from src.modules.imaging.detector import IrDetector, BoundingBox -import time -from typing import Tuple -from src.modules.imaging.analysis import ImageAnalysisDelegate -from src.modules.imaging.camera import RPiCamera -from src.modules.imaging.location import DebugLocationProvider -from PIL import Image, ImageDraw - - -def func(image: Image.Image, bb: BoundingBox): - top_left_corner: Tuple[float, float] = (bb.position.x, bb.position.y) - bottom_right_corner: Tuple[float, float] = (bb.position.x + bb.size.x, - bb.position.y + bb.size.y) - - draw = ImageDraw.Draw(image) - draw.rectangle((top_left_corner, bottom_right_corner), outline="red", width=3) - image.show() - time.sleep(3) - - -camera = RPiCamera() -detector = IrDetector() -location = DebugLocationProvider() - -analysis = ImageAnalysisDelegate(detector, camera, location) -analysis.subscribe(func) - -analysis.start() - - - -from src.modules.imaging.camera import GazeboCamera -import os - - -cam = GazeboCamera() - -os.makedirs("tmp/log", exist_ok=True) -dirs = os.listdir("tmp/log") -ft_num = len(dirs) -os.makedirs(f"tmp/log/{ft_num}") # no exist_ok bc. this folder should be new - - -cam.capture_to(f"tmp/log/{ft_num}/gazebocamera.png") - - - -from src.modules.imaging.bucket_detector import BucketDetector -from src.modules.imaging.camera import DebugCameraFromDir, RPiCamera, DebugCamera -from src.modules.imaging.location import DebugLocationProvider -from src.modules.imaging.analysis import ImageAnalysisDelegate - -from ultralytics import YOLO -import os - -import cv2 -from PIL import Image -import time - -cam = RPiCamera(0) -model_path = 'samples/models' -models = os.listdir(model_path) -models = ["best.pt"] -for file in models: - model = YOLO(os.path.join(model_path, file)) - i = len(os.listdir("photos")) - while True: - image = cam.capture() - image.save(f"photos/{i}.png") - results = model(image) - - result = results[0] # because one image - a = result.plot() - # b = Image.fromarray(a) - # b.save(f"results/{i}.png") - # - cv2.imshow(f'Result for model: {file}', a) - cv2.waitKey(0) - cv2.destroyAllWindows() - time.sleep(0.5) - i += 1 - # boxes = result.boxes - - # if boxes is not None and len(boxes) > 0: - # best_box = boxes[boxes.conf.argmax()] - # (x1, y1, x2, y2) = best_box.xyxy[0].tolist() # box [x1, y1, x2, y2] - # conf = best_box.conf.item() # confidence threshold - # if conf < 0.5: - # print("not confident") - # else: - # print("confident") - # else: - # print("no bounding box") - - - -import time -import os -import threading - -from dronekit import connect -import json - -from src.modules.imaging.camera import RPiCamera -# from src.modules.imaging.mavlink import MAVLinkDelegate -# from src.modules.imaging.location import MAVLinkLocationProvider - -CONN_STR = "tcp:127.0.0.1:14550" -MESSENGER_PORT = 14552 - -cam = RPiCamera() -#mavlink = MAVLinkDelegate() - -drone = connect(CONN_STR, wait_read=False) - -os.makedirs("tmp/log", exist_ok=True) -dirs = os.listdir("tmp/log") -ft_num = len(dirs) -os.makedirs(f"tmp/log/{ft_num}") # no exist_ok bc. this folder should be new - -i = 0 -last_picture = time.time() - - -def location_dump_to(path: str): - with open(path, 'w') as file: - location = {"location": str(drone.location.global_relative_frame)} - json.dump(location, file) - - -def take_picture(_): - global i - global last_picture - - cam.caputure_to(f"tmp/log/{ft_num}/{i}.png") - location_dump_to(f"tmp/log/{ft_num}/{i}.json") - print(i) - i += 1 - - -def picture_loop(sleep): - while True: - take_picture(None) - time.sleep(sleep) - - -thread_1 = threading.thread(picture_loop, 3) - -thread_1.start() - -#mavlink.run() - - - -from typing import Callable, Optional, List, Callable, Any - -import threading -# from multiprocessing import Process -from .detector import BaseDetector, BoundingBox -from .camera import CameraProvider -from .debug import ImageAnalysisDebugger -from ..georeference.inference_georeference import get_object_location -from .location import LocationProvider -from PIL import Image, ImageDraw - -import os - -from .analysis import CameraAttributes, Inference - - -class DebugImageAnalysisDelegate: - """ - Implements an imaging inference loops and provides several methods which - can be used to query the latest image analysis results. - - Responsible for capturing pictures regularly, detecting any landing pads in - those pictures and then providing the most recent estimate of the landing - pad location from the camera's perspective. - - Pass an `ImageAnalysisDebugger` when constructing to see a window with live - results. - - TODO: geolocate the landing pad using the drone's location. - """ - - def __init__(self, - detector: BaseDetector, - camera: CameraProvider, - location_provider: LocationProvider, - debugger: Optional[ImageAnalysisDebugger] = None, - ): - import os - self.detector = detector - self.camera = camera - self.debugger = debugger - self.location_provider = location_provider - self.subscribers: List[Callable[[Image.Image, float, float], Any]] = [] - self.camera_attributes = CameraAttributes() - - # log pictures taken - os.makedirs("tmp/log", exist_ok=True) - dirs = os.listdir("tmp/log") - current_path = f"tmp/log/{len(dirs)}" - os.makedirs(current_path) - - # path to store images taken during flight - self.img_path = f"{current_path}/images" - os.makedirs(self.img_path) - - # annotated bounding boxes - self.bb_img_path = f"{current_path}/bb" - os.makedirs(self.bb_img_path) - - # image number - self.i = 0 - - def get_inference(self, bounding_box: BoundingBox) -> Inference: - inference = Inference(bounding_box, self.location_provider.altitude()) - return inference - - def start(self): - """ - Will start the image analysis process in another thread. - """ - thread = threading.Thread(target=self._analysis_loop) - # process = Process(target=self._analysis_loop) - thread.start() - # process.start() - # Use `threading` to start `self._analysis_loop` in another thread. - - def _analyze_image(self): - """ - Actually performs the image analysis once. Only useful for testing, - should otherwise we run by `start()` which then starts - `_analysis_loop()` in another thread. - """ - im = self.camera.capture() - im.save(os.path.join(self.img_path, f"{self.i}.png")) - - bounding_box = self.detector.predict(im) - - if bounding_box: - draw = ImageDraw.Draw(im) - bb = (bounding_box.position.x, bounding_box.position.y, - bounding_box.size.x, bounding_box.size.y) - draw.rectangle(bb) - - im.save(os.path.join(self.bb_img_path, f"{self.i}.png")) - - self.i += 1 - - if self.debugger is not None: - self.debugger.set_image(im) - if bounding_box is not None: - self.debugger.set_bounding_box(bounding_box) - - for subscriber in self.subscribers: - if bounding_box: - inference = self.get_inference(bounding_box) - if inference: - x, y = get_object_location(self.camera_attributes, - inference) - subscriber(im, (x, y)) - else: - subscriber(im, None) - - def _analysis_loop(self): - """ - Indefinitely run image analysis. This should be run in another thread; - use `start()` to do so. - """ - while True: - self._analyze_image() - - def subscribe(self, callback: Callable): - """ - Subscribe to image analysis updates. For example: - - def myhandler(image: Image.Image, bounding_box: BoundingBox): - if bounding_box is None: - print("No bounding box detected") - else: - print("Bounding box detected") - - imaging_process.subscribe(myhandler) - """ - self.subscribers.append(callback) - - - -from src.modules.imaging.camera import WebcamCamera -# from src.modules.imaging.camera import RPiCamera -import time -import struct -import pickle -import socket - - -# interval in seconds -interval = 1 - -cam = WebcamCamera() -# cam = RPiCamera(0) - -client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) -client_socket.connect(('127.0.0.1', 8080)) - -while True: - img = cam.capture() - data = pickle.dumps(img) - message = struct.pack("Q", len(data)) + data - - client_socket.sendall(message) - - time.sleep(interval) - - - -from src.modules.imaging.bucket_detector import BucketDetector -from src.modules.imaging.camera import DebugCameraFromDir -from src.modules.imaging.location import DebugLocationProvider -# from src.modules.imaging.analysis import ImageAnalysisDelegate -from src.modules.imaging.debug_analysis import DebugImageAnalysisDelegate - - - -cam = DebugCameraFromDir("images") -det = BucketDetector("samples/models/n640.pt") -location = DebugLocationProvider() -analysis = DebugImageAnalysisDelegate(det, cam, location) -# analysis = ImageAnalysisDelegate(det, cam, location) - -def test(a1, a2): - if a1 != None and a2 != None: - print(a1) - print(a2) - -analysis.subscribe(test) - -analysis.start() - - - -from src.modules.imaging.detector import IrDetector -import time -from src.modules.imaging.analysis import ImageAnalysisDelegate -from src.modules.imaging.camera import RPiCamera -from src.modules.imaging.location import DebugLocationProvider -from src.modules.imaging.mavlink import MAVLinkDelegate -from pymavlink import mavutil - -from src.modules.autopilot import navigator - -from dronekit import connect, VehicleMode - - -CONN_STR = "udp:127.0.0.1:14551" -MESSENGER_PORT = 14552 - -drone = connect(CONN_STR, wait_ready=False) - -nav = navigator.Navigator(drone, MESSENGER_PORT) -mavlink = MAVLinkDelegate(conn_str = CONN_STR) - -time.sleep(2) - -bucket_avg = [[], []] -target_height = 0.9 - - -def moving_bucket_avg(_, pos): - if pos: - if len(bucket_avg[0]) > 0 and len(bucket_avg[1]) > 0: - num_x = len(bucket_avg[0]) - num_y = len(bucket_avg[1]) - - avg_x = sum(bucket_avg[0]) / num_x - avg_y = sum(bucket_avg[1]) / num_y - - new_x_avg = avg_x + pos[0] / (num_x + 1) - new_y_avg = avg_y + pos[1] / (num_y + 1) - - bucket_avg[0].append(new_x_avg) - bucket_avg[1].append(new_y_avg) - else: - bucket_avg[0].append(pos[0]) - bucket_avg[1].append(pos[1]) - - print(f"X: {bucket_avg[0]}") - print(f"Y: {bucket_avg[1]}") - time.sleep(1) - - -camera = RPiCamera(0) -detector = IrDetector() -location = DebugLocationProvider() - -analysis = ImageAnalysisDelegate(detector, camera, location) -analysis.subscribe(moving_bucket_avg) - -nav.send_status_message("Shepard is online") - -while not (drone.armed and drone.mode == VehicleMode("GUIDED")): - pass - -time.sleep(5) -analysis.start() - -nav.takeoff(30) - -type_mask = nav.generate_typemask([0, 1, 2]) - -nav.send_status_message("Executing") -current_alt = nav.get_local_position_ned()[2] - -delta = 0.5 -sleep_time = 2 - - -def bucket_descent(): - # VERIFY ALL OF THESE COORINDATE SYSTEMS BEFORE FLYING!!!!!!!!!!!!!!!!!!!!!!!!!! - - coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED - - nav.set_position_target_local_ned(x = bucket_avg[0][-1], - y = bucket_avg[1][-1], - z = 0, - type_mask = type_mask, - coordinate_frame = coordinate_frame) - - time.sleep(5) - - nav.set_position_target_local_ned(x = (bucket_avg[0][-1] - bucket_avg[0][0]), - y = (bucket_avg[1][-1] - bucket_avg[1][0]), - z = 10, - type_mask = type_mask, - coordinate_frame = coordinate_frame) - - time.sleep(5) - - nav.set_position_target_local_ned(x = (bucket_avg[0][-1] - bucket_avg[0][0]), - y = (bucket_avg[1][-1] - bucket_avg[1][0]), - z = 5, - type_mask = type_mask, - coordinate_frame = coordinate_frame) - - time.sleep(5) - - nav.set_position_target_local_ned(x = (bucket_avg[0][-1] - bucket_avg[0][0]), - y = (bucket_avg[1][-1] - bucket_avg[1][0]), - z = 5, - type_mask = type_mask, - coordinate_frame = coordinate_frame) - - time.sleep(5) - - nav.set_position_target_local_ned(x = (bucket_avg[0][-1] - bucket_avg[0][0]), - y = (bucket_avg[1][-1] - bucket_avg[1][0]), - z = 5, - type_mask = type_mask, - coordinate_frame = coordinate_frame) - - time.sleep(5) - - #Full commit - - nav.set_position_target_local_ned(x = 0, - y = 0, - z = 5 - target_height, - type_mask = type_mask, - coordinate_frame = coordinate_frame) - - time.sleep(5) - - -if len(bucket_avg[0]) > 0 and len(bucket_avg[1]) > 0: - bucket_descent() -else: - time.sleep(5) - bucket_descent() - -nav.return_to_launch() - -drone.close() - -nav.send_status_message("Flight test script execution terminated") - - - -from src.modules.emu import Emu -import time -import json - -emu = Emu("res") - -# def onConnect(): -# loadCurrent = { -# "type": "load", -# "uavStatus": { -# "connection": "no", -# "mode": "test", -# "imageCount": "2", -# "timeSinceMessage": "3" -# }, -# "imageName": "res/sample1.jpg" -# } -# emu.send_msg(json.dumps(loadCurrent)) - - -# emu.set_on_connect(onConnect) -emu.start_comms() -time.sleep(2) - -# test different logs -for i in range(6): - print(f"sending log {i}") - if i % 3 == 0: severity = "normal" - elif i % 3 == 1: severity = "warning" - else: severity = "error" - emu.send_log(f"log text {i}", severity) - time.sleep(1) - -while True: - print("sending image") - emu.send_log("sending image") - emu.send_image("test-image.jpeg") - time.sleep(2) - - - -""" -Functions for getting the location of objects we find in our images - -This is done by assuming that we are looking a flat field -We can find the angle from our camera lens based on the pixel location -We can find the angle of the camera based on the Autopilot Roll, Yaw and Pitch - -Note for this analysis, we use UTM coordinates. -This maps traditional Latitude and Longitude into an X Y coordinate grid -where X, Y are in meters -""" - -# TODO: Requires a circular-import... but we only need these for type annotations -import numpy as np -from math import cos, tan, atan, radians, sqrt -import pyproj -from pyproj import Geod - -from typing import TYPE_CHECKING - -if TYPE_CHECKING: - from ..imaging.analysis import CameraAttributes, Inference - - -def LonLat_To_XY(lon, lat, zone=12): - """ - Returns the Easting and Northing of given Lon, Lat on the WGS84 - Projection Model - - Input is in degrees, return is in meters - """ - P = pyproj.Proj(proj='utm', zone=zone, ellps='WGS84', preserve_units=True) - - return P(lon, lat) - - -def XY_To_LonLat(x, y, zone=12): - """ - Returns the Lon, Lat of given Easting, Northing on the WGS84 - Projection Model - - Input is in meters, return is in degrees - """ - P = pyproj.Proj(proj='utm', zone=zone, ellps='WGS84', preserve_units=True) - - return P(x, y, inverse=True) - - -def Geofence_to_XY(origin, geofence): - # Returns a new geofence which consists of position vectors in meters - # Guys the Earth is like mostly flat right?? - - try: - R = 6378137 # Radius of the Earth in meters - - new_fence = [] - - origin_lon = origin[0] - origin_lat = origin[1] - - origin_lon_rad = radians(origin_lon) - origin_lat_rad = radians(origin_lat) - - for point in geofence: - - point_lon_rad = radians(point[0]) - point_lat_rad = radians(point[1]) - - delta_lat = point_lat_rad - origin_lat_rad - delta_lon = point_lon_rad - origin_lon_rad - - x = delta_lat * R - y = delta_lon * cos((origin_lat_rad + point_lat_rad) / 2) * R - - new_fence.append((x, y)) - - return new_fence - - except TypeError: - return None - - -''' -def meters_to_LonLat(origin, points): - - R = 635900 - - new_points = [] - - origin_lon = origin[0] - origin_lat = origin[1] - - origin_lon_rad = radians(origin_lon) - origin_lat_rad = radians(origin_lat) - - for point in points: - - point_x = point[0] - point_y = point[1] - - delta_lat = point_x / R - delta_lon = point_y / (cos(origin_lat_rad + delta_lat / 2) * R) - - delta_lat = degrees(delta_lat) - delta_lon = degrees(delta_lon) - - new_points.append((delta_lat + origin_lat, delta_lon + origin_lon)) - - return new_points -''' - - -def meters_to_LonLat(origin, points): - g = Geod(ellps='clrk66') - new_points = [] - for point in points: - az = atan(point[1] / point[0]) - dist = sqrt((point[0] ** 2 + point[1] ** 2)) - - point_lon, point_lat, backaz = g.fwd(origin[0], origin[1], az, dist) - new_points.append((point_lat, point_lon)) - - return new_points - - -def pixel_to_rel_position(camera_attributes: 'CameraAttributes', - inference: 'Inference', fovh, fovv) -> np.array: - """ - Calculates the unit vector from an angled camera to an object at x, y pixel coordinates - x and y are the normalized pixel coordinates between 0 and 1 - both fovs' are in radians. - """ - - direction_vector = np.zeros(2) - - #calculating image height and width in meters - height = 2 * camera_attributes.focal_length * tan(fovv / 2) - width = 2 * camera_attributes.focal_length * tan(fovh / 2) - - #pixels to meters conversion - y_m = inference.y * height - x_m = inference.x * width - - if (y_m > height / 2): - y_m = height - y_m - theta_y = camera_attributes.angle - atan( - (height / 2 - y_m) / camera_attributes.focal_length) - elif (y_m <= height / 2): - theta_y = atan( - (height / 2 - y_m) / - camera_attributes.focal_length) + camera_attributes.angle - - if (x_m > width / 2): - x_m = width - x_m - theta_x = -1 * (atan( - (width / 2 - x_m) / camera_attributes.focal_length)) - elif (x_m <= width / 2): - theta_x = atan((width / 2 - x_m) / camera_attributes.focal_length) - - y_comp = inference.relative_alt * tan(theta_y) - x_comp = (inference.relative_alt / - cos(camera_attributes.angle)) * tan(theta_x) - - offset = calculate_object_offsets() - - # NOTE: This correction was made because of the camera being upside down - # This corrects the issues regarding the autopilot navigating to the wrong direction - - direction_vector[1] = -1 * (x_comp + offset[0]) - direction_vector[0] = y_comp + offset[1] - - return direction_vector - - -#TODO get measurements to calculate offset due to shifted position of camera from the gps -def calculate_object_offsets() -> np.array: - return np.array([0, 0]) - - -def get_object_location(camera_attributes: 'CameraAttributes', - inference: 'Inference') -> tuple: - """ - This calculates the location of the inference provided - and returns the longitude, latitude in degrees - """ - - H_FOV = radians(62.2) - V_FOV = radians(48.8) - - dir_vector = pixel_to_rel_position( - camera_attributes, - inference, - H_FOV, - V_FOV, - ) - print("dir_vector", dir_vector) - - #lon, lat = XY_To_LonLat(dir_vector[0], dir_vector[1]) - - return (dir_vector[0], dir_vector[1]) - - - -from typing import Tuple - -import pathlib -from PIL import Image -import numpy as np -import cv2 -import depthai as dai -from dataclasses import dataclass - - -class CameraProvider: - """ - Manage a camera source. This could be the raspberry pi camera, a web cam, - or a series of images. - """ - - def set_size(self, size: Tuple[int, int]): - """ - Set the pixel width and height of all images taken by this camera. - """ - # Should be implemented by deriving classes. - raise NotImplementedError() - - def capture(self) -> Image.Image: - """ - Captures a single image from the camera. This image will be of the size - set by `set_size`. - """ - # Should be implemented by deriving classes. - raise NotImplementedError() - - def capture_to(self, path: str | pathlib.Path): - """ - Captures a single image and saves it to `path`. - """ - self.capture().save(path) - - def caputure_as_ndarry(self) -> np.ndarray: - """ - Captures a single image returns it's numpy.ndarray representation. Will - have shape (height, width, colors). - """ - return np.array(self.capture()) - -@dataclass -class DepthCapture: - rgb: np.ndarray - point_cloud: np.ndarray - width: int - height: int - - def get_point(self, x: int, y: int) -> np.ndarray: - """Get the 3D coordinates relative to the camera frame (in mm) of a pixel). - - (x, y) are in pixel coordinates in the self.rgb frame.""" - idx = y * self.width + x - p = self.point_cloud[idx] - - # I am not sure if this is possible... - assert not np.any(np.isnan(p)), "Invalid depth at this point" - - return p - - def distance_between_points(self, x1: int, y1: int, x2: int, y2: int): - """Get the physical distance between points from pixels in the rgb - frame (x1, y1) and (x2, y2). - - Resulting distance is in mm.""" - p1 = self.get_point(x1, y1) - p2 = self.get_point(x2, y2) - - dist = np.linalg.norm(p1 - p2) - - return dist - - -class OakdCamera(CameraProvider): - """ - Manages an OAK-D device with on-demand capturing of 3D pictures (see DepthCapture). - """ - - def __init__(self, fps: int = 30): - self._init_pipeline(fps) - - def _init_pipeline(self, fps: int): - """Initialize the Depth AI pipeline (will be run on the OAK-D)""" - pipeline = dai.Pipeline() - - camRgb = pipeline.create(dai.node.ColorCamera) - monoLeft = pipeline.create(dai.node.MonoCamera) - monoRight = pipeline.create(dai.node.MonoCamera) - depth = pipeline.create(dai.node.StereoDepth) - pointcloud = pipeline.create(dai.node.PointCloud) - sync = pipeline.create(dai.node.Sync) - xOut = pipeline.create(dai.node.XLinkOut) - - camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P) - camRgb.setBoardSocket(dai.CameraBoardSocket.CAM_A) - camRgb.setIspScale(1, 3) - camRgb.setFps(fps) - - monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P) - monoLeft.setCamera("left") - monoLeft.setFps(fps) - - monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P) - monoRight.setCamera("right") - monoRight.setFps(fps) - - depth.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY) - depth.setLeftRightCheck(True) - depth.setSubpixel(True) - depth.setDepthAlign(dai.CameraBoardSocket.CAM_A) - - monoLeft.out.link(depth.left) - monoRight.out.link(depth.right) - depth.depth.link(pointcloud.inputDepth) - - camRgb.isp.link(sync.inputs["rgb"]) - pointcloud.outputPointCloud.link(sync.inputs["pcl"]) - - sync.out.link(xOut.input) - xOut.setStreamName("out") - - self.pipeline = pipeline - - def capture_with_depth(self) -> DepthCapture: - """Capture a current 3D frame on the OAK-D. - - NOTE: .start() must have been called first. If it has not, this will raise Exception.""" - if not self.device or self.device.isClosed(): - raise Exception("No oakD connection, perhaps you forgot to call the .start() function") - - msg = self.queue.get() - rgbFrame = msg["rgb"] - cv_frame = rgbFrame.getCvFrame() - rgb_frame = cv2.cvtColor(cv_frame, cv2.COLOR_BGR2RGB) - rgb = np.array(rgb_frame) - pcl = msg["pcl"] - - point_cloud = pcl.getPoints().astype(np.float64) - height, width, _ = rgb.shape - - capture = DepthCapture(rgb, point_cloud, width, height) - return capture - - def capture(self) -> Image.Image: - capture = self.capture_with_depth - img = Image.fromarray(capture.rgb, "RGB") - return img - - - def start(self): - """Start the depth-perception process on the OAK-D""" - print("Starting OAK-D Connection") - self.device = dai.Device(self.pipeline) - self.queue = self.device.getOutputQueue("out", maxSize=1, blocking=False) - - def stop(self): - """Stop the depth-perception process""" - self.device.close() - self.queue = None - -class DebugCamera(CameraProvider): - """ - Debug camera source which always returns the same image loaded from - `dummy_image_path`. - """ - - def __init__(self, dummy_image_path: str | pathlib.Path): - self.og_im = Image.open(dummy_image_path) - self.im = self.og_im # Keep a copy of the original image for resizing. - self.size = (self.im.width, self.im.height) - - def set_size(self, size: Tuple[int, int]): - # Always resize from the original "dummy" image - self.im = self.og_im.resize(size) - self.size = size - - def capture(self) -> Image.Image: - return self.im - - -class DebugCameraFromDir(CameraProvider): - """ - Debug camera that returns an image from directory 'image_dir' - containing only images - """ - def __init__(self, image_dir: str | pathlib.Path): - import os # used to get images in folder - self.image_dir = image_dir - self.imgs = os.listdir(image_dir) - self.imgs = [os.path.join(image_dir, file) for file in self.imgs] - if len(self.imgs) == 0: - raise ValueError('no files in directory') - self.index = 0 - - # set size at first based on first image - im = Image.open(self.imgs[self.index]) - self.size = (im.width, im.height) - - def set_size(self, size: Tuple[int, int]): - # set size as each image is resized on load - self.size = size - - def capture(self) -> Image.Image: - # return the next image in the directory - filename = self.imgs[self.index] - print(filename) - self.index = (self.index + 1) % len(self.imgs) - - return Image.open(filename).resize(self.size) - - -class GazeboCamera(CameraProvider): - """ - Video sourced from the Gazebo Simulator over UDP - """ - - def __init__(self): - self.port = 5600 - - gst_pipeline = ( - "udpsrc address=127.0.0.1 port=5600 ! " - "application/x-rtp, encoding-name=H264 ! " - "rtph264depay ! " - "avdec_h264 ! " - "videoconvert ! " - "appsink" - ) - self.size = (640, 480) - self.cap = cv2.VideoCapture(gst_pipeline, cv2.CAP_GSTREAMER) - - if not self.cap.isOpened(): - print("Failed to open UDP Stream") - exit() - - def set_size(self, size: Tuple[int, int]): - self.size = size - self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, size[0]) - self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, size[1]) - - def capture(self) -> Image.Image: - ret, frame = self.cap.read() - if ret: - frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) - return Image.fromarray(frame).resize(self.size) - else: - raise RuntimeError("Failed to capture image from webcam") - - def show_images(self): - while True: - ret, frame = self.cap.read() - if not ret: - print("Frame not received") - break - - cv2.imshow("Gazebo Video Stream", frame) - if cv2.waitKey(1) & 0xFF == ord('q'): - break - self.cap.release() - cv2.destroyAllWindows() - - -class WebcamCamera(CameraProvider): - """ - Debug camera source which uses the computer's webcam as the image source. - """ - - def __init__(self): - self.cap = cv2.VideoCapture(0) # 0 is typically the default webcam - self.size = (640, 480) - self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.size[0]) - self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.size[1]) - - def set_size(self, size: Tuple[int, int]): - self.size = size - self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, size[0]) - self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, size[1]) - - def capture(self) -> Image.Image: - ret, frame = self.cap.read() - if ret: - frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) - return Image.fromarray(frame).resize(self.size) - else: - raise RuntimeError("Failed to capture image from webcam") - - -class RPiCamera(CameraProvider): - """ - Note: Need picamera2 installed on the raspberry pi for this to work. - Production camera source which uses the raspberry pi camera as the image - source. - """ - - def __init__(self, cam_num: int): - from picamera2 import Picamera2 - self.camera = Picamera2(cam_num) - self.size = (640, 480) - self.configure_camera() - self.camera.start() - print(self.camera.capture_metadata()['ScalerCrop']) - print(self.camera.camera_controls['ScalerCrop']) - - def configure_camera(self): - # Configuring camera properties - config = self.camera.create_preview_configuration( - main={"size": self.size}) - self.camera.configure(config) - - def set_size(self, size: Tuple[int, int]): - self.size = size - self.configure_camera() - - def capture(self) -> Image.Image: - # Capture an image - self.camera.start() - capture_result = self.camera.capture_array() - image = Image.fromarray(capture_result) - return image - - - -from typing import Optional - -from PIL import Image - -from .detector import Vec2, BoundingBox, BaseDetector - -from ultralytics import YOLO - - -class BucketDetector(BaseDetector): - - def __init__(self, model_path): - print(f"model: {model_path}") - self.model = YOLO(model_path) - - def predict(self, image: Image.Image) -> Optional[BoundingBox]: - results = self.model(image, verbose = False) - - result = results[0] # because one image - - boxes = result.boxes - - if boxes is not None and len(boxes) > 0: - best_box = boxes[boxes.conf.argmax()] - (x1, y1, x2, y2) = best_box.xyxy[0].tolist() # box [x1, y1, x2, y2] - conf = best_box.conf.item() # confidence threshold - if conf < 0.75: - return None - else: - return BoundingBox(Vec2(x1, y1), Vec2(x2, y2)) - else: - return None - - - -from functools import lru_cache -from typing import Optional - -from PIL import Image -import numpy as np -import cv2 -from cv2 import aruco - - -@dataclass -class Vec2: - """2-component vector with float elements.""" - x: float - y: float - - def __add__(self, other: 'Vec2') -> 'Vec2': - return Vec2(self.x + other.x, self.y + other.y) - - def __sub__(self, other: 'Vec2') -> 'Vec2': - return Vec2(self.x - other.x, self.y - other.y) - - def __rmul__(self, scalar: float) -> 'Vec2': - return Vec2(self.x * scalar, self.y * scalar) - - def __rtruediv__(self, scalar: float) -> 'Vec2': - return Vec2(self.x / scalar, self.y / scalar) - - @cached_property - def norm(self) -> float: - """Return the euclidean norm of the vector""" - return math.sqrt(self.x**2 + self.y**2) - - def normalize(self) -> 'Vec2': - """Reduce the norm to 1 while preserving direction.""" - magnitude = self.norm - if magnitude != 0: - return Vec2(self.x / magnitude, self.y / magnitude) - else: - return Vec2(0, 0) - - @staticmethod - def dot(v1: 'Vec2', v2: 'Vec2') -> float: - """Compute the standard inner product between v1 and v2.""" - return v1.x * v2.x + v1.y * v2.y - - @staticmethod - def min(v1: 'Vec2', v2: 'Vec2') -> 'Vec2': - """Compute component-wise min of v1 and v2""" - return Vec2(min(v1.x, v2.x), min(v1.y, v2.y)) - - @staticmethod - def max(v1: 'Vec2', v2: 'Vec2') -> 'Vec2': - """Compute component-wise max of v1 and v2""" - return Vec2(max(v1.x, v2.x), max(v1.y, v2.y)) - - -class BoundingBox: - -def __init__(self, position: Vec2, size: Vec2): - self.position = position - self.size = size - -@lru_cache(maxsize=2) -def intersection(self, other: 'BoundingBox') -> float: - top_left = Vec2.max(self.position, other.position) - bottom_right = Vec2.min(self.position + self.size, - other.position + other.size) - - size = bottom_right - top_left - - intersection = size.x * size.y - return max(intersection, 0) - -def union(self, other: 'BoundingBox') -> float: - intersection = self.intersection(other) - if intersection == 0: - return 0 - - union = self.size.x * self.size.y + other.size.x * other.size.y - intersection - return union - -def intersection_over_union(self, pred: 'BoundingBox') -> Optional[float]: - intersection = self.intersection(pred) - if intersection == 0: - return 0 - iou = intersection / self.union(pred) - return iou - - -class BaseDetector: - def predict(self, image: Image.Image) -> Optional[BoundingBox]: - raise NotImplementedError() - - -class IrDetector(BaseDetector): - - def predict(self, image: Image.Image) -> Optional[BoundingBox]: - img = np.array(image) - - gray_img = cv2.cvtColor(img, cv2.COLOR_RGBA2GRAY) - max_val = np.max(gray_img) # returns maximum value of brightness - if max_val < 200: - return None # lower threshold for intensity - _, thresh = cv2.threshold(gray_img, max_val - 10, 255, cv2.THRESH_BINARY) - contours, _ = cv2.findContours(thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - - # cv2.drawContours(thresh, contours, -1, (0, 255, 0), 2) - - if len(contours) == 0: - return None - - for cnt in contours: - x, y, w, h = cv2.boundingRect(cnt) - - return BoundingBox(Vec2(x, y), Vec2(w, h)) - - -class ArucoDetector(): - - def predict(self, image: Image.Image) -> Optional[BoundingBox]: - img = cv2.cvtColor(np.array(image), cv2.COLOR_RGB2BGR) - - aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) - - params = cv2.aruco.DetectorParemeters() - - corners, ids, rejected = cv2.aruco.detectMarkers(img, aruco_dict, parameters=params) - - if ids: - for c in zip(corners, ids): - - pts = c[0] - - x_min = pts[:, 0].min() - x_max = pts[:, 0].max() - y_min = pts[:, 1].min() - y_max = pts[:, 1].max() - - x = (x_min + x_max) / 2 - y = (y_min + y_max) / 2 - w = (x_max - x_min) - h = (y_max - y_min) - - return BoundingBox(Vec2(x, y), Vec2(w, h)) - - - -import time - -from pymavlink import mavutil - -from src.modules.autopilot.navigator import Navigator -from src.modules import imaging -import math - - -class Lander: - """ - A class to handle everything regarding landing that is not already handled by ardupilot - """ - - HORIZONTAL_ANGLE = math.radians(30) - VERTICAL_ANGLE = math.radians(24) - - def __init__(self, nav, max_velocity, geofence): - self.__spiral_route = [] # Private attribute - self.__bounding_box_pos = [] - self.__buffer = [[], - []] - # self.landing_spot = landing_spot - self.nav = nav - self.i = 10 - self.max_velocity = max_velocity - self.bounding_box_detected = False - self.bounding_box_pos = [] - self.bounding_box_log = [] - self.bounding_box_log_og = [] - self.leave_frame = False - self.geofence = geofence - - self.null_radius = 10 # Radius in METERS of bounding box detection being ignored - - @property - def route(self): - return self.__spiral_route - - def generateSpiralSearch(self, numberOfLoops=10): - """ - Generate a landing route in a square spiral pattern. The generated route is to be multiplied by the current height in metres - to get the relative distance traveled in metres. - - :param numberOfLoops: The number of loops to be made, with a default value of 10 - :return: None - """ - - self.__spiral_route = [] # Clear the existing route - self.y, self.x = 0, 0 - - # verticalScanRatio = math.tan(Lander.VERTICAL_ANGLE) - # horizontalScanRatio = math.tan(Lander.HORIZONTAL_ANGLE) - - step_size = 5 - - steps_per_side = 2 - curr_side_iter = 0 - loops_completed = 0 - - axis = "x" - - dir_x = 1 - dir_y = 1 - - x, y = 0, 0 - - while loops_completed != numberOfLoops: - if axis == "x": - x += dir_x * step_size - for i in range(steps_per_side): - if len(self.__spiral_route) > 0: - last_x = self.__spiral_route[-1][0] - last_y = self.__spiral_route[-1][1] - else: - last_x = 0 - last_y = 0 - - self.__spiral_route.append((x + last_x, y + last_y)) - - axis = "y" - dir_x *= -1 - elif axis == "y": - y += dir_y * step_size - - for i in range(steps_per_side): - if len(self.__spiral_route) > 0: - last_x = self.__spiral_route[-1][0] - last_y = self.__spiral_route[-1][1] - else: - last_x = 0 - last_y = 0 - - self.__spiral_route.append((x + last_x, y + last_y)) - - axis = "x" - dir_y *= -1 - - curr_side_iter += 1 - - if curr_side_iter % 2 == 0: - steps_per_side += 1 - - if curr_side_iter % 5 == 0: - loops_completed += 1 - - self.x = x - self.y = y - x = 0 - y = 0 - - ''' - def executeSearch(self, altitude): - """ - Move the drone to the next position in the landing route. - - :param Navigator: An instance of the Navigator class. - :param route: A list of 2 elements representing the relative distance ratio to be specified as [north, east]. - :param altitude: The altitude in metres. - :return: None - """ - - type_mask = self.nav.generate_typemask([0, 1]) - i = 0 - while i <= len(self.__spiral_route) - 1: - - current_local_pos = self.nav.get_local_position_ned() - print(self.geofence_check((self.__spiral_route[i][0], self.__spiral_route[i][1]))) - print(self.__spiral_route[i]) - if self.bounding_box_detected: - print(self.bounding_box_pos) - new_x, new_y = self.bounding_box_pos[0], self.bounding_box_pos[1] - if len(self.bounding_box_log) == 0: - self.bounding_box_log.append((new_x, new_y)) - print(self.bounding_box_log) - for bounding_box in self.bounding_box_log: - delta_x = bounding_box[0] - new_x - current_local_pos[0] - delta_y = bounding_box[1] - new_y - current_local_pos[1] - - if math.sqrt((delta_x ** 2) + (delta_y ** 2)) >= self.null_radius: - self.boundingBoxAction() - time.sleep(1/(self.max_velocity)) - break - self.bounding_box_detected = False - else: - - if self.geofence_check((self.__spiral_route[i][0], self.__spiral_route[i][1])): - - self.nav.set_position_target_local_ned(x = self.__spiral_route[i][0], - y = self.__spiral_route[i][1], - type_mask=type_mask, - coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED) - - i += 1 - time.sleep(1 / (self.max_velocity)) - - return self.bounding_box_log - - #self.nav.set_position_relative(route[0], route[1]) - ''' - # Find the reason why double detection with one having high errors - # Make task 1 script ready for comp - # Make spiral take pictures as it goes around the IR emitter to revise accuracy - # Fix heading so that the camera can face towards the inside of the spiral - # Make it so that it is plug and play for this morning - - def executeSearch(self, altitude): - ''' - (location.north, location.east, location.down) - to always face inwards, always face the origin - ''' - type_mask = self.nav.generate_typemask([0, 1]) - i = 0 - origin = self.nav.get_local_position_ned() - try: - while i <= len(self.__spiral_route) - 1: - current_local_pos = self.nav.get_local_position_ned() - if self.bounding_box_detected: - print(self.bounding_box_pos) - new_x, new_y = self.bounding_box_pos[0], self.bounding_box_pos[1] - if len(self.bounding_box_log) == 0: - self.bounding_box_log.append((new_x, new_y)) - self.bounding_box_log_og.append((new_x + current_local_pos[0], new_y + current_local_pos[1])) - print(self.bounding_box_log) - for bounding_box in self.bounding_box_log: - delta_x = bounding_box[0] - new_x - current_local_pos[0] - delta_y = bounding_box[1] - new_y - current_local_pos[1] - - if math.sqrt((delta_x ** 2) + (delta_y ** 2)) >= self.null_radius: - self.boundingBoxAction() - time.sleep(2 / (self.max_velocity)) - break - self.bounding_box_detected = False - else: - # NOTE: THESE ARE ABSOLUTE COORDINATES - # Spiral search is in absolute coordinates in which it adds the offset to the origin - self.nav.set_position_target_local_ned(x = self.__spiral_route[i][0] + origin[0] - (self.__spiral_route[4][0] / 2), - y = self.__spiral_route[i][1] + origin[1] - (self.__spiral_route[9][1] / 2), - z = -altitude, - type_mask=type_mask, - coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_NED) - - i += 1 - - # Increase if the drone is overriding itself or moving too quickly - # Decrease if there is more "stop, start" than desirable - time.sleep(1.5 / (self.max_velocity)) - - print(self.bounding_box_log_og) - finally: - return self.bounding_box_log_og - - def boundingBoxAction(self): - # go to bounding box and go around it in a square - type_mask = self.nav.generate_typemask([0, 1]) - - current_local_pos = self.nav.get_local_position_ned() - time.sleep(1) - - x, y = self.bounding_box_pos[0], self.bounding_box_pos[1] - if self.geofence_check((x + 2, y)) and self.geofence_check((x, y + 2)) and self.geofence_check((x + 2, y + 2)): - self.nav.set_position_target_local_ned(x = x, - y = y, - type_mask=type_mask, - coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED) - time.sleep((math.sqrt(x ** 2 + y ** 2) / (self.max_velocity))) - self.nav.set_position_target_local_ned(x = 2, - y = 0, - type_mask=type_mask, - coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED) - time.sleep((3 / (self.max_velocity))) - self.nav.set_position_target_local_ned(x = 0, - y = 2, - type_mask=type_mask, - coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED) - time.sleep(3 / (self.max_velocity)) - self.nav.set_position_target_local_ned(x = -2, - y = 0, - type_mask=type_mask, - coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED) - time.sleep((3 / (self.max_velocity))) - self.nav.set_position_target_local_ned(x = 0, - y = -2, - type_mask=type_mask, - coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED) - time.sleep((3 / (self.max_velocity))) - - self.nav.set_position_target_local_ned(x = -x, - y = -y, - type_mask=type_mask, - coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED) - time.sleep((math.sqrt(x ** 2 + y ** 2) / (self.max_velocity))) - - else: - self.nav.set_position_target_local_ned(x = x, - y = y, - type_mask=type_mask, - coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED) - time.sleep((math.sqrt(x ** 2 + y ** 2) / (self.max_velocity))) - - self.bounding_box_detected = False - self.bounding_box_log.append((x, y)) - self.bounding_box_log_og.append((x + current_local_pos[0], y + current_local_pos[1])) - - def detectBoundingBox(self, _, bounding_box_pos): - if bounding_box_pos: - if not self.leave_frame: - self.bounding_box_detected = True - self.bounding_box_pos = bounding_box_pos - else: - - if self.leave_frame: - self.leave_frame = False - - def geofence_check(self, point): - # Adapted from code provided by Aiden - #lat, lon, alt = self.nav.get_global_position() - lat, lon = point[1], point[0] - hitcount = 0 - for i in range(len(self.geofence) - 1): - # Model geofence lines as y = mx + b - A = self.geofence[i] - B = self.geofence[i + 1] - - m = (B[1] - A[1]) / (B[0] - A[0]) - - # Handling the case of delta_lon == 0 - if abs(m) == float('inf'): - m = (B[1] - A[1]) / (B[0] - A[0] + 0.00000001) - - b = A[0] - m * A[0] - - x = (lat - b) / m - - if x > lon and lat > min(A[1], B[1]) and lat < max(A[1], B[1]): - hitcount += 1 - return bool(hitcount & 1) - - def enable_precision_land(self, Navigator): - - # NOTE: CHANGE THE CAMERA TYPE DURING ACTUAL USE - camera = imaging.camera.DebugCamera("./res/test-image.jpeg") - - analysis = imaging.analysis.ImageAnalysisDetector(camera = camera, nav = Navigator) - - analysis.subscribe(self._precision_land) - analysis.run() - - def _precision_land(self, im, lon, lat, x, y): - - # Append new values for position to the buffer and compute the moving average, taking the new values into account. - # Adjust buffer size depending on the refresh rate of the imaging script - - buffer_size = 5 - - self.__buffer[0].append(x) - self.__buffer[1].append(y) - - x = sum(self.__buffer[0]) / len(self._buffer[0]) - y = sum(self.__buffer[1] / len(self.__buffer[1])) - - if len(self.__buffer[0]) >= buffer_size and len(self.__buffer[1]) >= buffer_size: - type_mask = self.nav.generate_typemask([0, 1, 2]) - - self.nav.set_postion_target_local_NED(x = self.__buffer[0][-1], y = self.__buffer[1][-1], z = -(self.i), type_mask = type_mask) - self.i -= 1 - - # Maintain the size of the buffer - self.__buffer[0].pop(0) - self.__buffer[1].pop(0) - - -def main(): - LandingSpotFinder1 = Lander() - Navigator1 = Navigator() - - LandingSpotFinder1.generateRoute() # Call the method to generate the route - - for i in LandingSpotFinder1.route: - # add code to break the loop when landing spot is found - LandingSpotFinder1.goNext(Navigator1, i) - time.sleep(5) - ''' - if i == 0: - self.nav.set_heading(0) - time.sleep(5) - elif ((i - 1) % 5 == 0) and i != 1: - self.nav.set_heading_relative(90) - else: - self.nav.set_heading_relative(0) - - ''' - - -if __name__ == "__main__": - main() - - - -from typing import Callable, Optional, List, Any, Tuple - -import threading -# from multiprocessing import Process -from .detector import BaseDetector, BoundingBox -from .camera import CameraProvider -from .debug import ImageAnalysisDebugger -from ..georeference.inference_georeference import get_object_location -from .location import LocationProvider -from ..autopilot.navigator import Navigator -from PIL import Image - - -class CameraAttributes: - def __init__(self): - self.focal_length = 0.0034 - self.angle = 0 # in radians - self.resolution = (1920, 1080) - - -class Inference: - def __init__(self, bounding_box: BoundingBox, relative_alt): - camera_attributes = CameraAttributes() - position = bounding_box.position - size = bounding_box.size - self.x = (position.x + size.x / 2) / camera_attributes.resolution[0] - self.y = (position.y + size.y / 2) / camera_attributes.resolution[1] - self.relative_alt = relative_alt - - -class ImageAnalysisDelegate: - """ - Implements an imaging inference loops and provides several methods which - can be used to query the latest image analysis results. - - Responsible for capturing pictures regularly, detecting any landing pads in - those pictures and then providing the most recent estimate of the landing - pad location from the camera's perspective. - - Pass an `ImageAnalysisDebugger` when constructing to see a window with live - results. - - TODO: geolocate the landing pad using the drone's location. - """ - - def __init__(self, - detector: BaseDetector, - camera: CameraProvider, - location_provider: LocationProvider = None, - navigation_provider: Navigator = None, - debugger: Optional[ImageAnalysisDebugger] = None): - self.detector = detector - self.camera = camera - self.debugger = debugger - - if location_provider is None and navigation_provider is None: - raise ValueError("Either location_provider or navigation_provider must be provided.") - - self.location_provider = location_provider - self.navigation_provider = navigation_provider - - self.subscribers: List[Callable[[Image.Image, Optional[BoundingBox], Optional[Tuple[float, float]]], Any]] = [] - self.camera_attributes = CameraAttributes() - self.thread = None - self.loop = True - - def get_inference(self, bounding_box: BoundingBox) -> Inference: - if self.location_provider is not None: - altitude = self.location_provider.altitude() - elif self.navigation_provider is not None: - altitude = -1 * self.navigation_provider.get_local_position_ned()[2] - else: - raise ValueError("No altitude information provider available.") - - inference = Inference(bounding_box, altitude) - return inference - - def start(self): - """ - Will start the image analysis process in another thread. - """ - self.loop = True - self.thread = threading.Thread(target=self._analysis_loop) - # process = Process(target=self._analysis_loop) - self.thread.start() - # process.start() - # Use `threading` to start `self._analysis_loop` in another thread. - - def stop(self): - self.loop = False - self.thread.join() - - def _analyze_image(self): - """ - Actually performs the image analysis once. Only useful for testing, - should otherwise we run by `start()` which then starts - `_analysis_loop()` in another thread. - """ - im = self.camera.capture() - bounding_box = self.detector.predict(im) - - if self.debugger is not None: - self.debugger.set_image(im) - if bounding_box is not None: - self.debugger.set_bounding_box(bounding_box) - - for subscriber in self.subscribers: - if bounding_box: - inference = self.get_inference(bounding_box) - if inference: - x, y = get_object_location(self.camera_attributes, - inference) - subscriber(im, bounding_box, (x, y)) - else: - subscriber(im, None, None) - - def _analysis_loop(self): - """ - Indefinitely run image analysis. This should be run in another thread; - use `start()` to do so. - """ - while self.loop: - self._analyze_image() - - def subscribe(self, callback: Callable): - """ - Subscribe to image analysis updates. For example: - - def myhandler(image: Image.Image, bounding_box: BoundingBox): - if bounding_box is None: - print("No bounding box detected") - else: - print("Bounding box detected") - - imaging_process.subscribe(myhandler) - """ - self.subscribers.append(callback) - - - -from src.modules.imaging.bucket_detector import BucketDetector -import time -from src.modules.imaging.analysis import ImageAnalysisDelegate -from src.modules.imaging.camera import DebugCamera -from src.modules.imaging.mavlink import MAVLinkDelegate -from pymavlink import mavutil - -from src.modules.autopilot import navigator -from dronekit import connect, VehicleMode -import os -import RPi.GPIO as GPIO - - -CONN_STR = "udp:127.0.0.1:14551" -MESSENGER_PORT = 14552 -mavlink_str = "udp:127.0.0.1:14553" -GPIO_PIN = 23 - -drone = connect(CONN_STR, wait_ready=False) - -nav = navigator.Navigator(drone, MESSENGER_PORT) -mavlink = MAVLinkDelegate(conn_str = mavlink_str) - - -GPIO.setmode(GPIO.BCM) -GPIO.setup(GPIO_PIN, GPIO.OUT) -GPIO.output(23, GPIO.HIGH) - -time.sleep(2) - -GPIO.output(23, GPIO.LOW) - - -bucket_avg = [[], []] -big_bucket_height = 1 - -os.makedirs("tmp/log", exist_ok=True) -dirs = os.listdir("tmp/log") -ft_num = len(dirs) -os.makedirs(f"tmp/log/{ft_num}") - - -def moving_bucket_avg(_, pos): - - if pos: - if len(bucket_avg[0]) > 0 and len(bucket_avg[1]) > 0: - - num_x = len(bucket_avg[0]) - num_y = len(bucket_avg[1]) - - avg_x = sum(bucket_avg[0]) / num_x - avg_y = sum(bucket_avg[1]) / num_y - - new_x_avg = avg_x + pos[0] / (num_x + 1) - new_y_avg = avg_y + pos[1] / (num_y + 1) - - bucket_avg[0].append(new_x_avg) - bucket_avg[1].append(new_y_avg) - else: - bucket_avg[0].append(pos[0]) - bucket_avg[1].append(pos[1]) - - time.sleep(1) - - -camera = DebugCamera("photos/375.png") - -model_file = "best.pt" - -detector = BucketDetector(f"samples/models/{model_file}") - -analysis = ImageAnalysisDelegate(detector, camera, navigation_provider = nav) -analysis.subscribe(moving_bucket_avg) - -nav.send_status_message("Shepard is online") - -while not (drone.armed and drone.mode == VehicleMode("GUIDED")): - pass - -time.sleep(5) - -nav.takeoff(30) - -type_mask = nav.generate_typemask([0, 1, 2]) - -nav.send_status_message("Executing") -current_alt = nav.get_local_position_ned()[2] - -delta = 0.5 -sleep_time = 2 - - -def bucket_descent(target_height): - # VERIFY ALL OF THESE COORINDATE SYSTEMS BEFORE FLYING!!!!!!!!!!!!!!!!!!!!!!!!!! - - coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED - - nav.set_position_target_local_ned(x = bucket_avg[0][-1], - y = bucket_avg[1][-1], - z = 0, - type_mask = type_mask, - coordinate_frame = coordinate_frame) - - time.sleep(5) - alt = nav.get_local_position_ned()[2] - i = 0 - while -(alt) >= 11: - print(i) - - alt = nav.get_local_position_ned()[2] - - print(alt) - nav.set_position_target_local_ned(x = float(bucket_avg[0][-1] - bucket_avg[0][0]), - y = float(bucket_avg[1][-1] - bucket_avg[1][0]), - z = 5, - type_mask = type_mask, - coordinate_frame = coordinate_frame) - i += 1 - time.sleep(5) - - # Full Commit, drone is set to hover at the target height - # NOTE: COORDINATE SYSTEM HAS CHANGED. IT IS IN NED, DOWN IS POSITIVE - - nav.send_status_message("Comitting to bucket") - coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_NED - current_local_pos = nav.get_local_position_ned() - - nav.set_position_target_local_ned(x = current_local_pos[0], - y = current_local_pos[1], - z = -target_height, - type_mask = type_mask, - coordinate_frame = coordinate_frame) - - -def ActivatePump(duration): - coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_NED - current_local_pos = nav.get_local_position_ned() - # Runs the pump that is connected to GPIO 23 for a specified amount of time - - nav.set_position_target_local_ned(x = current_local_pos[0], - y = current_local_pos[1], - z = current_local_pos[2], - type_mask = type_mask, - coordinate_frame = coordinate_frame) - - GPIO.output(GPIO_PIN, GPIO.HIGH) - - # Make sure that the drone maintains its location above the bucket, hopefully avoiding drift due to wind or other sources - # Adjusts the drones position every half second, and the duration is equivalent to the duration passed to ActivatePump() - for _ in range(duration - 1): - - nav.set_position_target_local_ned(x = current_local_pos[0], - y = current_local_pos[1], - z = current_local_pos[2], - type_mask = type_mask, - coordinate_frame = coordinate_frame) - time.sleep(0.5) - - nav.set_position_target_local_ned(x = current_local_pos[0], - y = current_local_pos[1], - z = current_local_pos[2], - type_mask = type_mask, - coordinate_frame = coordinate_frame) - time.sleep(0.5) - - GPIO.output(GPIO_PIN, GPIO.LOW) - - -nav.send_status_message("Descending to bucket") -analysis.start() -time.sleep(5) -if len(bucket_avg[0]) > 0 and len(bucket_avg[1]) > 0: - bucket_descent(big_bucket_height) -else: - time.sleep(5) - bucket_descent(big_bucket_height) -analysis.stop() -nav.send_status_message("Stopped the analysis") - -nav.send_status_message("Activating Pump") - -ActivatePump(5) - -nav.return_to_launch() - -drone.close() - -nav.send_status_message("Flight test script execution terminated") - - - -import threading -import queue - -import asyncio -from typing import Callable, List - -from aiohttp import web -from aiohttp import web -import aiohttp - -import json - - -class Emu(): - """ - class representation of a connection to Emu - """ - def __init__(self, img_dir: str): - self.img_dir = img_dir - - self._send_queue = queue.Queue() - self._recv_queue = queue.Queue() - - # All messages in ._recv_queue are advertised to subscribers - self._subscribers: List[Callable] = [] - - # set default onConnect to be a no-op - self._on_connect = lambda: None - self._is_connected = False - - def start_comms(self): - self._comms_thread = threading.Thread(target=self._start_comms_loop, daemon=True) - self._comms_thread.start() - - def send_image(self, path: str): - """ - sends the image at specified path to Emu - the path sent should be accessable from within self.img_dir so it can be accessed through - /images/{filename} - """ - print(path) - img_url = "/images/" + path - print(img_url) - content = { - "type": "img", - "value": img_url - } - self._send_queue.put(json.dumps(content)) - - def send_log(self, message: str, severity: str="normal"): - """ - sends a log message to Emu - message: string of flog - severity: "normal", "warning", "error" - """ - content = { - "type": "log", - "message": message, - "severity": severity - } - self._send_queue.put(json.dumps(content)) - - def send_msg(self, message: str): - """ - sends message as it is, follow the proper JSON API messages - """ - self._send_queue.put(message) - - def set_on_connect(self, func: Callable): - self._on_connect = func - - def _start_comms_loop(self): - """ - starts connection loop with asyncio - """ - print("start_comms loop") - self.app = web.Application() - self.app.add_routes([web.static('/images', self.img_dir), - web.get('/ws', self.handle_websocket)]) - - web.run_app(self.app, handle_signals=False) - - def subscribe(self, subscriber: Callable): - self._subscribers.append(subscriber) - - async def producer_handler(self, ws): - """ - handles sending messages to the client - """ - event_loop = asyncio.get_running_loop() - while not ws.closed: - message = await asyncio.to_thread(self._send_queue.get) - - await ws.send_str(message) - - async def consumer_handler(self, ws): - async for msg in ws: - if msg.type == aiohttp.WSMsgType.TEXT: - for subscriber in self._subscribers: - subscriber(msg.data) - - elif msg.type == aiohttp.WSMsgType.ERROR: - print("WebSocket error:", ws.exception()) - - async def handle_websocket(self, request): - ws = web.WebSocketResponse() - await ws.prepare(request) - - self._is_connected = True - self._on_connect() - - consumer_task = asyncio.create_task(self.consumer_handler(ws)) - producer_task = asyncio.create_task(self.producer_handler(ws)) - - _, pending = await asyncio.wait( - [producer_task, consumer_task], - return_when=asyncio.FIRST_COMPLETED, - ) - - for task in pending: - task.cancel() - - print('websocket connection closed') - self._is_connected = False - - return ws - - -