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/.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..0f5506c 100644 --- a/analysis_options.yaml +++ b/analysis_options.yaml @@ -9,23 +9,24 @@ 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: + exclude: + - test/*.dart linter: rules: @@ -44,7 +45,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 index ae9f7d8..d924090 100644 --- a/bin/arcuo.dart +++ b/bin/arcuo.dart @@ -2,7 +2,7 @@ import "package:autonomy/autonomy.dart"; void main() async { final rover = RoverAutonomy(); - rover.drive = RoverDrive(collection: rover, useImu: false, useGps: false); + rover.drive = RoverDrive(collection: rover, useImu: false, useGps: false); await rover.init(); //await rover.waitForValue(); final didSeeAruco = await rover.drive.spinForAruco(); diff --git a/bin/latlong.dart b/bin/latlong.dart index eb5ce07..2c8bbf5 100644 --- a/bin/latlong.dart +++ b/bin/latlong.dart @@ -9,14 +9,14 @@ void printInfo(String name, double latitude) { GpsInterface.currentLatitude = 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"); + print(" Our max error in longitude would be ${GpsUtils.epsilonLongitude.toStringAsFixed(20)} degrees"); final isWithinRange = GpsInterface.gpsError <= GpsUtils.epsilonLongitude; print(" Our GPS has ${GpsInterface.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(" So our max error in latitude is always ${GpsUtils.epsilonLatitude.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..8ae1f28 100644 --- a/bin/path.dart +++ b/bin/path.dart @@ -4,7 +4,7 @@ import "package:autonomy/simulator.dart"; void main() { GpsUtils.maxErrorMeters = 1; - final destination = (1000, 1000).toGps(); + final destination = (lat: 1000, long: 1000).toGps(); final simulator = AutonomySimulator(); simulator.pathfinder = RoverPathfinder(collection: simulator); final path = simulator.pathfinder.getPath(destination); @@ -18,7 +18,7 @@ void main() { } var turnCount = 0; for (final step in path) { - if (step.direction == DriveDirection.left || step.direction == DriveDirection.right) { + if (step.instruction == DriveDirection.left || step.instruction == DriveDirection.right) { turnCount++; } } diff --git a/bin/random.dart b/bin/random.dart index fe52f37..c356d40 100644 --- a/bin/random.dart +++ b/bin/random.dart @@ -1,7 +1,6 @@ // ignore_for_file: avoid_print import "package:autonomy/interfaces.dart"; -import "package:autonomy/src/rover/corrector.dart"; const maxError = GpsInterface.gpsError; const maxSamples = 10; @@ -17,7 +16,7 @@ bool test() { corrector.addValue(value); if (verbose) { final calibrated = corrector.calibratedValue; - print("Current value: $value, Corrected value: $calibrated"); + print("Current value: $value, Corrected value: $calibrated"); print(" Difference: ${calibrated.toStringAsFixed(7)} < ${epsilon.toStringAsFixed(7)}"); } } diff --git a/bin/sensorless.dart b/bin/sensorless.dart index 0ddcd1f..1605430 100644 --- a/bin/sensorless.dart +++ b/bin/sensorless.dart @@ -1,63 +1,63 @@ -import "package:burt_network/logging.dart"; -import "package:burt_network/protobuf.dart"; +// import "package:burt_network/logging.dart"; +// import "package:burt_network/protobuf.dart"; -import "package:autonomy/simulator.dart"; -import "package:autonomy/rover.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(); +// 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); +// 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 +// // "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); +// 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(); -} +// await simulator.dispose(); +// } diff --git a/bin/task.dart b/bin/task.dart index 1cac4c9..3736494 100644 --- a/bin/task.dart +++ b/bin/task.dart @@ -1,27 +1,27 @@ -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; 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 index 12205b1..63fa627 100644 --- a/bin/test.dart +++ b/bin/test.dart @@ -1,23 +1,22 @@ -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"; +// import "package:autonomy/autonomy.dart"; +// import "package:burt_network/logging.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); +// 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(); +// await rover.init(); +// rover.logger.info("Waiting for readings"); +// // await rover.waitForReadings(); +// // await rover.waitForConnection(); - rover.logger.info("Starting"); - await rover.drive.turnLeft(); - await rover.drive.turnRight(); +// rover.logger.info("Starting"); +// await rover.drive.turnLeft(); +// await rover.drive.turnRight(); - rover.logger.info("Done"); - await rover.dispose(); -} +// rover.logger.info("Done"); +// await rover.dispose(); +// } diff --git a/lib/constants.dart b/lib/constants.dart new file mode 100644 index 0000000..03e23ad --- /dev/null +++ b/lib/constants.dart @@ -0,0 +1,27 @@ +class Constants { + /// The maximum error or "tolerance" for reaching the end goal + static const double maxErrorMeters = 1; + + /// 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: 3, milliseconds: 53); +} 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..38f5845 100644 --- a/lib/rover.dart +++ b/lib/rover.dart @@ -1,26 +1,23 @@ -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); + /// 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); + /// 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); diff --git a/lib/simulator.dart b/lib/simulator.dart index 908a8ae..8d3d068 100644 --- a/lib/simulator.dart +++ b/lib/simulator.dart @@ -1,20 +1,15 @@ -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/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); 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 100% rename from lib/src/interfaces/detector.dart rename to lib/src/detector/detector_interface.dart diff --git a/lib/src/detector/network_detector.dart b/lib/src/detector/network_detector.dart new file mode 100644 index 0000000..c91934d --- /dev/null +++ b/lib/src/detector/network_detector.dart @@ -0,0 +1,47 @@ +import "package:autonomy/autonomy.dart"; + +class NetworkDetector extends DetectorInterface { + final List _newObstacles = []; + bool _hasNewObstacles = false; + + NetworkDetector({required super.collection}); + + @override + bool canSeeAruco() => false; + + @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/rover/detector.dart b/lib/src/detector/rover_detector.dart similarity index 89% rename from lib/src/rover/detector.dart rename to lib/src/detector/rover_detector.dart index f28c508..3ed0161 100644 --- a/lib/src/rover/detector.dart +++ b/lib/src/detector/rover_detector.dart @@ -11,7 +11,7 @@ class RoverDetector extends DetectorInterface { @override // bool canSeeAruco() => collection.video.data.arucoDetected == BoolState.YES; - bool canSeeAruco() => collection.video.flag; + bool canSeeAruco() => collection.video.flag; @override Future init() async => true; diff --git a/lib/src/simulator/detector.dart b/lib/src/detector/sim_detector.dart similarity index 96% rename from lib/src/simulator/detector.dart rename to lib/src/detector/sim_detector.dart index 8234922..dfac538 100644 --- a/lib/src/simulator/detector.dart +++ b/lib/src/detector/sim_detector.dart @@ -1,5 +1,4 @@ import "package:autonomy/interfaces.dart"; -import "package:burt_network/protobuf.dart"; class SimulatedObstacle { final GpsCoordinates coordinates; diff --git a/lib/src/drive/drive_commands.dart b/lib/src/drive/drive_commands.dart new file mode 100644 index 0000000..3b149e7 --- /dev/null +++ b/lib/src/drive/drive_commands.dart @@ -0,0 +1,44 @@ +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(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"); + sendCommand(DriveCommand(left: left, setLeft: true)); + sendCommand(DriveCommand(right: right, setRight: true)); + } + + /// 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..eed787e --- /dev/null +++ b/lib/src/drive/drive_config.dart @@ -0,0 +1,57 @@ +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, + }); + + /// 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: 5500), + 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..8d250cf --- /dev/null +++ b/lib/src/drive/drive_interface.dart @@ -0,0 +1,96 @@ +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; + + /// Constructor for Drive Interface + DriveInterface({required this.collection, this.config = roverConfig}); + + /// 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() 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..c43a5fb --- /dev/null +++ b/lib/src/drive/rover_drive.dart @@ -0,0 +1,105 @@ +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() => sensorDrive.spinForAruco(); + + @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; + } + } +} diff --git a/lib/src/drive/sensor_drive.dart b/lib/src/drive/sensor_drive.dart new file mode 100644 index 0000000..7a9654f --- /dev/null +++ b/lib/src/drive/sensor_drive.dart @@ -0,0 +1,123 @@ +import "package:autonomy/autonomy.dart"; +import "package:autonomy/constants.dart"; +import "package:autonomy/interfaces.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 + 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 + Future waitFor(bool Function() predicate) async { + while (!predicate()) { + 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 timedOut = false; + await waitFor(() { + if (timedOut) return true; + moveForward(); + return collection.gps.isNear(position, Constants.intermediateStepTolerance); + }).timeout( + Constants.driveGPSTimeout, + onTimeout: () { + collection.logger.warning( + "GPS Drive timed out", + body: "Failed to reach ${position.prettyPrint()} after ${Constants.driveGPSTimeout}", + ); + timedOut = true; + }, + ); + await stop(); + return !timedOut; + } + + @override + Future faceOrientation(Orientation orientation) async { + collection.logger.info("Turning to face $orientation..."); + setThrottle(config.turnThrottle); + await waitFor(() => _tryToFace(orientation)); + await stop(); + return true; + } + + 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() async { + setThrottle(config.turnThrottle); + spinLeft(); + final result = await waitFor(() => collection.detector.canSeeAruco()) + .then((_) => true) + .timeout(config.turnDelay * 4, onTimeout: () => false); + await stop(); + return result; + } + + @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 (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..a9f0760 --- /dev/null +++ b/lib/src/drive/sim_drive.dart @@ -0,0 +1,44 @@ +import "package:autonomy/interfaces.dart"; + +/// 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); + + /// Whether or not to wait before updating virtual sensor readings, + /// this can be useful when simulating the individual steps of a path + final bool shouldDelay; + + /// Constructor for DriveSimulator, initializing the default fields, and whether or not it should delay + DriveSimulator({required super.collection, this.shouldDelay = false, super.config}); + + @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 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..b71812c --- /dev/null +++ b/lib/src/drive/timed_drive.dart @@ -0,0 +1,111 @@ +import "dart:math"; + +import "package:autonomy/interfaces.dart"; +import "package:autonomy/src/drive/drive_config.dart"; + +import "drive_commands.dart"; + +/// 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 + 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 75% rename from lib/src/interfaces/gps.dart rename to lib/src/gps/gps_interface.dart index 8ccef30..7ce52d1 100644 --- a/lib/src/interfaces/gps.dart +++ b/lib/src/gps/gps_interface.dart @@ -1,5 +1,5 @@ -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; @@ -12,9 +12,12 @@ abstract class GpsInterface extends Service with Receiver { double get latitude => coordinates.latitude; void update(GpsCoordinates newValue); + @visibleForTesting + void forceUpdate(GpsCoordinates newValue) {} + GpsCoordinates get coordinates; - bool isNear(GpsCoordinates other) => coordinates.isNear(other); + bool isNear(GpsCoordinates other, [double? tolerance]) => coordinates.isNear(other, tolerance); @override Future waitForValue() async { diff --git a/lib/src/gps/rover_gps.dart b/lib/src/gps/rover_gps.dart new file mode 100644 index 0000000..25e60d0 --- /dev/null +++ b/lib/src/gps/rover_gps.dart @@ -0,0 +1,46 @@ +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 94% rename from lib/src/simulator/gps.dart rename to lib/src/gps/sim_gps.dart index 0062db2..d90a7f5 100644 --- a/lib/src/simulator/gps.dart +++ b/lib/src/gps/sim_gps.dart @@ -1,4 +1,3 @@ -import "package:burt_network/protobuf.dart"; import "package:autonomy/interfaces.dart"; class GpsSimulator extends GpsInterface with ValueReporter { diff --git a/lib/src/imu/cardinal_direction.dart b/lib/src/imu/cardinal_direction.dart new file mode 100644 index 0000000..68017c6 --- /dev/null +++ b/lib/src/imu/cardinal_direction.dart @@ -0,0 +1,79 @@ + +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 54% rename from lib/src/interfaces/imu.dart rename to lib/src/imu/imu_interface.dart index 5bb6bba..9ff199b 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,14 @@ 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..d16dbcb --- /dev/null +++ b/lib/src/imu/rover_imu.dart @@ -0,0 +1,47 @@ +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; + _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 85% rename from lib/src/simulator/imu.dart rename to lib/src/imu/sim_imu.dart index 3c4cbac..8798446 100644 --- a/lib/src/simulator/imu.dart +++ b/lib/src/imu/sim_imu.dart @@ -1,4 +1,3 @@ -import "package:burt_network/protobuf.dart"; import "package:autonomy/interfaces.dart"; class ImuSimulator extends ImuInterface with ValueReporter { @@ -19,7 +18,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/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/orchestrator.dart b/lib/src/orchestrator/orchestrator_interface.dart similarity index 84% rename from lib/src/interfaces/orchestrator.dart rename to lib/src/orchestrator/orchestrator_interface.dart index 13ee8ed..ce8fcfe 100644 --- a/lib/src/interfaces/orchestrator.dart +++ b/lib/src/orchestrator/orchestrator_interface.dart @@ -1,7 +1,6 @@ import "dart:io"; import "package:autonomy/interfaces.dart"; -import "package:burt_network/burt_network.dart"; import "package:meta/meta.dart"; abstract class OrchestratorInterface extends Service { @@ -19,17 +18,19 @@ abstract class OrchestratorInterface extends Service { } 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.BETWEEN_GATES: break; // TODO + case AutonomyTask.AUTONOMY_TASK_UNDEFINED: break; 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 } } diff --git a/lib/src/rover/orchestrator.dart b/lib/src/orchestrator/rover_orchestrator.dart similarity index 62% rename from lib/src/rover/orchestrator.dart rename to lib/src/orchestrator/rover_orchestrator.dart index 2fd9a2f..f3f417f 100644 --- a/lib/src/rover/orchestrator.dart +++ b/lib/src/orchestrator/rover_orchestrator.dart @@ -1,5 +1,7 @@ +import "dart:math"; + +import "package:autonomy/constants.dart"; import "package:autonomy/interfaces.dart"; -import "package:burt_network/protobuf.dart"; import "dart:async"; class RoverOrchestrator extends OrchestratorInterface with ValueReporter { @@ -36,14 +38,18 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { @override Future handleGpsTask(AutonomyCommand command) async { final destination = command.destination; - collection.logger.info("Got GPS Task: Go to ${destination.prettyPrint()}"); + 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); + // detect obstacles before and after resolving orientation, as a "scan" + collection.detector.findObstacles(); + await collection.drive.resolveOrientation(); + collection.detector.findObstacles(); 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) { @@ -55,7 +61,7 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { } // 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("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()); @@ -64,10 +70,45 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { var count = 0; for (final state in path) { collection.logger.debug(state.toString()); - await collection.drive.goDirection(state.direction); + // 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"); + 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.inMeters - collection.gps.coordinates.inMeters; + + final angle = atan2(difference.lat, difference.long) * 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)) { + break; + } + if (currentCommand == null || currentPath == null) { + collection.logger.info("Aborting path, command was canceled"); + return; + } traversed.add(state.position); - if (state.direction != DriveDirection.forward) continue; - if (count++ == 5) break; + // 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..."); diff --git a/lib/src/simulator/orchestrator.dart b/lib/src/orchestrator/sim_orchestrator.dart similarity index 93% rename from lib/src/simulator/orchestrator.dart rename to lib/src/orchestrator/sim_orchestrator.dart index 56420d6..ae2dd75 100644 --- a/lib/src/simulator/orchestrator.dart +++ b/lib/src/orchestrator/sim_orchestrator.dart @@ -1,5 +1,4 @@ import "package:autonomy/interfaces.dart"; -import "package:burt_network/protobuf.dart"; class OrchestratorSimulator extends OrchestratorInterface { OrchestratorSimulator({required super.collection}); diff --git a/lib/src/interfaces/pathfinding.dart b/lib/src/pathfinding/pathfinding_interface.dart similarity index 92% rename from lib/src/interfaces/pathfinding.dart rename to lib/src/pathfinding/pathfinding_interface.dart index a3b4c73..25bce8e 100644 --- a/lib/src/interfaces/pathfinding.dart +++ b/lib/src/pathfinding/pathfinding_interface.dart @@ -1,5 +1,4 @@ import "package:autonomy/interfaces.dart"; -import "package:burt_network/protobuf.dart"; abstract class PathfindingInterface extends Service { final AutonomyInterface collection; diff --git a/lib/src/pathfinding/rover_pathfinding.dart b/lib/src/pathfinding/rover_pathfinding.dart new file mode 100644 index 0000000..afe429d --- /dev/null +++ b/lib/src/pathfinding/rover_pathfinding.dart @@ -0,0 +1,48 @@ +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..3df3970 --- /dev/null +++ b/lib/src/pathfinding/sim_pathfinding.dart @@ -0,0 +1,24 @@ +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/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/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/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/utils/a_star.dart b/lib/src/utils/a_star.dart new file mode 100644 index 0000000..41b132f --- /dev/null +++ b/lib/src/utils/a_star.dart @@ -0,0 +1,196 @@ +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
+ ///
+ /// Case 1:
+ /// 0 X
+ /// X R
+ /// Assuming the rover is facing 0 and trying to drive forward, will return false
+ ///
+ /// Case 2:
+ /// 0 X
+ /// X R
+ /// Assuming the rover is facing north and trying to turn 45 degrees left, will return false
+ ///
+ /// Case 3:
+ /// 0 X
+ /// 0 R
+ /// If the rover is facing left but trying to turn 45 degrees right, will return false
+ ///
+ /// Case 4:
+ /// 0 X 0
+ /// 0 R 0
+ /// If the rover is facing northeast to 0 and trying to turn left, will return false + bool willDriveThroughObstacle(AutonomyAStarState state) { + final isTurn = state.instruction != DriveDirection.forward; + final isQuarterTurn = state.instruction == DriveDirection.quarterLeft || state.instruction == DriveDirection.quarterRight; + + if ( + // Can't hit an obstacle while turning + state.instruction != DriveDirection.forward + + // Forward drive across the perpendicular axis + || (!isTurn && state.orientation.isPerpendicular) + + // Not encountering any sort of diagonal angle + || (isTurn && isQuarterTurn && state.orientation.isPerpendicular) + + // No diagonal movement, won't drive between obstacles + || (!isQuarterTurn && orientation.isPerpendicular) + ) { + return false; + } + + final CardinalDirection orientation1; + final CardinalDirection orientation2; + + // Case 1, trying to drive while facing a 45 degree angle + if (!isTurn) { + orientation1 = state.orientation.turnQuarterLeft(); + orientation2 = state.orientation.turnQuarterRight(); + } else if (isQuarterTurn) { // Case 2 and Case 3 + orientation1 = orientation; + orientation2 = (state.instruction == DriveDirection.quarterLeft) + ? orientation1.turnLeft() + : orientation1.turnRight(); + } else { // Case 4 + orientation1 = (state.instruction == DriveDirection.left) + ? orientation.turnQuarterLeft() + : orientation.turnQuarterRight(); + orientation2 = (state.instruction == DriveDirection.left) + ? state.orientation.turnQuarterLeft() + : 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 + return collection.pathfinder.isObstacle(position.goForward(orientation1)) + || collection.pathfinder.isObstacle(position.goForward(orientation2)); + } + + bool isValidState(AutonomyAStarState state) => + !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 88% rename from lib/src/rover/corrector.dart rename to lib/src/utils/corrector.dart index 46b59e1..7339f02 100644 --- a/lib/src/rover/corrector.dart +++ b/lib/src/utils/corrector.dart @@ -1,13 +1,14 @@ import "dart:collection"; -class ErrorCorrector { // non-nullable +class ErrorCorrector { final int maxSamples; final double maxDeviation; 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 +33,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..b91a0bf --- /dev/null +++ b/lib/src/utils/gps_utils.dart @@ -0,0 +1,112 @@ + +import "dart:math"; + +import "package:autonomy/constants.dart"; +import "package:autonomy/interfaces.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 const GpsMeters eastMeters = (lat: 0, long: Constants.moveLengthMeters); + static const GpsMeters westMeters = (lat: 0, long: -Constants.moveLengthMeters); + static const GpsMeters northMeters = (lat: Constants.moveLengthMeters, long: 0); + static const GpsMeters southMeters = (lat: -Constants.moveLengthMeters, long: 0); + static final GpsMeters northEastMeters = northMeters + eastMeters; + static final GpsMeters northWestMeters = northMeters + westMeters; + static final GpsMeters southEastMeters = southMeters + eastMeters; + static final GpsMeters 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; + + double distanceTo(GpsCoordinates other) { + final deltaMeters = inMeters - other.inMeters; + + return sqrt(pow(deltaMeters.long, 2) + pow(deltaMeters.lat, 2)); + } + + double heuristicDistance(GpsCoordinates other) { + var distance = 0.0; + final delta = inMeters - other.inMeters; + final deltaLat = delta.lat.abs(); + final deltaLong = delta.long.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 = inMeters - other.inMeters; + return delta.lat.toDouble().abs() + delta.long.abs(); + } + + double octileDistance(GpsCoordinates other) { + final delta = inMeters - other.inMeters; + final dx = delta.long.abs() ~/ Constants.moveLengthMeters; + final dy = delta.lat.abs() ~/ Constants.moveLengthMeters; + + return max(dx, dy) + (sqrt2 - 1) * min(dx, dy); + } + + bool isNear(GpsCoordinates other, [double? tolerance]) { + tolerance ??= Constants.maxErrorMeters; + final currentMeters = inMeters; + final otherMeters = other.inMeters; + + final delta = currentMeters - otherMeters; + + final distance = sqrt(pow(delta.long, 2) + pow(delta.lat, 2)); + + return distance < tolerance; + } + + GpsCoordinates operator +(GpsCoordinates other) => GpsCoordinates( + latitude: latitude + other.latitude, + longitude: longitude + other.longitude, + ); + + GpsCoordinates operator -(GpsCoordinates other) => GpsCoordinates( + latitude: latitude - other.latitude, + longitude: longitude - other.longitude, + ); + + String prettyPrint() => toProto3Json().toString(); + + GpsCoordinates goForward(CardinalDirection orientation) => (inMeters + + 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..084bc29 --- /dev/null +++ b/lib/src/utils/imu_utils.dart @@ -0,0 +1,39 @@ +import "package:autonomy/autonomy.dart"; +import "package:autonomy/constants.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/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/rover/video.dart b/lib/src/video/rover_video.dart similarity index 93% rename from lib/src/rover/video.dart rename to lib/src/video/rover_video.dart index 0f829ac..b9442d2 100644 --- a/lib/src/rover/video.dart +++ b/lib/src/video/rover_video.dart @@ -1,7 +1,6 @@ import "dart:async"; import "package:autonomy/interfaces.dart"; -import "package:burt_network/burt_network.dart"; class RoverVideo extends VideoInterface { RoverVideo({required super.collection}); diff --git a/lib/src/simulator/realsense.dart b/lib/src/video/sim_video.dart similarity index 90% rename from lib/src/simulator/realsense.dart rename to lib/src/video/sim_video.dart index f425102..c7d5f1c 100644 --- a/lib/src/simulator/realsense.dart +++ b/lib/src/video/sim_video.dart @@ -1,7 +1,6 @@ import "dart:typed_data"; import "package:autonomy/interfaces.dart"; -import "package:burt_network/protobuf.dart"; class VideoSimulator extends VideoInterface { VideoSimulator({required super.collection}); diff --git a/lib/src/interfaces/video.dart b/lib/src/video/video_interface.dart similarity index 70% rename from lib/src/interfaces/video.dart rename to lib/src/video/video_interface.dart index 6f3fc32..77ea2ad 100644 --- a/lib/src/interfaces/video.dart +++ b/lib/src/video/video_interface.dart @@ -1,5 +1,4 @@ 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 { @@ -12,6 +11,6 @@ abstract class VideoInterface extends Service with Receiver { void updateFrame(VideoData newData); - // double get arucoSize => data.arucoSize; - // double get arucoPosition => data.arucoPosition; + double get arucoSize => 0; // data.arucoSize; + double get arucoPosition => 0; // data.arucoPosition; } diff --git a/lib/utils.dart b/lib/utils.dart new file mode 100644 index 0000000..4829620 --- /dev/null +++ b/lib/utils.dart @@ -0,0 +1,9 @@ +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/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..c1f41ab 100644 --- a/pubspec.yaml +++ b/pubspec.yaml @@ -9,7 +9,7 @@ environment: # 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 diff --git a/test/network_test.dart b/test/network_test.dart index 8e482df..8c64bb6 100644 --- a/test/network_test.dart +++ b/test/network_test.dart @@ -1,6 +1,8 @@ +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"; @@ -33,31 +35,40 @@ 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(); } } 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,36 +103,43 @@ 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: 5, () 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 = (lat: 1, long: 0).toGps(); 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 = simulator.drive.driveForward(oneMeter); + 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); + await forwardFuture; + await Future.delayed(simulator.drive.config.oneMeterDelay * 0.5); + expect(subsystems.throttleFlag, isFalse); expect(subsystems.throttle, 0); expect(subsystems.left, 0); expect(subsystems.right, 0); @@ -129,6 +147,7 @@ void main() => group("[Network]", tags: ["network"], () { 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..f8419b5 100644 --- a/test/orchestrator_test.dart +++ b/test/orchestrator_test.dart @@ -1,10 +1,12 @@ -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); + setUp(() => Logger.level = LogLevel.info); tearDown(() => Logger.level = LogLevel.off); test("Fails for invalid destinations", () async { @@ -13,9 +15,9 @@ void main() => group("[Orchestrator]", tags: ["orchestrator"], () { 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); @@ -37,13 +39,13 @@ void main() => group("[Orchestrator]", tags: ["orchestrator"], () { 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 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); @@ -56,9 +58,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,9 +70,9 @@ 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.pathfinder = RoverPathfinder(collection: simulator); @@ -78,7 +80,7 @@ void main() => group("[Orchestrator]", tags: ["orchestrator"], () { 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); @@ -89,12 +91,13 @@ 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 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, useGps: true, useImu: false, config: tankConfig); await simulator.init(); expect(simulator.gps.hasValue, isFalse); @@ -104,13 +107,15 @@ void main() => group("[Orchestrator]", tags: ["orchestrator"], () { expect(GpsInterface.currentLatitude, 0); 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); + unawaited(simulator.orchestrator.onCommand(command)); + await Future.delayed(Duration.zero); + expect(simulator.orchestrator.currentCommand, isNotNull); expect(GpsInterface.currentLatitude, start.latitude); - expect(simulator.orchestrator.currentState, AutonomyState.AT_DESTINATION); + 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..c664735 100644 --- a/test/path_test.dart +++ b/test/path_test.dart @@ -1,3 +1,6 @@ +import "dart:math"; + +import "package:autonomy/constants.dart"; import "package:burt_network/burt_network.dart"; import "package:test/test.dart"; @@ -5,14 +8,22 @@ import "package:autonomy/interfaces.dart"; import "package:autonomy/rover.dart"; import "package:autonomy/simulator.dart"; +extension DriveFollowPath on DriveInterface { + Future followPath(List path) async { + for (final step in path) { + await driveState(step); + } + } +} + 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 = (lat: 5, long: 5).toGps(); + simulator.logger.info("Each step is ${GpsUtils.northMeters.toGps().latitude.toStringAsFixed(5)}"); simulator.logger.info("Going to ${destination.prettyPrint()}"); simulator.pathfinder = RoverPathfinder(collection: simulator); final path = simulator.pathfinder.getPath(destination); @@ -20,35 +31,36 @@ void main() => group("[Pathfinding]", tags: ["path"], () { }); test("Small paths are efficient", () { - final oldError = GpsUtils.maxErrorMeters; + const oldError = Constants.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(); + final destination = (lat: 5, long: 5).toGps(); simulator.logger.info("Going to ${destination.prettyPrint()}"); final path = simulator.pathfinder.getPath(destination); 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); + expect(path.length, 7); GpsUtils.maxErrorMeters = oldError; }); test("Following path gets to the end", () async { final simulator = AutonomySimulator(); - final destination = (5, 5).toGps(); + final destination = (lat: 5, long: 5).toGps(); simulator.pathfinder = RoverPathfinder(collection: simulator); final path = simulator.pathfinder.getPath(destination); @@ -64,18 +76,21 @@ void main() => group("[Pathfinding]", tags: ["path"], () { test("Avoid obstacles but reach the goal", () async { // Logger.level = LogLevel.all; final simulator = AutonomySimulator(); - final destination = (5, 0).toGps(); + final destination = (lat: 5, long: 0).toGps(); simulator.pathfinder = RoverPathfinder(collection: simulator); - simulator.pathfinder.recordObstacle((1, 0).toGps()); - simulator.pathfinder.recordObstacle((2, 0).toGps()); + simulator.pathfinder.recordObstacle((lat: 1, long: 0).toGps()); + simulator.pathfinder.recordObstacle((lat: 2, long: 0).toGps()); 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"); + expect(path.length, 10, reason: "1 turn + 1 forward + 1 turn + 4 forward + 1 45 degree turn + 1 forward + 1 stop = 10 steps total"); await simulator.drive.followPath(path); expect(simulator.gps.isNear(destination), isTrue); await simulator.dispose(); @@ -83,27 +98,30 @@ void main() => group("[Pathfinding]", tags: ["path"], () { test("Stress test", () async { final oldError = GpsUtils.maxErrorMeters; + final oldMoveLength = GpsUtils.moveLengthMeters; GpsUtils.maxErrorMeters = 1; + GpsUtils.moveLengthMeters = 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(); + final destination = (lat: 1000, long: 1000).toGps(); simulator.logger.info("Going to ${destination.prettyPrint()}"); final path = simulator.pathfinder.getPath(destination); expect(path, isNotNull); await simulator.dispose(); GpsUtils.maxErrorMeters = oldError; + GpsUtils.moveLengthMeters = oldMoveLength; }); test("Impossible paths are reported", () async { final simulator = AutonomySimulator(); simulator.pathfinder = RoverPathfinder(collection: simulator); - final destination = (5, 5).toGps(); + final destination = (lat: 5, long: 5).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(), + (lat: 1, long: -1).toGps(), (lat: 1, long: 0).toGps(), (lat: 1, long: 1).toGps(), + (lat: 0, long: -1).toGps(), /* Rover */ (lat: 0, long: 1).toGps(), + (lat: -1, long: -1).toGps(), (lat: -1, long: 0).toGps(), (lat: -1, long: 1).toGps(), }; for (final obstacle in obstacles) { simulator.pathfinder.recordObstacle(obstacle); @@ -112,4 +130,113 @@ void main() => group("[Pathfinding]", tags: ["path"], () { 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 = (lat: 5, long: 5).toGps(); + 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 = (lat: 5, long: 5).toGps(); + final obstacles = { + (lat: 1, long: 0).toGps(), /* Destination */ + /* Rover */ (lat: 0, long: 1).toGps(), + }; + for (final obstacle in obstacles) { + simulator.pathfinder.recordObstacle(obstacle); + } + 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 = (lat: 5, long: 5).toGps(); + final obstacles = { + (lat: 1, long: 0).toGps(), /* Destination */ + /* Rover */ + }; + for (final obstacle in obstacles) { + simulator.pathfinder.recordObstacle(obstacle); + } + 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: (lat: 0, long: 0).toGps(), + goal: (lat: 0, long: 0).toGps(), + collection: simulator, + instruction: DriveDirection.quarterLeft, + orientation: CardinalDirection.northEast, + depth: sqrt2, + ), + AutonomyAStarState( + position: (lat: 0, long: 0).toGps(), + goal: (lat: 0, long: 0).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: (lat: 0, long: 0).toGps(), + goal: (lat: 0, long: 0).toGps(), + collection: simulator, + instruction: DriveDirection.quarterLeft, + orientation: CardinalDirection.northEast, + depth: sqrt2, + ), + AutonomyAStarState( + position: (lat: 0, long: 0).toGps(), + goal: (lat: 0, long: 0).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..2563f64 100644 --- a/test/rover_test.dart +++ b/test/rover_test.dart @@ -21,7 +21,7 @@ void main() => group("[Rover]", tags: ["rover"], () { final simulator = AutonomySimulator(); simulator.pathfinder = RoverPathfinder(collection: simulator); await testPath(simulator); - simulator.gps.update((0, 0).toGps()); + simulator.gps.update((lat: 0, long: 0).toGps()); simulator.imu.update(Orientation()); await testPath2(simulator); await simulator.dispose(); @@ -29,7 +29,7 @@ void main() => group("[Rover]", tags: ["rover"], () { test("Waits for sensor data", () async { final rover = RoverAutonomy(); - final position = (5, 5).toGps(); + final position = (lat: 5, long: 5).toGps(); final orientation = Orientation(); final data = VideoData(); @@ -37,13 +37,13 @@ void main() => group("[Rover]", tags: ["rover"], () { expect(rover.hasValue, isFalse); expect(rover.gps.hasValue, isFalse); - rover.gps.update(position); + rover.gps.forceUpdate(position); 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); @@ -58,7 +58,7 @@ void main() => group("[Rover]", tags: ["rover"], () { }); Future testPath(AutonomyInterface simulator) async { - final destination = (5, 5).toGps(); + final destination = (lat: 5, long: 5).toGps(); final result = simulator.pathfinder.getPath(destination); expect(simulator.gps.latitude, 0); expect(simulator.gps.longitude, 0); @@ -68,20 +68,20 @@ Future testPath(AutonomyInterface simulator) async { 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); + await simulator.drive.driveState(transition); 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); + expect(simulator.imu.nearest, 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 destination = (lat: 4, long: 0).toGps(); + simulator.pathfinder.recordObstacle((lat: 2, long: 0).toGps()); + simulator.pathfinder.recordObstacle((lat: 4, long: -1).toGps()); + simulator.pathfinder.recordObstacle((lat: 4, long: 1).toGps()); final result = simulator.pathfinder.getPath(destination); expect(simulator.gps.latitude, 0); expect(simulator.gps.longitude, 0); @@ -91,10 +91,10 @@ Future testPath2(AutonomyInterface simulator) async { for (final transition in result) { simulator.logger.debug(transition.toString()); simulator.logger.trace(" From: ${simulator.gps.coordinates.prettyPrint()}"); - await simulator.drive.goDirection(transition.direction); + await simulator.drive.driveState(transition); expect(simulator.gps.isNear(transition.position), isTrue); expect(simulator.pathfinder.isObstacle(simulator.gps.coordinates), isFalse); - expect(simulator.imu.orientation, transition.orientation); + expect(simulator.imu.nearest, transition.orientation); simulator.logger.trace(" To: ${simulator.gps.coordinates.prettyPrint()}"); } } diff --git a/test/sensor_test.dart b/test/sensor_test.dart index 6375b9e..0f37ff8 100644 --- a/test/sensor_test.dart +++ b/test/sensor_test.dart @@ -1,5 +1,4 @@ -import "package:autonomy/src/rover/imu.dart"; -import "package:burt_network/logging.dart"; +import "package:autonomy/autonomy.dart"; import "package:test/test.dart"; import "package:burt_network/protobuf.dart"; @@ -16,13 +15,13 @@ void main() => group("[Sensors]", tags: ["sensors"], () { tearDown(() => Logger.level = LogLevel.off); test("GpsUtils.isNear", () { - final origin = (0, 0).toGps(); + final origin = (lat: 0, long: 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(); + final a = (lat: 5, long: 5).toGps(); + final a2 = (lat: 5, long: 5).toGps(); + final b = (lat: 5, long: 6.5).toGps(); expect(a.isNear(a), isTrue); expect(a.isNear(a2), isTrue); @@ -36,8 +35,8 @@ void main() => group("[Sensors]", tags: ["sensors"], () { expect(c.isNear(e), isFalse); expect(d.isNear(e), isFalse); - final f = (12, 12).toGps(); - final g = (12.2, 12.2).toGps(); + final f = (lat: 12, long: 12).toGps(); + final g = (lat: 12.2, long: 12.2).toGps(); expect(f.isNear(g), isTrue); }); @@ -82,8 +81,8 @@ void main() => group("[Sensors]", tags: ["sensors"], () { final simulator = AutonomySimulator(); final simulatedImu = ImuSimulator(collection: simulator, maxError: imuError); final realImu = RoverImu(collection: simulator); - final north = OrientationUtils.north; - simulatedImu.update(north); + final north = CardinalDirection.north; + simulatedImu.update(north.orientation); var count = 0; for (var i = 0; i < 1000; i++) { @@ -91,10 +90,10 @@ void main() => group("[Sensors]", tags: ["sensors"], () { 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}"); + simulator.logger.trace(" Real position: ${north.angle}"); if (i < 10) continue; - simulator.logger.trace(" Values are similar: ${realImu.isNear(north.heading)}"); - if (realImu.isNear(north.heading)) count++; + simulator.logger.trace(" Values are similar: ${realImu.isNear(CardinalDirection.north)}"); + if (realImu.isNear(north)) count++; } final percentage = count / 1000; @@ -102,12 +101,14 @@ void main() => group("[Sensors]", tags: ["sensors"], () { 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); + realImu.forceUpdate(OrientationUtils.south); + expect(realImu.isNear(north), isTrue); await simulator.dispose(); }); - test("GPS noise when moving", () async { + test("GPS noise when moving", + skip: "GPS noise is reduced enough with RTK where filtering is not necessary", + () async { // Set up a simulated and real GPS, both starting at (0, 0) final oldError = GpsUtils.maxErrorMeters; GpsUtils.maxErrorMeters = 3; @@ -116,7 +117,7 @@ void main() => group("[Sensors]", tags: ["sensors"], () { final simulatedGps = GpsSimulator(collection: simulator, maxError: GpsInterface.gpsError); var realCoordinates = GpsCoordinates(); simulatedGps.update(realCoordinates); - realGps.update(realCoordinates); + realGps.forceUpdate(realCoordinates); expect(realGps.coordinates.isNear(realCoordinates), isTrue); // For each step forward, use the noisy GPS to update the real GPS. @@ -124,7 +125,7 @@ void main() => group("[Sensors]", tags: ["sensors"], () { for (var step = 0; step < 1000; step++) { realCoordinates += GpsUtils.north; simulatedGps.update(realCoordinates); - realGps.update(simulatedGps.coordinates); + realGps.forceUpdate(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; @@ -142,12 +143,12 @@ void main() => group("[Sensors]", tags: ["sensors"], () { GpsUtils.maxErrorMeters = oldError; }); - test("IMU noise when moving", () async { + test("IMU noise when moving", skip: "IMU is currently accurate enough to not need filtering", () async { Logger.level = LogLevel.off; final simulator = AutonomySimulator(); final simulatedImu = ImuSimulator(collection: simulator, maxError: imuError); final realImu = RoverImu(collection: simulator); - final orientation = OrientationUtils.north; + final orientation = CardinalDirection.north.orientation; simulatedImu.update(orientation); var count = 0; @@ -155,13 +156,13 @@ void main() => group("[Sensors]", tags: ["sensors"], () { orientation.z += 1; simulatedImu.update(orientation); final newData = simulatedImu.raw; - realImu.update(newData); + realImu.forceUpdate(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++; + simulator.logger.trace(" Values are similar: ${realImu.isNear(CardinalDirection.north)}"); + if (realImu.isNear(CardinalDirection.north)) count++; } final percentage = count / 350; @@ -170,9 +171,9 @@ void main() => group("[Sensors]", tags: ["sensors"], () { final badData = Orientation(z: 10); simulator.logger.info("Final orientation: ${realImu.heading}"); simulator.logger.info("Bad orientation: ${badData.heading}"); - realImu.update(badData); + realImu.forceUpdate(badData); simulator.logger.info("Unaffected orientation: ${realImu.heading}"); - expect(realImu.isNear(orientation.heading), isTrue); + expect(realImu.isNear(CardinalDirection.north), isTrue); await simulator.dispose(); }); @@ -181,7 +182,7 @@ void main() => group("[Sensors]", tags: ["sensors"], () { const utahLatitude = 38.406683; final utah = GpsCoordinates(latitude: utahLatitude); - simulator.gps.update(utah); + simulator.gps.forceUpdate(utah); expect(simulator.hasValue, isFalse); expect(GpsInterface.currentLatitude, 0); @@ -205,7 +206,7 @@ void main() => group("[Sensors]", tags: ["sensors"], () { expect(simulator.gps.isNear(newYork), isFalse); expect(ocean.isNear(newYork), isFalse); - simulator.gps.update(newYork); + simulator.gps.forceUpdate(newYork); expect(simulator.gps.isNear(ocean), isFalse); expect(simulator.gps.isNear(newYork), isTrue); @@ -232,15 +233,15 @@ void main() => group("[Sensors]", tags: ["sensors"], () { 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(); - }); + // 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(); + // }); });