diff --git a/lib/src/interfaces/a_star.dart b/lib/src/interfaces/a_star.dart index e36d4bf..af7bae8 100644 --- a/lib/src/interfaces/a_star.dart +++ b/lib/src/interfaces/a_star.dart @@ -5,14 +5,14 @@ import "package:autonomy/interfaces.dart"; class AutonomyAStarState extends AStarState { final DriveDirection direction; - final DriveOrientation orientation; + final CardinalDirection orientation; final GpsCoordinates position; final GpsCoordinates goal; final AutonomyInterface collection; AutonomyAStarState({ - required this.position, - required this.goal, + required this.position, + required this.goal, required this.collection, required this.direction, required this.orientation, @@ -23,11 +23,11 @@ class AutonomyAStarState extends AStarState { required AutonomyInterface collection, required GpsCoordinates goal, }) => AutonomyAStarState( - position: collection.gps.coordinates, - goal: goal, - collection: collection, - direction: DriveDirection.stop, - orientation: collection.imu.orientation!, + position: collection.gps.coordinates, + goal: goal, + collection: collection, + direction: DriveDirection.stop, + orientation: collection.imu.orientation!, depth: 0, ); @@ -48,12 +48,12 @@ class AutonomyAStarState extends AStarState { @override bool isGoal() => position.isNear(goal); - AutonomyAStarState copyWith({required DriveDirection direction, required DriveOrientation orientation, required GpsCoordinates position}) => AutonomyAStarState( + AutonomyAStarState copyWith({required DriveDirection direction, required CardinalDirection orientation, required GpsCoordinates position}) => AutonomyAStarState( collection: collection, position: position, - orientation: orientation, + orientation: orientation, direction: direction, - goal: goal, + goal: goal, depth: direction == DriveDirection.forward ? depth + 1 : depth + 2, ); @@ -61,6 +61,6 @@ class AutonomyAStarState extends AStarState { 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), + 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 index d7dd8b2..15f7505 100644 --- a/lib/src/interfaces/drive.dart +++ b/lib/src/interfaces/drive.dart @@ -2,22 +2,22 @@ import "package:autonomy/interfaces.dart"; import "package:burt_network/generated.dart"; enum DriveDirection { - forward, + forward, left, right, stop, } -enum DriveOrientation { +enum CardinalDirection { north(0), west(90), south(180), east(270); final int angle; - const DriveOrientation(this.angle); + const CardinalDirection(this.angle); - static DriveOrientation? fromRaw(Orientation orientation) { + static CardinalDirection? fromRaw(Orientation orientation) { // TODO: Make this more precise. for (final value in values) { if (orientation.isNear(value.angle.toDouble())) return value; @@ -25,14 +25,14 @@ enum DriveOrientation { return null; } - DriveOrientation turnLeft() => switch (this) { + CardinalDirection turnLeft() => switch (this) { north => west, west => south, south => east, east => north, }; - DriveOrientation turnRight() => switch (this) { + CardinalDirection turnRight() => switch (this) { north => east, west => north, south => west, @@ -42,7 +42,7 @@ enum DriveOrientation { abstract class DriveInterface extends Service { AutonomyInterface collection; - DriveOrientation orientation = DriveOrientation.north; + CardinalDirection orientation = CardinalDirection.north; DriveInterface({required this.collection}); Future goDirection(DriveDirection direction) async => switch (direction) { @@ -53,13 +53,13 @@ abstract class DriveInterface extends Service { }; Future faceNorth(); - + Future goForward(); Future turnLeft(); Future turnRight(); Future stop(); - Future faceDirection(DriveOrientation orientation) async { + Future faceDirection(CardinalDirection orientation) async { this.orientation = orientation; } diff --git a/lib/src/interfaces/gps_utils.dart b/lib/src/interfaces/gps_utils.dart index a303a6a..e119491 100644 --- a/lib/src/interfaces/gps_utils.dart +++ b/lib/src/interfaces/gps_utils.dart @@ -10,16 +10,16 @@ extension RecordToGps on (num, num) { extension GpsUtils on GpsCoordinates { static double maxErrorMeters = 1; - static double get epsilonLatitude => maxErrorMeters * latitudePerMeter; - static double get epsilonLongitude => maxErrorMeters * longitudePerMeter; + static double get epsilonLatitude => maxErrorMeters * latitudePerMeter; + static double get epsilonLongitude => maxErrorMeters * longitudePerMeter; - static GpsCoordinates get east => + static GpsCoordinates get east => GpsCoordinates(longitude: -1 / metersPerLongitude); - static GpsCoordinates get west => + static GpsCoordinates get west => GpsCoordinates(longitude: 1 / metersPerLongitude); - static GpsCoordinates get north => + static GpsCoordinates get north => GpsCoordinates(latitude: 1 * latitudePerMeter); - static GpsCoordinates get south => + static GpsCoordinates get south => GpsCoordinates(latitude: -1 * latitudePerMeter); // Taken from https://stackoverflow.com/a/39540339/9392211 @@ -28,20 +28,20 @@ extension GpsUtils on GpsCoordinates { 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 + + double manhattanDistance(GpsCoordinates other) => + (latitude - other.latitude).abs() * metersPerLatitude + (longitude - other.longitude).abs() * metersPerLongitude; - bool isNear(GpsCoordinates other) => + bool isNear(GpsCoordinates other) => (latitude - other.latitude).abs() < epsilonLatitude && (longitude - other.longitude).abs() < epsilonLongitude; @@ -53,10 +53,10 @@ extension GpsUtils on GpsCoordinates { // 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, + GpsCoordinates goForward(CardinalDirection orientation) => this + switch(orientation) { + CardinalDirection.north => GpsUtils.north, + CardinalDirection.south => GpsUtils.south, + CardinalDirection.west => GpsUtils.west, + CardinalDirection.east => GpsUtils.east, }; } diff --git a/lib/src/interfaces/imu.dart b/lib/src/interfaces/imu.dart index 5bb6bba..30e1195 100644 --- a/lib/src/interfaces/imu.dart +++ b/lib/src/interfaces/imu.dart @@ -7,9 +7,9 @@ abstract class ImuInterface extends Service with Receiver { double get heading => raw.z; Orientation get raw; - DriveOrientation? get orientation { + CardinalDirection? get orientation { collection.logger.trace("Trying to find orientation at $heading"); - return DriveOrientation.fromRaw(raw); + return CardinalDirection.fromRaw(raw); } void update(Orientation newValue); bool isNear(double angle) => raw.isNear(angle); diff --git a/lib/src/rover/drive.dart b/lib/src/rover/drive.dart index 7cef09e..93972c1 100644 --- a/lib/src/rover/drive.dart +++ b/lib/src/rover/drive.dart @@ -47,7 +47,7 @@ class RoverDrive extends DriveInterface { @override - Future faceDirection(DriveOrientation orientation) async { + Future faceDirection(CardinalDirection orientation) async { if (useImu) { await sensorDrive.faceDirection(orientation); } else { diff --git a/lib/src/rover/drive/sensor.dart b/lib/src/rover/drive/sensor.dart index 7a6ea93..3da1a2a 100644 --- a/lib/src/rover/drive/sensor.dart +++ b/lib/src/rover/drive/sensor.dart @@ -55,7 +55,7 @@ class SensorDrive extends DriveInterface with RoverMotors { } @override - Future faceDirection(DriveOrientation orientation) async { + Future faceDirection(CardinalDirection orientation) async { collection.logger.info("Turning to face $orientation..."); setThrottle(turnThrottle); setSpeeds(left: -1, right: 1); diff --git a/lib/src/rover/sensorless.dart b/lib/src/rover/sensorless.dart index 2beab09..d065aa6 100644 --- a/lib/src/rover/sensorless.dart +++ b/lib/src/rover/sensorless.dart @@ -5,8 +5,8 @@ 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}) : + + SensorlessDrive({required super.collection, bool useGps = true, bool useImu = true}) : simulatedDrive = DriveSimulator(collection: collection), realDrive = RoverDrive(collection: collection, useGps: useGps, useImu: useImu); @@ -44,7 +44,7 @@ class SensorlessDrive extends DriveInterface { @override - Future faceDirection(DriveOrientation orientation) async { + Future faceDirection(CardinalDirection orientation) async { await simulatedDrive.faceDirection(orientation); await realDrive.faceDirection(orientation); await super.faceDirection(orientation); diff --git a/lib/src/simulator/pathfinding.dart b/lib/src/simulator/pathfinding.dart index 8c64b6a..17f0502 100644 --- a/lib/src/simulator/pathfinding.dart +++ b/lib/src/simulator/pathfinding.dart @@ -1,9 +1,9 @@ import "package:autonomy/interfaces.dart"; import "package:burt_network/generated.dart"; -class PathfindingSimulator extends PathfindingInterface { +class PathfindingSimulator extends PathfindingInterface { static int i = 0; - + PathfindingSimulator({required super.collection}); @override @@ -11,15 +11,15 @@ class PathfindingSimulator extends PathfindingInterface { @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), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 0).toGps(), orientation: CardinalDirection.north, direction: DriveDirection.right, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 0).toGps(), orientation: CardinalDirection.east, direction: DriveDirection.forward, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 1).toGps(), orientation: CardinalDirection.east, direction: DriveDirection.forward, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 2).toGps(), orientation: CardinalDirection.east, direction: DriveDirection.left, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 2).toGps(), orientation: CardinalDirection.north, direction: DriveDirection.forward, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (1, 2).toGps(), orientation: CardinalDirection.north, direction: DriveDirection.forward, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (2, 2).toGps(), orientation: CardinalDirection.north, direction: DriveDirection.left, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (2, 2).toGps(), orientation: CardinalDirection.west, direction: DriveDirection.forward, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (2, 1).toGps(), orientation: CardinalDirection.west, direction: DriveDirection.right, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (2, 1).toGps(), orientation: CardinalDirection.north, direction: DriveDirection.forward, collection: collection), ]; } diff --git a/pubspec.lock b/pubspec.lock index 0f65b3b..71f0a4b 100644 --- a/pubspec.lock +++ b/pubspec.lock @@ -57,7 +57,7 @@ packages: burt_network: dependency: "direct main" description: - path: "../burt_network" + path: "../../burt_network" relative: true source: path version: "2.0.0" @@ -217,10 +217,10 @@ packages: dependency: "direct main" description: name: meta - sha256: bdb68674043280c3428e9ec998512fb681678676b3c54e773629ffe74419f8c7 + sha256: e3641ec5d63ebf0d9b41bd43201a66e3fc79a65db5f61fc181f04cd27aab950c url: "https://pub.dev" source: hosted - version: "1.15.0" + version: "1.16.0" mime: dependency: transitive description: @@ -240,9 +240,11 @@ packages: opencv_ffi: dependency: "direct main" description: - path: "../opencv_ffi" - relative: true - source: path + path: "." + ref: HEAD + resolved-ref: fb721ce518faa62b470c73ae58edfe825a5f9052 + url: "https://github.com/BinghamtonRover/OpenCV-FFI.git" + source: git version: "1.2.0" package_config: dependency: transitive @@ -424,10 +426,10 @@ packages: dependency: transitive description: name: vm_service - sha256: "5c5f338a667b4c644744b661f309fb8080bb94b18a7e91ef1dbd343bed00ed6d" + sha256: f6be3ed8bd01289b34d679c2b62226f63c0e69f9fd2e50a6b3c1c729a961041b url: "https://pub.dev" source: hosted - version: "14.2.5" + version: "14.3.0" watcher: dependency: transitive description: @@ -440,10 +442,10 @@ packages: dependency: transitive description: name: web - sha256: d43c1d6b787bf0afad444700ae7f4db8827f701bc61c255ac8d328c6f4d52062 + sha256: cd3543bd5798f6ad290ea73d210f423502e71900302dde696f8bff84bf89a1cb url: "https://pub.dev" source: hosted - version: "1.0.0" + version: "1.1.0" web_socket: dependency: transitive description: diff --git a/test/network_test.dart b/test/network_test.dart index 8e482df..5108b1c 100644 --- a/test/network_test.dart +++ b/test/network_test.dart @@ -107,7 +107,7 @@ void main() => group("[Network]", tags: ["network"], () { await simulator.init(); await simulator.drive.faceNorth(); - expect(simulator.imu.orientation, DriveOrientation.north); + expect(simulator.imu.orientation, CardinalDirection.north); final origin = GpsCoordinates(latitude: 0, longitude: 0); final oneMeter = (1, 0).toGps();