diff --git a/.github/pubspec_overrides.yaml b/.github/pubspec_overrides.yaml index b7c9d12..e25d215 100644 --- a/.github/pubspec_overrides.yaml +++ b/.github/pubspec_overrides.yaml @@ -3,5 +3,5 @@ resolution: dependency_overrides: burt_network: git: - url: https://github.com/BinghamtonRover/Networking - ref: 2.3.1 \ No newline at end of file + url: https://github.com/BinghamtonRover/Rover-Code.git + path: burt_network \ No newline at end of file diff --git a/.github/workflows/analyze.yml b/.github/workflows/analyze.yml index b822d66..6761abc 100644 --- a/.github/workflows/analyze.yml +++ b/.github/workflows/analyze.yml @@ -10,14 +10,16 @@ jobs: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - name: Clone repo + uses: actions/checkout@v4 # 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 + - name: Setup dart + uses: dart-lang/setup-dart@v1 with: - sdk: 3.6.0 + sdk: 3.9.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 @@ -36,6 +38,5 @@ jobs: # 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 + - name: Run tests + run: dart test diff --git a/.github/workflows/documentation.yml b/.github/workflows/documentation.yml index 0267dfe..4d21499 100644 --- a/.github/workflows/documentation.yml +++ b/.github/workflows/documentation.yml @@ -24,26 +24,13 @@ jobs: - name: Install Dart uses: dart-lang/setup-dart@v1 with: - sdk: 3.5.2 + sdk: 3.9.0 - 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 + run: dartdoc --ignore 'unresolved-doc-reference,not-implemented,no-documentable-libraries,ambiguous-reexport' --exclude 'dart:async,dart:collection,dart:convert,dart:core,dart:developer,dart:io,dart:isolate,dart:math,dart:typed_data,dart:ui,dart:html,dart:js,dart:ffi,dart:js_util' --quiet --json --output docs --no-validate-links --no-verbose-warnings --no-allow-non-local-warnings - name: Commit and push files run: | diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..14029d2 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "cSpell.words": [ + "setpoint" + ] +} \ No newline at end of file diff --git a/analysis_options.yaml b/analysis_options.yaml index 76649d1..6a2b793 100644 --- a/analysis_options.yaml +++ b/analysis_options.yaml @@ -9,24 +9,22 @@ include: package:very_good_analysis/analysis_options.yaml # has more lints analyzer: language: - # Strict casts isn't helpful with null safety. It only notifies you on `dynamic`, - # which happens all the time in JSON. - # + # Strict casts isn't helpful with null safety. It only notifies you on `dynamic`, + # which happens all the time in JSON. + # # See https://github.com/dart-lang/language/blob/main/resources/type-system/strict-casts.md strict-casts: false # Don't let any types be inferred as `dynamic`. - # + # # See https://github.com/dart-lang/language/blob/main/resources/type-system/strict-inference.md strict-inference: true - # Don't let Dart infer the wrong type on the left side of an assignment. - # + # Don't let Dart infer the wrong type on the left side of an assignment. + # # See https://github.com/dart-lang/language/blob/main/resources/type-system/strict-raw-types.md strict-raw-types: true - exclude: - linter: rules: # Rules NOT in package:very_good_analysis @@ -44,7 +42,7 @@ linter: sort_constructors_first: false # final properties, then constructor avoid_dynamic_calls: false # this lint takes over errors in the IDE cascade_invocations: false # cascades are often less readable - + # Temporarily disabled lints public_member_api_docs: false # until we are ready to document flutter_style_todos: false # until we are ready to address them diff --git a/bin/arcuo.dart b/bin/arcuo.dart deleted file mode 100644 index ae9f7d8..0000000 --- a/bin/arcuo.dart +++ /dev/null @@ -1,16 +0,0 @@ -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/bin/aruco.dart b/bin/aruco.dart index 5f9fdcb..15ca03c 100644 --- a/bin/aruco.dart +++ b/bin/aruco.dart @@ -4,7 +4,7 @@ void main() async { final rover = RoverAutonomy(); await rover.init(); await rover.waitForValue(); - final didSeeAruco = await rover.drive.spinForAruco(); + final didSeeAruco = await rover.drive.spinForAruco(0); if (didSeeAruco) { rover.logger.info("Found aruco"); await rover.drive.approachAruco(); diff --git a/bin/autonomy.dart b/bin/autonomy.dart index 31bd0b6..5bf6d3d 100644 --- a/bin/autonomy.dart +++ b/bin/autonomy.dart @@ -1,6 +1,8 @@ import "package:autonomy/rover.dart"; +import "package:burt_network/logging.dart"; void main() async { + Logger.level = Level.debug; final rover = RoverAutonomy(); await rover.init(); } diff --git a/bin/latlong.dart b/bin/latlong.dart index eb5ce07..3606ebc 100644 --- a/bin/latlong.dart +++ b/bin/latlong.dart @@ -1,22 +1,34 @@ // ignore_for_file: avoid_print +import "package:autonomy/constants.dart"; import "package:autonomy/interfaces.dart"; const binghamtonLatitude = 42.0877327; const utahLatitude = 38.406683; void printInfo(String name, double latitude) { - GpsInterface.currentLatitude = latitude; + final metersPerLongitude = GpsToMeters.metersPerLongitude(latitude); print("At $name:"); - print(" There are ${GpsUtils.metersPerLongitude.toStringAsFixed(2)} meters per 1 degree of longitude"); - print(" Our max error in longitude would be ${GpsUtils.epsilonLongitude} degrees"); - final isWithinRange = GpsInterface.gpsError <= GpsUtils.epsilonLongitude; - print(" Our GPS has ${GpsInterface.gpsError} degrees of error, so this would ${isWithinRange ? 'work' : 'not work'}"); + print( + " There are ${metersPerLongitude.toStringAsFixed(2)} meters per 1 degree of longitude", + ); + print( + " Our max error in longitude would be ${(Constants.maxErrorMeters / metersPerLongitude).toStringAsFixed(20)} degrees", + ); + final isWithinRange = + Constants.gpsError <= Constants.maxErrorMeters / metersPerLongitude; + print( + " Our GPS has ${Constants.gpsError} degrees of error, so this would ${isWithinRange ? 'work' : 'not work'}", + ); } void main() { - print("There are always ${GpsUtils.metersPerLatitude} meters in 1 degree of latitude"); - print(" So our max error in latitude is always ${GpsUtils.epsilonLatitude} degrees"); + print( + "There are always ${GpsToMeters.metersPerLatitude} meters in 1 degree of latitude", + ); + print( + " So our max error in latitude is always ${(Constants.maxErrorMeters / GpsToMeters.metersPerLatitude).toStringAsFixed(20)} degrees", + ); printInfo("the equator", 0); printInfo("Binghamton", binghamtonLatitude); printInfo("Utah", utahLatitude); diff --git a/bin/path.dart b/bin/path.dart index cf0b5be..4fd1bb0 100644 --- a/bin/path.dart +++ b/bin/path.dart @@ -1,15 +1,22 @@ import "package:autonomy/interfaces.dart"; import "package:autonomy/rover.dart"; import "package:autonomy/simulator.dart"; +import "package:coordinate_converter/coordinate_converter.dart"; void main() { - GpsUtils.maxErrorMeters = 1; - final destination = (1000, 1000).toGps(); + final destination = UTMCoordinates( + x: 5 + 1000, + y: 5 + 1000, + zoneNumber: 31, + ).toGps(); final simulator = AutonomySimulator(); simulator.pathfinder = RoverPathfinder(collection: simulator); + simulator.gps.update(UTMCoordinates(x: 5, y: 5, zoneNumber: 31).toGps()); final path = simulator.pathfinder.getPath(destination); if (path == null) { - simulator.logger.critical("Could not find path to ${destination.prettyPrint()}"); + simulator.logger.critical( + "Could not find path to ${destination.prettyPrint()}", + ); return; } if (path.isEmpty) { @@ -18,9 +25,11 @@ void main() { } var turnCount = 0; for (final step in path) { - if (step.direction == DriveDirection.left || step.direction == DriveDirection.right) { + if (step.instruction.isTurn) { turnCount++; } } - simulator.logger.info("Got a path with ${path.length} steps and $turnCount turn(s)"); + simulator.logger.info( + "Got a path with ${path.length} steps and $turnCount turn(s)", + ); } diff --git a/bin/random.dart b/bin/random.dart index fe52f37..0df68e0 100644 --- a/bin/random.dart +++ b/bin/random.dart @@ -1,11 +1,13 @@ // ignore_for_file: avoid_print +import "package:autonomy/constants.dart"; import "package:autonomy/interfaces.dart"; -import "package:autonomy/src/rover/corrector.dart"; -const maxError = GpsInterface.gpsError; +const maxError = Constants.gpsError; const maxSamples = 10; -final epsilon = GpsUtils.epsilonLatitude; // we need to be accurate within 1 meter +const epsilon = + Constants.maxErrorMeters / + GpsToMeters.metersPerLatitude; // we need to be accurate within 1 meter const n = 1000; bool verbose = false; @@ -17,8 +19,10 @@ bool test() { corrector.addValue(value); if (verbose) { final calibrated = corrector.calibratedValue; - print("Current value: $value, Corrected value: $calibrated"); - print(" Difference: ${calibrated.toStringAsFixed(7)} < ${epsilon.toStringAsFixed(7)}"); + print("Current value: $value, Corrected value: $calibrated"); + print( + " Difference: ${calibrated.toStringAsFixed(7)} < ${epsilon.toStringAsFixed(7)}", + ); } } return corrector.calibratedValue.abs() <= epsilon; @@ -31,5 +35,5 @@ void main(List args) { if (test()) count++; } final percentage = (count / n * 100).toStringAsFixed(2); - print("Average performance: %$percentage"); + print("Average performance: $percentage%"); } diff --git a/bin/sensorless.dart b/bin/sensorless.dart deleted file mode 100644 index 0ddcd1f..0000000 --- a/bin/sensorless.dart +++ /dev/null @@ -1,63 +0,0 @@ -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/bin/simulator.dart b/bin/simulator.dart new file mode 100644 index 0000000..c0d6e12 --- /dev/null +++ b/bin/simulator.dart @@ -0,0 +1,23 @@ +import "package:autonomy/autonomy.dart"; +import "package:autonomy/src/drive/drive_config.dart"; +import "package:burt_network/burt_network.dart"; + +void main() async { + Logger.level = Level.debug; + final simulator = AutonomySimulator(); + + simulator.pathfinder = RoverPathfinder(collection: simulator); + simulator.orchestrator = RoverOrchestrator(collection: simulator); + simulator.drive = DriveSimulator( + collection: simulator, + method: SimulationMethod.intermediate, + config: roverConfig.copyWith( + oneMeterDelay: const Duration(milliseconds: 500), + turnDelay: const Duration(milliseconds: 750), + subsystemsAddress: "127.0.0.1", + ), + ); + simulator.detector = NetworkDetector(collection: simulator); + + await simulator.init(); +} diff --git a/bin/task.dart b/bin/task.dart index 1cac4c9..7d1c77a 100644 --- a/bin/task.dart +++ b/bin/task.dart @@ -1,27 +1,30 @@ -import "package:autonomy/interfaces.dart"; import "package:autonomy/simulator.dart"; import "package:autonomy/rover.dart"; -import "package:burt_network/logging.dart"; +import "package:burt_network/burt_network.dart"; -final chair = (2, 0).toGps(); +final chair = (lat: 2, long: 0).toGps(); final obstacles = [ - SimulatedObstacle(coordinates: (2, 0).toGps(), radius: 3), - SimulatedObstacle(coordinates: (6, -1).toGps(), radius: 3), - SimulatedObstacle(coordinates: (6, 1).toGps(), radius: 3), + SimulatedObstacle(coordinates: (lat: 6, long: -1).toGps(), radius: 3), + SimulatedObstacle(coordinates: (lat: 6, long: 1).toGps(), radius: 3), ]; // Enter in the Dashboard: Destination = (lat=7, long=0); void main() async { - Logger.level = LogLevel.debug; + Logger.level = LogLevel.trace; final simulator = RoverAutonomy(); - simulator.detector = DetectorSimulator(collection: simulator, obstacles: obstacles); + simulator.detector = NetworkDetector(collection: simulator); simulator.pathfinder = RoverPathfinder(collection: simulator); simulator.orchestrator = RoverOrchestrator(collection: simulator); - simulator.drive = RoverDrive(collection: simulator, useGps: false); + simulator.drive = RoverDrive( + collection: simulator, + useGps: false, + useImu: false, + ); simulator.gps = GpsSimulator(collection: simulator); -// simulator.drive = DriveSimulator(collection: simulator); + simulator.imu = ImuSimulator(collection: simulator); + simulator.video = VideoSimulator(collection: simulator); + await simulator.init(); await simulator.imu.waitForValue(); -// await simulator.drive.faceNorth(); await simulator.server.waitForConnection(); } diff --git a/bin/test.dart b/bin/test.dart deleted file mode 100644 index 12205b1..0000000 --- a/bin/test.dart +++ /dev/null @@ -1,23 +0,0 @@ -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/dart_test.yaml b/dart_test.yaml index 4bbaf05..4dae084 100644 --- a/dart_test.yaml +++ b/dart_test.yaml @@ -1,7 +1,8 @@ -tags: - path: - sensors: +tags: + corrector: + path: rover: simulator: orchestrator: network: + state_machine: diff --git a/lib/autonomy.dart b/lib/autonomy.dart index 2238505..de71f6b 100644 --- a/lib/autonomy.dart +++ b/lib/autonomy.dart @@ -1,3 +1,4 @@ +export "constants.dart"; export "interfaces.dart"; export "rover.dart"; export "simulator.dart"; diff --git a/lib/constants.dart b/lib/constants.dart new file mode 100644 index 0000000..b6e121f --- /dev/null +++ b/lib/constants.dart @@ -0,0 +1,53 @@ +import "package:autonomy/autonomy.dart"; + +/// Constant variables for Autonomy. +/// +/// These constants control the tolerances and behaviors of the +/// pathfinding and path following, such as move lengths, maximum +/// error distance allowed, timeouts, etc. +class Constants { + /// Private internal constructor for constants + Constants._(); + + /// The maximum error (in degrees) of our GPS + static const gpsError = 0.00003; + + /// The maximum error or "tolerance" for reaching the end goal + static const double maxErrorMeters = 1; + + /// The closest distance the pathfinding algorithm will allow + /// the rover to go near an obstacle + static const double obstacleAvoidanceRadius = 0.75; + + /// How close the rover should get to a drive coordinate before + /// continuing with the path + static const double intermediateStepTolerance = 0.25; + + /// The amount of meters to move per path step + static const double moveLengthMeters = 1; + + /// Replan the path if the rover's position is this far away from the path + static const double replanErrorMeters = 3; + + /// The IMU angle tolerance for a turn during autonomy + static const double turnEpsilon = 3; + + /// The IMU angle tolerance when turning to re-correct to the + /// proper orientation before driving forward + static const double driveRealignmentEpsilon = 5; + + /// The maximum time to spend waiting for the drive to reach a desired GPS coordinate. + /// + /// Only applies to individual "drive forward" steps, to prevent indefinite driving + /// if it never reaches within [maxErrorMeters] of its desired position. + static const Duration driveGPSTimeout = Duration( + seconds: 4, + milliseconds: 500, + ); + + /// The maximum time to spend searching for an aruco tag + static const Duration arucoSearchTimeout = Duration(seconds: 20); + + /// The camera that should be used to detect Aruco tags + static const CameraName arucoDetectionCamera = CameraName.ROVER_FRONT; +} diff --git a/lib/interfaces.dart b/lib/interfaces.dart index bd903f0..90b06b4 100644 --- a/lib/interfaces.dart +++ b/lib/interfaces.dart @@ -1,16 +1,12 @@ -export "src/interfaces/a_star.dart"; -export "src/interfaces/autonomy.dart"; -export "src/interfaces/detector.dart"; -export "src/interfaces/drive.dart"; -export "src/interfaces/error.dart"; -export "src/interfaces/gps.dart"; -export "src/interfaces/gps_utils.dart"; -export "src/interfaces/imu.dart"; -export "src/interfaces/imu_utils.dart"; -export "src/interfaces/pathfinding.dart"; -export "src/interfaces/server.dart"; -export "src/interfaces/video.dart"; -export "src/interfaces/receiver.dart"; -export "src/interfaces/reporter.dart"; -export "src/interfaces/service.dart"; -export "src/interfaces/orchestrator.dart"; +export "src/autonomy_interface.dart"; +export "src/detector/detector_interface.dart"; +export "src/drive/drive_interface.dart"; +export "src/gps/gps_interface.dart"; +export "src/imu/imu_interface.dart"; +export "src/imu/cardinal_direction.dart"; +export "src/pathfinding/pathfinding_interface.dart"; +export "src/video/video_interface.dart"; +export "src/orchestrator/orchestrator_interface.dart"; + +export "utils.dart"; +export "package:burt_network/protobuf.dart"; diff --git a/lib/rover.dart b/lib/rover.dart index 9f664d9..d212542 100644 --- a/lib/rover.dart +++ b/lib/rover.dart @@ -1,33 +1,44 @@ -export "src/rover/drive.dart"; -export "src/rover/pathfinding.dart"; -export "src/rover/orchestrator.dart"; -export "src/rover/sensorless.dart"; +export "src/drive/rover_drive.dart"; +export "src/pathfinding/rover_pathfinding.dart"; +export "src/orchestrator/rover_orchestrator.dart"; +export "src/imu/rover_imu.dart"; +export "src/gps/rover_gps.dart"; +export "src/detector/network_detector.dart"; +export "src/video/rover_video.dart"; +export "src/detector/rover_detector.dart"; import "package:autonomy/interfaces.dart"; +import "package:autonomy/rover.dart"; import "package:burt_network/burt_network.dart"; -import "src/rover/pathfinding.dart"; -import "src/rover/drive.dart"; -import "src/rover/gps.dart"; -import "src/rover/imu.dart"; -import "src/rover/orchestrator.dart"; -import "src/rover/video.dart"; -import "src/rover/detector.dart"; - /// A collection of all the different services used by the autonomy program. class RoverAutonomy extends AutonomyInterface { - /// A server to communicate with the dashboard and receive data from the subsystems. - // @override late final AutonomyServer server = AutonomyServer(collection: this); - @override late final server = RoverSocket(port: 8003, device: Device.AUTONOMY, collection: this); - /// A helper class to handle driving the rover. - @override late DriveInterface drive = RoverDrive(collection: this); - @override late GpsInterface gps = RoverGps(collection: this); - @override late ImuInterface imu = RoverImu(collection: this); - @override late final logger = BurtLogger(socket: server); - @override late PathfindingInterface pathfinder = RoverPathfinder(collection: this); - @override late DetectorInterface detector = RoverDetector(collection: this); - @override late VideoInterface video = RoverVideo(collection: this); - @override late OrchestratorInterface orchestrator = RoverOrchestrator(collection: this); + /// A server to communicate with the dashboard and receive data from the subsystems. + // @override late final AutonomyServer server = AutonomyServer(collection: this); + @override + late final server = RoverSocket( + port: 8003, + device: Device.AUTONOMY, + collection: this, + ); + + /// A helper class to handle driving the rover. + @override + late DriveInterface drive = RoverDrive(collection: this); + @override + late GpsInterface gps = RoverGps(collection: this); + @override + late ImuInterface imu = RoverImu(collection: this); + @override + late final logger = BurtLogger(socket: server); + @override + late PathfindingInterface pathfinder = RoverPathfinder(collection: this); + @override + late DetectorInterface detector = RoverDetector(collection: this); + @override + late VideoInterface video = RoverVideo(collection: this); + @override + late OrchestratorInterface orchestrator = RoverOrchestrator(collection: this); @override Future onDisconnect() async { diff --git a/lib/simulator.dart b/lib/simulator.dart index 908a8ae..5e10a26 100644 --- a/lib/simulator.dart +++ b/lib/simulator.dart @@ -1,36 +1,64 @@ -export "src/simulator/detector.dart"; -export "src/simulator/drive.dart"; -export "src/simulator/gps.dart"; -export "src/simulator/imu.dart"; -export "src/simulator/realsense.dart"; +export "src/detector/sim_detector.dart"; +export "src/detector/network_detector.dart"; +export "src/drive/sim_drive.dart"; +export "src/gps/sim_gps.dart"; +export "src/imu/sim_imu.dart"; +export "src/video/sim_video.dart"; +export "src/pathfinding/sim_pathfinding.dart"; +export "src/orchestrator/sim_orchestrator.dart"; import "package:autonomy/interfaces.dart"; -import "package:autonomy/src/simulator/orchestrator.dart"; +import "package:autonomy/simulator.dart"; import "package:burt_network/burt_network.dart"; -import "src/simulator/detector.dart"; -import "src/simulator/drive.dart"; -import "src/simulator/gps.dart"; -import "src/simulator/imu.dart"; -import "src/simulator/pathfinding.dart"; -import "src/simulator/realsense.dart"; - class AutonomySimulator extends AutonomyInterface { - @override late final logger = BurtLogger(socket: server); - @override late final server = RoverSocket(port: 8003, device: Device.AUTONOMY, collection: this); - @override late GpsInterface gps = GpsSimulator(collection: this); - @override late ImuInterface imu = ImuSimulator(collection: this); - @override late DriveInterface drive = DriveSimulator(collection: this); - @override late PathfindingInterface pathfinder = PathfindingSimulator(collection: this); - @override late DetectorInterface detector = DetectorSimulator(collection: this, obstacles: []); - @override late VideoInterface video = VideoSimulator(collection: this); - @override late OrchestratorInterface orchestrator = OrchestratorSimulator(collection: this); + @override + late final logger = BurtLogger(socket: server); + + @override + late final server = RoverSocket( + port: 8003, + device: Device.AUTONOMY, + collection: this, + ); + + @override + late GpsInterface gps = GpsSimulator(collection: this); + + @override + late ImuInterface imu = ImuSimulator(collection: this); + + @override + late DriveInterface drive = DriveSimulator(collection: this); + + @override + late PathfindingInterface pathfinder = PathfindingSimulator(collection: this); + + @override + late DetectorInterface detector = DetectorSimulator( + collection: this, + obstacles: [], + ); + + @override + late VideoInterface video = VideoSimulator(collection: this); + + @override + late OrchestratorInterface orchestrator = OrchestratorSimulator( + collection: this, + ); bool isInitialized = false; @override - Future init() { isInitialized = true; return super.init(); } + Future init() { + isInitialized = true; + return super.init(); + } @override - Future dispose() { isInitialized = false; return super.dispose(); } + Future dispose() { + isInitialized = false; + return super.dispose(); + } } diff --git a/lib/src/interfaces/autonomy.dart b/lib/src/autonomy_interface.dart similarity index 85% rename from lib/src/interfaces/autonomy.dart rename to lib/src/autonomy_interface.dart index ae69aba..dbafe47 100644 --- a/lib/src/interfaces/autonomy.dart +++ b/lib/src/autonomy_interface.dart @@ -52,15 +52,17 @@ abstract class AutonomyInterface extends Service with Receiver { await init(); } + List get _receivers => [gps, imu, video]; + @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"); + for (final receiver in _receivers) { + await receiver.waitForValue(); + } + logger.info("Received all necessary values"); } @override - bool get hasValue => gps.hasValue && imu.hasValue && video.hasValue; + bool get hasValue => _receivers.every((r) => r.hasValue); } diff --git a/lib/src/interfaces/detector.dart b/lib/src/detector/detector_interface.dart similarity index 91% rename from lib/src/interfaces/detector.dart rename to lib/src/detector/detector_interface.dart index c42a694..8d5909e 100644 --- a/lib/src/interfaces/detector.dart +++ b/lib/src/detector/detector_interface.dart @@ -5,6 +5,5 @@ abstract class DetectorInterface extends Service { DetectorInterface({required this.collection}); bool findObstacles(); - bool canSeeAruco(); bool isOnSlope(); } diff --git a/lib/src/detector/network_detector.dart b/lib/src/detector/network_detector.dart new file mode 100644 index 0000000..37e7796 --- /dev/null +++ b/lib/src/detector/network_detector.dart @@ -0,0 +1,44 @@ +import "package:autonomy/autonomy.dart"; + +class NetworkDetector extends DetectorInterface { + final List _newObstacles = []; + bool _hasNewObstacles = false; + + NetworkDetector({required super.collection}); + + @override + Future dispose() async {} + + @override + bool findObstacles() { + for (final obstacle in _newObstacles) { + collection.pathfinder.recordObstacle(obstacle); + } + final newObstacles = _hasNewObstacles; + _hasNewObstacles = false; + + _newObstacles.clear(); + + return newObstacles; + } + + @override + Future init() async { + collection.server.messages.onMessage( + name: AutonomyData().messageName, + constructor: AutonomyData.fromBuffer, + callback: _onDataReceived, + ); + return true; + } + + void _onDataReceived(AutonomyData data) { + if (data.obstacles.isNotEmpty) { + _newObstacles.addAll(data.obstacles); + _hasNewObstacles = true; + } + } + + @override + bool isOnSlope() => false; +} diff --git a/lib/src/detector/rover_detector.dart b/lib/src/detector/rover_detector.dart new file mode 100644 index 0000000..48dedd4 --- /dev/null +++ b/lib/src/detector/rover_detector.dart @@ -0,0 +1,130 @@ +import "dart:async"; +import "dart:math"; + +import "package:autonomy/interfaces.dart"; +import "package:coordinate_converter/coordinate_converter.dart"; + +class RoverDetector extends DetectorInterface { + StreamSubscription? _subscription; + + LidarPointCloud cloudCache = LidarPointCloud(); + + Set queuedObstacles = {}; + Set temporaryObstacles = {}; + + RoverDetector({required super.collection}); + + void _handleLidarCloud(LidarPointCloud cloud) { + if (cloud.cartesian.isNotEmpty) { + cloudCache.cartesian.clear(); + cloudCache.cartesian.addAll(cloud.cartesian); + } + if (cloud.polar.isNotEmpty) { + cloudCache.polar.clear(); + cloudCache.polar.addAll(cloud.polar); + } + + _queueObstacles(); + } + + void _queueObstacles() { + final cartesian = cloudCache.cartesian; + final polar = cloudCache.polar; + + if (cartesian.isEmpty || polar.isEmpty) { + return; + } + + queuedObstacles.clear(); + + for (final point in cartesian) { + final angle = atan2(point.y, point.x) * 180 / pi; + final magnitude = sqrt(pow(point.x, 2) + pow(point.y, 2)); + + if (angle >= pi / 2) { + continue; + } + + if (magnitude <= 0.1) { + continue; + } + + final matchingPolar = polar.where( + (e) => + (e.angle - angle.roundToDouble()).abs() <= 1 && + (e.angle - angle.roundToDouble()).abs() != 0, + ); + + // no polar coordinates are near the cartesian coordinate + if (matchingPolar.isEmpty) { + continue; + } + + // nearby polar coordinates do not match the cartesian distance, likely a false speck + if (!matchingPolar.any((e) => (e.distance - magnitude).abs() < 0.1)) { + continue; + } + + final imuAngleRad = collection.imu.heading * pi / 180 + pi / 2; + + final roverToPoint = UTMCoordinates( + x: point.x * cos(imuAngleRad) - point.y * sin(imuAngleRad), + y: point.y * cos(imuAngleRad) + point.x * sin(imuAngleRad), + zoneNumber: 1, + ); + + queuedObstacles.add( + (collection.gps.coordinates.toUTM() + roverToPoint).toGps(), + ); + } + + cloudCache.cartesian.clear(); + cloudCache.polar.clear(); + } + + @override + bool isOnSlope() => false; + + @override + bool findObstacles() { + if (queuedObstacles.isEmpty) return false; + + final roverUtm = collection.gps.coordinates.toUTM(); + + final toRemove = temporaryObstacles.where((coordinates) { + final delta = coordinates.toUTM() - roverUtm; + final roverToPoint = (atan2(delta.y, delta.x) - pi / 2) * 180 / pi; + final relativeAngle = (collection.imu.heading + roverToPoint) + .clampHalfAngle(); + + return relativeAngle > -135 && relativeAngle < 135; + }).toList(); + collection.pathfinder.obstacles.removeAll(toRemove); + temporaryObstacles.removeAll(toRemove); + + for (final obstacle in queuedObstacles) { + collection.pathfinder.recordObstacle(obstacle); + } + + temporaryObstacles.addAll(queuedObstacles); + + queuedObstacles.clear(); + + return true; + } + + @override + Future init() async { + _subscription = collection.server.messages.onMessage( + name: LidarPointCloud().messageName, + constructor: LidarPointCloud.fromBuffer, + callback: _handleLidarCloud, + ); + return true; + } + + @override + Future dispose() async { + await _subscription?.cancel(); + } +} diff --git a/lib/src/simulator/detector.dart b/lib/src/detector/sim_detector.dart similarity index 72% rename from lib/src/simulator/detector.dart rename to lib/src/detector/sim_detector.dart index 8234922..7cdf293 100644 --- a/lib/src/simulator/detector.dart +++ b/lib/src/detector/sim_detector.dart @@ -1,13 +1,11 @@ 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; + bool isNear(GpsCoordinates other) => coordinates.distanceTo(other) <= radius; } class DetectorSimulator extends DetectorInterface { @@ -30,7 +28,10 @@ class DetectorSimulator extends DetectorInterface { final coordinates = collection.gps.coordinates; var result = false; for (final obstacle in obstacles) { - if (!obstacle.isNear(coordinates) || found.contains(obstacle.coordinates)) continue; + if (!obstacle.isNear(coordinates) || + found.contains(obstacle.coordinates)) { + continue; + } result = true; found.add(obstacle.coordinates); collection.pathfinder.recordObstacle(obstacle.coordinates); @@ -39,8 +40,5 @@ class DetectorSimulator extends DetectorInterface { } @override - bool canSeeAruco() => false; // if can see [arucoPosition] - - @override - bool isOnSlope() => false; // if on [slopedLatitude] + bool isOnSlope() => false; // if on [slopedLatitude] } diff --git a/lib/src/drive/drive_commands.dart b/lib/src/drive/drive_commands.dart new file mode 100644 index 0000000..47cd4fa --- /dev/null +++ b/lib/src/drive/drive_commands.dart @@ -0,0 +1,49 @@ +import "package:autonomy/interfaces.dart"; + +/// Utility methods for a [DriveInterface] to send motor commands directly to the Subsystems program +mixin RoverDriveCommands on DriveInterface { + /// 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"); + sendCommand(DriveCommand(setThrottle: true, throttle: throttle)); + } + + /// 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"); + sendCommand( + DriveCommand(setLeft: true, setRight: true, left: left, right: right), + ); + } + + /// Stops the motors, setting the throttle and speeds to 0 + void stopMotors() { + setThrottle(0); + _setSpeeds(left: 0, right: 0); + } + + /// Sets the speeds of the wheels to spin the rover left + void spinLeft() => _setSpeeds(left: -1, right: 1); + + /// Sets the speeds of the wheels to spin the rover right + void spinRight() => _setSpeeds(left: 1, right: -1); + + /// Sets the speeds of the wheels to move the rover in a straight line + void moveForward() => _setSpeeds(left: 1, right: 1); + + /// 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); + sendCommand(command); + } +} diff --git a/lib/src/drive/drive_config.dart b/lib/src/drive/drive_config.dart new file mode 100644 index 0000000..2f37a72 --- /dev/null +++ b/lib/src/drive/drive_config.dart @@ -0,0 +1,74 @@ +import "dart:io"; + +import "package:burt_network/burt_network.dart"; + +/// Configuration for the Rover's drive behavior +/// +/// Each rover or device that is tested will have different behaviors +/// depending on the mechanical and electrical setup, ranging from subsystems +/// program address, to drive speeds, or the time it takes to move forward +/// and turn. +/// +/// This class is to make testing on different devices easier +class DriveConfig { + /// The throttle to set the drive to when moving forward + final double forwardThrottle; + + /// The throttle to set the drive to when turning + final double turnThrottle; + + /// The time it takes to turn 90 degrees + final Duration turnDelay; + + /// The time it takes to move one meter forward + final Duration oneMeterDelay; + + /// The IP address for the subsystems program + final String subsystemsAddress; + + /// Constructor for DriveConfig initializing all final fields + const DriveConfig({ + required this.forwardThrottle, + required this.turnThrottle, + required this.turnDelay, + required this.oneMeterDelay, + required this.subsystemsAddress, + }); + + /// Creates a copy of this drive config with the provided fields replaced + DriveConfig copyWith({ + double? forwardThrottle, + double? turnThrottle, + Duration? oneMeterDelay, + Duration? turnDelay, + String? subsystemsAddress, + }) => DriveConfig( + forwardThrottle: forwardThrottle ?? this.forwardThrottle, + turnThrottle: turnThrottle ?? this.turnThrottle, + turnDelay: turnDelay ?? this.turnDelay, + oneMeterDelay: oneMeterDelay ?? this.oneMeterDelay, + subsystemsAddress: subsystemsAddress ?? this.subsystemsAddress, + ); + + /// The [SocketInfo] for Subsystems, created using [subsystemsAddress] and port 8001 + SocketInfo get subsystems => + SocketInfo(address: InternetAddress(subsystemsAddress), port: 8001); +} + +/// The [DriveConfig] for the rover +const roverConfig = DriveConfig( + forwardThrottle: 0.2, + turnThrottle: 0.075, + oneMeterDelay: Duration(milliseconds: 2250 ~/ 2), + turnDelay: Duration(milliseconds: 4500), + subsystemsAddress: "192.168.1.20", +); + +/// The [DriveConfig] for the tank +const tankConfig = DriveConfig( + forwardThrottle: 0.3, + turnThrottle: 0.35, + turnDelay: Duration(milliseconds: 1000), + oneMeterDelay: Duration(milliseconds: 2000), + subsystemsAddress: "127.0.0.1", +); diff --git a/lib/src/drive/drive_interface.dart b/lib/src/drive/drive_interface.dart new file mode 100644 index 0000000..c87ca3b --- /dev/null +++ b/lib/src/drive/drive_interface.dart @@ -0,0 +1,142 @@ +import "package:autonomy/interfaces.dart"; + +import "drive_config.dart"; + +/// An enum for the drive direction or "instruction" for how the rover should drive +enum DriveDirection { + /// Move forward + forward, + + /// Turn 90 degrees left + left, + + /// Turn 90 degrees right + right, + + /// Turn 45 degrees left + quarterLeft, + + /// Turn 45 degrees right + quarterRight, + + /// Stop moving + stop; + + /// Whether or not this is a turn + bool get isTurn => this != forward && this != stop; + + /// Whether or not this is a quarter turn of 45 degrees + bool get isQuarterTurn => this == quarterLeft || this == quarterRight; +} + +/// An abstract class for driving. +/// +/// This allows for easy stubbing to simulate drive if certain sensors are not used. +abstract class DriveInterface extends Service { + /// The autonomy collection of the rover's sensors, pathfinders, loggers, and UDP sockets + AutonomyInterface collection; + + /// The drive configuration for the rover it is running on + DriveConfig config; + + /// Getter to access the state controller + StateController get controller => collection.orchestrator.controller; + + /// Constructor for Drive Interface + DriveInterface({required this.collection, this.config = roverConfig}); + + StateInterface driveStateState(AutonomyAStarState state) { + if (state.instruction == DriveDirection.stop) { + return FunctionalState( + controller, + onUpdate: (controller) => controller.popState(), + ); + } else if (state.instruction == DriveDirection.forward) { + return driveForwardState(state.position); + } else { + return turnStateState(state); + } + } + + /// State to turn the rover towards the nearest orientation + StateInterface resolveOrientationState() => + faceDirectionState(collection.imu.nearest); + + /// State to drive the rover forward towards [coordinates] + StateInterface driveForwardState(GpsCoordinates coordinates); + + /// State to turn the rover to face towards [direction] + StateInterface faceDirectionState(CardinalDirection direction) => + faceOrientationState(direction.orientation); + + /// State to face the rover towards [orientation] + StateInterface faceOrientationState(Orientation orientation); + + /// State to execute actions relating to the turning of an [AutonomyAStarState] + StateInterface turnStateState(AutonomyAStarState state) => + faceOrientationState(state.orientation.orientation); + + /// Stop the rover + Future stop(); + + /// Drive forward to [position], returns whether or not it successfully drove to the position + Future driveForward(GpsCoordinates position); + + /// Turn to face [orientation], returns whether or not it was able to turn + Future faceOrientation(Orientation orientation); + + /// Turn to face the orientation of [direction], returns whether or not it was able to turn + Future faceDirection(CardinalDirection direction) => + faceOrientation(direction.orientation); + + /// Utility method to send a command to subsystems + void sendCommand(Message message) => + collection.server.sendMessage(message, destination: config.subsystems); + + /// Spins the rover to the nearest IMU rotation + /// + /// This exists so the rover can generate a path based on a known + /// orientation that aligns to the possible orientations defined by [CardinalDirection] + Future resolveOrientation() => faceDirection(collection.imu.nearest); + + /// Turns to face the state's [AutonomyAStarState.orientation]. + /// + /// Exists so that the TimedDrive can implement this in terms of [AutonomyAStarState.instruction]. + /// + /// Returns whether or not the turn was successful + Future turnState(AutonomyAStarState state) => + faceDirection(state.orientation); + + /// Drives the rover based on the instruction and desired positions in [state] + /// + /// This determines based on the [state] whether it should move forward, turn, or stop + /// + /// Returns whether or not the drive was successful + Future driveState(AutonomyAStarState state) { + if (state.instruction == DriveDirection.stop) { + return stop(); + } else if (state.instruction == DriveDirection.forward) { + return driveForward(state.position); + } else { + return turnState(state); + } + } + + /// Utility method to send a command to change the color of the LED strip + /// + /// This is used to signal the state of the autonomous driving outside of the rover + void setLedStrip(ProtoColor color, {bool blink = false}) { + final command = DriveCommand( + color: color, + blink: blink ? BoolState.YES : BoolState.NO, + ); + sendCommand(command); + } + + /// Spin to face an Aruco tag, returns whether or not it was able to face the tag + Future spinForAruco(int arucoId, {CameraName? desiredCamera}) async => + false; + + /// Drive forward to approach an Aruco tag + Future approachAruco() async {} +} diff --git a/lib/src/drive/rover_drive.dart b/lib/src/drive/rover_drive.dart new file mode 100644 index 0000000..9106471 --- /dev/null +++ b/lib/src/drive/rover_drive.dart @@ -0,0 +1,145 @@ +import "package:autonomy/interfaces.dart"; +import "package:autonomy/rover.dart"; + +import "sensor_drive.dart"; +import "timed_drive.dart"; +import "sim_drive.dart"; + +/// A helper class to send drive commands to the rover with a simpler API. +class RoverDrive extends DriveInterface { + /// Whether or not it should use the GPS while driving forward + final bool useGps; + + /// Whether or not it should use the IMU while turning + final bool useImu; + + /// A [SensorDrive] used during movements that rely on sensors + late final sensorDrive = SensorDrive(collection: collection, config: config); + + /// A [TimedDrive] used during movements that do not use sensors + late final timedDrive = TimedDrive(collection: collection, config: config); + + /// A simulator used in conjunction with [timedDrive] to simulate the sensor + /// readings of the drive when driving without sensors + late final simDrive = DriveSimulator(collection: collection, config: config); + + /// Constructor for RoverDrive + /// + /// Takes in parameters for whether or not to use the GPS and imu + /// These will determine when to use [sensorDrive] or [timedDrive] + RoverDrive({ + required super.collection, + this.useGps = true, + this.useImu = true, + super.config, + }); + + /// Initializes the rover's drive subsystems. + @override + Future init() async { + if (!useImu && collection.imu is RoverImu) { + collection.logger.critical( + "Cannot use Rover IMU with simulated turning", + body: "Set useImu to true, or use the simulated IMU", + ); + return false; + } + if (!useGps && collection.imu is RoverGps) { + collection.logger.critical( + "Cannot use Rover GPS with simulated driving", + body: "Set useGps to true, or use the simulated GPS", + ); + return false; + } + + var result = true; + result &= await sensorDrive.init(); + result &= await timedDrive.init(); + result &= await simDrive.init(); + return result; + } + + /// Stops the rover from driving. + @override + Future dispose() async { + await sensorDrive.dispose(); + await timedDrive.dispose(); + await simDrive.dispose(); + } + + @override + Future stop() async { + var result = true; + result &= await sensorDrive.stop(); + result &= await timedDrive.stop(); + result &= await simDrive.stop(); + return result; + } + + @override + Future spinForAruco(int arucoId, {CameraName? desiredCamera}) => + sensorDrive.spinForAruco(arucoId, desiredCamera: desiredCamera); + + @override + Future approachAruco() => sensorDrive.approachAruco(); + + @override + Future faceOrientation(Orientation orientation) async { + if (useImu) { + return sensorDrive.faceOrientation(orientation); + } else { + return simDrive.faceOrientation(orientation); + } + } + + @override + Future driveForward(GpsCoordinates position) async { + if (useGps) { + return sensorDrive.driveForward(position); + } else { + var result = true; + result &= await timedDrive.driveForward(position); + result &= await simDrive.driveForward(position); + return result; + } + } + + @override + StateInterface driveForwardState(GpsCoordinates coordinates) { + if (useGps) { + return sensorDrive.driveForwardState(coordinates); + } else { + return SequenceState( + controller, + steps: [ + timedDrive.driveForwardState(coordinates), + simDrive.driveForwardState(coordinates), + ], + ); + } + } + + @override + StateInterface turnStateState(AutonomyAStarState state) { + if (useImu) { + return sensorDrive.turnStateState(state); + } else { + return SequenceState( + controller, + steps: [ + timedDrive.turnStateState(state), + simDrive.turnStateState(state), + ], + ); + } + } + + @override + StateInterface faceOrientationState(Orientation orientation) { + if (useImu) { + return sensorDrive.faceOrientationState(orientation); + } else { + return simDrive.faceOrientationState(orientation); + } + } +} diff --git a/lib/src/drive/sensor_drive.dart b/lib/src/drive/sensor_drive.dart new file mode 100644 index 0000000..3b5302f --- /dev/null +++ b/lib/src/drive/sensor_drive.dart @@ -0,0 +1,192 @@ +import "package:autonomy/autonomy.dart"; + +import "drive_commands.dart"; + +/// An implementation of [DriveInterface] that uses the rover's sensors to +/// determine its direction to move in and whether or not it has moved in its +/// desired direction/orientation +/// +/// When this is driving, it assumes that the rover is constantly getting new sensor +/// readings, if not, this will continue moving indefinitely +class SensorDrive extends DriveInterface with RoverDriveCommands { + /// The default period to check for a condition to become true + static const predicateDelay = Duration(milliseconds: 10); + + /// Default constructor for SensorDrive + SensorDrive({required super.collection, super.config}); + + @override + StateInterface driveForwardState(GpsCoordinates coordinates) => + TimeoutDecorator( + child: SensorForwardState( + controller, + collection: collection, + position: coordinates, + drive: this, + ), + onTimeout: (controller) { + if (collection.orchestrator is RoverOrchestrator) { + (collection.orchestrator as RoverOrchestrator).triggerPathReplan(); + } + controller.popState(); + }, + timeout: Constants.driveGPSTimeout, + ); + + @override + StateInterface faceOrientationState(Orientation orientation) => + SensorTurnState( + controller, + collection: collection, + orientation: orientation, + drive: this, + ); + + @override + Future stop() async { + stopMotors(); + return true; + } + + /// Will periodically check for a condition to become true. This can be + /// thought of as a "wait until", where the rover will periodically check + /// if it has reached its desired position or orientation. + /// + /// The [predicate] method is intended to manuever the rover until its condition + /// becomes true. To prevent infinite driving, this will return false if either + /// the command is canceled, or [stopNearObstacle] is true and the rover becomes + /// too close to an obstacle. + /// + /// Returns whether or not the feedback loop reached its desired state. + Future runFeedback( + bool Function() predicate, { + bool stopNearObstacle = false, + }) async { + while (true) { + if (collection.orchestrator.currentCommand == null) { + return false; + } + if (stopNearObstacle && + collection.pathfinder.isObstacle(collection.gps.coordinates)) { + return false; + } + + if (predicate()) { + return true; + } + + await Future.delayed(predicateDelay); + } + } + + @override + Future init() async => true; + + @override + Future dispose() async {} + + @override + Future driveForward(GpsCoordinates position) async { + collection.logger.info("Driving forward one meter"); + setThrottle(config.forwardThrottle); + var succeeded = true; + succeeded = + await runFeedback(() { + if (!succeeded) return true; + moveForward(); + return collection.gps.isNear( + position, + Constants.intermediateStepTolerance, + ); + // ignore: require_trailing_commas + }, stopNearObstacle: true).timeout( + Constants.driveGPSTimeout, + onTimeout: () { + collection.logger.warning( + "GPS Drive timed out", + body: + "Failed to reach ${position.prettyPrint()} after ${Constants.driveGPSTimeout}", + ); + return false; + }, + ); + await stop(); + return succeeded; + } + + @override + Future faceOrientation(Orientation orientation) async { + collection.logger.info("Turning to face $orientation..."); + setThrottle(config.turnThrottle); + final result = await runFeedback(() => _tryToFace(orientation)); + await stop(); + return result; + } + + bool _tryToFace(Orientation orientation) { + final current = collection.imu.heading; + final target = orientation.heading; + final error = (target - current).clampHalfAngle(); + if (error < 0) { + spinRight(); + } else { + spinLeft(); + } + // if (error.abs() < 180) { + // if (current < target) { + // spinRight(); + // } else { + // spinLeft(); + // } + // } else { + // if (current < target) { + // spinLeft(); + // } else { + // spinRight(); + // } + // } + collection.logger.trace("Current heading: $current"); + return collection.imu.isNear(orientation); + } + + @override + Future spinForAruco(int arucoId, {CameraName? desiredCamera}) async { + setThrottle(config.turnThrottle); + var foundAruco = true; + foundAruco = + await runFeedback(() { + if (!foundAruco) { + return true; + } + spinLeft(); + return collection.video.getArucoDetection( + arucoId, + desiredCamera: desiredCamera, + ) != + null; + }).timeout( + Constants.arucoSearchTimeout, + onTimeout: () { + foundAruco = false; + return false; + }, + ); + await stop(); + return foundAruco; + } + + @override + Future approachAruco() async { + // const sizeThreshold = 0.2; + // const epsilon = 0.00001; + // setThrottle(config.forwardThrottle); + // moveForward(); + // await waitFor(() { + // final size = collection.video.arucoSize; + // collection.logger.trace("The Aruco tag is at $size percent"); + // return true; + // return (size.abs() < epsilon && !collection.detector.canSeeAruco()) || size >= sizeThreshold; + // }).timeout(config.oneMeterDelay * 5); + await stop(); + } +} diff --git a/lib/src/drive/sim_drive.dart b/lib/src/drive/sim_drive.dart new file mode 100644 index 0000000..3626b49 --- /dev/null +++ b/lib/src/drive/sim_drive.dart @@ -0,0 +1,88 @@ +import "package:autonomy/interfaces.dart"; + +/// Enum to represent the method to use for drive simulation +enum SimulationMethod { + /// Will instantly simulate moving to a location with no delay + instant, + + /// Will wait a period of time before updating + constantDelay, + + /// Will continuously update with intermediate states during its + /// delay, as if the rover was actually moving + intermediate, +} + +/// An implementation of [DriveInterface] that will not move the rover, +/// and only update its sensor readings based on the desired values +/// +/// This assumes that the implementations for sensors are not expected to be updated from the rover, +/// otherwise, this can cause the rover to not follow its path properly +class DriveSimulator extends DriveInterface { + /// The amount of time to wait before updating the virtual sensor readings + static const delay = Duration(milliseconds: 500); + + /// The method to use for the drive simulation + final SimulationMethod method; + + /// Whether or not to wait before updating virtual sensor readings, + /// this can be useful when simulating the individual steps of a path + bool get shouldDelay => method != SimulationMethod.instant; + + /// Constructor for DriveSimulator, initializing the default fields, and whether or not it should delay + DriveSimulator({ + required super.collection, + this.method = SimulationMethod.instant, + super.config, + }); + + @override + StateInterface driveForwardState(GpsCoordinates coordinates) => + SimulationDriveForward( + controller, + collection: collection, + method: method, + destination: coordinates, + ); + + @override + StateInterface faceOrientationState(Orientation orientation) => + SimulationDriveTurn( + controller, + collection: collection, + method: method, + goalOrientation: orientation, + ); + + @override + Future init() async => true; + + @override + Future dispose() async {} + + @override + Future driveForward(GpsCoordinates position) async { + if (shouldDelay) await Future.delayed(delay); + collection.gps.update(position); + return true; + } + + @override + Future faceOrientation(Orientation orientation) async { + if (shouldDelay) { + await Future.delayed(const Duration(milliseconds: 500)); + } + collection.imu.update(orientation); + return true; + } + + @override + Future spinForAruco(int arucoId, {CameraName? desiredCamera}) async => + true; + + @override + Future stop() async { + collection.logger.debug("Stopping"); + return true; + } +} diff --git a/lib/src/drive/timed_drive.dart b/lib/src/drive/timed_drive.dart new file mode 100644 index 0000000..131ff8a --- /dev/null +++ b/lib/src/drive/timed_drive.dart @@ -0,0 +1,213 @@ +import "dart:math"; + +import "package:autonomy/interfaces.dart"; +import "package:autonomy/src/drive/drive_config.dart"; + +import "drive_commands.dart"; + +class _TimedOperationState extends RoverState { + DateTime _startTime = DateTime(0); + + final Duration time; + final void Function() operation; + final void Function()? onDone; + + _TimedOperationState( + super.controller, { + required this.time, + required this.operation, + this.onDone, + }); + + @override + void enter() { + _startTime = DateTime.now(); + } + + @override + void update() { + if (DateTime.now().difference(_startTime) >= time) { + controller.popState(); + return; + } + operation(); + } + + @override + void exit() => onDone?.call(); +} + +/// An implementation of [DriveInterface] that drives for a specified amount of time without using sensors +/// +/// The time to drive/turn for is defined by [DriveConfig.oneMeterDelay] and [DriveConfig.turnDelay] +/// +/// This should only be used if the rover is not using sensors for autonomous driving +class TimedDrive extends DriveInterface with RoverDriveCommands { + TimedDrive({required super.collection, super.config}); + + @override + StateInterface faceOrientationState(Orientation orientation) { + throw UnsupportedError( + "Cannot face any arbitrary direction using TimedDrive", + ); + } + + @override + StateInterface driveForwardState(GpsCoordinates coordinates) => + _TimedOperationState( + controller, + time: + config.oneMeterDelay * + (collection.imu.nearest.isPerpendicular ? 1 : sqrt2), + operation: () { + setThrottle(config.forwardThrottle); + moveForward(); + }, + ); + + @override + StateInterface turnStateState(AutonomyAStarState state) => + switch (state.instruction) { + DriveDirection.forward => throw UnimplementedError(), + + DriveDirection.left => _TimedOperationState( + controller, + time: config.turnDelay, + operation: () { + setThrottle(config.turnThrottle); + spinLeft(); + }, + onDone: stopMotors, + ), + + DriveDirection.right => _TimedOperationState( + controller, + time: config.turnDelay, + operation: () { + setThrottle(config.turnThrottle); + spinRight(); + }, + onDone: stopMotors, + ), + + DriveDirection.quarterLeft => _TimedOperationState( + controller, + time: config.turnDelay * 0.5, + operation: () { + setThrottle(config.turnThrottle); + spinLeft(); + }, + onDone: stopMotors, + ), + + DriveDirection.quarterRight => _TimedOperationState( + controller, + time: config.turnDelay * 0.5, + operation: () { + setThrottle(config.turnThrottle); + spinRight(); + }, + onDone: stopMotors, + ), + + DriveDirection.stop => throw UnimplementedError(), + }; + + @override + Future init() async => true; + + @override + Future dispose() async {} + + @override + Future driveForward(GpsCoordinates position) async { + await goForward(collection.imu.nearest.isPerpendicular ? 1 : sqrt2); + return true; + } + + @override + Future turnState(AutonomyAStarState state) async { + switch (state.instruction) { + case DriveDirection.left: + await turnLeft(); + case DriveDirection.right: + await turnRight(); + case DriveDirection.quarterLeft: + await turnQuarterLeft(); + case DriveDirection.quarterRight: + await turnQuarterRight(); + case DriveDirection.forward || DriveDirection.stop: + break; + } + return true; + } + + @override + Future stop() async { + stopMotors(); + return true; + } + + /// Moves forward for the amount of time it will take to drive the specified [distance] + /// + /// This will set the speeds to move forward, and wait for the amount of + /// time specified by the [DriveConfig.oneMeterDelay] + Future goForward([double distance = 1]) async { + collection.logger.info("Driving forward $distance meters"); + setThrottle(config.forwardThrottle); + moveForward(); + await Future.delayed(config.oneMeterDelay * distance); + await stop(); + return true; + } + + /// Turns left for the amount of time it will take to spin left 90 degrees + /// + /// This will set the speeds to turn left, and wait for the amount of + /// time specified by the [DriveConfig.turnDelay] + Future turnLeft() async { + setThrottle(config.turnThrottle); + spinLeft(); + await Future.delayed(config.turnDelay); + await stop(); + } + + /// Turns right for the amount of time it will take to spin right 90 degrees + /// + /// This will set the speeds to turn right, and wait for the amount of + /// time defined by the [DriveConfig.turnDelay] + Future turnRight() async { + setThrottle(config.turnThrottle); + spinRight(); + await Future.delayed(config.turnDelay); + await stop(); + } + + /// Turns left for the amount of time it will take to spin left 45 degrees + /// + /// This will set the speeds to turn left, and wait for the amount of + /// time defined by the [DriveConfig.turnDelay] / 2 + Future turnQuarterLeft() async { + setThrottle(config.turnThrottle); + spinLeft(); + await Future.delayed(config.turnDelay * 0.5); + await stop(); + } + + /// Turns right for the amount of time it will take to spin right 45 degrees + /// + /// This will set the speeds to turn right, and wait for the amount of + /// time defined by the [DriveConfig.turnDelay] / 2 + Future turnQuarterRight() async { + setThrottle(config.turnThrottle); + spinRight(); + await Future.delayed(config.turnDelay * 0.5); + await stop(); + } + + @override + Future faceOrientation(Orientation orientation) => + throw UnsupportedError( + "Cannot face any arbitrary direction using TimedDrive", + ); +} diff --git a/lib/src/interfaces/gps.dart b/lib/src/gps/gps_interface.dart similarity index 56% rename from lib/src/interfaces/gps.dart rename to lib/src/gps/gps_interface.dart index 8ccef30..7b4b772 100644 --- a/lib/src/interfaces/gps.dart +++ b/lib/src/gps/gps_interface.dart @@ -1,26 +1,23 @@ -import "package:burt_network/protobuf.dart"; import "package:autonomy/interfaces.dart"; +import "package:meta/meta.dart"; abstract class GpsInterface extends Service with Receiver { - static const gpsError = 0.00003; - static double currentLatitude = 0; - final AutonomyInterface collection; - GpsInterface({required this.collection}); + + GpsCoordinates get coordinates; double get longitude => coordinates.longitude; double get latitude => coordinates.latitude; + GpsInterface({required this.collection}); + void update(GpsCoordinates newValue); - GpsCoordinates get coordinates; - bool isNear(GpsCoordinates other) => coordinates.isNear(other); + @visibleForTesting + void forceUpdate(GpsCoordinates newValue) {} - @override - Future waitForValue() async { - await super.waitForValue(); - currentLatitude = coordinates.latitude; - } + bool isNear(GpsCoordinates other, [double? tolerance]) => + coordinates.isNear(other, tolerance); @override Future init() async => true; diff --git a/lib/src/gps/rover_gps.dart b/lib/src/gps/rover_gps.dart new file mode 100644 index 0000000..900062a --- /dev/null +++ b/lib/src/gps/rover_gps.dart @@ -0,0 +1,47 @@ +import "package:autonomy/interfaces.dart"; + +class RoverGps extends GpsInterface { + final _latitudeCorrector = ErrorCorrector.disabled(); + final _longitudeCorrector = ErrorCorrector.disabled(); + + RoverGps({required super.collection}); + + @override + Future init() async { + collection.server.messages.onMessage( + name: RoverPosition().messageName, + constructor: RoverPosition.fromBuffer, + callback: _internalUpdate, + ); + return super.init(); + } + + @override + Future dispose() async { + _latitudeCorrector.clear(); + _longitudeCorrector.clear(); + } + + @override + void update(GpsCoordinates newValue) { + // Do nothing, since this should only be internally updated + } + + @override + void forceUpdate(GpsCoordinates newValue) => + _internalUpdate(RoverPosition(gps: newValue)); + + void _internalUpdate(RoverPosition newValue) { + if (!newValue.hasGps()) return; + final position = newValue.gps; + _latitudeCorrector.addValue(position.latitude); + _longitudeCorrector.addValue(position.longitude); + hasValue = true; + } + + @override + GpsCoordinates get coordinates => GpsCoordinates( + latitude: _latitudeCorrector.calibratedValue, + longitude: _longitudeCorrector.calibratedValue, + ); +} diff --git a/lib/src/simulator/gps.dart b/lib/src/gps/sim_gps.dart similarity index 73% rename from lib/src/simulator/gps.dart rename to lib/src/gps/sim_gps.dart index 0062db2..11c37df 100644 --- a/lib/src/simulator/gps.dart +++ b/lib/src/gps/sim_gps.dart @@ -1,15 +1,14 @@ -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); + GpsSimulator({required super.collection, double maxError = 0}) + : _error = RandomError(maxError); @override RoverPosition getMessage() => RoverPosition(gps: coordinates); - GpsCoordinates _coordinates = GpsCoordinates(); + final GpsCoordinates _coordinates = GpsCoordinates(); @override GpsCoordinates get coordinates => GpsCoordinates( @@ -18,7 +17,8 @@ class GpsSimulator extends GpsInterface with ValueReporter { ); @override - void update(GpsCoordinates newValue) => _coordinates = newValue; + void update(GpsCoordinates newValue) => + _coordinates.mergeFromMessage(newValue); @override Future init() async { diff --git a/lib/src/imu/cardinal_direction.dart b/lib/src/imu/cardinal_direction.dart new file mode 100644 index 0000000..ce30dc1 --- /dev/null +++ b/lib/src/imu/cardinal_direction.dart @@ -0,0 +1,78 @@ +import "package:autonomy/interfaces.dart"; + +enum CardinalDirection { + north(0), + west(90), + south(180), + east(-90), + northEast(-45), + northWest(45), + southWest(180 - 45), + southEast(180 + 45); + + final double angle; + const CardinalDirection(this.angle); + + Orientation get orientation => Orientation(z: angle); + + static CardinalDirection nearest(Orientation orientation) { + var smallestDiff = double.infinity; + var closestOrientation = CardinalDirection.north; + + for (final value in values) { + final diff = (value.angle - orientation.z).clampHalfAngle(); + if (diff.abs() < smallestDiff) { + smallestDiff = diff.abs(); + closestOrientation = value; + } + } + + return closestOrientation; + } + + bool get isPerpendicular => angle.abs() % 90 == 0; + + CardinalDirection turnLeft() => switch (this) { + north => west, + west => south, + south => east, + east => north, + northEast => northWest, + northWest => southWest, + southWest => southEast, + southEast => northEast, + }; + + CardinalDirection turnRight() => switch (this) { + north => east, + west => north, + south => west, + east => south, + northEast => southEast, + southEast => southWest, + southWest => northWest, + northWest => northEast, + }; + + CardinalDirection turnQuarterLeft() => switch (this) { + north => northWest, + northWest => west, + west => southWest, + southWest => south, + south => southEast, + southEast => east, + east => northEast, + northEast => north, + }; + + CardinalDirection turnQuarterRight() => switch (this) { + north => northEast, + northEast => east, + east => southEast, + southEast => south, + south => southWest, + southWest => west, + west => northWest, + northWest => north, + }; +} diff --git a/lib/src/interfaces/imu.dart b/lib/src/imu/imu_interface.dart similarity index 53% rename from lib/src/interfaces/imu.dart rename to lib/src/imu/imu_interface.dart index 5bb6bba..15bb4a2 100644 --- a/lib/src/interfaces/imu.dart +++ b/lib/src/imu/imu_interface.dart @@ -1,5 +1,5 @@ import "package:autonomy/interfaces.dart"; -import "package:burt_network/burt_network.dart"; +import "package:meta/meta.dart"; abstract class ImuInterface extends Service with Receiver { final AutonomyInterface collection; @@ -7,12 +7,16 @@ abstract class ImuInterface extends Service with Receiver { double get heading => raw.z; Orientation get raw; - DriveOrientation? get orientation { - collection.logger.trace("Trying to find orientation at $heading"); - return DriveOrientation.fromRaw(raw); - } + + CardinalDirection get nearest => CardinalDirection.nearest(raw); + void update(Orientation newValue); - bool isNear(double angle) => raw.isNear(angle); + + @visibleForTesting + void forceUpdate(Orientation newValue) {} + + bool isNear(Orientation orientation, [double? tolerance]) => + raw.isNear(orientation.heading, tolerance); @override Future init() async => true; diff --git a/lib/src/imu/rover_imu.dart b/lib/src/imu/rover_imu.dart new file mode 100644 index 0000000..8363110 --- /dev/null +++ b/lib/src/imu/rover_imu.dart @@ -0,0 +1,54 @@ +import "package:autonomy/interfaces.dart"; + +class RoverImu extends ImuInterface { + final _xCorrector = ErrorCorrector.disabled(); + final _yCorrector = ErrorCorrector.disabled(); + final _zCorrector = ErrorCorrector.disabled(); + + RoverImu({required super.collection}); + + @override + Future init() async { + collection.server.messages.onMessage( + name: RoverPosition().messageName, + constructor: RoverPosition.fromBuffer, + callback: _internalUpdate, + ); + return super.init(); + } + + @override + Future dispose() async { + _zCorrector.clear(); + } + + @override + void update(Orientation newValue) { + // Do nothing, since this should only be internally updated + } + + @override + void forceUpdate(Orientation newValue) => + _internalUpdate(RoverPosition(orientation: newValue)); + + void _internalUpdate(RoverPosition newValue) { + if (!newValue.hasOrientation()) return; + // Angles are always between -180 and +180 + if (newValue.orientation.x.abs() > 180 || + newValue.orientation.y.abs() > 180 || + newValue.orientation.z.abs() > 180) { + return; + } + _xCorrector.addValue(newValue.orientation.x); + _yCorrector.addValue(newValue.orientation.y); + _zCorrector.addValue(newValue.orientation.z); + hasValue = true; + } + + @override + Orientation get raw => Orientation( + x: _xCorrector.calibratedValue.clampHalfAngle(), + y: _yCorrector.calibratedValue.clampHalfAngle(), + z: _zCorrector.calibratedValue.clampHalfAngle(), + ); +} diff --git a/lib/src/simulator/imu.dart b/lib/src/imu/sim_imu.dart similarity index 81% rename from lib/src/simulator/imu.dart rename to lib/src/imu/sim_imu.dart index 3c4cbac..466b9eb 100644 --- a/lib/src/simulator/imu.dart +++ b/lib/src/imu/sim_imu.dart @@ -1,10 +1,10 @@ -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); + + ImuSimulator({required super.collection, double maxError = 0}) + : _error = RandomError(maxError); @override RoverPosition getMessage() => RoverPosition(orientation: raw); @@ -19,7 +19,7 @@ class ImuSimulator extends ImuInterface with ValueReporter { ); @override - void update(Orientation newValue) => _orientation = newValue.clampHeading(); + void update(Orientation newValue) => _orientation = newValue; @override Future init() async { diff --git a/lib/src/interfaces/a_star.dart b/lib/src/interfaces/a_star.dart deleted file mode 100644 index 6c7f16e..0000000 --- a/lib/src/interfaces/a_star.dart +++ /dev/null @@ -1,66 +0,0 @@ -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/lib/src/interfaces/drive.dart b/lib/src/interfaces/drive.dart deleted file mode 100644 index 08b677e..0000000 --- a/lib/src/interfaces/drive.dart +++ /dev/null @@ -1,79 +0,0 @@ -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/lib/src/interfaces/gps_utils.dart b/lib/src/interfaces/gps_utils.dart deleted file mode 100644 index dc4fc74..0000000 --- a/lib/src/interfaces/gps_utils.dart +++ /dev/null @@ -1,62 +0,0 @@ - -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/lib/src/interfaces/imu_utils.dart b/lib/src/interfaces/imu_utils.dart deleted file mode 100644 index 588189d..0000000 --- a/lib/src/interfaces/imu_utils.dart +++ /dev/null @@ -1,27 +0,0 @@ -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/lib/src/interfaces/pathfinding.dart b/lib/src/interfaces/pathfinding.dart deleted file mode 100644 index a3b4c73..0000000 --- a/lib/src/interfaces/pathfinding.dart +++ /dev/null @@ -1,18 +0,0 @@ -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/lib/src/interfaces/receiver.dart b/lib/src/interfaces/receiver.dart deleted file mode 100644 index 59ec05b..0000000 --- a/lib/src/interfaces/receiver.dart +++ /dev/null @@ -1,9 +0,0 @@ -mixin Receiver { - bool hasValue = false; - - Future waitForValue() async { - while (!hasValue) { - await Future.delayed(const Duration(seconds: 1)); - } - } -} diff --git a/lib/src/interfaces/server.dart b/lib/src/interfaces/server.dart deleted file mode 100644 index cec6850..0000000 --- a/lib/src/interfaces/server.dart +++ /dev/null @@ -1,25 +0,0 @@ -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/lib/src/interfaces/service.dart b/lib/src/interfaces/service.dart deleted file mode 100644 index 19cbaca..0000000 --- a/lib/src/interfaces/service.dart +++ /dev/null @@ -1 +0,0 @@ -export "package:burt_network/burt_network.dart" show Service; diff --git a/lib/src/interfaces/video.dart b/lib/src/interfaces/video.dart deleted file mode 100644 index 6f3fc32..0000000 --- a/lib/src/interfaces/video.dart +++ /dev/null @@ -1,17 +0,0 @@ -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/lib/src/interfaces/orchestrator.dart b/lib/src/orchestrator/orchestrator_interface.dart similarity index 50% rename from lib/src/interfaces/orchestrator.dart rename to lib/src/orchestrator/orchestrator_interface.dart index 13ee8ed..1c2a291 100644 --- a/lib/src/interfaces/orchestrator.dart +++ b/lib/src/orchestrator/orchestrator_interface.dart @@ -1,35 +1,49 @@ -import "dart:io"; +import "dart:async"; 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; + PeriodicTimer? executionTimer; + + StateController controller = StateController(); + + OrchestratorInterface({required this.collection}); + 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"); + collection.logger.error( + "Already executing a command", + body: "Abort first if you want to switch tasks", + ); return; } if (!collection.hasValue) { + // We don't wait here because this was explicitly requested by the operator. + // Users expect immediate feedback, so we give an error instead of freezing. collection.logger.error("Sensors haven't gotten any readings yet!"); currentState = AutonomyState.NO_SOLUTION; return; } - await collection.drive.faceNorth(); + + // await collection.drive.resolveOrientation(); 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 + case AutonomyTask.BETWEEN_GATES: + break; // TODO + case AutonomyTask.AUTONOMY_TASK_UNDEFINED: + break; + case AutonomyTask.GPS_ONLY: + handleGpsTask(command); + case AutonomyTask.VISUAL_MARKER: + handleArucoTask(command); } } @@ -44,18 +58,28 @@ abstract class OrchestratorInterface extends Service { } @mustCallSuper - Future abort() async { + void onCommandEnd() { currentCommand = null; + executionTimer?.cancel(); + executionTimer = null; + controller.clearStates(); + collection.drive.stop(); + } + + @mustCallSuper + Future abort() async { collection.logger.warning("Aborting task!"); + onCommandEnd(); currentState = AutonomyState.ABORTING; - await collection.drive.stop(); - await collection.dispose(); - exit(1); + // await collection.dispose(); + // await collection.init(); + // exit(1); } - Future handleGpsTask(AutonomyCommand command); - Future handleArucoTask(AutonomyCommand command); - Future handleHammerTask(AutonomyCommand command); - Future handleBottleTask(AutonomyCommand command); + void handleGpsTask(AutonomyCommand command); + void handleArucoTask(AutonomyCommand command); + void handleHammerTask(AutonomyCommand command); + void handleBottleTask(AutonomyCommand command); + AutonomyData get statusMessage; } diff --git a/lib/src/orchestrator/rover_orchestrator.dart b/lib/src/orchestrator/rover_orchestrator.dart new file mode 100644 index 0000000..8e05835 --- /dev/null +++ b/lib/src/orchestrator/rover_orchestrator.dart @@ -0,0 +1,432 @@ +import "dart:math"; + +import "package:autonomy/constants.dart"; +import "package:autonomy/interfaces.dart"; +import "dart:async"; + +import "package:coordinate_converter/coordinate_converter.dart"; + +class RoverOrchestrator extends OrchestratorInterface with ValueReporter { + /// The GPS coordinates that the rover has traversed during the task + final List traversed = []; + + /// The current path that the rover is following + List? currentPath; + + /// Whether or not the rover should replan the path, this is managed by the behavior tree + bool replanPath = true; + + RoverOrchestrator({required super.collection}); + + @override + Future dispose() async { + currentPath = null; + currentCommand = null; + currentState = AutonomyState.AUTONOMY_STATE_UNDEFINED; + traversed.clear(); + await super.dispose(); + } + + @override + Future abort() async { + currentPath = null; + return super.abort(); + } + + @override + void onCommandEnd() { + super.onCommandEnd(); + currentPath = null; + replanPath = false; + } + + @override + AutonomyData get statusMessage => AutonomyData( + destination: currentCommand?.destination, + state: currentState, + obstacles: [ + ...collection.pathfinder.obstacles, + ...collection.pathfinder.lockedObstacles, + ], + path: { + ...traversed, + for (final transition in currentPath ?? []) + transition.position, + }, + task: currentCommand?.task, + crash: false, // TODO: Investigate if this is used and how to use it better + ); + + @override + Message getMessage() => statusMessage; + + /// Triggers a path replan, calling this method will signal the + /// orchestrator to replan a new path while path following + void triggerPathReplan() => replanPath = true; + + /// Finds new obstacles and locks them if any intersect with the current path + /// + /// If the implementation of the obstacle detector has detected any obstacles, + /// it will "lock" any obstacles that intersect with the current path, to prevent + /// future paths from being planned in that area. + bool findAndLockObstacles() { + if (!collection.detector.findObstacles()) { + return false; + } + + if (currentPath == null) return true; + + final toLock = {}; + + for (final step in currentPath!.map((state) => state.position)) { + // Since we're iterating over the obstacles that we also want to lock, + // we have to create a copy of the ones we want to lock, otherwise we'll + // be modifying the array while iterating over it + toLock.addAll( + collection.pathfinder.obstacles.where( + (obstacle) => collection.pathfinder.isObstacle(step), + ), + ); + } + + toLock.forEach(collection.pathfinder.lockObstacle); + + return true; + } + + Future calculateAndFollowPath( + GpsCoordinates goal, { + bool abortOnError = true, + bool Function()? alternateEndCondition, + }) async { + await collection.drive.resolveOrientation(); + collection.detector.findObstacles(); + while (!collection.gps.coordinates.isNear(goal) && + !(alternateEndCondition?.call() ?? false)) { + // Calculate a path + collection.logger.debug("Finding a path"); + currentState = AutonomyState.PATHING; + final path = collection.pathfinder.getPath(goal); + 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 ${goal.prettyPrint()}", + ); + if (abortOnError) { + currentState = AutonomyState.NO_SOLUTION; + currentCommand = null; + } + return false; + } + // Try to take that path + final current = collection.gps.coordinates; + collection.logger.debug( + "Found a path from ${current.prettyPrint()} to ${goal.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()); + // Alternate end condition may have hit between steps + if (alternateEndCondition?.call() ?? false) { + break; + } + // Replan if too far from start point + final distanceError = collection.gps.coordinates.distanceTo( + state.startPostition, + ); + if (distanceError >= Constants.replanErrorMeters) { + collection.logger.info( + "Replanning Path", + body: "Rover is $distanceError meters off the path", + ); + findAndLockObstacles(); + break; + } + // Re-align to desired start orientation if angle is too far + if (state.instruction == DriveDirection.forward) { + Orientation targetOrientation; + // if it has RTK, point towards the next coordinate + if (collection.gps.coordinates.hasRTK) { + final difference = + state.position.toUTM() - collection.gps.coordinates.toUTM(); + + final angle = atan2(difference.y, difference.x) * 180 / pi; + + targetOrientation = Orientation(z: angle); + } else { + targetOrientation = state.orientation.orientation; + } + + if (!collection.imu.isNear( + targetOrientation, + Constants.driveRealignmentEpsilon, + )) { + collection.logger.info("Re-aligning IMU to correct orientation"); + await collection.drive.faceOrientation(targetOrientation); + } + } + // If there was an error (usually a timeout) while driving, replan path + if (!await collection.drive.driveState(state)) { + findAndLockObstacles(); + break; + } + if (currentCommand == null || currentPath == null) { + collection.logger.info("Aborting path, command was canceled"); + return false; + } + traversed.add(state.position); + // if (state.direction != DriveDirection.forward) continue; + if (++count >= 5) { + findAndLockObstacles(); + break; + } + final foundObstacle = findAndLockObstacles(); + if (foundObstacle) { + collection.logger.debug("Found an obstacle. Recalculating path..."); + break; // calculate a new path + } + } + } + return true; + } + + @override + void handleGpsTask(AutonomyCommand command) { + final destination = command.destination; + collection.logger.info( + "Received GPS Task", + body: "Go to ${destination.prettyPrint()}", + ); + collection.logger.debug( + "Currently at ${collection.gps.coordinates.prettyPrint()}", + ); + traversed.clear(); + collection.drive.setLedStrip(ProtoColor.RED); + replanPath = false; + controller.pushState( + SequenceState( + controller, + steps: [ + collection.drive.resolveOrientationState(), + PathingState( + controller, + collection: collection, + orchestrator: this, + destination: destination, + ), + ], + ), + ); + executionTimer = PeriodicTimer(const Duration(milliseconds: 10), ( + timer, + ) async { + if (currentCommand == null) { + collection.logger.warning( + "Execution timer running while command is null", + body: "Canceling timer", + ); + onCommandEnd(); + timer.cancel(); + return; + } + + if (!controller.hasState()) { + currentState = AutonomyState.NO_SOLUTION; + onCommandEnd(); + timer.cancel(); + return; + } + + controller.update(); + + if (collection.gps.isNear(destination, Constants.maxErrorMeters)) { + timer.cancel(); + collection.logger.info("Task complete"); + onCommandEnd(); + currentState = AutonomyState.AT_DESTINATION; + collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); + return; + } + }); + // detect obstacles before and after resolving orientation, as a "scan" + // collection.detector.findObstacles(); + // await collection.drive.resolveOrientation(); + // collection.detector.findObstacles(); + + // if (!await calculateAndFollowPath(command.destination)) { + // return; + // } + } + + @override + Future handleArucoTask(AutonomyCommand command) async { + collection.drive.setLedStrip(ProtoColor.RED); + + // Go to GPS coordinates + collection.logger.info("Got ArUco Task"); + + DetectedObject? detectedAruco; + + if (command.destination != GpsCoordinates(latitude: 0, longitude: 0)) { + if (!await calculateAndFollowPath( + command.destination, + abortOnError: false, + )) { + collection.logger.error( + "Failed to follow path towards initial destination", + ); + currentState = AutonomyState.NO_SOLUTION; + currentCommand = null; + return; + } + } + + currentState = AutonomyState.SEARCHING; + collection.logger.info("Searching for ArUco tag"); + final didSeeAruco = await collection.drive.spinForAruco( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ); + detectedAruco = collection.video.getArucoDetection( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ); + + if (!didSeeAruco || detectedAruco == null) { + collection.logger.error("Could not find desired Aruco tag"); + currentState = AutonomyState.NO_SOLUTION; + currentCommand = null; + return; + } + + collection.logger.info("Found aruco"); + currentState = AutonomyState.APPROACHING; + final arucoOrientation = Orientation( + z: collection.imu.heading - detectedAruco.yaw, + ); + await collection.drive.faceOrientation(arucoOrientation); + detectedAruco = await collection.video.waitForAruco( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + timeout: const Duration(seconds: 3), + ); + + if (detectedAruco == null || !detectedAruco.hasBestPnpResult()) { + // TODO: handle this condition properly + collection.logger.error( + "Could not find desired Aruco tag after rotating towards it", + ); + currentState = AutonomyState.NO_SOLUTION; + currentCommand = null; + return; + } + + collection.logger.debug( + "Planning path to Aruco ID ${command.arucoId}", + body: "Detection: ${detectedAruco.toProto3Json()}", + ); + + // In theory we could just find the relative position with the translation x and z, + // however if the tag's rotation relative to itself is off (which can be common + // when facing it head on), then it will be extremely innacurate. Since the SolvePnP's + // distance is always extremely accurate, it is more reliable to use the distance + // hypotenuse to the camera combined with trig of the tag's angle relative to the camera. + final cameraToTag = detectedAruco.bestPnpResult.cameraToTarget; + final distanceToTag = + sqrt( + pow(cameraToTag.translation.z, 2) + pow(cameraToTag.translation.x, 2), + ) - + 1; // don't drive *into* the tag + + if (distanceToTag < 1) { + // well that was easy + collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); + currentState = AutonomyState.AT_DESTINATION; + currentCommand = null; + return; + } + + final relativeX = + -distanceToTag * + sin((collection.imu.heading - detectedAruco.yaw) * pi / 180); + final relativeY = + distanceToTag * + cos((collection.imu.heading - detectedAruco.yaw) * pi / 180); + + final destinationCoordinates = + (collection.gps.coordinates.toUTM() + + UTMCoordinates(y: relativeY, x: relativeX, zoneNumber: 1)) + .toGps(); + + if (!await calculateAndFollowPath( + destinationCoordinates, + abortOnError: false, + alternateEndCondition: () { + detectedAruco = collection.video.getArucoDetection( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ); + if (detectedAruco == null) { + return false; + } + final cameraToTag = detectedAruco!.bestPnpResult.cameraToTarget; + final distanceToTag = sqrt( + pow(cameraToTag.translation.z, 2) + pow(cameraToTag.translation.x, 2), + ); + return distanceToTag < 1; + }, + )) { + collection.logger.error("Could not spin towards ArUco tag"); + currentState = AutonomyState.NO_SOLUTION; + currentCommand = null; + return; + } + collection.logger.info("Arrived at estimated Aruco position"); + detectedAruco = collection.video.getArucoDetection( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ); + if (detectedAruco == null) { + collection.logger.info("Re-spinning to find Aruco"); + await collection.drive.spinForAruco( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ); + } + + detectedAruco = collection.video.getArucoDetection( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ); + if (detectedAruco != null) { + collection.logger.info("Rotating towards Aruco"); + await collection.drive.faceOrientation( + Orientation(z: collection.imu.heading - detectedAruco!.yaw), + ); + } else { + collection.logger.warning("Could not find Aruco after following path"); + } + + collection.logger.info( + "Successfully reached within ${Constants.maxErrorMeters} meters of the Aruco tag", + ); + collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); + currentState = AutonomyState.AT_DESTINATION; + + currentCommand = null; + } + + @override + Future handleHammerTask(AutonomyCommand command) async {} + + @override + Future handleBottleTask(AutonomyCommand command) async {} +} diff --git a/lib/src/simulator/orchestrator.dart b/lib/src/orchestrator/sim_orchestrator.dart similarity index 64% rename from lib/src/simulator/orchestrator.dart rename to lib/src/orchestrator/sim_orchestrator.dart index 56420d6..8020d36 100644 --- a/lib/src/simulator/orchestrator.dart +++ b/lib/src/orchestrator/sim_orchestrator.dart @@ -1,32 +1,23 @@ import "package:autonomy/interfaces.dart"; -import "package:burt_network/protobuf.dart"; class OrchestratorSimulator extends OrchestratorInterface { OrchestratorSimulator({required super.collection}); @override - Future dispose() async { } + Future dispose() async {} @override AutonomyData get statusMessage => AutonomyData(); @override - Future handleGpsTask(AutonomyCommand command) async { - - } + Future handleGpsTask(AutonomyCommand command) async {} @override - Future handleArucoTask(AutonomyCommand command) async { - - } + Future handleArucoTask(AutonomyCommand command) async {} @override - Future handleHammerTask(AutonomyCommand command) async { - - } + Future handleHammerTask(AutonomyCommand command) async {} @override - Future handleBottleTask(AutonomyCommand command) async { - - } + Future handleBottleTask(AutonomyCommand command) async {} } diff --git a/lib/src/pathfinding/pathfinding_interface.dart b/lib/src/pathfinding/pathfinding_interface.dart new file mode 100644 index 0000000..a308b64 --- /dev/null +++ b/lib/src/pathfinding/pathfinding_interface.dart @@ -0,0 +1,39 @@ +import "package:autonomy/constants.dart"; +import "package:autonomy/interfaces.dart"; + +abstract class PathfindingInterface extends Service { + final AutonomyInterface collection; + PathfindingInterface({required this.collection}); + + List? getPath( + GpsCoordinates destination, { + bool verbose = false, + }); + + Set obstacles = {}; + final Set _lockedObstacles = {}; + + Set get lockedObstacles => _lockedObstacles; + + void recordObstacle(GpsCoordinates coordinates) => obstacles.add(coordinates); + + void lockObstacle(GpsCoordinates coordinates) { + _lockedObstacles.add(coordinates); + obstacles.remove(coordinates); + } + + bool isObstacle(GpsCoordinates coordinates) => + obstacles.any( + (obstacle) => + obstacle.isNear(coordinates, Constants.obstacleAvoidanceRadius), + ) || + _lockedObstacles.any( + (obstacle) => + obstacle.isNear(coordinates, Constants.obstacleAvoidanceRadius), + ); + + @override + Future dispose() async { + obstacles.clear(); + } +} diff --git a/lib/src/pathfinding/rover_pathfinding.dart b/lib/src/pathfinding/rover_pathfinding.dart new file mode 100644 index 0000000..8aa6efc --- /dev/null +++ b/lib/src/pathfinding/rover_pathfinding.dart @@ -0,0 +1,54 @@ +import "package:a_star/a_star.dart"; + +import "package:autonomy/interfaces.dart"; + +class RoverPathfinder extends PathfindingInterface { + RoverPathfinder({required super.collection}); + + @override + Future init() async => true; + + List optimizePath(Iterable original) { + final newPath = []; + AutonomyAStarState? previous; + for (final step in original) { + if (previous != null && + step.instruction.isQuarterTurn && + previous.instruction.isQuarterTurn && + step.instruction == previous.instruction) { + newPath.last = AutonomyAStarState( + position: previous.position, + goal: previous.goal, + collection: collection, + instruction: step.instruction == DriveDirection.quarterLeft + ? DriveDirection.left + : DriveDirection.right, + orientation: step.orientation, + depth: step.depth, + ); + } else { + newPath.add(step); + } + previous = newPath.last; + } + + return newPath; + } + + @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(); + final optimized = optimizePath(transitions); + return optimized; + } +} diff --git a/lib/src/pathfinding/sim_pathfinding.dart b/lib/src/pathfinding/sim_pathfinding.dart new file mode 100644 index 0000000..42ac80b --- /dev/null +++ b/lib/src/pathfinding/sim_pathfinding.dart @@ -0,0 +1,97 @@ +import "package:autonomy/interfaces.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: (lat: 2, long: 1).toGps(), + position: (lat: 0, long: 0).toGps(), + orientation: CardinalDirection.north, + instruction: DriveDirection.right, + collection: collection, + ), + AutonomyAStarState( + depth: i++, + goal: (lat: 2, long: 1).toGps(), + position: (lat: 0, long: 0).toGps(), + orientation: CardinalDirection.east, + instruction: DriveDirection.forward, + collection: collection, + ), + AutonomyAStarState( + depth: i++, + goal: (lat: 2, long: 1).toGps(), + position: (lat: 0, long: 1).toGps(), + orientation: CardinalDirection.east, + instruction: DriveDirection.forward, + collection: collection, + ), + AutonomyAStarState( + depth: i++, + goal: (lat: 2, long: 1).toGps(), + position: (lat: 0, long: 2).toGps(), + orientation: CardinalDirection.east, + instruction: DriveDirection.left, + collection: collection, + ), + AutonomyAStarState( + depth: i++, + goal: (lat: 2, long: 1).toGps(), + position: (lat: 0, long: 2).toGps(), + orientation: CardinalDirection.north, + instruction: DriveDirection.forward, + collection: collection, + ), + AutonomyAStarState( + depth: i++, + goal: (lat: 2, long: 1).toGps(), + position: (lat: 1, long: 2).toGps(), + orientation: CardinalDirection.north, + instruction: DriveDirection.forward, + collection: collection, + ), + AutonomyAStarState( + depth: i++, + goal: (lat: 2, long: 1).toGps(), + position: (lat: 2, long: 2).toGps(), + orientation: CardinalDirection.north, + instruction: DriveDirection.left, + collection: collection, + ), + AutonomyAStarState( + depth: i++, + goal: (lat: 2, long: 1).toGps(), + position: (lat: 2, long: 2).toGps(), + orientation: CardinalDirection.west, + instruction: DriveDirection.forward, + collection: collection, + ), + AutonomyAStarState( + depth: i++, + goal: (lat: 2, long: 1).toGps(), + position: (lat: 2, long: 1).toGps(), + orientation: CardinalDirection.west, + instruction: DriveDirection.right, + collection: collection, + ), + AutonomyAStarState( + depth: i++, + goal: (lat: 2, long: 1).toGps(), + position: (lat: 2, long: 1).toGps(), + orientation: CardinalDirection.north, + instruction: DriveDirection.forward, + collection: collection, + ), + ]; +} diff --git a/lib/src/rover/detector.dart b/lib/src/rover/detector.dart deleted file mode 100644 index f28c508..0000000 --- a/lib/src/rover/detector.dart +++ /dev/null @@ -1,21 +0,0 @@ -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/lib/src/rover/drive.dart b/lib/src/rover/drive.dart deleted file mode 100644 index 7cef09e..0000000 --- a/lib/src/rover/drive.dart +++ /dev/null @@ -1,67 +0,0 @@ -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/lib/src/rover/drive/motors.dart b/lib/src/rover/drive/motors.dart deleted file mode 100644 index 12ae541..0000000 --- a/lib/src/rover/drive/motors.dart +++ /dev/null @@ -1,26 +0,0 @@ -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/lib/src/rover/drive/sensor.dart b/lib/src/rover/drive/sensor.dart deleted file mode 100644 index 207e4fc..0000000 --- a/lib/src/rover/drive/sensor.dart +++ /dev/null @@ -1,148 +0,0 @@ -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/lib/src/rover/drive/timed.dart b/lib/src/rover/drive/timed.dart deleted file mode 100644 index 142a1ff..0000000 --- a/lib/src/rover/drive/timed.dart +++ /dev/null @@ -1,51 +0,0 @@ -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/lib/src/rover/gps.dart b/lib/src/rover/gps.dart deleted file mode 100644 index 2704b85..0000000 --- a/lib/src/rover/gps.dart +++ /dev/null @@ -1,41 +0,0 @@ -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/lib/src/rover/imu.dart b/lib/src/rover/imu.dart deleted file mode 100644 index 48f6666..0000000 --- a/lib/src/rover/imu.dart +++ /dev/null @@ -1,43 +0,0 @@ -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/lib/src/rover/orchestrator.dart b/lib/src/rover/orchestrator.dart deleted file mode 100644 index 2fd9a2f..0000000 --- a/lib/src/rover/orchestrator.dart +++ /dev/null @@ -1,113 +0,0 @@ -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/lib/src/rover/pathfinding.dart b/lib/src/rover/pathfinding.dart deleted file mode 100644 index 2eb0673..0000000 --- a/lib/src/rover/pathfinding.dart +++ /dev/null @@ -1,22 +0,0 @@ -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/lib/src/rover/sensorless.dart b/lib/src/rover/sensorless.dart deleted file mode 100644 index 2beab09..0000000 --- a/lib/src/rover/sensorless.dart +++ /dev/null @@ -1,64 +0,0 @@ -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/lib/src/rover/video.dart b/lib/src/rover/video.dart deleted file mode 100644 index 0f829ac..0000000 --- a/lib/src/rover/video.dart +++ /dev/null @@ -1,32 +0,0 @@ -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/lib/src/simulator/drive.dart b/lib/src/simulator/drive.dart deleted file mode 100644 index 16eecaa..0000000 --- a/lib/src/simulator/drive.dart +++ /dev/null @@ -1,54 +0,0 @@ -// 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/lib/src/simulator/pathfinding.dart b/lib/src/simulator/pathfinding.dart deleted file mode 100644 index 155f20e..0000000 --- a/lib/src/simulator/pathfinding.dart +++ /dev/null @@ -1,25 +0,0 @@ -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/lib/src/simulator/realsense.dart b/lib/src/simulator/realsense.dart deleted file mode 100644 index f425102..0000000 --- a/lib/src/simulator/realsense.dart +++ /dev/null @@ -1,22 +0,0 @@ -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/lib/src/state_machine/decorators.dart b/lib/src/state_machine/decorators.dart new file mode 100644 index 0000000..e1f927b --- /dev/null +++ b/lib/src/state_machine/decorators.dart @@ -0,0 +1,45 @@ +import "package:autonomy/src/state_machine/rover_state_machine.dart"; + +/// State to add a timeout to a state +/// +/// The [child] of this state will be updated each time update is +/// called on this state, but when [timeout] amount of time elapses +/// after starting, the state will be popped. +class TimeoutDecorator extends RoverState { + /// The child state the timeout will be applied to + final RoverState child; + + /// The timeout duration + final Duration timeout; + + /// The function to execute when the timeout is reached + final StateCallback onTimeout; + + DateTime _startTime = DateTime(0); + + /// Constructor for [TimeoutDecorator] + TimeoutDecorator({ + required this.child, + required this.timeout, + required this.onTimeout, + }) : super(child.controller); + + @override + void enter() { + _startTime = DateTime.now(); + child.enter(); + } + + @override + void update() { + if (DateTime.now().difference(_startTime) > timeout) { + return onTimeout(controller); + } + child.update(); + } + + @override + void exit() { + child.exit(); + } +} diff --git a/lib/src/state_machine/rover_state.dart b/lib/src/state_machine/rover_state.dart new file mode 100644 index 0000000..e3b2730 --- /dev/null +++ b/lib/src/state_machine/rover_state.dart @@ -0,0 +1,36 @@ +import "package:autonomy/src/state_machine/rover_state_machine.dart"; + +/// Abstracted version of a state +abstract class StateInterface { + /// The controller for the state machine + StateController get controller; + + /// Called when the state is initially entered + void enter(); + + /// Called when the state is updated from the controller + void update(); + + /// Called when the state is exited or removed from the stack + void exit(); +} + +/// An implementation of [StateInterface] to be used for the rover +/// +/// Stores an own internal [StateController] +class RoverState implements StateInterface { + @override + final StateController controller; + + /// Constructor for [RoverState] initializing its controller + RoverState(this.controller); + + @override + void enter() {} + + @override + void update() {} + + @override + void exit() {} +} diff --git a/lib/src/state_machine/rover_state_machine.dart b/lib/src/state_machine/rover_state_machine.dart new file mode 100644 index 0000000..801b0d5 --- /dev/null +++ b/lib/src/state_machine/rover_state_machine.dart @@ -0,0 +1,16 @@ +import "package:autonomy/src/state_machine/state_controller.dart"; + +export "state_controller.dart"; +export "rover_state.dart"; +export "decorators.dart"; +export "state_utils.dart"; + +export "rover_states/navigation.dart"; +export "rover_states/pathing.dart"; +export "rover_states/sensor_drive_forward.dart"; +export "rover_states/sensor_drive_turn.dart"; +export "rover_states/simulation_drive_forward.dart"; +export "rover_states/simulation_drive_turn.dart"; + +/// Callback for a state method that takes in a controller +typedef StateCallback = void Function(StateController controller); diff --git a/lib/src/state_machine/rover_states/navigation.dart b/lib/src/state_machine/rover_states/navigation.dart new file mode 100644 index 0000000..c20f81d --- /dev/null +++ b/lib/src/state_machine/rover_states/navigation.dart @@ -0,0 +1,191 @@ +import "dart:math"; + +import "package:autonomy/constants.dart"; +import "package:autonomy/interfaces.dart"; +import "package:autonomy/rover.dart"; + +/// State to manage the navigation of the rover +/// +/// This state should be pushed after [PathingState], as it depends +/// on having a path already made for the rover to follow. +/// +/// This state will manage following each individual step of the path as well +/// as performing necessary corrections and replanning. +/// +/// When the path has to be replanned, this state will transition to [PathingState] +class NavigationState extends RoverState { + /// The collection for the state + final AutonomyInterface collection; + + /// The orchestrator for the state + final RoverOrchestrator orchestrator; + + /// The final destination to navigate to + final GpsCoordinates destination; + + /// Whether or not the state has performed pre-step correction + bool hasCorrected = false; + + /// Whether or not the state has just completed following a path step + bool hasFollowed = false; + + /// The index of the waypoint being followed + int waypointIndex = 0; + + /// The current step of the path being followed + AutonomyAStarState? get currentPathState { + if (orchestrator.currentPath == null || + waypointIndex >= orchestrator.currentPath!.length) { + return null; + } + return orchestrator.currentPath![waypointIndex]; + } + + /// Default constructor for [NavigationState] + NavigationState( + super.controller, { + required this.collection, + required this.orchestrator, + required this.destination, + }); + + @override + void enter() { + waypointIndex = 0; + hasCorrected = false; + hasFollowed = false; + + orchestrator.currentState = AutonomyState.DRIVING; + } + + /// Checks if the rover is oriented properly before driving the [state] + /// + /// This is assuming that the step's instruction is to drive forward. + /// + /// If the rover is not facing the proper direction, a new state will be pushed + /// to re-correct the rover's orientation + bool checkOrientation(AutonomyAStarState state) { + Orientation targetOrientation; + // if it has RTK, point towards the next coordinate + if (collection.gps.coordinates.hasRTK) { + final difference = + state.position.toUTM() - collection.gps.coordinates.toUTM(); + + final angle = atan2(difference.y, difference.x) * 180 / pi; + + targetOrientation = Orientation(z: angle); + } else { + targetOrientation = state.orientation.orientation; + } + + if (!collection.imu.isNear( + targetOrientation, + Constants.driveRealignmentEpsilon, + )) { + collection.logger.info("Re-aligning IMU to correct orientation"); + controller.pushState( + collection.drive.faceOrientationState(targetOrientation), + ); + return true; + } + return false; + } + + /// Checks if the rover is within a certain distance of [state]'s starting position + /// + /// If the rover is not within [Constants.replanErrorMeters] of the state's starting + /// position, the path will be replanned + bool checkPosition(AutonomyAStarState state) { + final distanceError = collection.gps.coordinates.distanceTo( + state.startPostition, + ); + if (distanceError > Constants.replanErrorMeters) { + collection.logger.info( + "Replanning Path", + body: "Rover is $distanceError meters off the path", + ); + controller.transitionTo( + PathingState( + controller, + collection: collection, + orchestrator: orchestrator, + destination: destination, + ), + ); + return true; + } + return false; + } + + /// Check's the position and orientation of [state] before following it + /// + /// If the instruction of [state] is to move forward, it will check if the + /// orientation is correct using [checkOrientation], otherwise, it will check + /// the position using [checkPosition] + bool checkCurrentPosition(AutonomyAStarState state) { + if (state.instruction == DriveDirection.forward) { + return checkOrientation(state); + } else { + return checkPosition(state); + } + } + + @override + void update() { + if (currentPathState == null) { + controller.popState(); + return; + } + + if (!hasCorrected) { + hasCorrected = true; + if (checkCurrentPosition(currentPathState!)) return; + } + + if (!hasFollowed) { + hasFollowed = true; + collection.logger.debug(currentPathState!.toString()); + controller.pushState(collection.drive.driveStateState(currentPathState!)); + return; + } + + orchestrator.traversed.add(currentPathState!.position); + + if (orchestrator.replanPath) { + controller.transitionTo( + PathingState( + controller, + collection: collection, + orchestrator: orchestrator, + destination: destination, + ), + ); + return; + } + + if (waypointIndex >= orchestrator.currentPath!.length - 1 || + waypointIndex >= 5 || + orchestrator.findAndLockObstacles()) { + controller.transitionTo( + PathingState( + controller, + collection: collection, + orchestrator: orchestrator, + destination: destination, + ), + ); + return; + } + + waypointIndex++; + hasCorrected = false; + hasFollowed = false; + } + + @override + void exit() { + if (currentPathState != null) { + orchestrator.traversed.add(currentPathState!.position); + } + } +} diff --git a/lib/src/state_machine/rover_states/pathing.dart b/lib/src/state_machine/rover_states/pathing.dart new file mode 100644 index 0000000..8116603 --- /dev/null +++ b/lib/src/state_machine/rover_states/pathing.dart @@ -0,0 +1,65 @@ +import "package:autonomy/autonomy.dart"; + +/// State for when the rover is finding a path +/// +/// When ran the state will attempt to plan a path to its given [destination], +/// if successful, it will transition to [NavigationState], if unsuccessful, +/// it will be popped from the stack. +class PathingState extends RoverState { + /// The autonomy collection for the state + final AutonomyInterface collection; + + /// The orchestrator for the state + final RoverOrchestrator orchestrator; + + /// The destination to plan a path to + final GpsCoordinates destination; + + /// Default constructor for [PathingState] + PathingState( + super.controller, { + required this.collection, + required this.orchestrator, + required this.destination, + }); + + @override + void enter() { + orchestrator.replanPath = false; + orchestrator.currentState = AutonomyState.PATHING; + orchestrator.findAndLockObstacles(); + } + + @override + void update() { + final current = collection.gps.coordinates; + orchestrator.currentPath = collection.pathfinder.getPath( + orchestrator.currentCommand!.destination, + ); + if (orchestrator.currentPath == null) { + collection.logger.error( + "Could not find a path", + body: + "No path found from ${current.prettyPrint()} to ${destination.prettyPrint()}", + ); + collection.drive.stop(); + controller.popState(); + } else { + collection.logger.debug( + "Found a path from ${current.prettyPrint()} to ${destination.prettyPrint()}: ${orchestrator.currentPath!.length} steps", + ); + collection.logger.debug("Here is a summary of the path"); + for (final step in orchestrator.currentPath!) { + collection.logger.debug(step.toString()); + } + controller.transitionTo( + NavigationState( + controller, + collection: collection, + orchestrator: orchestrator, + destination: destination, + ), + ); + } + } +} diff --git a/lib/src/state_machine/rover_states/sensor_drive_forward.dart b/lib/src/state_machine/rover_states/sensor_drive_forward.dart new file mode 100644 index 0000000..e0c7782 --- /dev/null +++ b/lib/src/state_machine/rover_states/sensor_drive_forward.dart @@ -0,0 +1,38 @@ +import "package:autonomy/constants.dart"; +import "package:autonomy/interfaces.dart"; +import "package:autonomy/src/drive/drive_commands.dart"; +import "package:autonomy/src/drive/drive_config.dart"; +import "package:autonomy/src/orchestrator/rover_orchestrator.dart"; + +class SensorForwardState extends RoverState { + final AutonomyInterface collection; + final GpsCoordinates position; + + final RoverDriveCommands drive; + + DriveConfig get config => collection.drive.config; + + SensorForwardState( + super.controller, { + required this.collection, + required this.position, + required this.drive, + }); + + @override + void update() { + drive.setThrottle(config.forwardThrottle); + drive.moveForward(); + + if (collection.pathfinder.isObstacle(collection.gps.coordinates)) { + if (collection.orchestrator is RoverOrchestrator) { + (collection.orchestrator as RoverOrchestrator).triggerPathReplan(); + } + controller.popState(); + } + + if (collection.gps.isNear(position, Constants.intermediateStepTolerance)) { + controller.popState(); + } + } +} diff --git a/lib/src/state_machine/rover_states/sensor_drive_turn.dart b/lib/src/state_machine/rover_states/sensor_drive_turn.dart new file mode 100644 index 0000000..e3bd8b5 --- /dev/null +++ b/lib/src/state_machine/rover_states/sensor_drive_turn.dart @@ -0,0 +1,42 @@ +import "package:autonomy/interfaces.dart"; +import "package:autonomy/src/drive/drive_commands.dart"; +import "package:autonomy/src/drive/drive_config.dart"; + +class SensorTurnState extends RoverState { + final AutonomyInterface collection; + final Orientation orientation; + + final RoverDriveCommands drive; + + DriveConfig get config => collection.drive.config; + + SensorTurnState( + super.controller, { + required this.collection, + required this.orientation, + required this.drive, + }); + + @override + void update() { + drive.setThrottle(config.turnThrottle); + + final current = collection.imu.heading; + final target = orientation.heading; + final error = (target - current).clampHalfAngle(); + if (error < 0) { + drive.spinRight(); + } else { + drive.spinLeft(); + } + + if (collection.imu.isNear(orientation)) { + controller.popState(); + } + } + + @override + void exit() { + drive.stopMotors(); + } +} diff --git a/lib/src/state_machine/rover_states/simulation_drive_forward.dart b/lib/src/state_machine/rover_states/simulation_drive_forward.dart new file mode 100644 index 0000000..d8c760b --- /dev/null +++ b/lib/src/state_machine/rover_states/simulation_drive_forward.dart @@ -0,0 +1,58 @@ +import "package:autonomy/autonomy.dart"; +import "package:coordinate_converter/coordinate_converter.dart"; + +class SimulationDriveForward extends RoverState { + final SimulationMethod method; + final AutonomyInterface collection; + final GpsCoordinates destination; + + Duration get delay => collection.drive.config.oneMeterDelay; + + DateTime _startTime = DateTime(0); + GpsCoordinates _startCoordinates = GpsCoordinates(); + + SimulationDriveForward( + super.controller, { + required this.collection, + required this.method, + required this.destination, + }); + + @override + void enter() { + _startTime = DateTime.now(); + _startCoordinates = collection.gps.coordinates; + } + + @override + void update() { + if (method == SimulationMethod.instant) { + collection.gps.update(destination); + controller.popState(); + return; + } + + if (DateTime.now().difference(_startTime) >= delay) { + collection.gps.update(destination); + controller.popState(); + return; + } + + if (method == SimulationMethod.intermediate) { + final deltaCoordinates = destination.toUTM() - _startCoordinates.toUTM(); + final deltaTime = DateTime.now().difference(_startTime); + final timeFraction = + 1.0 * deltaTime.inMicroseconds / delay.inMicroseconds; + + final intermediateCoordinates = + _startCoordinates.toUTM() + + UTMCoordinates( + x: deltaCoordinates.x * timeFraction, + y: deltaCoordinates.y * timeFraction, + zoneNumber: 1, + ); + + collection.gps.update(intermediateCoordinates.toGps()); + } + } +} diff --git a/lib/src/state_machine/rover_states/simulation_drive_turn.dart b/lib/src/state_machine/rover_states/simulation_drive_turn.dart new file mode 100644 index 0000000..ebcdd24 --- /dev/null +++ b/lib/src/state_machine/rover_states/simulation_drive_turn.dart @@ -0,0 +1,53 @@ +import "package:autonomy/autonomy.dart"; + +class SimulationDriveTurn extends RoverState { + final SimulationMethod method; + final AutonomyInterface collection; + final Orientation goalOrientation; + + Duration get delay => collection.drive.config.turnDelay; + + DateTime _startTime = DateTime(0); + Orientation _startOrientation = Orientation(); + + SimulationDriveTurn( + super.controller, { + required this.collection, + required this.method, + required this.goalOrientation, + }); + + @override + void enter() { + _startTime = DateTime.now(); + _startOrientation = collection.imu.raw; + } + + @override + void update() { + if (method == SimulationMethod.instant) { + collection.imu.update(goalOrientation); + controller.popState(); + return; + } + + if (DateTime.now().difference(_startTime) >= delay) { + collection.imu.update(goalOrientation); + controller.popState(); + return; + } + + if (method == SimulationMethod.intermediate) { + final deltaOrientation = (goalOrientation.z - _startOrientation.z) + .clampHalfAngle(); + final deltaTime = DateTime.now().difference(_startTime); + final timeFraction = + 1.0 * deltaTime.inMicroseconds / delay.inMicroseconds; + + final intermediateRotation = + _startOrientation.z + deltaOrientation * timeFraction; + + collection.imu.update(Orientation(z: intermediateRotation)); + } + } +} diff --git a/lib/src/state_machine/state_controller.dart b/lib/src/state_machine/state_controller.dart new file mode 100644 index 0000000..3bb82df --- /dev/null +++ b/lib/src/state_machine/state_controller.dart @@ -0,0 +1,77 @@ +import "package:autonomy/src/state_machine/rover_state_machine.dart"; +import "package:meta/meta.dart"; + +/// Class to control a Stack based State Machine, handling state pushes, +/// pops, and transitions +/// +/// The class maintains a stack of [StateInterface], where the most recent +/// state, or the one at the top of the stack, is the one being updated. +/// +/// When a new state is pushed, it will be added to the top of the state +/// stack, and becomes the new state being updated. This means that any +/// states underneath will no longer be updated. +/// +/// When a state is popped, it is removed from the top of the stack, and will be exited. +class StateController { + @visibleForTesting + List get stack => _stateStack; + + final List _stateStack = []; + + /// Pushes a new state to the top of the stack, and enters it + void pushState(StateInterface state) { + _stateStack.add(state); + state.enter(); + } + + /// Pops and exits the latest state from the top of the stack and replaces it with [state] + void transitionTo(StateInterface state) { + _stateStack.removeLast().exit(); + _stateStack.add(state); + state.enter(); + } + + /// Clears all states from the stack + void clearStates() { + final originalLength = _stateStack.length; + for (var i = 0; i < originalLength; i++) { + popState(); + } + } + + /// Pops the latest state from the top of the stack + void popState() { + if (_stateStack.isEmpty) { + return; + } + _stateStack.removeLast().exit(); + } + + /// Pops all states the come before a state of type [T], not including [T] + void popUntil() { + final index = _stateStack.lastIndexWhere((element) => element is T); + + if (index == -1) { + return; + } + + final amountToRemove = _stateStack.length - index - 1; + for (var i = 0; i < amountToRemove; i++) { + popState(); + } + } + + /// Updates the state controller, calling the update + /// method on the last element of the state stack + void update() { + if (_stateStack.isNotEmpty) { + _stateStack.last.update(); + } + } + + /// Whether or not there are any states in the stack + bool hasState() => _stateStack.isNotEmpty; + + /// Whether or not there is a substate, or more than one state, in the stack + bool hasSubstate() => _stateStack.length > 1; +} diff --git a/lib/src/state_machine/state_utils.dart b/lib/src/state_machine/state_utils.dart new file mode 100644 index 0000000..fbbff6a --- /dev/null +++ b/lib/src/state_machine/state_utils.dart @@ -0,0 +1,90 @@ +import "package:autonomy/src/state_machine/rover_state_machine.dart"; + +/// State that can only be ran for [delayTime] amount of time +/// after entering before it automatically pops itself from the +/// state stack +/// +/// This is commonly used in sequences to add a delay between actions. +/// This state has no functional logic to it. +class DelayedState extends RoverState { + /// The amount of time to wait for before popping + final Duration delayTime; + + DateTime _startTime = DateTime(0); + + /// Default constructor for [DelayedState], initializing the delay time + DelayedState(super.controller, {required this.delayTime}); + + @override + void enter() { + _startTime = DateTime.now(); + } + + @override + void update() { + if (DateTime.now().difference(_startTime) >= delayTime) { + controller.popState(); + } + } +} + +/// A state with no internal logic that operates only on [StateCallback]s given by the user. +/// +/// This state is to allow for simple states to be created inline rather than from a subclass. +class FunctionalState extends RoverState { + /// [StateCallback] for when the state is entered + final StateCallback? onEnter; + + /// [StateCallback] for when the state is updated + final StateCallback? onUpdate; + + /// [StateCallback] for when the state is exited + final StateCallback? onExit; + + /// Default constructor for [FunctionalState], initializing callbacks + /// for entering, updating, and exiting + FunctionalState(super.controller, {this.onEnter, this.onUpdate, this.onExit}); + + @override + void enter() => onEnter?.call(controller); + + @override + void update() => onUpdate?.call(controller); + + @override + void exit() => onExit?.call(controller); +} + +/// A state which pushes multiple sub-states in order +/// +/// This will push and execute the provided [steps] in order, +/// until all states have been entered and finished +class SequenceState extends RoverState { + /// The list of states to execute in order + final List steps; + + int _stepIndex = 0; + + /// Default constructor for [SequenceState], initializing the steps + SequenceState(super.controller, {required this.steps}); + + @override + void enter() { + _stepIndex = 0; + if (steps.isNotEmpty) { + controller.pushState(steps[_stepIndex]); + } + } + + @override + void update() { + _stepIndex++; + // There's another state left in the sequence, push it + if (_stepIndex < steps.length) { + controller.pushState(steps[_stepIndex]); + } else { + // sequence is done + controller.popState(); + } + } +} diff --git a/lib/src/utils/a_star.dart b/lib/src/utils/a_star.dart new file mode 100644 index 0000000..2449d16 --- /dev/null +++ b/lib/src/utils/a_star.dart @@ -0,0 +1,163 @@ +import "dart:math"; + +import "package:a_star/a_star.dart"; +import "package:autonomy/constants.dart"; + +import "package:autonomy/interfaces.dart"; + +class AutonomyAStarState extends AStarState { + static double getCost(DriveDirection direction) { + if (direction == DriveDirection.stop) { + return 0; + } + if (direction == DriveDirection.forward) { + return 1; + } else if (direction == DriveDirection.quarterLeft || + direction == DriveDirection.quarterRight) { + return sqrt2; + } else { + return 2 * sqrt2; + } + } + + final DriveDirection instruction; + final GpsCoordinates position; + final CardinalDirection orientation; + final GpsCoordinates goal; + final AutonomyInterface collection; + + GpsCoordinates get startPostition { + if (instruction != DriveDirection.forward) { + return position; + } + + return position.goForward(orientation.turnRight().turnRight()); + } + + AutonomyAStarState({ + required this.position, + required this.goal, + required this.collection, + required this.instruction, + required this.orientation, + required super.depth, + }); + + factory AutonomyAStarState.start({ + required AutonomyInterface collection, + required GpsCoordinates goal, + }) => AutonomyAStarState( + position: collection.gps.coordinates, + goal: goal, + collection: collection, + instruction: DriveDirection.stop, + orientation: collection.imu.nearest, + depth: 0, + ); + + AutonomyAStarState copyWith({ + required DriveDirection direction, + required CardinalDirection orientation, + required GpsCoordinates position, + }) => AutonomyAStarState( + collection: collection, + position: position, + orientation: orientation, + instruction: direction, + goal: goal, + depth: depth + getCost(direction), + ); + + @override + String toString() => switch (instruction) { + DriveDirection.forward => "Go forward to ${position.prettyPrint()}", + DriveDirection.left => "Turn left to face $instruction", + DriveDirection.right => "Turn right to face $instruction", + DriveDirection.stop => "Start/Stop at ${position.prettyPrint()}", + DriveDirection.quarterLeft => "Turn 45 degrees left to face $instruction", + DriveDirection.quarterRight => "Turn 45 degrees right to face $instruction", + }; + + @override + double heuristic() => position.octileDistance(goal); + + @override + String hash() => "${position.prettyPrint()} ($orientation) ($instruction)"; + + @override + bool isGoal() => position.isNear( + goal, + min(Constants.moveLengthMeters, Constants.maxErrorMeters), + ); + + /// Returns whether or not the rover will drive between or right next to an obstacle diagonally
+ ///
+ /// Example 1:
+ /// 0 X
+ /// X R
+ /// Assuming the rover is facing 0 and trying to drive forward, will return false
+ ///
+ /// Example 2:
+ /// 0 0
+ /// X R
+ /// Assuming the rover is facing 0 and trying to drive forward, will return false
+ ///
+ bool willDriveThroughObstacle(AutonomyAStarState state) { + // Can't hit an obstacle while turning + final isTurn = state.instruction != DriveDirection.forward; + // Forward drive across the perpendicular axis + final isPerpendicular = state.orientation.isPerpendicular; + if (isTurn || isPerpendicular) { + return false; + } + + final orientation1 = state.orientation.turnQuarterLeft(); + final orientation2 = state.orientation.turnQuarterRight(); + + // Since the state being passed has a position of moving after the + // turn, we have to check the position of where it started (our current position) + return collection.pathfinder.isObstacle(position.goForward(orientation1)) || + collection.pathfinder.isObstacle(position.goForward(orientation2)); + } + + bool isValidState(AutonomyAStarState state) => + !(state.instruction == DriveDirection.forward && + collection.pathfinder.isObstacle(state.position)) && + !willDriveThroughObstacle(state); + + Iterable _allNeighbors() => [ + 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, + ), + copyWith( + direction: DriveDirection.quarterLeft, + orientation: orientation.turnQuarterLeft(), + position: position, + ), + copyWith( + direction: DriveDirection.quarterRight, + orientation: orientation.turnQuarterRight(), + position: position, + ), + copyWith( + direction: DriveDirection.stop, + orientation: orientation, + position: position, + ), + ]; + + @override + Iterable expand() => _allNeighbors().where(isValidState); +} diff --git a/lib/src/rover/corrector.dart b/lib/src/utils/corrector.dart similarity index 79% rename from lib/src/rover/corrector.dart rename to lib/src/utils/corrector.dart index 46b59e1..ac7d451 100644 --- a/lib/src/rover/corrector.dart +++ b/lib/src/utils/corrector.dart @@ -1,13 +1,17 @@ import "dart:collection"; -class ErrorCorrector { // non-nullable +class ErrorCorrector { final int maxSamples; final double maxDeviation; - ErrorCorrector({required this.maxSamples, this.maxDeviation = double.infinity}); - + ErrorCorrector({ + required this.maxSamples, + this.maxDeviation = double.infinity, + }); + factory ErrorCorrector.disabled() => ErrorCorrector(maxSamples: 1); + double calibratedValue = 0; final Queue recentSamples = DoubleLinkedQueue(); - + void addValue(double value) { if (recentSamples.isEmpty) { recentSamples.add(value); @@ -32,7 +36,7 @@ extension on Iterable { num sum = 0; var count = 0; for (final element in this) { - sum += element; + sum += element; count++; } return sum / count; diff --git a/lib/src/interfaces/error.dart b/lib/src/utils/error.dart similarity index 100% rename from lib/src/interfaces/error.dart rename to lib/src/utils/error.dart diff --git a/lib/src/utils/gps_utils.dart b/lib/src/utils/gps_utils.dart new file mode 100644 index 0000000..8b53271 --- /dev/null +++ b/lib/src/utils/gps_utils.dart @@ -0,0 +1,130 @@ +import "dart:math"; + +import "package:autonomy/constants.dart"; +import "package:autonomy/interfaces.dart"; +import "package:coordinate_converter/coordinate_converter.dart"; + +/// An alias for gps coordinates measured in meters +typedef GpsMeters = ({num lat, num long}); + +/// Utility math methods for GpsMeters +extension GpsMetersUtil on GpsMeters { + /// Add 2 [GpsMeters] together + GpsMeters operator +(GpsMeters other) => + (lat: lat + other.lat, long: long + other.long); + + /// Subtract 2 [GpsMeters] from each other + GpsMeters operator -(GpsMeters other) => + (lat: lat - other.lat, long: long - other.long); +} + +extension GpsUtils on GpsCoordinates { + static UTMCoordinates eastMeters = UTMCoordinates( + y: 0, + x: Constants.moveLengthMeters, + zoneNumber: 1, + ); + static UTMCoordinates westMeters = UTMCoordinates( + y: 0, + x: -Constants.moveLengthMeters, + zoneNumber: 1, + ); + static UTMCoordinates northMeters = UTMCoordinates( + y: Constants.moveLengthMeters, + x: 0, + zoneNumber: 1, + ); + static UTMCoordinates southMeters = UTMCoordinates( + y: -Constants.moveLengthMeters, + x: 0, + zoneNumber: 1, + ); + static final UTMCoordinates northEastMeters = northMeters + eastMeters; + static final UTMCoordinates northWestMeters = northMeters + westMeters; + static final UTMCoordinates southEastMeters = southMeters + eastMeters; + static final UTMCoordinates southWestMeters = southMeters + westMeters; + + /// Whether or not the coordinates is fused with the RTK algorithm + bool get hasRTK => + rtkMode == RTKMode.RTK_FIXED || rtkMode == RTKMode.RTK_FLOAT; + + /// The distance to [other] using the haversine formula + double distanceTo(GpsCoordinates other) { + // Radius of the earth in meters + const double earthRadius = 6371000; + // Calculate the differences between the coordinates + final deltaLat = (latitude - other.latitude) * pi / 180; + final deltaLong = (longitude - other.longitude) * pi / 180; + + // Apply the Haversine formula + final a = + pow(sin(deltaLat / 2), 2) + + cos(other.latitude * pi / 180) * + cos(latitude * pi / 180) * + pow(sin(deltaLong / 2), 2); + final c = 2 * atan2(sqrt(a), sqrt(1 - a)); + + return earthRadius * c; + } + + double heuristicDistance(GpsCoordinates other) { + var distance = 0.0; + final delta = toUTM() - other.toUTM(); + final deltaLat = delta.y.abs(); + final deltaLong = delta.x.abs(); + + final minimumDistance = min(deltaLat, deltaLong); + if (minimumDistance >= Constants.moveLengthMeters) { + distance += (minimumDistance ~/ Constants.moveLengthMeters) * sqrt2; + } + + final translationDelta = (deltaLat - deltaLong).abs(); + + if (translationDelta >= Constants.moveLengthMeters) { + distance += translationDelta ~/ Constants.moveLengthMeters; + } + + return distance; + } + + double manhattanDistance(GpsCoordinates other) { + final delta = toUTM() - other.toUTM(); + return delta.x.abs() + delta.y.abs(); + } + + double octileDistance(GpsCoordinates other) { + final delta = toUTM() - other.toUTM(); + final dx = delta.x.abs() ~/ Constants.moveLengthMeters; + final dy = delta.y.abs() ~/ Constants.moveLengthMeters; + + return max(dx, dy) + (sqrt2 - 1) * min(dx, dy); + } + + bool isNear(GpsCoordinates other, [double? tolerance]) { + tolerance ??= Constants.maxErrorMeters; + + return distanceTo(other) <= tolerance; + } + + GpsCoordinates operator +(GpsCoordinates other) => + (toUTM() + other.toUTM()).toGps(); + + GpsCoordinates operator -(GpsCoordinates other) => + (toUTM() - other.toUTM()).toGps(); + + String prettyPrint() => toProto3Json().toString(); + + GpsCoordinates goForward(CardinalDirection orientation) => + (toUTM() + + switch (orientation) { + CardinalDirection.north => GpsUtils.northMeters, + CardinalDirection.south => GpsUtils.southMeters, + CardinalDirection.west => GpsUtils.westMeters, + CardinalDirection.east => GpsUtils.eastMeters, + CardinalDirection.northEast => GpsUtils.northEastMeters, + CardinalDirection.northWest => GpsUtils.northWestMeters, + CardinalDirection.southEast => GpsUtils.southEastMeters, + CardinalDirection.southWest => GpsUtils.southWestMeters, + }) + .toGps(); +} diff --git a/lib/src/utils/imu_utils.dart b/lib/src/utils/imu_utils.dart new file mode 100644 index 0000000..9b7a539 --- /dev/null +++ b/lib/src/utils/imu_utils.dart @@ -0,0 +1,42 @@ +import "package:autonomy/autonomy.dart"; + +extension OrientationUtils on Orientation { + /// North orientation + static final north = Orientation(z: CardinalDirection.north.angle); + + /// East orientation + static final west = Orientation(z: CardinalDirection.west.angle); + + /// South Orientation + static final south = Orientation(z: CardinalDirection.south.angle); + + /// East orientation + static final east = Orientation(z: CardinalDirection.east.angle); + + /// The heading of the orientation, or the compass direction we are facing + double get heading => z; + + /// Whether or not this orientation is within [epsilon] degrees of [value] + bool isNear(double value, [double? epsilon]) { + epsilon ??= Constants.turnEpsilon; + final error = (value - z).clampHalfAngle(); + + return error.abs() <= epsilon; + // if (value > 270 && z < 90) { + // return (z + 360 - value).abs() < epsilon; + // } else if (value < 90 && z > 270) { + // return (z - value - 360).abs() < epsilon; + // } else { + // return (z - value).abs() < epsilon; + // } + } +} + +/// Utility methods for an angle +extension AngleUtils on double { + /// The angle clamped between (-180, 180) + double clampHalfAngle() => ((this + 180) % 360) - 180; + + /// The angle clamped between (0, 360) + double clampAngle() => ((this % 360) + 360) % 360; +} diff --git a/lib/src/utils/periodic_timer.dart b/lib/src/utils/periodic_timer.dart new file mode 100644 index 0000000..efaecf4 --- /dev/null +++ b/lib/src/utils/periodic_timer.dart @@ -0,0 +1,64 @@ +import "dart:async"; + +/// An alternative to [Timer.periodic] that ensures its callback is only being run once. +/// +/// Using [Timer.periodic] can cause your function to run in parallel with itself. For example: +/// +/// ```dart +/// void main() => Timer.periodic(Duration(seconds: 1), twoSecondDelay); +/// +/// void twoSecondDelay(_) async { +/// final id = Random().nextInt(100); +/// print("Starting task $id"); +/// await Future.delayed(Duration(seconds: 2)); +/// print("Finished tick $id"); +/// } +/// ``` +/// +/// Running the above code will cause `twoSecondDelay` to be started before the previous call has +/// even finished. If your function uses a handle to a blocking resource, then the second call will +/// crash or stall while the first one is still running. This class ensures that each call to +/// [function] finishes running before the next one begins, while still making sure that they +/// are called approximately every [interval]. +class PeriodicTimer { + /// The interval at which to run [function]. + final Duration interval; + + /// The function to run. Can be synchronous or asynchronous. + final FutureOr Function(PeriodicTimer timer) function; + + /// A timer that runs the next [_tick] at exactly the right time. + Timer? timer; + + /// Whether this timer is currently running. + bool isRunning = true; + + /// Creates a periodic timer and starts the next tick asynchronously. + PeriodicTimer(this.interval, this.function) { + Timer.run(_tick); + } + + /// Calls [function] once and ensures it is called again exactly after [interval]. + /// + /// This function calls [function] and measures how long it takes to run. Afterwards, + /// it waits a delay so that the delay plus the elapsed time equals [interval]. + Future _tick() async { + final startTime = DateTime.timestamp(); + await function(this); + final delay = interval - DateTime.timestamp().difference(startTime); + if (!isRunning) return; + timer = Timer(delay, _tick); + } + + /// Cancels the timer. + void cancel() { + isRunning = false; + timer?.cancel(); + } + + /// Restarts the timer and begins the next tick asynchronously. + void restart() { + isRunning = true; + Timer.run(_tick); + } +} diff --git a/lib/src/utils/receiver.dart b/lib/src/utils/receiver.dart new file mode 100644 index 0000000..c5837d8 --- /dev/null +++ b/lib/src/utils/receiver.dart @@ -0,0 +1,19 @@ +import "dart:async"; + +mixin Receiver { + final Completer _completer = Completer(); + + bool _hasValue = false; + + set hasValue(bool value) { + _hasValue = value; + if (!value) return; + if (!_completer.isCompleted) { + _completer.complete(true); + } + } + + bool get hasValue => _hasValue; + + Future waitForValue() => _completer.future; +} diff --git a/lib/src/interfaces/reporter.dart b/lib/src/utils/reporter.dart similarity index 92% rename from lib/src/interfaces/reporter.dart rename to lib/src/utils/reporter.dart index 4524b60..744eb80 100644 --- a/lib/src/interfaces/reporter.dart +++ b/lib/src/utils/reporter.dart @@ -1,5 +1,4 @@ import "dart:async"; -import "package:burt_network/protobuf.dart"; import "package:autonomy/interfaces.dart"; diff --git a/lib/src/video/rover_video.dart b/lib/src/video/rover_video.dart new file mode 100644 index 0000000..e3363e4 --- /dev/null +++ b/lib/src/video/rover_video.dart @@ -0,0 +1,73 @@ +import "dart:async"; + +import "package:autonomy/constants.dart"; +import "package:autonomy/interfaces.dart"; +import "package:collection/collection.dart"; + +class RoverVideo extends VideoInterface { + final List _cachedResults = []; + + RoverVideo({required super.collection}); + + @override + void updateFrame(VideoData result) { + hasValue = true; + if (result.hasFrame() && result.frame.isNotEmpty) return; + + _cachedResults.removeWhere((e) => e.details.name == result.details.name); + if (result.detectedObjects.isEmpty) return; + + _cachedResults.add(result); + } + + @override + DetectedObject? getArucoDetection(int id, {CameraName? desiredCamera}) { + for (final result in _cachedResults.where( + (e) => e.details.name == (desiredCamera ?? e.details.name), + )) { + for (final object in result.detectedObjects) { + if (object.objectType == DetectedObjectType.ARUCO && + object.arucoTagId == id) { + return object; + } + } + } + return null; + } + + @override + Future waitForAruco( + int id, { + CameraName? desiredCamera, + Duration timeout = Constants.arucoSearchTimeout, + }) async { + final completer = Completer(); + + late final StreamSubscription resultSubscription; + + resultSubscription = collection.server.messages.onMessage( + name: VideoData().messageName, + constructor: VideoData.fromBuffer, + callback: (result) async { + if (result.hasFrame() && result.frame.isNotEmpty) return; + if (result.details.name != (desiredCamera ?? result.details.name)) { + return; + } + final object = result.detectedObjects.firstWhereOrNull( + (e) => e.objectType == DetectedObjectType.ARUCO && e.arucoTagId == id, + ); + if (object != null) { + await resultSubscription.cancel(); + completer.complete(object); + } + }, + ); + + try { + return await completer.future.timeout(timeout); + } on TimeoutException { + await resultSubscription.cancel(); + return null; + } + } +} diff --git a/lib/src/video/sim_video.dart b/lib/src/video/sim_video.dart new file mode 100644 index 0000000..ca6b17b --- /dev/null +++ b/lib/src/video/sim_video.dart @@ -0,0 +1,14 @@ +import "package:autonomy/interfaces.dart"; + +class VideoSimulator extends VideoInterface { + VideoSimulator({required super.collection}); + + @override + Future init() async { + hasValue = true; + return super.init(); + } + + @override + void updateFrame(VideoData result) {} +} diff --git a/lib/src/video/video_interface.dart b/lib/src/video/video_interface.dart new file mode 100644 index 0000000..4344054 --- /dev/null +++ b/lib/src/video/video_interface.dart @@ -0,0 +1,38 @@ +import "dart:async"; + +import "package:autonomy/constants.dart"; +import "package:autonomy/interfaces.dart"; + +/// Handles obstacle detection data and ArUco data from video +abstract class VideoInterface extends Service with Receiver { + final AutonomyInterface collection; + StreamSubscription? _dataSubscription; + + VideoInterface({required this.collection}); + + @override + Future init() async { + _dataSubscription = collection.server.messages.onMessage( + name: VideoData().messageName, + constructor: VideoData.fromBuffer, + callback: updateFrame, + ); + return true; + } + + @override + Future dispose() async { + await _dataSubscription?.cancel(); + } + + void updateFrame(VideoData result); + + DetectedObject? getArucoDetection(int id, {CameraName? desiredCamera}) => + null; + + Future waitForAruco( + int id, { + CameraName? desiredCamera, + Duration timeout = Constants.arucoSearchTimeout, + }) => Future.value(); +} diff --git a/lib/utils.dart b/lib/utils.dart new file mode 100644 index 0000000..bf82279 --- /dev/null +++ b/lib/utils.dart @@ -0,0 +1,11 @@ +export "src/state_machine/rover_state_machine.dart"; +export "src/utils/a_star.dart"; +export "src/utils/corrector.dart"; +export "src/utils/error.dart"; +export "src/utils/gps_utils.dart"; +export "src/utils/imu_utils.dart"; +export "src/utils/periodic_timer.dart"; +export "src/utils/receiver.dart"; +export "src/utils/reporter.dart"; + +export "package:burt_network/burt_network.dart" show Service; diff --git a/pubspec.yaml b/pubspec.yaml index 9dfa7b7..0c65f9e 100644 --- a/pubspec.yaml +++ b/pubspec.yaml @@ -5,13 +5,15 @@ publish_to: none resolution: workspace environment: - sdk: ^3.6.0 + sdk: ^3.9.0 # Try not to edit this section by hand. Use `dart pub add my_package` dependencies: - burt_network: ^2.3.1 + burt_network: ^2.4.0 a_star: ^3.0.0 meta: ^1.11.0 + collection: ^1.19.1 + coordinate_converter: ^1.1.2 dev_dependencies: test: ^1.21.0 diff --git a/test/corrector_test.dart b/test/corrector_test.dart new file mode 100644 index 0000000..ee4c5c7 --- /dev/null +++ b/test/corrector_test.dart @@ -0,0 +1,30 @@ +import "package:autonomy/autonomy.dart"; +import "package:test/test.dart"; + +void main() => group("[Corrector]", tags: ["corrector"], () { + test("Sample Average", () { + final random = RandomError(5); + final corrector = ErrorCorrector(maxSamples: 10); + + corrector.addValue(0); + + var count = 0; + for (var i = 0; i < 1000; i++) { + corrector.addValue(random.value); + + if (corrector.calibratedValue.abs() < 1.25) count++; + } + final percentage = count / 1000; + expect(percentage, greaterThan(0.75)); + }); + + test("Rejects Outliers", () { + final corrector = ErrorCorrector(maxSamples: 10, maxDeviation: 5); + + corrector.addValue(100); + corrector.addValue(-50); + corrector.addValue(20); + + expect(corrector.calibratedValue, 100); + }); +}); diff --git a/test/network_test.dart b/test/network_test.dart index 8e482df..78912f2 100644 --- a/test/network_test.dart +++ b/test/network_test.dart @@ -1,9 +1,13 @@ +import "dart:async"; import "dart:io"; import "package:autonomy/autonomy.dart"; +import "package:autonomy/src/drive/drive_config.dart"; import "package:burt_network/burt_network.dart"; import "package:test/test.dart"; +import "test_util.dart"; + class MockSubsystems extends Service { final socket = RoverSocket( device: Device.AUTONOMY, @@ -33,31 +37,52 @@ class MockSubsystems extends Service { if (!enabled) return; if (command.setLeft) left = command.left; if (command.setRight) right = command.right; - if (command.setThrottle) throttle = command.throttle; - if (throttle > 0) throttleFlag = true; + if (command.setThrottle) { + throttle = command.throttle; + throttleFlag = throttle > 0; + } } @override Future dispose() async { - left = 0; right = 0; - throttle = 0; throttleFlag = false; + left = 0; + right = 0; + throttle = 0; + throttleFlag = false; + enabled = false; await socket.dispose(); } } +class StopState extends RoverState { + final AutonomyInterface collection; + + StopState(super.controller, {required this.collection}); + + @override + void update() { + collection.drive.stop(); + controller.popState(); + } +} + void main() => group("[Network]", tags: ["network"], () { - final subsystems = MockSubsystems(); + var subsystems = MockSubsystems(); final rover = RoverAutonomy(); - rover.drive = SensorlessDrive(collection: rover, useGps: false, useImu: false); + rover.drive = RoverDrive(collection: rover, useGps: false, useImu: false); - Future setup() async { + setUp(() async { Logger.level = LogLevel.off; + await subsystems.dispose(); + subsystems = MockSubsystems(); await subsystems.init(); await rover.init(); - } + }); - setUp(setup); - tearDown(() async { await subsystems.dispose(); await rover.dispose(); }); + tearDown(() async { + await subsystems.dispose(); + await rover.dispose(); + }); test("Rover waits for all data to arrive", () async { final gps = GpsCoordinates(latitude: 1, longitude: 2); @@ -92,43 +117,87 @@ void main() => group("[Network]", tags: ["network"], () { expect(rover.imu.hasValue, isTrue); expect(rover.video.hasValue, isTrue); expect(rover.hasValue, isTrue); + + await Future.delayed(const Duration(seconds: 1)); }); - test("Rover can drive", () async { + test("Rover can drive", retry: 10, () async { subsystems.enabled = true; - ServerUtils.subsystemsDestination = SocketInfo( - address: InternetAddress.loopbackIPv4, - port: 8001, - ); final simulator = AutonomySimulator(); simulator.gps = GpsSimulator(collection: simulator); simulator.imu = ImuSimulator(collection: simulator); - simulator.drive = SensorlessDrive(collection: simulator, useGps: false, useImu: false); + simulator.drive = RoverDrive( + collection: simulator, + useGps: false, + useImu: false, + config: tankConfig, + ); await simulator.init(); - await simulator.drive.faceNorth(); - expect(simulator.imu.orientation, DriveOrientation.north); - - final origin = GpsCoordinates(latitude: 0, longitude: 0); - final oneMeter = (1, 0).toGps(); + final oneMeter = origin.plus(x: 0, y: 1); + simulator.gps.update(origin); expect(subsystems.throttle, 0); expect(subsystems.left, 0); expect(subsystems.right, 0); expect(simulator.gps.isNear(origin), isTrue); expect(simulator.gps.isNear(oneMeter), isFalse); - const driveDelay = Duration(milliseconds: 10); expect(subsystems.throttleFlag, isFalse); - await simulator.drive.goForward(); - await Future.delayed(driveDelay); + final forwardFuture = Future.delayed( + simulator.drive.config.oneMeterDelay, + ); + final forwardState = simulator.drive.driveForwardState(oneMeter); + simulator.orchestrator.controller.pushState(forwardState); + simulator.orchestrator.controller.update(); + simulator.orchestrator.controller.update(); + simulator.orchestrator.controller.update(); + + await Future.delayed(simulator.drive.config.oneMeterDelay * 0.5); + expect(subsystems.throttleFlag, isTrue); + expect(subsystems.throttle, isNot(0)); + expect(subsystems.left, isNot(0)); + expect(subsystems.right, isNot(0)); + expect(simulator.gps.isNear(origin), isTrue); + expect(simulator.gps.isNear(oneMeter), isFalse); + + simulator.orchestrator.controller.update(); + simulator.orchestrator.controller.update(); + simulator.orchestrator.controller.update(); + + await forwardFuture; + + simulator.orchestrator.controller.update(); + simulator.orchestrator.controller.update(); + simulator.orchestrator.controller.update(); + + await Future.delayed(Duration.zero); + + expect(subsystems.throttleFlag, isTrue); + expect(subsystems.throttle, isNot(0)); + expect(subsystems.left, isNot(0)); + expect(subsystems.right, isNot(0)); + + simulator.orchestrator.controller.pushState( + StopState(simulator.orchestrator.controller, collection: simulator), + ); + + simulator.orchestrator.controller.update(); + simulator.orchestrator.controller.update(); + simulator.orchestrator.controller.update(); + + // Future events need to pump for the call to drive.stop() to execute + await Future.delayed(Duration.zero); + expect(subsystems.throttle, 0); expect(subsystems.left, 0); expect(subsystems.right, 0); - expect(simulator.gps.isNear(origin), isFalse); + + expect(simulator.gps.isNear(origin, 0.5), isFalse); expect(simulator.gps.isNear(oneMeter), isTrue); subsystems.enabled = false; + await subsystems.dispose(); await simulator.dispose(); }); }); diff --git a/test/orchestrator_test.dart b/test/orchestrator_test.dart index 27549f5..e6a1c40 100644 --- a/test/orchestrator_test.dart +++ b/test/orchestrator_test.dart @@ -1,21 +1,26 @@ -import "package:autonomy/src/rover/gps.dart"; +import "dart:async"; + +import "package:autonomy/src/drive/drive_config.dart"; import "package:test/test.dart"; import "package:autonomy/autonomy.dart"; import "package:burt_network/burt_network.dart"; -void main() => group("[Orchestrator]", tags: ["orchestrator"], () { - setUp(() => Logger.level = LogLevel.off); +void main() => group("[Orchestrator]", skip: true, tags: ["orchestrator"], () { + setUp(() => Logger.level = LogLevel.info); tearDown(() => Logger.level = LogLevel.off); test("Fails for invalid destinations", () async { - Logger.level = LogLevel.off; // this test can log critical messages + Logger.level = LogLevel.off; // this test can log critical messages final simulator = AutonomySimulator(); await simulator.init(); simulator.pathfinder = RoverPathfinder(collection: simulator); simulator.orchestrator = RoverOrchestrator(collection: simulator); - simulator.pathfinder.recordObstacle((2, 0).toGps()); + simulator.pathfinder.recordObstacle((lat: 2, long: 0).toGps()); // Test blocked command: - final command = AutonomyCommand(destination: (2, 0).toGps(), task: AutonomyTask.GPS_ONLY); + final command = AutonomyCommand( + destination: (lat: 2, long: 0).toGps(), + task: AutonomyTask.GPS_ONLY, + ); expect(simulator.gps.latitude, 0); expect(simulator.gps.longitude, 0); expect(simulator.imu.heading, 0); @@ -32,19 +37,22 @@ void main() => group("[Orchestrator]", tags: ["orchestrator"], () { }); test("Works for GPS task", () async { - Logger.level = LogLevel.off; // this test can log critical messages + Logger.level = LogLevel.off; // this test can log critical messages final simulator = AutonomySimulator(); simulator.pathfinder = RoverPathfinder(collection: simulator); simulator.orchestrator = RoverOrchestrator(collection: simulator); simulator.pathfinder.obstacles.addAll([ - (2, 0).toGps(), - (4, -1).toGps(), - (4, 1).toGps(), + (lat: 2, long: 0).toGps(), + (lat: 4, long: -1).toGps(), + (lat: 4, long: 1).toGps(), ]); await simulator.init(); // Test normal command: - final destination = (4, 0).toGps(); - final command = AutonomyCommand(destination: destination, task: AutonomyTask.GPS_ONLY); + final destination = (lat: 4, long: 0).toGps(); + final command = AutonomyCommand( + destination: destination, + task: AutonomyTask.GPS_ONLY, + ); expect(simulator.gps.latitude, 0); expect(simulator.gps.longitude, 0); expect(simulator.imu.heading, 0); @@ -56,9 +64,9 @@ void main() => group("[Orchestrator]", tags: ["orchestrator"], () { expect(status1.task, AutonomyTask.AUTONOMY_TASK_UNDEFINED); expect(status1.destination, GpsCoordinates()); expect(status1.obstacles, [ - (2, 0).toGps(), - (4, -1).toGps(), - (4, 1).toGps(), + (lat: 2, long: 0).toGps(), + (lat: 4, long: -1).toGps(), + (lat: 4, long: 1).toGps(), ]); expect(status1.state, AutonomyState.AT_DESTINATION); await simulator.dispose(); @@ -68,20 +76,26 @@ void main() => group("[Orchestrator]", tags: ["orchestrator"], () { Logger.level = LogLevel.off; final simulator = AutonomySimulator(); final obstacles = [ - SimulatedObstacle(coordinates: (2, 0).toGps(), radius: 1), - SimulatedObstacle(coordinates: (6, -1).toGps(), radius: 1), - SimulatedObstacle(coordinates: (6, 1).toGps(), radius: 1), + SimulatedObstacle(coordinates: (lat: 2, long: 0).toGps(), radius: 1), + SimulatedObstacle(coordinates: (lat: 6, long: -1).toGps(), radius: 1), + SimulatedObstacle(coordinates: (lat: 6, long: 1).toGps(), radius: 1), ]; - simulator.detector = DetectorSimulator(collection: simulator, obstacles: obstacles); + simulator.detector = DetectorSimulator( + collection: simulator, + obstacles: obstacles, + ); simulator.pathfinder = RoverPathfinder(collection: simulator); simulator.orchestrator = RoverOrchestrator(collection: simulator); simulator.drive = DriveSimulator(collection: simulator); await simulator.init(); final origin = GpsCoordinates(); - final destination = (0, 7).toGps(); + final destination = (lat: 0, long: 7).toGps(); expect(simulator.gps.isNear(origin), isTrue); expect(simulator.imu.heading, 0); - final command = AutonomyCommand(destination: destination, task: AutonomyTask.GPS_ONLY); + final command = AutonomyCommand( + destination: destination, + task: AutonomyTask.GPS_ONLY, + ); await simulator.orchestrator.onCommand(command); expect(simulator.gps.isNear(destination), isTrue); await simulator.dispose(); @@ -89,30 +103,40 @@ void main() => group("[Orchestrator]", tags: ["orchestrator"], () { test("Rejects commands until latitude has been determined", () async { final simulator = AutonomySimulator(); - final start = (5, 0).toGps(); - final destination = (5, 5).toGps(); - final command = AutonomyCommand(destination: destination, task: AutonomyTask.GPS_ONLY); + final start = (lat: 5, long: 0).toGps(); + final destination = (lat: 5, long: 5).toGps(); + final command = AutonomyCommand( + destination: destination, + task: AutonomyTask.GPS_ONLY, + ); simulator.orchestrator = RoverOrchestrator(collection: simulator); simulator.pathfinder = RoverPathfinder(collection: simulator); simulator.gps = RoverGps(collection: simulator); + simulator.drive = RoverDrive( + collection: simulator, + useImu: false, + config: tankConfig, + ); await simulator.init(); expect(simulator.gps.hasValue, isFalse); await simulator.orchestrator.onCommand(command); expect(simulator.gps.hasValue, isFalse); - expect(GpsInterface.currentLatitude, 0); - expect(simulator.orchestrator.statusMessage.state, AutonomyState.NO_SOLUTION); + expect( + simulator.orchestrator.statusMessage.state, + AutonomyState.NO_SOLUTION, + ); - simulator.gps.update(start); + simulator.gps.forceUpdate(start); await simulator.init(); await simulator.waitForValue(); - await simulator.orchestrator.onCommand(command); expect(simulator.hasValue, isTrue); - expect(GpsInterface.currentLatitude, start.latitude); - expect(simulator.orchestrator.currentState, AutonomyState.AT_DESTINATION); + unawaited(simulator.orchestrator.onCommand(command)); + await Future.delayed(Duration.zero); + expect(simulator.orchestrator.currentCommand, isNotNull); + expect(simulator.orchestrator.currentState, AutonomyState.DRIVING); - GpsInterface.currentLatitude = 0; await simulator.dispose(); }); }); diff --git a/test/path_test.dart b/test/path_test.dart index e5be9d4..b91634b 100644 --- a/test/path_test.dart +++ b/test/path_test.dart @@ -1,115 +1,250 @@ +import "dart:math"; + +import "package:autonomy/constants.dart"; import "package:burt_network/burt_network.dart"; +import "package:coordinate_converter/coordinate_converter.dart"; import "package:test/test.dart"; import "package:autonomy/interfaces.dart"; import "package:autonomy/rover.dart"; import "package:autonomy/simulator.dart"; +import "test_util.dart"; + void main() => group("[Pathfinding]", tags: ["path"], () { setUp(() => Logger.level = LogLevel.off); tearDown(() => Logger.level = LogLevel.off); test("Simple path from (0, 0) to (5, 5) exists", () { final simulator = AutonomySimulator(); - final destination = (5, 5).toGps(); - simulator.logger.info("Each step is ${GpsUtils.north.latitude.toStringAsFixed(5)}"); + final destination = origin.plus(x: 5, y: 5); + simulator.logger.info( + "Each step is ${GpsUtils.northMeters.toGps().latitude.toStringAsFixed(5)}", + ); simulator.logger.info("Going to ${destination.prettyPrint()}"); - simulator.pathfinder = RoverPathfinder(collection: simulator); + simulator.pathfinder = RoverPathfinder(collection: simulator); + simulator.gps.update(origin); final path = simulator.pathfinder.getPath(destination); expect(path, isNotNull); }); test("Small paths are efficient", () { - final oldError = GpsUtils.maxErrorMeters; - GpsUtils.maxErrorMeters = 1; final simulator = AutonomySimulator(); // Plan a path from (0, 0) to (5, 5) - simulator.pathfinder = RoverPathfinder(collection: simulator); - final destination = (5, 5).toGps(); + simulator.pathfinder = RoverPathfinder(collection: simulator); + final destination = origin.plus(x: 5, y: 5); simulator.logger.info("Going to ${destination.prettyPrint()}"); + simulator.gps.update(origin); final path = simulator.pathfinder.getPath(destination); - expect(path, isNotNull); if (path == null) return; + expect(path, isNotNull); + + if (path == null) return; var turnCount = 0; for (final step in path) { - if (step.direction == DriveDirection.left || step.direction == DriveDirection.right) { + if (step.instruction.isTurn) { turnCount++; } simulator.logger.trace(step.toString()); } // start + 5 forward + 1 turn + 5 right = 12 steps + // start + quarter turn left + 7 forward = 8 steps expect(turnCount, 1); - expect(path.length, 11); - - GpsUtils.maxErrorMeters = oldError; - }); - - test("Following path gets to the end", () async { - final simulator = AutonomySimulator(); - final destination = (5, 5).toGps(); - simulator.pathfinder = RoverPathfinder(collection: simulator); - final path = simulator.pathfinder.getPath(destination); - - expect(path, isNotNull); if (path == null) return; - expect(simulator.gps.isNear(GpsCoordinates()), isTrue); - - await simulator.drive.followPath(path); - expect(simulator.gps.isNear(destination), isTrue); - - await simulator.dispose(); + expect(path.length, 7); }); test("Avoid obstacles but reach the goal", () async { // Logger.level = LogLevel.all; final simulator = AutonomySimulator(); - final destination = (5, 0).toGps(); + final destination = origin.plus(x: 0, y: 5); simulator.pathfinder = RoverPathfinder(collection: simulator); - simulator.pathfinder.recordObstacle((1, 0).toGps()); - simulator.pathfinder.recordObstacle((2, 0).toGps()); + simulator.pathfinder.recordObstacle(origin.plus(x: 0, y: 1)); + simulator.pathfinder.recordObstacle(origin.plus(x: 0, y: 2)); + simulator.gps.update(origin); final path = simulator.pathfinder.getPath(destination); - expect(path, isNotNull); if (path == null) return; + expect(path, isNotNull); + if (path == null) { + return; + } expect(path, isNotEmpty); for (final step in path) { simulator.logger.trace(step.toString()); expect(simulator.pathfinder.isObstacle(step.position), isFalse); } - expect(path.length, 10, reason: "1 Stop + 5 detour + 4 forward = 10 steps total"); - await simulator.drive.followPath(path); - expect(simulator.gps.isNear(destination), isTrue); - await simulator.dispose(); + expect( + path.length, + 9, + reason: + "1 turn + 1 forward + 1 turn + 5 forward + 1 stop = 9 steps total", + ); + expect( + path.last.position.distanceTo(destination), + lessThan(Constants.maxErrorMeters), + ); }); test("Stress test", () async { - final oldError = GpsUtils.maxErrorMeters; - GpsUtils.maxErrorMeters = 1; final simulator = AutonomySimulator(); simulator.pathfinder = RoverPathfinder(collection: simulator); - simulator.logger.trace("Starting from ${simulator.gps.coordinates.prettyPrint()}"); - simulator.logger.trace("Each step is +/- ${GpsUtils.north.prettyPrint()}"); - final destination = (1000, 1000).toGps(); + simulator.logger.trace( + "Starting from ${simulator.gps.coordinates.prettyPrint()}", + ); + final destination = origin.plus(x: 1000, y: 1000); simulator.logger.info("Going to ${destination.prettyPrint()}"); + simulator.gps.update(origin); final path = simulator.pathfinder.getPath(destination); expect(path, isNotNull); await simulator.dispose(); - GpsUtils.maxErrorMeters = oldError; }); test("Impossible paths are reported", () async { final simulator = AutonomySimulator(); simulator.pathfinder = RoverPathfinder(collection: simulator); - final destination = (5, 5).toGps(); + final destination = UTMCoordinates(x: 10, y: 10, zoneNumber: 31).toGps(); final obstacles = { - (1, -1).toGps(), (1, 0).toGps(), (1, 1).toGps(), - (0, -1).toGps(), /* Rover */ (0, 1).toGps(), - (-1, -1).toGps(), (-1, 0).toGps(), (-1, 1).toGps(), + // dart format off + origin.plus(x: -1, y: 1), origin.plus(x: 0, y: 1), origin.plus(x: 1, y: 1), + origin.plus(x: -1, y: 0), /* Rover */ origin.plus(x: 1, y: 0), + origin.plus(x: -1, y: -1), origin.plus(x: 0, y: -1), origin.plus(x: 1, y: -1), + // dart format on }; for (final obstacle in obstacles) { simulator.pathfinder.recordObstacle(obstacle); } + simulator.gps.update(origin); final path = simulator.pathfinder.getPath(destination); expect(path, isNull); await simulator.dispose(); }); + + group("diagonal turns", () { + test("path chooses to move diagonally", () async { + final simulator = AutonomySimulator(); + simulator.pathfinder = RoverPathfinder(collection: simulator); + final destination = origin.plus(x: 5, y: 5); + simulator.gps.update(origin); + final path = simulator.pathfinder.getPath(destination); + expect(path, isNotNull); + expect( + path! + .where((state) => state.instruction == DriveDirection.forward) + .length, + 5, + ); + expect(path[1].instruction, DriveDirection.quarterRight); + await simulator.dispose(); + }); + + test("doesn't drive through an obstacle", () async { + final simulator = AutonomySimulator(); + simulator.pathfinder = RoverPathfinder(collection: simulator); + final destination = UTMCoordinates(x: 10, y: 10, zoneNumber: 31).toGps(); + final obstacles = { + // dart format off + origin.plus(x: 0, y: 1), /* Destination */ + /* Rover */ origin.plus(x: 1, y: 0), + // dart format on + }; + for (final obstacle in obstacles) { + simulator.pathfinder.recordObstacle(obstacle); + } + simulator.gps.update(origin); + final path = simulator.pathfinder.getPath(destination); + expect(path, isNotNull); + expect( + path! + .where((state) => state.instruction == DriveDirection.forward) + .length, + greaterThan(2), + ); + await simulator.dispose(); + }); + + test("doesn't drive through an obstacle", () async { + final simulator = AutonomySimulator(); + simulator.pathfinder = RoverPathfinder(collection: simulator); + final destination = UTMCoordinates(x: 10, y: 10, zoneNumber: 31).toGps(); + final obstacles = { + origin /* Destination */, + /* Rover */ + }; + for (final obstacle in obstacles) { + simulator.pathfinder.recordObstacle(obstacle); + } + simulator.gps.update(origin); + final path = simulator.pathfinder.getPath(destination); + expect(path, isNotNull); + expect( + path! + .where((state) => state.instruction == DriveDirection.forward) + .length, + greaterThan(1), + ); + await simulator.dispose(); + }); + }); + + group("optimizer", () { + test("replaces equal and duplicate quarter turns", () async { + final simulator = AutonomySimulator(); + final pathfinder = RoverPathfinder(collection: simulator); + simulator.pathfinder = pathfinder; + + final originalPath = [ + AutonomyAStarState( + position: UTMCoordinates(x: 0, y: 0, zoneNumber: 31).toGps(), + goal: UTMCoordinates(x: 0, y: 0, zoneNumber: 31).toGps(), + collection: simulator, + instruction: DriveDirection.quarterLeft, + orientation: CardinalDirection.northEast, + depth: sqrt2, + ), + AutonomyAStarState( + position: UTMCoordinates(x: 0, y: 0, zoneNumber: 31).toGps(), + goal: UTMCoordinates(x: 0, y: 0, zoneNumber: 31).toGps(), + collection: simulator, + instruction: DriveDirection.quarterLeft, + orientation: CardinalDirection.east, + depth: sqrt2, + ), + ]; + final optimizedPath = pathfinder.optimizePath(originalPath); + expect(optimizedPath.length, 1); + expect(optimizedPath.first.instruction, DriveDirection.left); + expect(optimizedPath.first.orientation, CardinalDirection.east); + await simulator.dispose(); + }); + + test("does not replace non-equal turns", () async { + final simulator = AutonomySimulator(); + final pathfinder = RoverPathfinder(collection: simulator); + simulator.pathfinder = pathfinder; + + final originalPath = [ + AutonomyAStarState( + position: UTMCoordinates(x: 0, y: 0, zoneNumber: 31).toGps(), + goal: UTMCoordinates(x: 0, y: 0, zoneNumber: 31).toGps(), + collection: simulator, + instruction: DriveDirection.quarterLeft, + orientation: CardinalDirection.northEast, + depth: sqrt2, + ), + AutonomyAStarState( + position: UTMCoordinates(x: 0, y: 0, zoneNumber: 31).toGps(), + goal: UTMCoordinates(x: 0, y: 0, zoneNumber: 31).toGps(), + collection: simulator, + instruction: DriveDirection.quarterRight, + orientation: CardinalDirection.north, + depth: sqrt2, + ), + ]; + final optimizedPath = pathfinder.optimizePath(originalPath); + expect(optimizedPath.length, 2); + expect(optimizedPath.first.instruction, DriveDirection.quarterLeft); + expect(optimizedPath.first.orientation, CardinalDirection.northEast); + await simulator.dispose(); + }); + }); }); diff --git a/test/rover_test.dart b/test/rover_test.dart index f384fb3..f59630d 100644 --- a/test/rover_test.dart +++ b/test/rover_test.dart @@ -1,11 +1,11 @@ import "package:test/test.dart"; -import "package:burt_network/protobuf.dart"; import "package:burt_network/logging.dart"; import "package:autonomy/interfaces.dart"; import "package:autonomy/rover.dart"; -import "package:autonomy/simulator.dart"; + +import "test_util.dart"; void main() => group("[Rover]", tags: ["rover"], () { test("Can be restarted", () async { @@ -16,20 +16,8 @@ void main() => group("[Rover]", tags: ["rover"], () { await rover.dispose(); }); - test("Real pathfinding is coherent", () async { - Logger.level = LogLevel.off; - final simulator = AutonomySimulator(); - simulator.pathfinder = RoverPathfinder(collection: simulator); - await testPath(simulator); - simulator.gps.update((0, 0).toGps()); - simulator.imu.update(Orientation()); - await testPath2(simulator); - await simulator.dispose(); - }); - test("Waits for sensor data", () async { final rover = RoverAutonomy(); - final position = (5, 5).toGps(); final orientation = Orientation(); final data = VideoData(); @@ -37,13 +25,13 @@ void main() => group("[Rover]", tags: ["rover"], () { expect(rover.hasValue, isFalse); expect(rover.gps.hasValue, isFalse); - rover.gps.update(position); + rover.gps.forceUpdate(origin); expect(rover.gps.hasValue, isTrue); expect(rover.hasValue, isFalse); expect(rover.hasValue, isFalse); expect(rover.imu.hasValue, isFalse); - rover.imu.update(orientation); + rover.imu.forceUpdate(orientation); expect(rover.imu.hasValue, isTrue); expect(rover.hasValue, isFalse); @@ -56,45 +44,3 @@ void main() => group("[Rover]", tags: ["rover"], () { await rover.dispose(); }); }); - -Future testPath(AutonomyInterface simulator) async { - final destination = (5, 5).toGps(); - final result = simulator.pathfinder.getPath(destination); - expect(simulator.gps.latitude, 0); - expect(simulator.gps.longitude, 0); - expect(simulator.imu.heading, 0); - expect(result, isNotNull); if (result == null) return; - expect(result, isNotEmpty); - for (final transition in result) { - simulator.logger.trace(" From: ${simulator.gps.coordinates.prettyPrint()} facing ${simulator.imu.heading}"); - simulator.logger.debug(" $transition"); - await simulator.drive.goDirection(transition.direction); - expect(simulator.gps.isNear(transition.position), isTrue); - simulator.logger.trace("New orientation: ${simulator.imu.heading}"); - simulator.logger.trace("Expected orientation: ${transition.orientation}"); - expect(simulator.imu.orientation, transition.orientation); - } -} - -Future testPath2(AutonomyInterface simulator) async { - // Logger.level = LogLevel.all; - final destination = (4, 0).toGps(); - simulator.pathfinder.recordObstacle((2, 0).toGps()); - simulator.pathfinder.recordObstacle((4, -1).toGps()); - simulator.pathfinder.recordObstacle((4, 1).toGps()); - final result = simulator.pathfinder.getPath(destination); - expect(simulator.gps.latitude, 0); - expect(simulator.gps.longitude, 0); - expect(simulator.imu.heading, 0); - expect(result, isNotNull); if (result == null) return; - expect(result, isNotEmpty); - for (final transition in result) { - simulator.logger.debug(transition.toString()); - simulator.logger.trace(" From: ${simulator.gps.coordinates.prettyPrint()}"); - await simulator.drive.goDirection(transition.direction); - expect(simulator.gps.isNear(transition.position), isTrue); - expect(simulator.pathfinder.isObstacle(simulator.gps.coordinates), isFalse); - expect(simulator.imu.orientation, transition.orientation); - simulator.logger.trace(" To: ${simulator.gps.coordinates.prettyPrint()}"); - } -} diff --git a/test/sensor_test.dart b/test/sensor_test.dart deleted file mode 100644 index 6375b9e..0000000 --- a/test/sensor_test.dart +++ /dev/null @@ -1,246 +0,0 @@ -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/test/simulator_test.dart b/test/simulator_test.dart index 0d986a1..b76ee46 100644 --- a/test/simulator_test.dart +++ b/test/simulator_test.dart @@ -4,7 +4,7 @@ import "package:autonomy/autonomy.dart"; import "package:burt_network/burt_network.dart"; void main() => group("[Simulator]", tags: ["simulator"], () { - test("Simulator can be restarted", () async { + test("Simulator can be restarted", () async { Logger.level = LogLevel.off; final simulator = AutonomySimulator(); expect(simulator.isInitialized, isFalse); @@ -15,55 +15,12 @@ void main() => group("[Simulator]", tags: ["simulator"], () { await simulator.dispose(); expect(simulator.isInitialized, isFalse); }); - - test("Simulated drive test with simulated GPS", () async { - Logger.level = LogLevel.off; - final simulator = AutonomySimulator(); - var position = GpsCoordinates(); - final gps = simulator.gps; - expect(gps.coordinates.isNear(position), isTrue); - // Turning left takes you +90 degrees - await simulator.drive.turnLeft(); - expect(simulator.imu.heading, 90); - expect(gps.coordinates.isNear(position), isTrue); - // Turning right takes you -90 degrees - await simulator.drive.turnRight(); - expect(simulator.imu.heading, 0); - expect(gps.coordinates.isNear(position), isTrue); - // Going straight takes you 1 cell forward - await simulator.drive.goForward(); - position += GpsUtils.north; - expect(gps.coordinates.isNear(position), isTrue); - expect(simulator.imu.heading, 0); - // Going forward at 90 degrees - await simulator.drive.turnLeft(); - await simulator.drive.goForward(); - position += GpsUtils.west; - expect(gps.coordinates.isNear(position), isTrue); - expect(simulator.imu.heading, 90); - // Going forward at 180 degrees - await simulator.drive.turnLeft(); - await simulator.drive.goForward(); - position += GpsUtils.south; - expect(gps.coordinates.isNear(position), isTrue); - expect(simulator.imu.heading, 180); - // Going forward at 270 degrees - await simulator.drive.turnLeft(); - await simulator.drive.goForward(); - position += GpsUtils.east; - expect(gps.coordinates.isNear(position), isTrue); - expect(simulator.imu.heading, 270); - // 4 lefts go back to north - await simulator.drive.turnLeft(); - expect(simulator.imu.heading, 0); - expect(gps.coordinates.isNear(position), isTrue); - await simulator.dispose(); - }); test("Already has values", () async { final simulator = AutonomySimulator(); expect(simulator.hasValue, isFalse); await simulator.init(); expect(simulator.hasValue, isTrue); + await simulator.dispose(); }); }); diff --git a/test/state_machine_test.dart b/test/state_machine_test.dart new file mode 100644 index 0000000..097a9b9 --- /dev/null +++ b/test/state_machine_test.dart @@ -0,0 +1,182 @@ +import "package:autonomy/src/state_machine/rover_state_machine.dart"; +import "package:test/test.dart"; + +class TrackingState extends RoverState { + int enterCount = 0; + int updateCount = 0; + int exitCount = 0; + + TrackingState(super.controller); + + @override + void enter() => enterCount++; + + @override + void update() => updateCount++; + + @override + void exit() => exitCount++; +} + +void main() => group("[State Machine]", tags: ["state_machine"], () { + final controller = StateController(); + + test("Pushing and popping", () { + final state = TrackingState(controller); + + controller.pushState(state); + + expect(state.enterCount, 1); + controller.update(); + expect(state.updateCount, 1); + controller.popState(); + expect(state.exitCount, 1); + }); + + test("Pushing multiple", () { + final state1 = TrackingState(controller); + final state2 = TrackingState(controller); + + controller.pushState(state1); + + expect(state1.enterCount, 1); + controller.update(); + expect(state1.updateCount, 1); + + controller.pushState(state2); + + expect(state1.exitCount, 0); + + expect(state2.enterCount, 1); + controller.update(); + expect(state2.updateCount, 1); + controller.popState(); + expect(state2.exitCount, 1); + expect(state1.exitCount, 0); + + controller.popState(); + + expect(state1.exitCount, 1); + }); + + test("Sequence", () { + final state1 = TrackingState(controller); + final state2 = TrackingState(controller); + final sequence = SequenceState(controller, steps: [state1, state2]); + + controller.pushState(sequence); + + // State 1 gets pushed, state 2 remains idle + expect(state1.enterCount, 1); + expect(state2.enterCount, 0); + + controller.update(); + + // State 1 gets updated + expect(state1.updateCount, 1); + expect(state2.updateCount, 0); + + controller.popState(); + + // State 1 exits, state 2 doesn't enter until next update + expect(state1.exitCount, 1); + expect(state2.enterCount, 0); + + controller.update(); + + // State 2 enters, doesn't update until next controller update + expect(state1.updateCount, 1); + expect(state2.enterCount, 1); + expect(state2.updateCount, 0); + + controller.update(); + + expect(state2.updateCount, 1); + }); + + test("Timeout", () async { + const timeoutDuration = Duration(milliseconds: 10); + + var timedOut = false; + + final state1 = TrackingState(controller); + final timeout = TimeoutDecorator( + child: state1, + timeout: timeoutDuration, + onTimeout: (_) { + timedOut = true; + }, + ); + + controller.pushState(timeout); + controller.update(); + expect(timedOut, false); + + await Future.delayed(timeoutDuration); + + // Doesn't time out until next update + expect(timedOut, false); + controller.update(); + expect(timedOut, true); + }); + + test("Delayed State", () async { + const delayTime = Duration(milliseconds: 10); + + final state1 = TrackingState(controller); + final state2 = TrackingState(controller); + final sequence = SequenceState( + controller, + steps: [ + state1, + DelayedState(controller, delayTime: delayTime), + state2, + ], + ); + + controller.pushState(sequence); + + expect(state1.enterCount, 1); + expect(state2.enterCount, 0); + + controller.update(); + + expect(state1.updateCount, 1); + expect(state2.updateCount, 0); + + controller.popState(); + + expect(state1.exitCount, 1); + expect(state2.enterCount, 0); + + controller.update(); + + expect(state1.updateCount, 1); + expect(state2.enterCount, 0); + expect(state2.updateCount, 0); + + controller.update(); + + expect(state1.updateCount, 1); + expect(state2.enterCount, 0); + expect(state2.updateCount, 0); + + await Future.delayed(delayTime); + + controller.update(); + + expect(state1.updateCount, 1); + expect(state2.enterCount, 0); + expect(state2.updateCount, 0); + + controller.update(); + + expect(state1.updateCount, 1); + expect(state2.enterCount, 1); + expect(state2.updateCount, 0); + + controller.update(); + + expect(state2.updateCount, 1); + }); +}); diff --git a/test/test_util.dart b/test/test_util.dart new file mode 100644 index 0000000..1f64c5d --- /dev/null +++ b/test/test_util.dart @@ -0,0 +1,13 @@ +import "package:autonomy/autonomy.dart"; +import "package:coordinate_converter/coordinate_converter.dart"; + +final GpsCoordinates origin = UTMCoordinates( + x: 5, + y: 5, + zoneNumber: 31, +).toGps(); + +extension GpsTransformUtil on GpsCoordinates { + GpsCoordinates plus({required double x, required double y}) => + (toUTM() + UTMCoordinates(x: x, y: y, zoneNumber: 31)).toGps(); +}