Aruco Task Implementation

bin/autonomy.dart

@@ -1,6 +1,8 @@
 import "package:autonomy/rover.dart";
+import "package:burt_network/logging.dart";
 
 void main() async {
+  Logger.level = Level.debug;
   final rover = RoverAutonomy();
   await rover.init();
 }

lib/constants.dart

@@ -1,3 +1,5 @@
+import "package:autonomy/autonomy.dart";
+
 class Constants {
   /// The maximum error or "tolerance" for reaching the end goal
   static const double maxErrorMeters = 1;
@@ -23,5 +25,11 @@ class Constants {
   ///
   /// 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);
+  static const Duration driveGPSTimeout = Duration(seconds: 4, milliseconds: 500);
+
+  /// The maximum time to spend searching for an aruco tag
+  static const Duration arucoSearchTimeout = Duration(seconds: 20);
+
+  /// The camera that should be used to detect Aruco tags
+  static const CameraName arucoDetectionCamera = CameraName.ROVER_FRONT;
 }

lib/src/detector/detector_interface.dart

@@ -5,6 +5,5 @@ abstract class DetectorInterface extends Service {
   DetectorInterface({required this.collection});
 
   bool findObstacles();
-  bool canSeeAruco();
   bool isOnSlope();
 }

lib/src/detector/network_detector.dart

@@ -6,9 +6,6 @@ class NetworkDetector extends DetectorInterface {
 
   NetworkDetector({required super.collection});
 
-  @override
-  bool canSeeAruco() => false;
-
   @override
   Future<void> dispose() async {}
 

lib/src/detector/rover_detector.dart

@@ -9,10 +9,6 @@ class RoverDetector extends DetectorInterface {
   @override
   bool findObstacles() => false;
 
-  @override
-//  bool canSeeAruco() => collection.video.data.arucoDetected == BoolState.YES;
-  bool canSeeAruco() => collection.video.flag;
-
   @override
   Future<bool> init() async => true;
 

lib/src/detector/sim_detector.dart

@@ -37,9 +37,6 @@ class DetectorSimulator extends DetectorInterface {
     return result;
   }
 
-  @override
-  bool canSeeAruco() => false;  // if can see [arucoPosition]
-
   @override
   bool isOnSlope() => false;  // if on [slopedLatitude]
 }

lib/src/drive/drive_config.dart

@@ -42,7 +42,7 @@ class DriveConfig {
 const roverConfig = DriveConfig(
   forwardThrottle: 0.2,
   turnThrottle: 0.075,
-  oneMeterDelay: Duration(milliseconds: 5500),
+  oneMeterDelay: Duration(milliseconds: 5500 ~/ 2),
   turnDelay: Duration(milliseconds: 4500),
   subsystemsAddress: "192.168.1.20",
 );

lib/src/drive/drive_interface.dart

@@ -89,7 +89,7 @@ abstract class DriveInterface extends Service {
   }
 
   /// Spin to face an Aruco tag, returns whether or not it was able to face the tag
-  Future<bool> spinForAruco() async => false;
+  Future<bool> spinForAruco(int arucoId, {CameraName? desiredCamera}) async => false;
 
   /// Drive forward to approach an Aruco tag
   Future<void> approachAruco() async { }

lib/src/drive/rover_drive.dart

@@ -77,7 +77,10 @@ class RoverDrive extends DriveInterface {
   }
 
   @override
-  Future<bool> spinForAruco() => sensorDrive.spinForAruco();
+  Future<bool> spinForAruco(
+    int arucoId, {
+    CameraName? desiredCamera,
+  }) => sensorDrive.spinForAruco(arucoId, desiredCamera: desiredCamera);
 
   @override
   Future<void> approachAruco() => sensorDrive.approachAruco();

lib/src/drive/sensor_drive.dart

@@ -97,27 +97,38 @@ class SensorDrive extends DriveInterface with RoverDriveCommands {
   }
 
   @override
-  Future<bool> spinForAruco() async {
+  Future<bool> spinForAruco(
+    int arucoId, {
+    CameraName? desiredCamera,
+  }) async {
     setThrottle(config.turnThrottle);
-    spinLeft();
-    final result = await waitFor(() => collection.detector.canSeeAruco())
-      .then((_) => true)
-      .timeout(config.turnDelay * 4, onTimeout: () => false);
+    var foundAruco = true;
+    await waitFor(() {
+      if (!foundAruco) {
+        return true;
+      }
+      spinLeft();
+      return collection.video.getArucoDetection(arucoId, desiredCamera: desiredCamera) != null;
+    }).timeout(
+      Constants.arucoSearchTimeout,
+      onTimeout: () => foundAruco = false,
+    );
     await stop();
-    return result;
+    return foundAruco;
   }
 
   @override
   Future<void> 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);
+    // const sizeThreshold = 0.2;
+    // const epsilon = 0.00001;
+    // setThrottle(config.forwardThrottle);
+    // moveForward();
+    // await waitFor(() {
+    //   final size = collection.video.arucoSize;
+    //   collection.logger.trace("The Aruco tag is at $size percent");
+    //   return true;
+    //   return (size.abs() < epsilon && !collection.detector.canSeeAruco()) || size >= sizeThreshold;
+    // }).timeout(config.oneMeterDelay * 5);
     await stop();
   }
 }

lib/src/drive/sim_drive.dart

@@ -36,6 +36,9 @@ class DriveSimulator extends DriveInterface {
     return true;
   }
 
+  @override
+  Future<bool> spinForAruco(int arucoId, {CameraName? desiredCamera}) async => true;
+
   @override
   Future<bool> stop() async {
     collection.logger.debug("Stopping");

lib/src/orchestrator/rover_orchestrator.dart

@@ -35,33 +35,31 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter {
   @override
   Message getMessage() => statusMessage;
 
-  @override
-  Future<void> handleGpsTask(AutonomyCommand command) async {
-    final destination = command.destination;
-    collection.logger.info("Received GPS Task", body: "Go to ${destination.prettyPrint()}");
-    collection.logger.debug("Currently at ${collection.gps.coordinates.prettyPrint()}");
-    traversed.clear();
-    collection.drive.setLedStrip(ProtoColor.RED);
-    // detect obstacles before and after resolving orientation, as a "scan"
-    collection.detector.findObstacles();
+  Future<bool> calculateAndFollowPath(
+    GpsCoordinates goal, {
+    bool abortOnError = true,
+    bool Function()? alternateEndCondition,
+  }) async {
     await collection.drive.resolveOrientation();
     collection.detector.findObstacles();
-    while (!collection.gps.coordinates.isNear(destination)) {
+    while (!collection.gps.coordinates.isNear(goal) && !(alternateEndCondition?.call() ?? false)) {
       // Calculate a path
       collection.logger.debug("Finding a path");
       currentState = AutonomyState.PATHING;
-      final path = collection.pathfinder.getPath(destination);
+      final path = collection.pathfinder.getPath(goal);
       currentPath = path;  // also use local variable path for promotion
       if (path == null) {
         final current = collection.gps.coordinates;
-        collection.logger.error("Could not find a path", body: "No path found from ${current.prettyPrint()} to ${destination.prettyPrint()}");
-        currentState = AutonomyState.NO_SOLUTION;
-        currentCommand = null;
-        return;
+        collection.logger.error("Could not find a path", body: "No path found from ${current.prettyPrint()} to ${goal.prettyPrint()}");
+        if (abortOnError) {
+          currentState = AutonomyState.NO_SOLUTION;
+          currentCommand = null;
+        }
+        return false;
       }
       // Try to take that path
       final current = collection.gps.coordinates;
-      collection.logger.debug("Found a path from ${current.prettyPrint()} to ${destination.prettyPrint()}: ${path.length} steps");
+      collection.logger.debug("Found a path from ${current.prettyPrint()} to ${goal.prettyPrint()}: ${path.length} steps");
       collection.logger.debug("Here is a summary of the path");
       for (final step in path) {
         collection.logger.debug(step.toString());
@@ -70,6 +68,10 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter {
       var count = 0;
       for (final state in path) {
         collection.logger.debug(state.toString());
+        // Alternate end condition may have hit between steps
+        if (alternateEndCondition?.call() ?? false) {
+          break;
+        }
         // Replan if too far from start point
         final distanceError = collection.gps.coordinates.distanceTo(state.startPostition);
         if (distanceError >= Constants.replanErrorMeters) {
@@ -104,7 +106,7 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter {
         }
         if (currentCommand == null || currentPath == null) {
           collection.logger.info("Aborting path, command was canceled");
-          return;
+          return false;
         }
         traversed.add(state.position);
         // if (state.direction != DriveDirection.forward) continue;
@@ -116,6 +118,25 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter {
         }
       }
     }
+    return true;
+  }
+
+  @override
+  Future<void> handleGpsTask(AutonomyCommand command) async {
+    final destination = command.destination;
+    collection.logger.info("Received GPS Task", body: "Go to ${destination.prettyPrint()}");
+    collection.logger.debug("Currently at ${collection.gps.coordinates.prettyPrint()}");
+    traversed.clear();
+    collection.drive.setLedStrip(ProtoColor.RED);
+    // detect obstacles before and after resolving orientation, as a "scan"
+    collection.detector.findObstacles();
+    await collection.drive.resolveOrientation();
+    collection.detector.findObstacles();
+
+    if (!await calculateAndFollowPath(command.destination)) {
+      return;
+    }
+
     collection.logger.info("Task complete");
     collection.drive.setLedStrip(ProtoColor.GREEN, blink: true);
     currentState = AutonomyState.AT_DESTINATION;
@@ -127,19 +148,127 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter {
     collection.drive.setLedStrip(ProtoColor.RED);
 
     // Go to GPS coordinates
-    // await handleGpsTask(command);
     collection.logger.info("Got ArUco Task");
+    if (command.destination != GpsCoordinates(latitude: 0, longitude: 0)) {
+      if (!await calculateAndFollowPath(command.destination, abortOnError: false)) {
+        collection.logger.error("Failed to follow path towards initial destination");
+        currentState = AutonomyState.NO_SOLUTION;
+        currentCommand = null;
+        return;
+      }
+    }
 
     currentState = AutonomyState.SEARCHING;
     collection.logger.info("Searching for ArUco tag");
-    final didSeeAruco = await collection.drive.spinForAruco();
-    if (didSeeAruco) {
+    final didSeeAruco = await collection.drive.spinForAruco(
+      command.arucoId,
+      desiredCamera: Constants.arucoDetectionCamera,
+    );
+    var detectedAruco = collection.video.getArucoDetection(
+      command.arucoId,
+      desiredCamera: Constants.arucoDetectionCamera,
+    );
+
+    if (didSeeAruco && detectedAruco != null) {
       collection.logger.info("Found aruco");
       currentState = AutonomyState.APPROACHING;
-      await collection.drive.approachAruco();
+      final arucoOrientation = Orientation(z: collection.imu.heading - detectedAruco.yaw);
+      await collection.drive.faceOrientation(arucoOrientation);
+      detectedAruco = await collection.video.waitForAruco(
+        command.arucoId,
+        desiredCamera: Constants.arucoDetectionCamera,
+        timeout: const Duration(seconds: 3),
+      );
+
+      if (detectedAruco == null || !detectedAruco.hasBestPnpResult()) {
+        // TODO: handle this condition properly
+        collection.logger.error("Could not find desired Aruco tag");
+        return;
+      }
+
+      collection.logger.debug(
+        "Planning path to Aruco ID ${command.arucoId}",
+        body: "Detection: ${detectedAruco.toProto3Json()}",
+      );
+
+      // In theory we could just find the relative position with the translation x and z,
+      // however if the tag's rotation relative to itself is off (which can be common
+      // when facing it head on), then it will be extremely innacurate. Since the SolvePnP's
+      // distance is always extremely accurate, it is more reliable to use the distance
+      // hypotenuse to the camera combined with trig of the tag's angle relative to the camera.
+      final cameraToTag = detectedAruco.bestPnpResult.cameraToTarget;
+      final distanceToTag = sqrt(
+        pow(cameraToTag.translation.z, 2) + pow(cameraToTag.translation.x, 2),
+      ) - 1; // don't drive *into* the tag
+
+      if (distanceToTag < 1) {
+        // well that was easy
+        collection.drive.setLedStrip(ProtoColor.GREEN, blink: true);
+        currentState = AutonomyState.AT_DESTINATION;
+        return;
+      }
+
+      final relativeX = -distanceToTag * sin((collection.imu.heading - detectedAruco.yaw) * pi / 180);
+      final relativeY = distanceToTag * cos((collection.imu.heading - detectedAruco.yaw) * pi / 180);
+
+      final destinationCoordinates = (collection.gps.coordinates.inMeters + (lat: relativeY, long: relativeX)).toGps();
+
+      if (!await calculateAndFollowPath(
+        destinationCoordinates,
+        abortOnError: false,
+        alternateEndCondition: () {
+          detectedAruco = collection.video.getArucoDetection(
+            command.arucoId,
+            desiredCamera: Constants.arucoDetectionCamera,
+          );
+          if (detectedAruco == null) {
+            return false;
+          }
+          final cameraToTag = detectedAruco!.bestPnpResult.cameraToTarget;
+          final distanceToTag = sqrt(
+            pow(cameraToTag.translation.z, 2) +
+                pow(cameraToTag.translation.x, 2),
+          );
+          return distanceToTag < 1;
+        },
+      )) {
+        collection.logger.error("Could not spin towards ArUco tag");
+        currentCommand = null;
+        return;
+      }
+      collection.logger.info("Arrived at estimated Aruco position");
+      detectedAruco = collection.video.getArucoDetection(
+        command.arucoId,
+        desiredCamera: Constants.arucoDetectionCamera,
+      );
+      if (detectedAruco == null) {
+        collection.logger.info("Re-spinning to find Aruco");
+        await collection.drive.spinForAruco(
+          command.arucoId,
+          desiredCamera: Constants.arucoDetectionCamera,
+        );
+      }
+
+      detectedAruco = collection.video.getArucoDetection(
+        command.arucoId,
+        desiredCamera: Constants.arucoDetectionCamera,
+      );
+      if (detectedAruco != null) {
+        collection.logger.info("Rotating towards Aruco");
+        await collection.drive.faceOrientation(
+          Orientation(
+            z: collection.imu.heading - detectedAruco!.yaw,
+          ),
+        );
+      } else {
+        collection.logger.warning("Could not find Aruco after following path");
+      }
+
+      collection.logger.info("Successfully reached within ${Constants.maxErrorMeters} meters of the Aruco tag");
       collection.drive.setLedStrip(ProtoColor.GREEN, blink: true);
       currentState = AutonomyState.AT_DESTINATION;
     }
+    currentCommand = null;
   }
 
   @override