Vision task

autonomy/lib/constants.dart

@@ -48,6 +48,28 @@ class Constants {
   /// The maximum time to spend searching for an aruco tag
   static const Duration arucoSearchTimeout = Duration(seconds: 20);
 
+  /// The maximum time to spend searching for a vision object
+  static const Duration visionSearchTimeout = Duration(seconds: 20);
+
   /// The camera that should be used to detect Aruco tags
   static const CameraName arucoDetectionCamera = CameraName.ROVER_FRONT;
+
+  /// The size of the search area in meters for the lawnmower pattern
+  static const double searchAreaMeters = 20;
+
+  /// The width of each strip in the lawnmower pattern in meters
+  /// TODO: calibrate based on camera detection range testing in lab
+  static const double searchStripWidthMeters = 3;
+
+  /// How close (in degrees) the detected object yaw must be to center
+  static const double visionCenterYawEpsilon = 3;
+
+  /// How close (in pixels) the detected object center must be to frame center
+  static const int visionCenterPixelEpsilon = 20;
+
+  /// How close (normalized -1..1) the detected object x position must be
+  static const double visionCenterXPositionEpsilon = 0.1;
+
+  /// How many consecutive frames the target can be missing before we stop
+  static const int visionMissingFramesToArrive = 3;
 }

autonomy/lib/src/drive/drive_interface.dart

@@ -139,4 +139,10 @@ abstract class DriveInterface extends Service {
 
   /// Drive forward to approach an Aruco tag
   Future<void> approachAruco() async {}
+
+  /// Spin to face a detected object, returns whether or not it was found
+  Future<bool> spinForObject(
+    List<DetectedObjectType> types, {
+    CameraName? desiredCamera,
+  }) async => false;
 }

autonomy/lib/src/drive/rover_drive.dart

