Optimize turning behavior

lib/src/interfaces/a_star.dart

@@ -5,14 +5,14 @@ import "package:autonomy/interfaces.dart";
 
 class AutonomyAStarState extends AStarState<AutonomyAStarState> {
   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<AutonomyAStarState> {
     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,19 +48,19 @@ class AutonomyAStarState extends AStarState<AutonomyAStarState> {
   @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,
   );
 
   @override
   Iterable<AutonomyAStarState> 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));
 }

lib/src/interfaces/drive.dart

@@ -2,37 +2,37 @@ 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;
     }
     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<void> goDirection(DriveDirection direction) async => switch (direction) {
@@ -53,13 +53,13 @@ abstract class DriveInterface extends Service {
   };
 
   Future<void> faceNorth();
-  
+
   Future<void> goForward();
   Future<void> turnLeft();
   Future<void> turnRight();
   Future<void> stop();
 
-  Future<void> faceDirection(DriveOrientation orientation) async {
+  Future<void> faceDirection(CardinalDirection orientation) async {
     this.orientation = orientation;
   }
 

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,
   };
 }

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);

lib/src/rover/drive.dart

@@ -47,7 +47,7 @@ class RoverDrive extends DriveInterface {
 
 
   @override
-  Future<void> faceDirection(DriveOrientation orientation) async {
+  Future<void> faceDirection(CardinalDirection orientation) async {
     if (useImu) {
       await sensorDrive.faceDirection(orientation);
     } else {

lib/src/rover/drive/sensor.dart

@@ -55,7 +55,7 @@ class SensorDrive extends DriveInterface with RoverMotors {
   }
 
   @override
-  Future<void> faceDirection(DriveOrientation orientation) async {
+  Future<void> faceDirection(CardinalDirection orientation) async {
     collection.logger.info("Turning to face $orientation...");
     setThrottle(turnThrottle);
     setSpeeds(left: -1, right: 1);

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<void> faceDirection(DriveOrientation orientation) async {
+  Future<void> faceDirection(CardinalDirection orientation) async {
     await simulatedDrive.faceDirection(orientation);
     await realDrive.faceDirection(orientation);
     await super.faceDirection(orientation);

lib/src/simulator/pathfinding.dart

@@ -1,25 +1,25 @@
 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
   Future<bool> init() async => true;
 
   @override
   List<AutonomyAStarState> 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),
   ];
 }

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:

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();