From 81354abe4bd2d687c75152e4fce7f7219880169b Mon Sep 17 00:00:00 2001 From: preston Date: Wed, 21 Jan 2026 09:25:00 -0500 Subject: [PATCH 1/2] Add parallel compilation --- bin/rover.dart | 72 +++++++++++++++++++++++------------------ subsystems/src/Makefile | 6 ++-- temp.service | 19 +++++++++++ video/build.sh | 4 +-- 4 files changed, 65 insertions(+), 36 deletions(-) create mode 100644 temp.service diff --git a/bin/rover.dart b/bin/rover.dart index 4f37e476..608000da 100644 --- a/bin/rover.dart +++ b/bin/rover.dart @@ -55,40 +55,50 @@ Future compileAllPrograms(String? only) async { await runCommand("sudo", ["apt", "update", "-y"]); } - for (final program in programs) { - final name = program.name; - if (only != null && name != only) continue; - logger.info("Processing the $name program"); - - // Stop the service if it was already running - if (await isServiceRunning(name)) { - logger.debug("Stopping $name..."); - await runCommand("sudo", ["systemctl", "stop", name]); - await runCommand("sudo", ["systemctl", "disable", name], hideOutput: true); // always uses stderr - } + final programsToCompile = only != null + ? programs.where((p) => p.name == only).toList() + : programs; - // Run any pre-requisite commands - final extraCommands = program.extraCommands; - if (extraCommands != null) { - for (final extraCommand in extraCommands) { - if (offline && extraCommand.requiresInternet) { - logger.debug("Skipping '${extraCommand.task}' because it requires internet"); - continue; - } - logger.info("- ${extraCommand.task}..."); - await runCommand(extraCommand.command, extraCommand.args, workingDirectory: program.sourceDir); - } - } + logger.info("Starting parallel compilation of ${programsToCompile.length} programs..."); + + await Future.wait(programsToCompile.map((program) async { + await compileProgram(program); + })); +} + +Future compileProgram(RoverProgram program) async { + final name = program.name; + logger.info("Processing the $name program"); - // Compile the program - final command = program.compileCommand; - if (command != null) { - logger.info("- Compiling. This could take a few minutes..."); - final (cmd, args) = command; - await runCommand(cmd, args, workingDirectory: program.sourceDir); + // Stop the service if it was already running + if (await isServiceRunning(name)) { + logger.debug("Stopping $name..."); + await runCommand("sudo", ["systemctl", "stop", name]); + await runCommand("sudo", ["systemctl", "disable", name], hideOutput: true); // always uses stderr + } + + // Run any pre-requisite commands + final extraCommands = program.extraCommands; + if (extraCommands != null) { + for (final extraCommand in extraCommands) { + if (offline && extraCommand.requiresInternet) { + logger.debug("Skipping '${extraCommand.task}' because it requires internet"); + continue; + } + logger.info("- $name: ${extraCommand.task}..."); + await runCommand(extraCommand.command, extraCommand.args, workingDirectory: program.sourceDir); } + } - // Save, enable, and start the systemd service - await writeSystemdFile(program); + // Compile the program + final command = program.compileCommand; + if (command != null) { + logger.info("- $name: Compiling. This could take a few minutes..."); + final (cmd, args) = command; + await runCommand(cmd, args, workingDirectory: program.sourceDir); } + + // Save, enable, and start the systemd service + await writeSystemdFile(program); + logger.info("- $name: Compilation complete!"); } diff --git a/subsystems/src/Makefile b/subsystems/src/Makefile index 7286cb3b..4ce2928c 100644 --- a/subsystems/src/Makefile +++ b/subsystems/src/Makefile @@ -3,14 +3,14 @@ all: burt_can/burt_can.so libserialport.so install burt_can/burt_can.so: burt_can/*.h burt_can/*.cpp burt_can/*.hpp - make -C burt_can shared + make -j$(nproc) -C burt_can shared clean: rm -f *.o *.so *.exp *.lib *.obj *.out rm -f burt_can/*.so rm -f libserialport/*.so rm -f ../*.so - make -C burt_can clean + make -j$(nproc) -C burt_can clean SerialCommand = gcc SerialCommand += libserialport/serialport.c @@ -29,4 +29,4 @@ libserialport.so: libserialport/serialport.c libserialport/timing.c libserialpor install: burt_can/burt_can.so libserialport.so sudo cp libserialport.so /usr/lib - make -C burt_can install + make -j$(nproc) -C burt_can install diff --git a/temp.service b/temp.service new file mode 100644 index 00000000..b54cc294 --- /dev/null +++ b/temp.service @@ -0,0 +1,19 @@ +# AUTO-GENERATED FILE. DO NOT EDIT. +# To re-generate this file, edit `~/rover/lib/all_programs.dart` and run `dart run :systemd` +# +# This file defines a program that automatically starts on boot. +# Place this file in /etc/systemd/system. +# +# For more information, see: https://bing-rover.gitbook.io/docs/v/software/onboard-computers/configuring-systemd + +[Unit] +Description=Navigates the field autonomously +After=network-online.target + +[Service] +WorkingDirectory=/Users/preston/Projects/Rover-Code/autonomy +ExecStart=/Users/preston/autonomy +TimeoutStopSec=3 + +[Install] +WantedBy=multi-user.target diff --git a/video/build.sh b/video/build.sh index fd32013b..8bdd50c4 100644 --- a/video/build.sh +++ b/video/build.sh @@ -24,5 +24,5 @@ then fi # Build our FFI wrappers -make -C src clean -make -C src shared +make -j$(nproc) -C src clean +make -j$(nproc) -C src shared From 7999aa21e5daf5fedca563ff78691536684cd7bd Mon Sep 17 00:00:00 2001 From: preston Date: Wed, 21 Jan 2026 09:45:32 -0500 Subject: [PATCH 2/2] Fix parallel compilation apt package manager conflicts --- autonomy/.github/pubspec_overrides.yaml | 7 + autonomy/.github/workflows/analyze.yml | 41 +++ autonomy/.github/workflows/documentation.yml | 55 ++++ autonomy/bin/arcuo.dart | 16 ++ autonomy/bin/sensorless.dart | 63 +++++ autonomy/bin/test.dart | 23 ++ autonomy/lib/src/interfaces/a_star.dart | 66 +++++ autonomy/lib/src/interfaces/autonomy.dart | 66 +++++ autonomy/lib/src/interfaces/detector.dart | 10 + autonomy/lib/src/interfaces/drive.dart | 79 ++++++ autonomy/lib/src/interfaces/error.dart | 14 + autonomy/lib/src/interfaces/gps.dart | 27 ++ autonomy/lib/src/interfaces/gps_utils.dart | 62 +++++ autonomy/lib/src/interfaces/imu.dart | 19 ++ autonomy/lib/src/interfaces/imu_utils.dart | 27 ++ autonomy/lib/src/interfaces/orchestrator.dart | 61 +++++ autonomy/lib/src/interfaces/pathfinding.dart | 18 ++ autonomy/lib/src/interfaces/receiver.dart | 9 + autonomy/lib/src/interfaces/reporter.dart | 23 ++ autonomy/lib/src/interfaces/server.dart | 25 ++ autonomy/lib/src/interfaces/service.dart | 1 + autonomy/lib/src/interfaces/video.dart | 17 ++ autonomy/lib/src/rover/corrector.dart | 40 +++ autonomy/lib/src/rover/detector.dart | 21 ++ autonomy/lib/src/rover/drive.dart | 67 +++++ autonomy/lib/src/rover/drive/motors.dart | 26 ++ autonomy/lib/src/rover/drive/sensor.dart | 148 +++++++++++ autonomy/lib/src/rover/drive/timed.dart | 51 ++++ autonomy/lib/src/rover/gps.dart | 41 +++ autonomy/lib/src/rover/imu.dart | 43 +++ autonomy/lib/src/rover/orchestrator.dart | 113 ++++++++ autonomy/lib/src/rover/pathfinding.dart | 22 ++ autonomy/lib/src/rover/sensorless.dart | 64 +++++ autonomy/lib/src/rover/video.dart | 32 +++ autonomy/lib/src/simulator/detector.dart | 46 ++++ autonomy/lib/src/simulator/drive.dart | 54 ++++ autonomy/lib/src/simulator/gps.dart | 28 ++ autonomy/lib/src/simulator/imu.dart | 35 +++ autonomy/lib/src/simulator/orchestrator.dart | 32 +++ autonomy/lib/src/simulator/pathfinding.dart | 25 ++ autonomy/lib/src/simulator/realsense.dart | 22 ++ autonomy/test/sensor_test.dart | 246 ++++++++++++++++++ bin/rover.dart | 38 ++- 43 files changed, 1922 insertions(+), 1 deletion(-) create mode 100644 autonomy/.github/pubspec_overrides.yaml create mode 100644 autonomy/.github/workflows/analyze.yml create mode 100644 autonomy/.github/workflows/documentation.yml create mode 100644 autonomy/bin/arcuo.dart create mode 100644 autonomy/bin/sensorless.dart create mode 100644 autonomy/bin/test.dart create mode 100644 autonomy/lib/src/interfaces/a_star.dart create mode 100644 autonomy/lib/src/interfaces/autonomy.dart create mode 100644 autonomy/lib/src/interfaces/detector.dart create mode 100644 autonomy/lib/src/interfaces/drive.dart create mode 100644 autonomy/lib/src/interfaces/error.dart create mode 100644 autonomy/lib/src/interfaces/gps.dart create mode 100644 autonomy/lib/src/interfaces/gps_utils.dart create mode 100644 autonomy/lib/src/interfaces/imu.dart create mode 100644 autonomy/lib/src/interfaces/imu_utils.dart create mode 100644 autonomy/lib/src/interfaces/orchestrator.dart create mode 100644 autonomy/lib/src/interfaces/pathfinding.dart create mode 100644 autonomy/lib/src/interfaces/receiver.dart create mode 100644 autonomy/lib/src/interfaces/reporter.dart create mode 100644 autonomy/lib/src/interfaces/server.dart create mode 100644 autonomy/lib/src/interfaces/service.dart create mode 100644 autonomy/lib/src/interfaces/video.dart create mode 100644 autonomy/lib/src/rover/corrector.dart create mode 100644 autonomy/lib/src/rover/detector.dart create mode 100644 autonomy/lib/src/rover/drive.dart create mode 100644 autonomy/lib/src/rover/drive/motors.dart create mode 100644 autonomy/lib/src/rover/drive/sensor.dart create mode 100644 autonomy/lib/src/rover/drive/timed.dart create mode 100644 autonomy/lib/src/rover/gps.dart create mode 100644 autonomy/lib/src/rover/imu.dart create mode 100644 autonomy/lib/src/rover/orchestrator.dart create mode 100644 autonomy/lib/src/rover/pathfinding.dart create mode 100644 autonomy/lib/src/rover/sensorless.dart create mode 100644 autonomy/lib/src/rover/video.dart create mode 100644 autonomy/lib/src/simulator/detector.dart create mode 100644 autonomy/lib/src/simulator/drive.dart create mode 100644 autonomy/lib/src/simulator/gps.dart create mode 100644 autonomy/lib/src/simulator/imu.dart create mode 100644 autonomy/lib/src/simulator/orchestrator.dart create mode 100644 autonomy/lib/src/simulator/pathfinding.dart create mode 100644 autonomy/lib/src/simulator/realsense.dart create mode 100644 autonomy/test/sensor_test.dart diff --git a/autonomy/.github/pubspec_overrides.yaml b/autonomy/.github/pubspec_overrides.yaml new file mode 100644 index 00000000..b7c9d120 --- /dev/null +++ b/autonomy/.github/pubspec_overrides.yaml @@ -0,0 +1,7 @@ +resolution: + +dependency_overrides: + burt_network: + git: + url: https://github.com/BinghamtonRover/Networking + ref: 2.3.1 \ No newline at end of file diff --git a/autonomy/.github/workflows/analyze.yml b/autonomy/.github/workflows/analyze.yml new file mode 100644 index 00000000..b822d66a --- /dev/null +++ b/autonomy/.github/workflows/analyze.yml @@ -0,0 +1,41 @@ +name: Dart Analyzer + +on: + push: + branches: [ "main" ] + pull_request: + +jobs: + analyze: + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + + # Note: This workflow uses the latest stable version of the Dart SDK. + # You can specify other versions if desired, see documentation here: + # https://github.com/dart-lang/setup-dart/blob/main/README.md + - uses: dart-lang/setup-dart@v1 + with: + sdk: 3.6.0 + + # This package is part of a Pub Workspace. However, CI still needs to + # run on this repo by itself, so we want to override burt_network to use + # a Git dependency ONLY on GitHub Actions. + # + # To get around this, we commit the overrides to the .github folder where + # Dart can't find them, then copy them as part of the CI workflow. + - name: Install dependencies + run: | + mv .github/pubspec_overrides.yaml . + dart pub get + + - name: Analyze project source + run: dart analyze --fatal-infos + + # Your project will need to have tests in test/ and a dependency on + # package:test for this step to succeed. Note that Flutter projects will + # want to change this to 'flutter test'. + # FIXME: Disabling tests for now + # - name: Run tests + # run: dart test diff --git a/autonomy/.github/workflows/documentation.yml b/autonomy/.github/workflows/documentation.yml new file mode 100644 index 00000000..0267dfea --- /dev/null +++ b/autonomy/.github/workflows/documentation.yml @@ -0,0 +1,55 @@ +name: Generate Dart Documentation + +on: + push: + branches: [ "main" ] + +jobs: + build: + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + with: + fetch-depth: 0 + + - name: Git Setup + run: | + git config user.name "GitHub Actions Bot" + git config user.email "<>" + git branch --all + git switch --track origin/documentation + git reset --hard origin/main + + - name: Install Dart + uses: dart-lang/setup-dart@v1 + with: + sdk: 3.5.2 + + - name: Install dependencies + run: dart pub get + + - name: Analyze Dart code + run: dart analyze --fatal-infos + + - name: Output error + if: failure() + run: echo "::error The code or is missing documentation. Run flutter analyze --dartdocs" + + - name: Generate documentation + run: dart doc --output=docs + + # Your project will need to have tests in test/ and a dependency on + # package:test for this step to succeed. Note that Flutter projects will + # want to change this to 'flutter test'. + - name: Run tests + run: dart test + + - name: Commit and push files + run: | + cd docs + cd .. + git status + git stage --force docs + git commit -a -m "Generated documentation" + git push --force diff --git a/autonomy/bin/arcuo.dart b/autonomy/bin/arcuo.dart new file mode 100644 index 00000000..ae9f7d8c --- /dev/null +++ b/autonomy/bin/arcuo.dart @@ -0,0 +1,16 @@ +import "package:autonomy/autonomy.dart"; + +void main() async { + final rover = RoverAutonomy(); + rover.drive = RoverDrive(collection: rover, useImu: false, useGps: false); + await rover.init(); + //await rover.waitForValue(); + final didSeeAruco = await rover.drive.spinForAruco(); + if (didSeeAruco) { + rover.logger.info("Found aruco"); + await rover.drive.approachAruco(); + } else { + rover.logger.warning("COuld not find aruco"); + } + rover.logger.info("Done"); +} diff --git a/autonomy/bin/sensorless.dart b/autonomy/bin/sensorless.dart new file mode 100644 index 00000000..0ddcd1f0 --- /dev/null +++ b/autonomy/bin/sensorless.dart @@ -0,0 +1,63 @@ +import "package:burt_network/logging.dart"; +import "package:burt_network/protobuf.dart"; + +import "package:autonomy/simulator.dart"; +import "package:autonomy/rover.dart"; + +void main() async { + Logger.level = LogLevel.debug; + final simulator = AutonomySimulator(); + simulator.drive = SensorlessDrive(collection: simulator, useGps: false, useImu: false); + await simulator.init(); + await simulator.server.waitForConnection(); + + final message = AutonomyData( + destination: GpsCoordinates(latitude: 0, longitude: 0), + state: AutonomyState.DRIVING, + task: AutonomyTask.GPS_ONLY, + obstacles: [], + path: [ + for (double x = 0; x < 3; x++) + for (double y = 0; y < 3; y++) + GpsCoordinates(latitude: y, longitude: x), + ], + ); + simulator.server.sendMessage(message); + + // "Snakes" around a 3x3 meter box. (0, 0), North + await simulator.drive.goForward(); // (1, 0), North + await simulator.drive.goForward(); // (2, 0), North + await simulator.drive.turnRight(); // (2, 0), East + await simulator.drive.goForward(); // (2, 1), East + await simulator.drive.turnRight(); // (2, 1), South + await simulator.drive.goForward(); // (1, 1), South + await simulator.drive.goForward(); // (0, 1), South + await simulator.drive.turnLeft(); // (0, 1), East + await simulator.drive.goForward(); // (0, 2), East + await simulator.drive.turnLeft(); // (0, 2), North + await simulator.drive.goForward(); // (1, 2), North + await simulator.drive.goForward(); // (2, 2), North + await simulator.drive.turnLeft(); // (2, 2), West + await simulator.drive.goForward(); // (2, 1), West + await simulator.drive.goForward(); // (2, 0), West + await simulator.drive.turnLeft(); // (2, 0), South + await simulator.drive.goForward(); // (1, 0), South + await simulator.drive.goForward(); // (0, 0), South + await simulator.drive.turnLeft(); // (0, 0), East + await simulator.drive.turnLeft(); // (0, 0), North + + final message2 = AutonomyData( + state: AutonomyState.AT_DESTINATION, + task: AutonomyTask.AUTONOMY_TASK_UNDEFINED, + obstacles: [], + path: [ + for (double x = 0; x < 3; x++) + for (double y = 0; y < 3; y++) + GpsCoordinates(latitude: y, longitude: x), + ], + ); + simulator.server.sendMessage(message2); + simulator.server.sendMessage(message2); + + await simulator.dispose(); +} diff --git a/autonomy/bin/test.dart b/autonomy/bin/test.dart new file mode 100644 index 00000000..12205b1d --- /dev/null +++ b/autonomy/bin/test.dart @@ -0,0 +1,23 @@ +import "package:autonomy/autonomy.dart"; +import "package:burt_network/logging.dart"; +import "package:autonomy/src/rover/gps.dart"; +import "package:autonomy/src/rover/imu.dart"; + +void main() async { + Logger.level = LogLevel.all; + final rover = RoverAutonomy(); + rover.gps = RoverGps(collection: rover); + rover.imu = RoverImu(collection: rover); + rover.drive = RoverDrive(collection: rover, useGps: false); + + await rover.init(); +// await rover.waitForReadings(); +// await rover.waitForConnection(); + + rover.logger.info("Starting"); + await rover.drive.turnLeft(); + await rover.drive.turnRight(); + + rover.logger.info("Done"); + await rover.dispose(); +} diff --git a/autonomy/lib/src/interfaces/a_star.dart b/autonomy/lib/src/interfaces/a_star.dart new file mode 100644 index 00000000..6c7f16ec --- /dev/null +++ b/autonomy/lib/src/interfaces/a_star.dart @@ -0,0 +1,66 @@ +import "package:a_star/a_star.dart"; + +import "package:burt_network/protobuf.dart"; +import "package:autonomy/interfaces.dart"; + +class AutonomyAStarState extends AStarState { + final DriveDirection direction; + final DriveOrientation orientation; + final GpsCoordinates position; + final GpsCoordinates goal; + final AutonomyInterface collection; + + AutonomyAStarState({ + required this.position, + required this.goal, + required this.collection, + required this.direction, + required this.orientation, + required super.depth, + }); + + factory AutonomyAStarState.start({ + required AutonomyInterface collection, + required GpsCoordinates goal, + }) => AutonomyAStarState( + position: collection.gps.coordinates, + goal: goal, + collection: collection, + direction: DriveDirection.stop, + orientation: collection.imu.orientation!, + depth: 0, + ); + + @override + String toString() => switch(direction) { + DriveDirection.forward => "Go forward to ${position.prettyPrint()}", + DriveDirection.left => "Turn left to face $direction", + DriveDirection.right => "Turn right to face $direction", + DriveDirection.stop => "Start/Stop at ${position.prettyPrint()}", + }; + + @override + double heuristic() => position.manhattanDistance(goal); + + @override + String hash() => "${position.prettyPrint()} ($orientation)"; + + @override + bool isGoal() => position.isNear(goal); + + AutonomyAStarState copyWith({required DriveDirection direction, required DriveOrientation orientation, required GpsCoordinates position}) => AutonomyAStarState( + collection: collection, + position: position, + orientation: orientation, + direction: direction, + goal: goal, + depth: direction == DriveDirection.forward ? depth + 1 : depth + 2, + ); + + @override + Iterable expand() => [ + copyWith(direction: DriveDirection.forward, orientation: orientation, position: position.goForward(orientation)), + copyWith(direction: DriveDirection.left, orientation: orientation.turnLeft(), position: position), + copyWith(direction: DriveDirection.right, orientation: orientation.turnRight(), position: position), + ].where((state) => !collection.pathfinder.isObstacle(state.position)); +} diff --git a/autonomy/lib/src/interfaces/autonomy.dart b/autonomy/lib/src/interfaces/autonomy.dart new file mode 100644 index 00000000..ae69aba6 --- /dev/null +++ b/autonomy/lib/src/interfaces/autonomy.dart @@ -0,0 +1,66 @@ +import "package:burt_network/burt_network.dart"; + +import "package:autonomy/interfaces.dart"; + +abstract class AutonomyInterface extends Service with Receiver { + BurtLogger get logger; + GpsInterface get gps; + ImuInterface get imu; + DriveInterface get drive; + RoverSocket get server; + PathfindingInterface get pathfinder; + DetectorInterface get detector; + VideoInterface get video; + OrchestratorInterface get orchestrator; + + @override + Future init() async { + var result = true; + result &= await gps.init(); + result &= await imu.init(); + result &= await drive.init(); + result &= await server.init(); + result &= await pathfinder.init(); + result &= await detector.init(); + result &= await video.init(); + logger.info("Init orchestrator"); + await orchestrator.init(); + logger.info("Init orchestrator done"); + if (result) { + logger.info("Autonomy initialized"); + } else { + logger.warning("Autonomy could not initialize"); + } + return result; + } + + @override + Future dispose() async { + await gps.dispose(); + await imu.dispose(); + await drive.dispose(); + await pathfinder.dispose(); + await detector.dispose(); + await video.dispose(); + await orchestrator.dispose(); + logger.info("Autonomy disposed"); + await server.dispose(); + } + + Future restart() async { + await dispose(); + await init(); + } + + @override + Future waitForValue() async { + logger.info("Waiting for readings..."); + await gps.waitForValue(); + await imu.waitForValue(); + await video.waitForValue(); + logger.info("Received GPS and IMU values"); + } + + @override + bool get hasValue => gps.hasValue && imu.hasValue && video.hasValue; +} diff --git a/autonomy/lib/src/interfaces/detector.dart b/autonomy/lib/src/interfaces/detector.dart new file mode 100644 index 00000000..c42a6943 --- /dev/null +++ b/autonomy/lib/src/interfaces/detector.dart @@ -0,0 +1,10 @@ +import "package:autonomy/interfaces.dart"; + +abstract class DetectorInterface extends Service { + AutonomyInterface collection; + DetectorInterface({required this.collection}); + + bool findObstacles(); + bool canSeeAruco(); + bool isOnSlope(); +} diff --git a/autonomy/lib/src/interfaces/drive.dart b/autonomy/lib/src/interfaces/drive.dart new file mode 100644 index 00000000..08b677ed --- /dev/null +++ b/autonomy/lib/src/interfaces/drive.dart @@ -0,0 +1,79 @@ +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; + +enum DriveDirection { + forward, + left, + right, + stop, +} + +enum DriveOrientation { + north(0), + west(90), + south(180), + east(270); + + final int angle; + const DriveOrientation(this.angle); + + static DriveOrientation? fromRaw(Orientation orientation) { + // TODO: Make this more precise. + for (final value in values) { + if (orientation.isNear(value.angle.toDouble())) return value; + } + return null; + } + + DriveOrientation turnLeft() => switch (this) { + north => west, + west => south, + south => east, + east => north, + }; + + DriveOrientation turnRight() => switch (this) { + north => east, + west => north, + south => west, + east => south, + }; +} + +abstract class DriveInterface extends Service { + AutonomyInterface collection; + DriveOrientation orientation = DriveOrientation.north; + DriveInterface({required this.collection}); + + Future goDirection(DriveDirection direction) async => switch (direction) { + DriveDirection.forward => await goForward(), + DriveDirection.left => await turnLeft(), + DriveDirection.right => await turnRight(), + DriveDirection.stop => await stop(), + }; + + Future faceNorth(); + + Future goForward(); + Future turnLeft(); + Future turnRight(); + Future stop(); + + Future faceDirection(DriveOrientation orientation) async { + this.orientation = orientation; + } + + Future followPath(List path) async { + for (final state in path) { + await goDirection(state.direction); + } + } + + void setLedStrip(ProtoColor color, {bool blink = false}) { + final command = DriveCommand(color: color, blink: blink ? BoolState.YES : BoolState.NO); + collection.server.sendCommand(command); + } + + Future spinForAruco() async => false; + Future approachAruco() async { } +} diff --git a/autonomy/lib/src/interfaces/error.dart b/autonomy/lib/src/interfaces/error.dart new file mode 100644 index 00000000..9d0c9ed3 --- /dev/null +++ b/autonomy/lib/src/interfaces/error.dart @@ -0,0 +1,14 @@ +import "dart:math"; + +class RandomError { + final double maxError; + final _random = Random(); + + RandomError(this.maxError); + + double get value { + final sign = _random.nextBool() ? 1 : -1; + final value = _random.nextDouble() * maxError; + return sign * value; + } +} diff --git a/autonomy/lib/src/interfaces/gps.dart b/autonomy/lib/src/interfaces/gps.dart new file mode 100644 index 00000000..8ccef300 --- /dev/null +++ b/autonomy/lib/src/interfaces/gps.dart @@ -0,0 +1,27 @@ +import "package:burt_network/protobuf.dart"; +import "package:autonomy/interfaces.dart"; + +abstract class GpsInterface extends Service with Receiver { + static const gpsError = 0.00003; + static double currentLatitude = 0; + + final AutonomyInterface collection; + GpsInterface({required this.collection}); + + double get longitude => coordinates.longitude; + double get latitude => coordinates.latitude; + + void update(GpsCoordinates newValue); + GpsCoordinates get coordinates; + + bool isNear(GpsCoordinates other) => coordinates.isNear(other); + + @override + Future waitForValue() async { + await super.waitForValue(); + currentLatitude = coordinates.latitude; + } + + @override + Future init() async => true; +} diff --git a/autonomy/lib/src/interfaces/gps_utils.dart b/autonomy/lib/src/interfaces/gps_utils.dart new file mode 100644 index 00000000..dc4fc74c --- /dev/null +++ b/autonomy/lib/src/interfaces/gps_utils.dart @@ -0,0 +1,62 @@ + +import "dart:math"; + +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; + +extension RecordToGps on (num, num) { + GpsCoordinates toGps() => GpsCoordinates(latitude: $1.toDouble() * GpsUtils.latitudePerMeter, longitude: $2.toDouble() * GpsUtils.longitudePerMeter); +} + +extension GpsUtils on GpsCoordinates { + static double maxErrorMeters = 1; + static double get epsilonLatitude => maxErrorMeters * latitudePerMeter; + static double get epsilonLongitude => maxErrorMeters * longitudePerMeter; + + static GpsCoordinates get east => + GpsCoordinates(longitude: -1 / metersPerLongitude); + static GpsCoordinates get west => + GpsCoordinates(longitude: 1 / metersPerLongitude); + static GpsCoordinates get north => + GpsCoordinates(latitude: 1 * latitudePerMeter); + static GpsCoordinates get south => + GpsCoordinates(latitude: -1 * latitudePerMeter); + + // Taken from https://stackoverflow.com/a/39540339/9392211 + // static const metersPerLatitude = 111.32 * 1000; // 111.32 km + static const metersPerLatitude = 1; + static const radiansPerDegree = pi / 180; + static double get metersPerLongitude => 1; +// 40075 * cos( GpsInterface.currentLatitude * radiansPerDegree ) / 360 * 1000; + + static double get latitudePerMeter => 1 / metersPerLatitude; + static double get longitudePerMeter => 1 / metersPerLongitude; + + double distanceTo(GpsCoordinates other) => sqrt( + pow(latitude - other.latitude, 2) + + pow(longitude - other.longitude, 2), + ); + + double manhattanDistance(GpsCoordinates other) => + (latitude - other.latitude).abs() * metersPerLatitude + + (longitude - other.longitude).abs() * metersPerLongitude; + + bool isNear(GpsCoordinates other) => + (latitude - other.latitude).abs() < epsilonLatitude && + (longitude - other.longitude).abs() < epsilonLongitude; + + GpsCoordinates operator +(GpsCoordinates other) => GpsCoordinates( + latitude: latitude + other.latitude, + longitude: longitude + other.longitude, + ); + +// String prettyPrint() => "(lat=${(latitude * GpsUtils.metersPerLatitude).toStringAsFixed(2)}, long=${(longitude * GpsUtils.metersPerLongitude).toStringAsFixed(2)})"; + String prettyPrint() => toProto3Json().toString(); + + GpsCoordinates goForward(DriveOrientation orientation) => this + switch(orientation) { + DriveOrientation.north => GpsUtils.north, + DriveOrientation.south => GpsUtils.south, + DriveOrientation.west => GpsUtils.west, + DriveOrientation.east => GpsUtils.east, + }; +} diff --git a/autonomy/lib/src/interfaces/imu.dart b/autonomy/lib/src/interfaces/imu.dart new file mode 100644 index 00000000..5bb6bba2 --- /dev/null +++ b/autonomy/lib/src/interfaces/imu.dart @@ -0,0 +1,19 @@ +import "package:autonomy/interfaces.dart"; +import "package:burt_network/burt_network.dart"; + +abstract class ImuInterface extends Service with Receiver { + final AutonomyInterface collection; + ImuInterface({required this.collection}); + + double get heading => raw.z; + Orientation get raw; + DriveOrientation? get orientation { + collection.logger.trace("Trying to find orientation at $heading"); + return DriveOrientation.fromRaw(raw); + } + void update(Orientation newValue); + bool isNear(double angle) => raw.isNear(angle); + + @override + Future init() async => true; +} diff --git a/autonomy/lib/src/interfaces/imu_utils.dart b/autonomy/lib/src/interfaces/imu_utils.dart new file mode 100644 index 00000000..588189d3 --- /dev/null +++ b/autonomy/lib/src/interfaces/imu_utils.dart @@ -0,0 +1,27 @@ +import "package:burt_network/protobuf.dart"; + +extension OrientationUtils on Orientation { + static const double epsilon = 10; + + static final north = Orientation(z: 0); + static final west = Orientation(z: 90); + static final south = Orientation(z: 180); + static final east = Orientation(z: 270); + + double get heading => z; + + bool get isEmpty => x == 0 && y == 0 && z == 0; + + Orientation clampHeading() { + var adjustedHeading = heading; + if (heading >= 360) adjustedHeading -= 360; + if (heading < 0) adjustedHeading = 360 + heading; + return Orientation(x: x, y: y, z: adjustedHeading); + } + + bool isNear(double value) => value > 270 && z < 90 + ? (z + 360 - value).abs() < epsilon + : value < 90 && z > 270 + ? (z - value - 360).abs() < epsilon + : (z - value).abs() < epsilon; +} diff --git a/autonomy/lib/src/interfaces/orchestrator.dart b/autonomy/lib/src/interfaces/orchestrator.dart new file mode 100644 index 00000000..13ee8ed6 --- /dev/null +++ b/autonomy/lib/src/interfaces/orchestrator.dart @@ -0,0 +1,61 @@ +import "dart:io"; + +import "package:autonomy/interfaces.dart"; +import "package:burt_network/burt_network.dart"; +import "package:meta/meta.dart"; + +abstract class OrchestratorInterface extends Service { + final AutonomyInterface collection; + OrchestratorInterface({required this.collection}); + + AutonomyCommand? currentCommand; + AutonomyState currentState = AutonomyState.AUTONOMY_STATE_UNDEFINED; + Future onCommand(AutonomyCommand command) async { + collection.server.sendMessage(command); + if (command.abort) return abort(); + if (currentCommand != null) { + collection.logger.error("Already executing a command", body: "Abort first if you want to switch tasks"); + return; + } + + if (!collection.hasValue) { + collection.logger.error("Sensors haven't gotten any readings yet!"); + currentState = AutonomyState.NO_SOLUTION; + return; + } + await collection.drive.faceNorth(); + currentCommand = command; + switch (command.task) { + case AutonomyTask.GPS_ONLY: await handleGpsTask(command); + case AutonomyTask.VISUAL_MARKER: await handleArucoTask(command); + // TODO: Add more tasks + default: collection.logger.error("Unrecognized task: ${command.task}"); // ignore: no_default_cases + } + } + + @override + Future init() async { + collection.server.messages.onMessage( + name: AutonomyCommand().messageName, + constructor: AutonomyCommand.fromBuffer, + callback: onCommand, + ); + return true; + } + + @mustCallSuper + Future abort() async { + currentCommand = null; + collection.logger.warning("Aborting task!"); + currentState = AutonomyState.ABORTING; + await collection.drive.stop(); + await collection.dispose(); + exit(1); + } + + Future handleGpsTask(AutonomyCommand command); + Future handleArucoTask(AutonomyCommand command); + Future handleHammerTask(AutonomyCommand command); + Future handleBottleTask(AutonomyCommand command); + AutonomyData get statusMessage; +} diff --git a/autonomy/lib/src/interfaces/pathfinding.dart b/autonomy/lib/src/interfaces/pathfinding.dart new file mode 100644 index 00000000..a3b4c73a --- /dev/null +++ b/autonomy/lib/src/interfaces/pathfinding.dart @@ -0,0 +1,18 @@ +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; + +abstract class PathfindingInterface extends Service { + final AutonomyInterface collection; + PathfindingInterface({required this.collection}); + + List? getPath(GpsCoordinates destination, {bool verbose = false}); + + Set obstacles = {}; + void recordObstacle(GpsCoordinates coordinates) => obstacles.add(coordinates); + bool isObstacle(GpsCoordinates coordinates) => obstacles.any((obstacle) => obstacle.isNear(coordinates)); + + @override + Future dispose() async { + obstacles.clear(); + } +} diff --git a/autonomy/lib/src/interfaces/receiver.dart b/autonomy/lib/src/interfaces/receiver.dart new file mode 100644 index 00000000..59ec05bf --- /dev/null +++ b/autonomy/lib/src/interfaces/receiver.dart @@ -0,0 +1,9 @@ +mixin Receiver { + bool hasValue = false; + + Future waitForValue() async { + while (!hasValue) { + await Future.delayed(const Duration(seconds: 1)); + } + } +} diff --git a/autonomy/lib/src/interfaces/reporter.dart b/autonomy/lib/src/interfaces/reporter.dart new file mode 100644 index 00000000..4524b60d --- /dev/null +++ b/autonomy/lib/src/interfaces/reporter.dart @@ -0,0 +1,23 @@ +import "dart:async"; +import "package:burt_network/protobuf.dart"; + +import "package:autonomy/interfaces.dart"; + +mixin ValueReporter on Service { + AutonomyInterface get collection; + Message getMessage(); + + Timer? timer; + Duration get reportInterval => const Duration(milliseconds: 10); + + @override + Future init() async { + timer = Timer.periodic(reportInterval, (timer) => _reportValue()); + return super.init(); + } + + @override + Future dispose() async => timer?.cancel(); + + void _reportValue() => collection.server.sendMessage(getMessage()); +} diff --git a/autonomy/lib/src/interfaces/server.dart b/autonomy/lib/src/interfaces/server.dart new file mode 100644 index 00000000..cec6850e --- /dev/null +++ b/autonomy/lib/src/interfaces/server.dart @@ -0,0 +1,25 @@ +import "dart:io"; + +import "package:burt_network/burt_network.dart"; + +extension ServerUtils on RoverSocket { + static SocketInfo subsystemsDestination = SocketInfo( + address: InternetAddress("192.168.1.20"), + port: 8001, + ); + + void sendCommand(Message message) => sendMessage(message, destination: subsystemsDestination); + + Future waitForConnection() async { + logger.info("Waiting for connection..."); + while (!isConnected) { + await Future.delayed(const Duration(milliseconds: 100)); + } + return; + } + + void sendDone() { + final message = AutonomyData(state: AutonomyState.AT_DESTINATION, task: AutonomyTask.AUTONOMY_TASK_UNDEFINED); + sendMessage(message); + } +} diff --git a/autonomy/lib/src/interfaces/service.dart b/autonomy/lib/src/interfaces/service.dart new file mode 100644 index 00000000..19cbacaa --- /dev/null +++ b/autonomy/lib/src/interfaces/service.dart @@ -0,0 +1 @@ +export "package:burt_network/burt_network.dart" show Service; diff --git a/autonomy/lib/src/interfaces/video.dart b/autonomy/lib/src/interfaces/video.dart new file mode 100644 index 00000000..6f3fc32b --- /dev/null +++ b/autonomy/lib/src/interfaces/video.dart @@ -0,0 +1,17 @@ +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; + +/// Handles obstacle detection data and ArUco data from video +abstract class VideoInterface extends Service with Receiver { + bool flag = false; + + final AutonomyInterface collection; + VideoInterface({required this.collection}); + + VideoData data = VideoData(); + + void updateFrame(VideoData newData); + + // double get arucoSize => data.arucoSize; + // double get arucoPosition => data.arucoPosition; +} diff --git a/autonomy/lib/src/rover/corrector.dart b/autonomy/lib/src/rover/corrector.dart new file mode 100644 index 00000000..46b59e1f --- /dev/null +++ b/autonomy/lib/src/rover/corrector.dart @@ -0,0 +1,40 @@ +import "dart:collection"; + +class ErrorCorrector { // non-nullable + final int maxSamples; + final double maxDeviation; + ErrorCorrector({required this.maxSamples, this.maxDeviation = double.infinity}); + + double calibratedValue = 0; + final Queue recentSamples = DoubleLinkedQueue(); + + void addValue(double value) { + if (recentSamples.isEmpty) { + recentSamples.add(value); + calibratedValue = value; + return; + } + final deviation = (calibratedValue - value).abs(); + if (deviation > maxDeviation) return; + if (recentSamples.length == maxSamples) recentSamples.removeLast(); + recentSamples.addFirst(value); + calibratedValue = recentSamples.weightedAverage(); + } + + void clear() { + calibratedValue = 0; + recentSamples.clear(); + } +} + +extension on Iterable { + double weightedAverage() { + num sum = 0; + var count = 0; + for (final element in this) { + sum += element; + count++; + } + return sum / count; + } +} diff --git a/autonomy/lib/src/rover/detector.dart b/autonomy/lib/src/rover/detector.dart new file mode 100644 index 00000000..f28c508b --- /dev/null +++ b/autonomy/lib/src/rover/detector.dart @@ -0,0 +1,21 @@ +import "package:autonomy/interfaces.dart"; + +class RoverDetector extends DetectorInterface { + RoverDetector({required super.collection}); + + @override + bool isOnSlope() => false; + + @override + bool findObstacles() => false; + + @override +// bool canSeeAruco() => collection.video.data.arucoDetected == BoolState.YES; + bool canSeeAruco() => collection.video.flag; + + @override + Future init() async => true; + + @override + Future dispose() async { } +} diff --git a/autonomy/lib/src/rover/drive.dart b/autonomy/lib/src/rover/drive.dart new file mode 100644 index 00000000..7cef09e4 --- /dev/null +++ b/autonomy/lib/src/rover/drive.dart @@ -0,0 +1,67 @@ +import "package:burt_network/burt_network.dart"; +import "package:autonomy/interfaces.dart"; + +import "drive/timed.dart"; +import "drive/sensor.dart"; + +/// A helper class to send drive commands to the rover with a simpler API. +class RoverDrive extends DriveInterface { + final bool useGps; + final bool useImu; + + final SensorDrive sensorDrive; + final TimedDrive timedDrive; + + // TODO: Calibrate these + RoverDrive({required super.collection, this.useGps = true, this.useImu = true}) : + sensorDrive = SensorDrive(collection: collection), + timedDrive = TimedDrive(collection: collection); + + /// Initializes the rover's drive subsystems. + @override + Future init() async => true; + + /// Stops the rover from driving. + @override + Future dispose() => stop(); + + /// Sets the angle of the front camera. + void setCameraAngle({required double swivel, required double tilt}) { + collection.logger.trace("Setting camera angles to $swivel (swivel) and $tilt (tilt)"); + final command = DriveCommand(frontSwivel: swivel, frontTilt: tilt); + collection.server.sendCommand(command); + } + + @override + Future stop() async { + await timedDrive.stop(); + } + + @override + Future faceNorth() => useImu ? sensorDrive.faceNorth() : timedDrive.faceNorth(); + + @override + Future spinForAruco() => sensorDrive.spinForAruco(); + @override + Future approachAruco() => sensorDrive.approachAruco(); + + + @override + Future faceDirection(DriveOrientation orientation) async { + if (useImu) { + await sensorDrive.faceDirection(orientation); + } else { + await timedDrive.faceDirection(orientation); + } + await super.faceDirection(orientation); + } + + @override + Future goForward() => useGps ? sensorDrive.goForward() : timedDrive.goForward(); + + @override + Future turnLeft() => useImu ? sensorDrive.turnLeft() : timedDrive.turnLeft(); + + @override + Future turnRight() => useImu ? sensorDrive.turnRight() : timedDrive.turnRight(); +} diff --git a/autonomy/lib/src/rover/drive/motors.dart b/autonomy/lib/src/rover/drive/motors.dart new file mode 100644 index 00000000..12ae541b --- /dev/null +++ b/autonomy/lib/src/rover/drive/motors.dart @@ -0,0 +1,26 @@ +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; + +mixin RoverMotors { + AutonomyInterface get collection; + + /// Sets the max speed of the rover. + /// + /// [setSpeeds] takes the speeds of each side of wheels. These numbers are percentages of the + /// max speed allowed by the rover, which we call the throttle. This function adjusts the + /// throttle, as a percentage of the rover's top speed. + void setThrottle(double throttle) { + collection.logger.trace("Setting throttle to $throttle"); + collection.server.sendCommand(DriveCommand(throttle: throttle, setThrottle: true)); + } + + /// Sets the speeds of the left and right wheels, using differential steering. + /// + /// These values are percentages of the max speed allowed by the rover. See [setThrottle]. + void setSpeeds({required double left, required double right}) { + right *= -1; + collection.logger.trace("Setting speeds to $left and $right"); + collection.server.sendCommand(DriveCommand(left: left, setLeft: true)); + collection.server.sendCommand(DriveCommand(right: right, setRight: true)); + } +} diff --git a/autonomy/lib/src/rover/drive/sensor.dart b/autonomy/lib/src/rover/drive/sensor.dart new file mode 100644 index 00000000..207e4fc5 --- /dev/null +++ b/autonomy/lib/src/rover/drive/sensor.dart @@ -0,0 +1,148 @@ +import "package:autonomy/autonomy.dart"; +import "package:autonomy/interfaces.dart"; + +import "motors.dart"; + +class SensorDrive extends DriveInterface with RoverMotors { + static const double maxThrottle = 0.1; + static const double turnThrottle = 0.1; + static const predicateDelay = Duration(milliseconds: 100); + static const turnDelay = Duration(milliseconds: 1500); + + SensorDrive({required super.collection}); + + @override + Future stop() async { + setThrottle(0); + setSpeeds(left: 0, right: 0); + } + + Future waitFor(bool Function() predicate) async { + while (!predicate()) { + await Future.delayed(predicateDelay); + await collection.imu.waitForValue(); + } + } + + @override + Future init() async => true; + + @override + Future dispose() async { } + + @override + Future goForward() async { + final orientation = collection.imu.orientation; + final currentCoordinates = collection.gps.coordinates; + final destination = currentCoordinates.goForward(orientation!); + + setThrottle(maxThrottle); + setSpeeds(left: 1, right: 1); + await waitFor(() => collection.gps.coordinates.isNear(destination)); + await stop(); + } + + @override + Future faceNorth() async { + collection.logger.info("Turning to face north..."); + setThrottle(turnThrottle); + setSpeeds(left: -1, right: 1); + await waitFor(() { + collection.logger.trace("Current heading: ${collection.imu.heading}"); + return collection.imu.raw.isNear(0); + }); + await stop(); + } + + @override + Future faceDirection(DriveOrientation orientation) async { + collection.logger.info("Turning to face $orientation..."); + setThrottle(turnThrottle); + setSpeeds(left: -1, right: 1); + await waitFor(() { + collection.logger.trace("Current heading: ${collection.imu.heading}"); + return collection.imu.raw.isNear(orientation.angle.toDouble()); + }); + await stop(); + await super.faceDirection(orientation); + } + + @override + Future turnLeft() async { + await collection.imu.waitForValue(); + + if (collection.imu.orientation == null) { + await faceNorth(); + await faceDirection(this.orientation); + } + await collection.imu.waitForValue(); + + final orientation = collection.imu.orientation; + final destination = orientation!.turnLeft(); // do NOT clamp! + setThrottle(maxThrottle); + setSpeeds(left: -1, right: 1); + await waitFor(() => collection.imu.orientation == destination); + await stop(); + this.orientation = this.orientation.turnLeft(); + } + + @override + Future turnRight() async { + await collection.imu.waitForValue(); + if (collection.imu.orientation == null) { + await faceNorth(); + await faceDirection(this.orientation); + } + await collection.imu.waitForValue(); + final orientation = collection.imu.orientation; + final destination = orientation!.turnRight(); // do NOT clamp! + setThrottle(maxThrottle); + setSpeeds(left: 1, right: -1); + await waitFor(() => collection.imu.orientation == destination); + await stop(); + this.orientation = this.orientation.turnRight(); + } + + @override + Future spinForAruco() async { + for (var i = 0; i < 16; i++) { + setThrottle(turnThrottle); + setSpeeds(left: -1, right: 1); + await Future.delayed(turnDelay); + await stop(); + + for (var j = 0; j < 300; j++) { + await Future.delayed(const Duration(milliseconds: 10)); + collection.logger.trace("Can see aruco? ${collection.detector.canSeeAruco()}"); + + if (collection.detector.canSeeAruco()) { + // Spin a bit more to center it + // print("We found it!"); + // setThrottle(0.1); + // setSpeeds(left: -1, right: 1); + // await waitFor(() { + // final pos = collection.video.arucoPosition; + // collection.logger.debug("aruco is at $pos"); + // return pos > 0.2; + // }); + // await stop(); + return true; + }} + } + return false; + } + + @override + Future approachAruco() async { + setThrottle(maxThrottle); + setSpeeds(left: 1, right: 1); + // const threshold = 0.2; + // await waitFor(() { + // final pos = collection.video.arucoSize; + // collection.logger.debug("It is at $pos percent"); + // return (pos.abs() < 0.00001 && !collection.detector.canSeeAruco()) || pos >= threshold; + // }); + await Future.delayed(const Duration(seconds: 10)); + await stop(); + } +} diff --git a/autonomy/lib/src/rover/drive/timed.dart b/autonomy/lib/src/rover/drive/timed.dart new file mode 100644 index 00000000..142a1ff4 --- /dev/null +++ b/autonomy/lib/src/rover/drive/timed.dart @@ -0,0 +1,51 @@ +import "package:autonomy/interfaces.dart"; + +import "motors.dart"; + +class TimedDrive extends DriveInterface with RoverMotors { + static const maxThrottle = 0.1; + static const turnThrottle = 0.1; + static const oneMeterDelay = Duration(milliseconds: 5500); + static const turnDelay = Duration(milliseconds: 4500); + + TimedDrive({required super.collection}); + + @override + Future init() async => true; + + @override + Future dispose() async { } + + @override + Future stop() async { + setThrottle(0); + setSpeeds(left: 0, right: 0); + } + + @override + Future faceNorth() async { /* Assume already facing north */ } + + @override + Future goForward() async { + setThrottle(maxThrottle); + setSpeeds(left: 1, right: 1); + await Future.delayed(oneMeterDelay); + await stop(); + } + + @override + Future turnLeft() async { + setThrottle(turnThrottle); + setSpeeds(left: -1, right: 1); + await Future.delayed(turnDelay); + await stop(); + } + + @override + Future turnRight() async { + setThrottle(turnThrottle); + setSpeeds(left: 1, right: -1); + await Future.delayed(turnDelay); + await stop(); + } +} diff --git a/autonomy/lib/src/rover/gps.dart b/autonomy/lib/src/rover/gps.dart new file mode 100644 index 00000000..2704b853 --- /dev/null +++ b/autonomy/lib/src/rover/gps.dart @@ -0,0 +1,41 @@ +import "package:burt_network/burt_network.dart"; +import "package:autonomy/interfaces.dart"; + +import "corrector.dart"; + +class RoverGps extends GpsInterface { + final _latitudeCorrector = ErrorCorrector(maxSamples: 1, maxDeviation: GpsInterface.gpsError * 10); + final _longitudeCorrector = ErrorCorrector(maxSamples: 1, maxDeviation: GpsInterface.gpsError * 10); + RoverGps({required super.collection}); + + @override + Future init() async { + collection.server.messages.onMessage( + name: RoverPosition().messageName, + constructor: RoverPosition.fromBuffer, + callback: (pos) { + if (pos.hasGps()) update(pos.gps); + }, + ); + return super.init(); + } + + @override + Future dispose() async { + _latitudeCorrector.clear(); + _longitudeCorrector.clear(); + } + + @override + void update(GpsCoordinates newValue) { + _latitudeCorrector.addValue(newValue.latitude); + _longitudeCorrector.addValue(newValue.longitude); + hasValue = true; + } + + @override + GpsCoordinates get coordinates => GpsCoordinates( + latitude: _latitudeCorrector.calibratedValue, + longitude: _longitudeCorrector.calibratedValue, + ); +} diff --git a/autonomy/lib/src/rover/imu.dart b/autonomy/lib/src/rover/imu.dart new file mode 100644 index 00000000..48f66666 --- /dev/null +++ b/autonomy/lib/src/rover/imu.dart @@ -0,0 +1,43 @@ +import "package:burt_network/burt_network.dart"; +import "package:autonomy/interfaces.dart"; + +import "corrector.dart"; + +class RoverImu extends ImuInterface { + final _zCorrector = ErrorCorrector(maxSamples: 10, maxDeviation: 15); + RoverImu({required super.collection}); + + Orientation value = Orientation(); + + @override + Future init() async { + collection.server.messages.onMessage( + name: RoverPosition().messageName, + constructor: RoverPosition.fromBuffer, + callback: (pos) { + if (pos.hasOrientation()) update(pos.orientation); + }, + ); + return super.init(); + } + + @override + Future dispose() async { + _zCorrector.clear(); + } + + @override + void update(Orientation newValue) { + // _zCorrector.addValue(newValue.heading); + // collection.logger.trace("Got IMU value"); + hasValue = true; + value = newValue; + } + + @override + Orientation get raw => Orientation( + x: 0, + y: 0, + z: value.z, + ).clampHeading(); +} diff --git a/autonomy/lib/src/rover/orchestrator.dart b/autonomy/lib/src/rover/orchestrator.dart new file mode 100644 index 00000000..2fd9a2f8 --- /dev/null +++ b/autonomy/lib/src/rover/orchestrator.dart @@ -0,0 +1,113 @@ +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; +import "dart:async"; + +class RoverOrchestrator extends OrchestratorInterface with ValueReporter { + final List traversed = []; + List? currentPath; + RoverOrchestrator({required super.collection}); + + @override + Future dispose() async { + currentPath = null; + currentCommand = null; + currentState = AutonomyState.AUTONOMY_STATE_UNDEFINED; + traversed.clear(); + await super.dispose(); + } + + @override + AutonomyData get statusMessage => AutonomyData( + destination: currentCommand?.destination, + state: currentState, + obstacles: collection.pathfinder.obstacles, + path: [ + for (final transition in currentPath ?? []) + transition.position, + ...traversed, + ], + task: currentCommand?.task, + crash: false, // TODO: Investigate if this is used and how to use it better + ); + + @override + Message getMessage() => statusMessage; + + @override + Future handleGpsTask(AutonomyCommand command) async { + final destination = command.destination; + collection.logger.info("Got GPS Task: Go to ${destination.prettyPrint()}"); + collection.logger.debug("Currently at ${collection.gps.coordinates.prettyPrint()}"); + collection.drive.setLedStrip(ProtoColor.RED); + while (!collection.gps.coordinates.isNear(destination)) { + // Calculate a path + collection.logger.debug("Finding a path"); + currentState = AutonomyState.PATHING; + await collection.drive.faceNorth(); + final path = collection.pathfinder.getPath(destination); + currentPath = path; // also use local variable path for promotion + if (path == null) { + final current = collection.gps.coordinates; + collection.logger.error("Could not find a path", body: "No path found from ${current.prettyPrint()} to ${destination.prettyPrint()}"); + currentState = AutonomyState.NO_SOLUTION; + currentCommand = null; + return; + } + // Try to take that path + final current = collection.gps.coordinates; + collection.logger.info("Found a path from ${current.prettyPrint()} to ${destination.prettyPrint()}: ${path.length} steps"); + collection.logger.debug("Here is a summary of the path"); + for (final step in path) { + collection.logger.debug(step.toString()); + } + currentState = AutonomyState.DRIVING; + var count = 0; + for (final state in path) { + collection.logger.debug(state.toString()); + await collection.drive.goDirection(state.direction); + traversed.add(state.position); + if (state.direction != DriveDirection.forward) continue; + if (count++ == 5) break; + final foundObstacle = collection.detector.findObstacles(); + if (foundObstacle) { + collection.logger.debug("Found an obstacle. Recalculating path..."); + break; // calculate a new path + } + } + } + collection.logger.info("Task complete"); + collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); + currentState = AutonomyState.AT_DESTINATION; + currentCommand = null; + } + + @override + Future handleArucoTask(AutonomyCommand command) async { + collection.drive.setLedStrip(ProtoColor.RED); + + // Go to GPS coordinates + // await handleGpsTask(command); + collection.logger.info("Got ArUco Task"); + + currentState = AutonomyState.SEARCHING; + collection.logger.info("Searching for ArUco tag"); + final didSeeAruco = await collection.drive.spinForAruco(); + if (didSeeAruco) { + collection.logger.info("Found aruco"); + currentState = AutonomyState.APPROACHING; + await collection.drive.approachAruco(); + collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); + currentState = AutonomyState.AT_DESTINATION; + } + } + + @override + Future handleHammerTask(AutonomyCommand command) async { + + } + + @override + Future handleBottleTask(AutonomyCommand command) async { + + } +} diff --git a/autonomy/lib/src/rover/pathfinding.dart b/autonomy/lib/src/rover/pathfinding.dart new file mode 100644 index 00000000..2eb06735 --- /dev/null +++ b/autonomy/lib/src/rover/pathfinding.dart @@ -0,0 +1,22 @@ +import "package:a_star/a_star.dart"; + +import "package:autonomy/interfaces.dart"; +import "package:burt_network/burt_network.dart"; +import "package:burt_network/protobuf.dart"; + +class RoverPathfinder extends PathfindingInterface { + RoverPathfinder({required super.collection}); + + @override + Future init() async => true; + + @override + List? getPath(GpsCoordinates destination, {bool verbose = false}) { + if (isObstacle(destination)) return null; + final state = AutonomyAStarState.start(collection: collection, goal: destination); + final result = aStar(state, verbose: verbose, limit: 50000); + if (result == null) return null; + final transitions = result.reconstructPath().toList(); + return transitions; + } +} diff --git a/autonomy/lib/src/rover/sensorless.dart b/autonomy/lib/src/rover/sensorless.dart new file mode 100644 index 00000000..2beab095 --- /dev/null +++ b/autonomy/lib/src/rover/sensorless.dart @@ -0,0 +1,64 @@ +import "package:autonomy/interfaces.dart"; +import "package:autonomy/rover.dart"; +import "package:autonomy/simulator.dart"; + +class SensorlessDrive extends DriveInterface { + final DriveInterface simulatedDrive; + final DriveInterface realDrive; + + SensorlessDrive({required super.collection, bool useGps = true, bool useImu = true}) : + simulatedDrive = DriveSimulator(collection: collection), + realDrive = RoverDrive(collection: collection, useGps: useGps, useImu: useImu); + + @override + Future init() async { + var result = true; + result &= await simulatedDrive.init(); + result &= await realDrive.init(); + return result; + } + + @override + Future dispose() async { + await simulatedDrive.dispose(); + await realDrive.dispose(); + } + + @override + Future goForward() async { + await simulatedDrive.goForward(); + await realDrive.goForward(); + } + + @override + Future stop() async { + await simulatedDrive.stop(); + await realDrive.stop(); + } + + @override + Future faceNorth() async { + await simulatedDrive.faceNorth(); + await realDrive.faceNorth(); + } + + + @override + Future faceDirection(DriveOrientation orientation) async { + await simulatedDrive.faceDirection(orientation); + await realDrive.faceDirection(orientation); + await super.faceDirection(orientation); + } + + @override + Future turnLeft() async { + await simulatedDrive.turnLeft(); + await realDrive.turnLeft(); + } + + @override + Future turnRight() async { + await simulatedDrive.turnRight(); + await realDrive.turnRight(); + } +} diff --git a/autonomy/lib/src/rover/video.dart b/autonomy/lib/src/rover/video.dart new file mode 100644 index 00000000..0f829acb --- /dev/null +++ b/autonomy/lib/src/rover/video.dart @@ -0,0 +1,32 @@ +import "dart:async"; + +import "package:autonomy/interfaces.dart"; +import "package:burt_network/burt_network.dart"; + +class RoverVideo extends VideoInterface { + RoverVideo({required super.collection}); + + @override + Future init() async { + collection.server.messages.onMessage( + name: VideoData().messageName, + constructor: VideoData.fromBuffer, + callback: updateFrame, + ); + return true; + } + + @override + Future dispose() async { } + + @override + void updateFrame(VideoData newData) { + data = newData; + // if (data.arucoDetected == BoolState.YES) { + // flag = true; + // Timer(const Duration(seconds: 3), () => flag = false); + // collection.logger.info("Is ArUco detected: ${data.arucoDetected}"); + // } + hasValue = true; + } +} diff --git a/autonomy/lib/src/simulator/detector.dart b/autonomy/lib/src/simulator/detector.dart new file mode 100644 index 00000000..82349221 --- /dev/null +++ b/autonomy/lib/src/simulator/detector.dart @@ -0,0 +1,46 @@ +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; + +class SimulatedObstacle { + final GpsCoordinates coordinates; + final int radius; + SimulatedObstacle({required this.coordinates, required this.radius}); + + bool isNear(GpsCoordinates other) => + coordinates.distanceTo(other) <= radius; +} + +class DetectorSimulator extends DetectorInterface { + static const arucoPosition = (10, 10); + static const slopedLatitude = -5; + + final List obstacles; + final Set found = {}; + + DetectorSimulator({required super.collection, required this.obstacles}); + + @override + Future init() async => true; + + @override + Future dispose() async => obstacles.clear(); + + @override + bool findObstacles() { + final coordinates = collection.gps.coordinates; + var result = false; + for (final obstacle in obstacles) { + if (!obstacle.isNear(coordinates) || found.contains(obstacle.coordinates)) continue; + result = true; + found.add(obstacle.coordinates); + collection.pathfinder.recordObstacle(obstacle.coordinates); + } + return result; + } + + @override + bool canSeeAruco() => false; // if can see [arucoPosition] + + @override + bool isOnSlope() => false; // if on [slopedLatitude] +} diff --git a/autonomy/lib/src/simulator/drive.dart b/autonomy/lib/src/simulator/drive.dart new file mode 100644 index 00000000..16eecaae --- /dev/null +++ b/autonomy/lib/src/simulator/drive.dart @@ -0,0 +1,54 @@ +// ignore_for_file: cascade_invocations + +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; + +class DriveSimulator extends DriveInterface { + final bool shouldDelay; + DriveSimulator({required super.collection, this.shouldDelay = false}); + + @override + Future init() async => true; + + @override + Future dispose() async { } + + @override + Future goForward() async { + final position = collection.gps.coordinates; + final orientation = collection.imu.orientation; + final newPosition = position.goForward(orientation!); + collection.logger.debug("Going forward"); + collection.logger.trace(" Old position: ${position.prettyPrint()}"); + collection.logger.trace(" Orientation: $orientation"); + collection.logger.trace(" New position: ${newPosition.prettyPrint()}"); + collection.gps.update(newPosition); + if (shouldDelay) await Future.delayed(const Duration(milliseconds: 500)); + } + + @override + Future faceNorth() async { + collection.imu.update(OrientationUtils.north); + } + + @override + Future turnLeft() async { + collection.logger.debug("Turning left"); + final heading = collection.imu.heading; + final orientation = Orientation(z: heading + 90); + collection.imu.update(orientation); + if (shouldDelay) await Future.delayed(const Duration(milliseconds: 500)); + } + + @override + Future turnRight() async { + collection.logger.debug("Turning right"); + final heading = collection.imu.heading; + final orientation = Orientation(z: heading - 90); + collection.imu.update(orientation); + if (shouldDelay) await Future.delayed(const Duration(milliseconds: 500)); + } + + @override + Future stop() async => collection.logger.debug("Stopping"); +} diff --git a/autonomy/lib/src/simulator/gps.dart b/autonomy/lib/src/simulator/gps.dart new file mode 100644 index 00000000..0062db25 --- /dev/null +++ b/autonomy/lib/src/simulator/gps.dart @@ -0,0 +1,28 @@ +import "package:burt_network/protobuf.dart"; +import "package:autonomy/interfaces.dart"; + +class GpsSimulator extends GpsInterface with ValueReporter { + final RandomError _error; + GpsSimulator({required super.collection, double maxError = 0}) : + _error = RandomError(maxError); + + @override + RoverPosition getMessage() => RoverPosition(gps: coordinates); + + GpsCoordinates _coordinates = GpsCoordinates(); + + @override + GpsCoordinates get coordinates => GpsCoordinates( + latitude: _coordinates.latitude + _error.value, + longitude: _coordinates.longitude + _error.value, + ); + + @override + void update(GpsCoordinates newValue) => _coordinates = newValue; + + @override + Future init() async { + hasValue = true; + return super.init(); + } +} diff --git a/autonomy/lib/src/simulator/imu.dart b/autonomy/lib/src/simulator/imu.dart new file mode 100644 index 00000000..3c4cbacc --- /dev/null +++ b/autonomy/lib/src/simulator/imu.dart @@ -0,0 +1,35 @@ +import "package:burt_network/protobuf.dart"; +import "package:autonomy/interfaces.dart"; + +class ImuSimulator extends ImuInterface with ValueReporter { + final RandomError _error; + ImuSimulator({required super.collection, double maxError = 0}) : + _error = RandomError(maxError); + + @override + RoverPosition getMessage() => RoverPosition(orientation: raw); + + Orientation _orientation = Orientation(); + + @override + Orientation get raw => Orientation( + x: _orientation.x + _error.value, + y: _orientation.y + _error.value, + z: _orientation.z + _error.value, + ); + + @override + void update(Orientation newValue) => _orientation = newValue.clampHeading(); + + @override + Future init() async { + hasValue = true; + return super.init(); + } + + @override + Future dispose() async { + _orientation = Orientation(); + await super.dispose(); + } +} diff --git a/autonomy/lib/src/simulator/orchestrator.dart b/autonomy/lib/src/simulator/orchestrator.dart new file mode 100644 index 00000000..56420d6b --- /dev/null +++ b/autonomy/lib/src/simulator/orchestrator.dart @@ -0,0 +1,32 @@ +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; + +class OrchestratorSimulator extends OrchestratorInterface { + OrchestratorSimulator({required super.collection}); + + @override + Future dispose() async { } + + @override + AutonomyData get statusMessage => AutonomyData(); + + @override + Future handleGpsTask(AutonomyCommand command) async { + + } + + @override + Future handleArucoTask(AutonomyCommand command) async { + + } + + @override + Future handleHammerTask(AutonomyCommand command) async { + + } + + @override + Future handleBottleTask(AutonomyCommand command) async { + + } +} diff --git a/autonomy/lib/src/simulator/pathfinding.dart b/autonomy/lib/src/simulator/pathfinding.dart new file mode 100644 index 00000000..155f20e0 --- /dev/null +++ b/autonomy/lib/src/simulator/pathfinding.dart @@ -0,0 +1,25 @@ +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; + +class PathfindingSimulator extends PathfindingInterface { + static int i = 0; + + PathfindingSimulator({required super.collection}); + + @override + Future init() async => true; + + @override + List getPath(GpsCoordinates destination, {bool verbose = false}) => [ + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 0).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.right, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 0).toGps(), orientation: DriveOrientation.east, direction: DriveDirection.forward, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 1).toGps(), orientation: DriveOrientation.east, direction: DriveDirection.forward, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 2).toGps(), orientation: DriveOrientation.east, direction: DriveDirection.left, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 2).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.forward, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (1, 2).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.forward, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (2, 2).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.left, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (2, 2).toGps(), orientation: DriveOrientation.west, direction: DriveDirection.forward, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (2, 1).toGps(), orientation: DriveOrientation.west, direction: DriveDirection.right, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (2, 1).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.forward, collection: collection), + ]; +} diff --git a/autonomy/lib/src/simulator/realsense.dart b/autonomy/lib/src/simulator/realsense.dart new file mode 100644 index 00000000..f4251028 --- /dev/null +++ b/autonomy/lib/src/simulator/realsense.dart @@ -0,0 +1,22 @@ +import "dart:typed_data"; + +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; + +class VideoSimulator extends VideoInterface { + VideoSimulator({required super.collection}); + + @override + Future init() async { + hasValue = true; + return true; + } + + @override + Future dispose() async => depthFrame = Uint16List.fromList([]); + + Uint16List depthFrame = Uint16List.fromList([]); + + @override + void updateFrame(VideoData newData) { } +} diff --git a/autonomy/test/sensor_test.dart b/autonomy/test/sensor_test.dart new file mode 100644 index 00000000..6375b9eb --- /dev/null +++ b/autonomy/test/sensor_test.dart @@ -0,0 +1,246 @@ +import "package:autonomy/src/rover/imu.dart"; +import "package:burt_network/logging.dart"; +import "package:test/test.dart"; + +import "package:burt_network/protobuf.dart"; + +import "package:autonomy/interfaces.dart"; +import "package:autonomy/simulator.dart"; +import "package:autonomy/src/rover/gps.dart"; + +const imuError = 2.5; +const gpsPrecision = 7; + +void main() => group("[Sensors]", tags: ["sensors"], () { + setUp(() => Logger.level = LogLevel.off); + tearDown(() => Logger.level = LogLevel.off); + + test("GpsUtils.isNear", () { + final origin = (0, 0).toGps(); + expect(GpsCoordinates(latitude: 0, longitude: 0), origin); + expect(origin.isNear(origin), isTrue); + + final a = (5, 5).toGps(); + final a2 = (5, 5).toGps(); + final b = (5, 6.5).toGps(); + + expect(a.isNear(a), isTrue); + expect(a.isNear(a2), isTrue); + expect(a.isNear(b), isFalse); + + final c = GpsCoordinates(latitude: 0, longitude: 0); + final d = GpsCoordinates(latitude: 45.123, longitude: 72.123); + final e = GpsCoordinates(latitude: 12.123, longitude: 24.123); + + expect(c.isNear(d), isFalse); + expect(c.isNear(e), isFalse); + expect(d.isNear(e), isFalse); + + final f = (12, 12).toGps(); + final g = (12.2, 12.2).toGps(); + expect(f.isNear(g), isTrue); + }); + + test("GPS noise when stationary", () async { + // Set up a simulated and real GPS, both starting at (0, 0) + final oldError = GpsUtils.maxErrorMeters; + GpsUtils.maxErrorMeters = 3; + Logger.level = LogLevel.off; + + final simulator = AutonomySimulator(); + final realGps = RoverGps(collection: simulator); + final simulatedGps = GpsSimulator(collection: simulator, maxError: GpsInterface.gpsError); + final origin = GpsCoordinates(); + simulatedGps.update(origin); + realGps.update(origin); + expect(realGps.isNear(origin), isTrue); + + // Feed many noisy signals to produce a cleaner signal. + var count = 0; + for (var step = 0; step < 1000; step++) { + realGps.update(simulatedGps.coordinates); + simulator.logger.trace("New coordinate: ${realGps.coordinates.latitude.toStringAsFixed(gpsPrecision)} vs real position: ${origin.latitude.toStringAsFixed(gpsPrecision)}"); + if (step < 10) continue; + if (realGps.isNear(origin)) count++; + } + + final percentage = count / 1000; + simulator.logger.info("Final coordinates: ${realGps.coordinates.latitude}"); + expect(percentage, greaterThan(0.75), reason: "GPS should be accurate >75% of the time: $percentage"); + + // Ensure that *very* noisy readings don't affect anything. + simulator.logger.debug("Adding 100, 100"); + simulator.gps.update(GpsCoordinates(latitude: 100, longitude: 100)); + expect(realGps.isNear(origin), isTrue); + + await simulator.dispose(); + GpsUtils.maxErrorMeters = oldError; + }); + + test("IMU noise when stationary", () async { + Logger.level = LogLevel.off; + final simulator = AutonomySimulator(); + final simulatedImu = ImuSimulator(collection: simulator, maxError: imuError); + final realImu = RoverImu(collection: simulator); + final north = OrientationUtils.north; + simulatedImu.update(north); + + var count = 0; + for (var i = 0; i < 1000; i++) { + final newData = simulatedImu.raw; + realImu.update(newData); + simulator.logger.trace("Got new value: ${newData.heading}"); + simulator.logger.trace(" New heading: ${realImu.heading}"); + simulator.logger.trace(" Real position: ${north.heading}"); + if (i < 10) continue; + simulator.logger.trace(" Values are similar: ${realImu.isNear(north.heading)}"); + if (realImu.isNear(north.heading)) count++; + } + + final percentage = count / 1000; + simulator.logger.info("Final orientation: ${realImu.heading}"); + expect(percentage, greaterThan(0.75), reason: "IMU should be accurate >75% of the time: $percentage"); + + + realImu.update(OrientationUtils.south); + expect(realImu.isNear(OrientationUtils.north.heading), isTrue); + await simulator.dispose(); + }); + + test("GPS noise when moving", () async { + // Set up a simulated and real GPS, both starting at (0, 0) + final oldError = GpsUtils.maxErrorMeters; + GpsUtils.maxErrorMeters = 3; + final simulator = AutonomySimulator(); + final realGps = RoverGps(collection: simulator); + final simulatedGps = GpsSimulator(collection: simulator, maxError: GpsInterface.gpsError); + var realCoordinates = GpsCoordinates(); + simulatedGps.update(realCoordinates); + realGps.update(realCoordinates); + expect(realGps.coordinates.isNear(realCoordinates), isTrue); + + // For each step forward, use the noisy GPS to update the real GPS. + var count = 0; + for (var step = 0; step < 1000; step++) { + realCoordinates += GpsUtils.north; + simulatedGps.update(realCoordinates); + realGps.update(simulatedGps.coordinates); + simulator.logger.trace("New coordinate: ${realGps.coordinates.latitude.toStringAsFixed(5)} vs real position: ${realCoordinates.latitude.toStringAsFixed(5)}"); + simulator.logger.trace(" Difference: ${(realGps.latitude - realCoordinates.latitude).abs().toStringAsFixed(5)} < ${GpsUtils.epsilonLatitude.toStringAsFixed(5)}"); + if (step < 10) continue; + if (realGps.isNear(realCoordinates)) count++; + } + + final percentage = count / 1000; + simulator.logger.info("Final coordinates: ${realGps.coordinates.prettyPrint()} vs actual: ${realCoordinates.prettyPrint()}"); + expect(percentage, greaterThan(0.75), reason: "GPS should be accurate >75% of the time: $percentage"); + + // Ensure that *very* noisy readings don't affect anything. + simulator.logger.debug("Adding 100, 100"); + simulator.gps.update(GpsCoordinates(latitude: 100, longitude: 100)); + expect(realGps.isNear(realCoordinates), isTrue); + GpsUtils.maxErrorMeters = oldError; + }); + + test("IMU noise when moving", () async { + Logger.level = LogLevel.off; + final simulator = AutonomySimulator(); + final simulatedImu = ImuSimulator(collection: simulator, maxError: imuError); + final realImu = RoverImu(collection: simulator); + final orientation = OrientationUtils.north; + simulatedImu.update(orientation); + + var count = 0; + for (var i = 0; i < 350; i++) { + orientation.z += 1; + simulatedImu.update(orientation); + final newData = simulatedImu.raw; + realImu.update(newData); + simulator.logger.trace("Got new value: ${newData.heading}"); + simulator.logger.trace(" New heading: ${realImu.heading}"); + simulator.logger.trace(" Real position: ${orientation.heading}"); + if (i < 10) continue; + simulator.logger.trace(" Values are similar: ${realImu.isNear(orientation.heading)}"); + if (realImu.isNear(orientation.heading)) count++; + } + + final percentage = count / 350; + expect(percentage, greaterThan(0.75), reason: "IMU should be accurate >75% of the time: $percentage"); + + final badData = Orientation(z: 10); + simulator.logger.info("Final orientation: ${realImu.heading}"); + simulator.logger.info("Bad orientation: ${badData.heading}"); + realImu.update(badData); + simulator.logger.info("Unaffected orientation: ${realImu.heading}"); + expect(realImu.isNear(orientation.heading), isTrue); + await simulator.dispose(); + }); + + test("GPS latitude is set properly", () async { + final simulator = AutonomySimulator(); + const utahLatitude = 38.406683; + final utah = GpsCoordinates(latitude: utahLatitude); + + simulator.gps.update(utah); + expect(simulator.hasValue, isFalse); + expect(GpsInterface.currentLatitude, 0); + + await simulator.init(); + await simulator.waitForValue(); + expect(simulator.hasValue, isTrue); + expect(GpsInterface.currentLatitude, utahLatitude); + + await simulator.dispose(); + GpsInterface.currentLatitude = 0; + }); + + test("GPS can start in a real location", () async { + final simulator = AutonomySimulator(); + final ocean = GpsCoordinates(latitude: 0, longitude: 0); + final newYork = GpsCoordinates(latitude: 45, longitude: 72); + simulator.gps = RoverGps(collection: simulator); + + await simulator.init(); + expect(simulator.gps.coordinates, ocean); + expect(simulator.gps.isNear(newYork), isFalse); + expect(ocean.isNear(newYork), isFalse); + + simulator.gps.update(newYork); + expect(simulator.gps.isNear(ocean), isFalse); + expect(simulator.gps.isNear(newYork), isTrue); + + await simulator.dispose(); + }); + + test("IMU can handle values on the edge", () async { + Logger.level = LogLevel.off; + final simulator = AutonomySimulator(); + final simulatedImu = ImuSimulator(collection: simulator, maxError: imuError); + final realImu = RoverImu(collection: simulator); + final orientation = Orientation(z: 360); + simulatedImu.update(orientation); + + var count = 0; + for (var i = 0; i < 1000; i++) { + final newData = simulatedImu.raw; + realImu.update(newData); + simulator.logger.trace("Got new value: ${newData.heading}"); + simulator.logger.trace(" New heading: ${realImu.heading}"); + simulator.logger.trace(" Real position: ${orientation.heading}"); + if (i < 10) continue; + simulator.logger.trace(" Values are similar: ${realImu.isNear(orientation.heading)}"); + if (realImu.isNear(orientation.heading)) count++; + } + + final percentage = count / 1000; + expect(percentage, greaterThan(0.75), reason: "IMU should be accurate >75% of the time: $percentage"); + + final badData = Orientation(z: 10); + simulator.logger.info("Final orientation: ${realImu.heading}"); + simulator.logger.info("Bad orientation: ${badData.heading}"); + realImu.update(badData); + simulator.logger.info("Unaffected orientation: ${realImu.heading}"); + expect(realImu.isNear(orientation.heading), isTrue); + await simulator.dispose(); + }); +}); diff --git a/bin/rover.dart b/bin/rover.dart index 608000da..120be73e 100644 --- a/bin/rover.dart +++ b/bin/rover.dart @@ -59,6 +59,11 @@ Future compileAllPrograms(String? only) async { ? programs.where((p) => p.name == only).toList() : programs; + // Install all apt dependencies first to avoid package manager conflicts + if (!offline) { + await installAllAptDependencies(programsToCompile); + } + logger.info("Starting parallel compilation of ${programsToCompile.length} programs..."); await Future.wait(programsToCompile.map((program) async { @@ -66,6 +71,28 @@ Future compileAllPrograms(String? only) async { })); } +Future installAllAptDependencies(List programsToCompile) async { + final aptCommands = []; + + for (final program in programsToCompile) { + final extraCommands = program.extraCommands; + if (extraCommands != null) { + for (final command in extraCommands) { + if (command.requiresInternet && + (command.command == "sudo" && command.args.isNotEmpty && + (command.args[0] == "apt-get" || command.args[0] == "apt"))) { + aptCommands.add(command); + } + } + } + } + + for (final aptCommand in aptCommands) { + logger.info("Installing dependencies: ${aptCommand.task}..."); + await runCommand(aptCommand.command, aptCommand.args); + } +} + Future compileProgram(RoverProgram program) async { final name = program.name; logger.info("Processing the $name program"); @@ -77,7 +104,7 @@ Future compileProgram(RoverProgram program) async { await runCommand("sudo", ["systemctl", "disable", name], hideOutput: true); // always uses stderr } - // Run any pre-requisite commands + // Run any pre-requisite commands (except apt commands which were already handled) final extraCommands = program.extraCommands; if (extraCommands != null) { for (final extraCommand in extraCommands) { @@ -85,6 +112,15 @@ Future compileProgram(RoverProgram program) async { logger.debug("Skipping '${extraCommand.task}' because it requires internet"); continue; } + + // Skip apt commands since they were already executed serially + if (extraCommand.requiresInternet && + extraCommand.command == "sudo" && extraCommand.args.isNotEmpty && + (extraCommand.args[0] == "apt-get" || extraCommand.args[0] == "apt")) { + logger.debug("Skipping apt command '${extraCommand.task}' (already installed)"); + continue; + } + logger.info("- $name: ${extraCommand.task}..."); await runCommand(extraCommand.command, extraCommand.args, workingDirectory: program.sourceDir); }