@@ -83,6 +83,12 @@ class RoverDrive extends DriveInterface {
   @override
   Future<void> approachAruco() => sensorDrive.approachAruco();
 
+  @override
+  Future<bool> spinForObject(
+    List<DetectedObjectType> types, {
+    CameraName? desiredCamera,
+  }) => sensorDrive.spinForObject(types, desiredCamera: desiredCamera);
+
   @override
   Future<bool> faceOrientation(Orientation orientation) async {
     if (useImu) {

autonomy/lib/src/drive/sensor_drive.dart

@@ -175,6 +175,36 @@ class SensorDrive extends DriveInterface with RoverDriveCommands {
     return foundAruco;
   }
 
+  @override
+  Future<bool> spinForObject(
+    List<DetectedObjectType> types, {
+    CameraName? desiredCamera,
+  }) async {
+    setThrottle(config.turnThrottle);
+    var foundObject = true;
+    foundObject =
+        await runFeedback(() {
+          if (!foundObject) return true;
+          spinLeft();
+          return types.any(
+            (type) =>
+                collection.video.getDetection(
+                  type,
+                  desiredCamera: desiredCamera,
+                ) !=
+                null,
+          );
+        }).timeout(
+          Constants.visionSearchTimeout,
+          onTimeout: () {
+            foundObject = false;
+            return false;
+          },
+        );
+    await stop();
+    return foundObject;
+  }
+
   @override
   Future<void> approachAruco() async {
     // const sizeThreshold = 0.2;

autonomy/lib/src/orchestrator/orchestrator_interface.dart

@@ -44,6 +44,12 @@ abstract class OrchestratorInterface extends Service {
         handleGpsTask(command);
       case AutonomyTask.VISUAL_MARKER:
         handleArucoTask(command);
+      case AutonomyTask.HAMMER_TARGET:
+        handleHammerTask(command);
+      case AutonomyTask.ROCK_HAMMER_TARGET:
+        handleHammerTask(command);
+      case AutonomyTask.BOTTLE_TARGET:
+        handleBottleTask(command);
     }
   }
 

autonomy/lib/src/orchestrator/rover_orchestrator.dart

@@ -1,8 +1,9 @@
+import "dart:async";
 import "dart:math";
 
 import "package:autonomy/constants.dart";
 import "package:autonomy/interfaces.dart";
-import "dart:async";
+import "package:autonomy/src/state_machine/rover_states/approach_visible_object.dart";
 
 import "package:coordinate_converter/coordinate_converter.dart";
 
@@ -198,6 +199,32 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter {
     return true;
   }
 
+  /// Generates GPS waypoints for a lawnmower search pattern centered on [center].
+  ///
+  /// Covers a [Constants.searchAreaMeters] x [Constants.searchAreaMeters] area,
+  /// sweeping back and forth in strips of [Constants.searchStripWidthMeters] width.
+  List<GpsCoordinates> generateLawnmowerWaypoints(GpsCoordinates center) {
+    final waypoints = <GpsCoordinates>[];
+    final centerUtm = center.toUTM();
+    const halfSize = Constants.searchAreaMeters / 2;
+    const stripWidth = Constants.searchStripWidthMeters;
+
+    var row = 0;
+    for (var y = -halfSize; y <= halfSize; y += stripWidth) {
+      final xStart = row.isEven ? -halfSize : halfSize;
+      final xEnd = row.isEven ? halfSize : -halfSize;
+      waypoints.add(
+        (centerUtm + UTMCoordinates(x: xStart, y: y, zoneNumber: 1)).toGps(),
+      );
+      waypoints.add(
+        (centerUtm + UTMCoordinates(x: xEnd, y: y, zoneNumber: 1)).toGps(),
+      );
+      row++;
+    }
+
+    return waypoints;
+  }
+
   @override
   void handleGpsTask(AutonomyCommand command) {
     final destination = command.destination;
@@ -291,7 +318,7 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter {
 
     currentState = AutonomyState.SEARCHING;
     collection.logger.info("Searching for ArUco tag");
-    final didSeeAruco = await collection.drive.spinForAruco(
+    await collection.drive.spinForAruco(
       command.arucoId,
       desiredCamera: Constants.arucoDetectionCamera,
     );
@@ -300,12 +327,37 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter {
       desiredCamera: Constants.arucoDetectionCamera,
     );
 
-    if (!didSeeAruco || detectedAruco == null) {
+    if (detectedAruco == null) {
+      collection.logger.info(
+        "ArUco not found after spin, starting lawnmower search",
+      );
+      final waypoints = generateLawnmowerWaypoints(collection.gps.coordinates);
+      for (final waypoint in waypoints) {
+        if (currentCommand == null) return;
+        await calculateAndFollowPath(
+          waypoint,
+          abortOnError: false,
+          alternateEndCondition: () =>
+              collection.video.getArucoDetection(
+                command.arucoId,
+                desiredCamera: Constants.arucoDetectionCamera,
+              ) !=
+              null,
+        );
+        detectedAruco = collection.video.getArucoDetection(
+          command.arucoId,
+          desiredCamera: Constants.arucoDetectionCamera,
+        );
+        if (detectedAruco != null) break;
+      }
+    }
+
+    if (detectedAruco == null) {
       collection.logger.error("Could not find desired Aruco tag");
       currentState = AutonomyState.NO_SOLUTION;
       currentCommand = null;
       return;
-    }
+    }    
 
     collection.logger.info("Found aruco");
     currentState = AutonomyState.APPROACHING;
@@ -425,8 +477,133 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter {
   }
 
   @override
-  Future<void> handleHammerTask(AutonomyCommand command) async {}
+  Future<void> handleHammerTask(AutonomyCommand command) async {
+    await _handleVisualObjectTask(
+      command: command,
+      targetTypes: [
+        DetectedObjectType.HAMMER,
+        DetectedObjectType.ROCK_HAMMER,
+      ],
+      label: "hammer",
+    );
+  }
 
   @override
-  Future<void> handleBottleTask(AutonomyCommand command) async {}
+  Future<void> handleBottleTask(AutonomyCommand command) async {
+    await _handleVisualObjectTask(
+      command: command,
+      targetTypes: [DetectedObjectType.BOTTLE],
+      label: "bottle",
+    );
+  }
+
+  Future<void> _handleVisualObjectTask({
+    required AutonomyCommand command,
+    required List<DetectedObjectType> targetTypes,
+    required String label,
+  }) async {
+    collection.drive.setLedStrip(ProtoColor.RED);
+
+    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 $label");
+    await collection.drive.spinForObject(
+      targetTypes,
+      desiredCamera: Constants.arucoDetectionCamera,
+    );
+    var detection = _firstTargetDetection(targetTypes);
+
+    if (detection == null) {
+      collection.logger.info(
+        "$label not found after spin, starting lawnmower search",
+      );
+      final waypoints = generateLawnmowerWaypoints(collection.gps.coordinates);
+      for (final waypoint in waypoints) {
+        if (currentCommand == null) return;
+        final reached = await calculateAndFollowPath(
+          waypoint,
+          abortOnError: false,
+          alternateEndCondition: () => _firstTargetDetection(targetTypes) != null,
+        );
+        detection = _firstTargetDetection(targetTypes);
+        if (detection != null) break;
+        if (!reached) {
+          await collection.drive.spinForObject(
+            targetTypes,
+            desiredCamera: Constants.arucoDetectionCamera,
+          );
+          detection = _firstTargetDetection(targetTypes);
+          if (detection != null) break;
+        }
+      }
+    }
+
+    if (detection == null) {
+      collection.logger.error("Could not find $label");
+      currentState = AutonomyState.NO_SOLUTION;
+      currentCommand = null;
+      return;
+    }
+
+    collection.logger.info("Found $label, approaching");
+    currentState = AutonomyState.APPROACHING;
+
+    controller.pushState(
+      ApproachVisibleObject(
+        controller,
+        collection: collection,
+        targetTypes: targetTypes,
+        desiredCamera: Constants.arucoDetectionCamera,
+      ),
+    );
+
+    executionTimer = PeriodicTimer(const Duration(milliseconds: 10), (
+      timer,
+    ) async {
+      if (currentCommand == null) {
+        onCommandEnd();
+        timer.cancel();
+        return;
+      }
+
+      if (!controller.hasState()) {
+        collection.logger.info("Reached target $label");
+        collection.drive.setLedStrip(ProtoColor.GREEN, blink: true);
+        currentState = AutonomyState.AT_DESTINATION;
+        onCommandEnd();
+        timer.cancel();
+        return;
+      }
+
+      controller.update();
+    });
+  }
+
+  DetectedObjectSnapshot? _firstTargetDetection(
+    List<DetectedObjectType> targetTypes,
+  ) {
+    for (final type in targetTypes) {
+      final detection = collection.video.getDetection(
+        type,
+        desiredCamera: Constants.arucoDetectionCamera,
+      );
+      if (detection != null) {
+        return detection;
+      }
+    }
+    return null;
+  }
 }