From 6cd9248220a90d9fcc20a3fc0c340111715e83f1 Mon Sep 17 00:00:00 2001 From: logan Date: Sat, 14 Feb 2026 19:11:52 -0500 Subject: [PATCH 1/6] Resolve merge conflict in rover_orchestrator.dart --- autonomy/lib/constants.dart | 12 ++ .../orchestrator/orchestrator_interface.dart | 6 + .../src/orchestrator/rover_orchestrator.dart | 126 +++++++++++++++++- .../rover_states/approach_visible_object.dart | 110 +++++++++++++++ .../rover_states/spin_for_object.dart | 71 ++++++++++ autonomy/lib/src/video/rover_video.dart | 27 +++- autonomy/lib/src/video/video_interface.dart | 14 ++ .../lib/src/generated/autonomy.pbenum.dart | 11 +- .../lib/src/generated/autonomy.pbjson.dart | 6 +- .../lib/src/generated/vision.pbenum.dart | 5 +- .../lib/src/generated/vision.pbjson.dart | 3 +- vision/lib/analyzer.py | 89 +++++++++---- vision/lib/analyzer_server.py | 8 +- vision/lib/network | 2 +- 14 files changed, 452 insertions(+), 38 deletions(-) create mode 100644 autonomy/lib/src/state_machine/rover_states/approach_visible_object.dart create mode 100644 autonomy/lib/src/state_machine/rover_states/spin_for_object.dart diff --git a/autonomy/lib/constants.dart b/autonomy/lib/constants.dart index b6e121fc..7abbb962 100644 --- a/autonomy/lib/constants.dart +++ b/autonomy/lib/constants.dart @@ -50,4 +50,16 @@ class Constants { /// The camera that should be used to detect Aruco tags static const CameraName arucoDetectionCamera = CameraName.ROVER_FRONT; + + /// How close (in degrees) the detected object yaw must be to center + static const double visionCenterYawEpsilon = 3.0; + + /// 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; } diff --git a/autonomy/lib/src/orchestrator/orchestrator_interface.dart b/autonomy/lib/src/orchestrator/orchestrator_interface.dart index 1c2a2918..3780a70e 100644 --- a/autonomy/lib/src/orchestrator/orchestrator_interface.dart +++ b/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); } } diff --git a/autonomy/lib/src/orchestrator/rover_orchestrator.dart b/autonomy/lib/src/orchestrator/rover_orchestrator.dart index 8e05835d..00df6e9e 100644 --- a/autonomy/lib/src/orchestrator/rover_orchestrator.dart +++ b/autonomy/lib/src/orchestrator/rover_orchestrator.dart @@ -1,8 +1,10 @@ +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:autonomy/src/state_machine/rover_states/spin_for_object.dart"; import "package:coordinate_converter/coordinate_converter.dart"; @@ -425,8 +427,126 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { } @override - Future handleHammerTask(AutonomyCommand command) async {} + Future handleHammerTask(AutonomyCommand command) async { + await _handleVisualObjectTask( + targetTypes: [ + DetectedObjectType.HAMMER, + DetectedObjectType.ROCK_HAMMER, + ], + label: "hammer", + ); + } @override - Future handleBottleTask(AutonomyCommand command) async {} + Future handleBottleTask(AutonomyCommand command) async { + await _handleVisualObjectTask( + targetTypes: [DetectedObjectType.BOTTLE], + label: "bottle", + ); + } + + Future _handleVisualObjectTask({ + required List targetTypes, + required String label, + }) async { + collection.drive.setLedStrip(ProtoColor.RED); + + var searchFailed = false; + + final objectTaskSequence = SequenceState( + controller, + steps: [ + FunctionalState( + controller, + onEnter: (controller) { + currentState = AutonomyState.SEARCHING; + controller.popState(); + }, + ), + SpinForObject( + controller, + collection: collection, + targetTypes: targetTypes, + desiredCamera: Constants.arucoDetectionCamera, + ), + FunctionalState( + controller, + onEnter: (controller) { + final detection = _firstTargetDetection(targetTypes); + if (detection != null) { + currentState = AutonomyState.APPROACHING; + controller.popState(); + return; + } + currentState = AutonomyState.NO_SOLUTION; + searchFailed = true; + controller.popState(); + }, + ), + ApproachVisibleObject( + controller, + collection: collection, + targetTypes: targetTypes, + desiredCamera: Constants.arucoDetectionCamera, + ), + FunctionalState( + controller, + onEnter: (controller) { + collection.logger.info("Reached target $label"); + currentState = AutonomyState.AT_DESTINATION; + collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); + controller.popState(); + }, + ), + ], + ); + + controller.pushState(objectTaskSequence); + + executionTimer = PeriodicTimer(const Duration(milliseconds: 10), ( + timer, + ) async { + if (currentCommand == null) { + collection.logger.warning( + "Execution timer running while command is null", + body: "Canceling timer", + ); + onCommandEnd(); + timer.cancel(); + return; + } + + if (searchFailed) { + collection.logger.error("Failed to find target $label"); + currentState = AutonomyState.NO_SOLUTION; + onCommandEnd(); + timer.cancel(); + return; + } + + if (!controller.hasState()) { + currentState = AutonomyState.NO_SOLUTION; + onCommandEnd(); + timer.cancel(); + return; + } + + controller.update(); + }); + } + + DetectedObjectSnapshot? _firstTargetDetection( + List targetTypes, + ) { + for (final type in targetTypes) { + final detection = collection.video.getDetection( + type, + desiredCamera: Constants.arucoDetectionCamera, + ); + if (detection != null) { + return detection; + } + } + return null; + } } diff --git a/autonomy/lib/src/state_machine/rover_states/approach_visible_object.dart b/autonomy/lib/src/state_machine/rover_states/approach_visible_object.dart new file mode 100644 index 00000000..38df312c --- /dev/null +++ b/autonomy/lib/src/state_machine/rover_states/approach_visible_object.dart @@ -0,0 +1,110 @@ +import "package:autonomy/constants.dart"; +import "package:autonomy/interfaces.dart"; +import "package:autonomy/rover.dart"; +import "package:autonomy/src/drive/drive_config.dart"; + +class ApproachVisibleObject extends RoverState { + final AutonomyInterface collection; + final List targetTypes; + final CameraName? desiredCamera; + final int? arucoId; + + DriveConfig get config => collection.drive.config; + + int _missingFrames = 0; + + ApproachVisibleObject( + super.controller, { + required this.collection, + required List targetTypes, + this.desiredCamera, + this.arucoId, + }) : targetTypes = targetTypes.isEmpty + ? [DetectedObjectType.DETECTION_TYPE_UNDEFINED] + : targetTypes; + + @override + void update() { + if (collection.drive is! RoverDrive) { + controller.popState(); + return; + } + + final drive = (collection.drive as RoverDrive).sensorDrive; + + DetectedObjectSnapshot? detection; + for (final type in targetTypes) { + detection = collection.video.getDetection( + type, + desiredCamera: desiredCamera, + arucoId: arucoId, + ); + if (detection != null) { + break; + } + } + + if (detection == null) { + _missingFrames += 1; + drive.stopMotors(); + if (_missingFrames >= Constants.visionMissingFramesToArrive) { + controller.popState(); + } + return; + } + + _missingFrames = 0; + + final object = detection.object; + final xPositionError = object.hasXPosition() ? object.xPosition : null; + final yawError = object.hasYaw() ? object.yaw : null; + + double? pixelError; + if (yawError == null && object.hasCenterX()) { + final width = detection.details.hasStreamWidth() + ? detection.details.streamWidth + : detection.details.resolutionWidth; + if (width > 0) { + pixelError = object.centerX - (width / 2.0); + } + } + + if (xPositionError == null && yawError == null && pixelError == null) { + collection.logger.warning( + "Detected object lacks centering data", + body: "Cannot determine yaw or center for ${object.objectType}", + ); + drive.stopMotors(); + return; + } + + final isCentered = xPositionError != null + ? xPositionError.abs() <= Constants.visionCenterXPositionEpsilon + : (yawError != null + ? yawError.abs() <= Constants.visionCenterYawEpsilon + : pixelError!.abs() <= Constants.visionCenterPixelEpsilon); + + if (!isCentered) { + drive.setThrottle(config.turnThrottle); + final error = xPositionError ?? yawError ?? pixelError!; + if (error > 0) { + drive.spinRight(); + } else { + drive.spinLeft(); + } + return; + } + + drive.setThrottle(config.forwardThrottle); + drive.moveForward(); + } + + @override + void exit() { + if (collection.drive is! RoverDrive) { + return; + } + final drive = (collection.drive as RoverDrive).sensorDrive; + drive.stopMotors(); + } +} diff --git a/autonomy/lib/src/state_machine/rover_states/spin_for_object.dart b/autonomy/lib/src/state_machine/rover_states/spin_for_object.dart new file mode 100644 index 00000000..24b3fca2 --- /dev/null +++ b/autonomy/lib/src/state_machine/rover_states/spin_for_object.dart @@ -0,0 +1,71 @@ +import "package:autonomy/constants.dart"; +import "package:autonomy/interfaces.dart"; +import "package:autonomy/rover.dart"; +import "package:autonomy/src/drive/drive_config.dart"; + +class SpinForObject extends RoverState { + final AutonomyInterface collection; + final List targetTypes; + final CameraName? desiredCamera; + + DriveConfig get config => collection.drive.config; + + double _startOrientation = 0; + bool _rotated180 = false; + + SpinForObject( + super.controller, { + required this.collection, + required this.targetTypes, + this.desiredCamera, + }); + + @override + void enter() { + _startOrientation = collection.imu.heading; + _rotated180 = false; + } + + @override + void update() { + if (collection.drive is! RoverDrive) { + controller.popState(); + return; + } + + final drive = (collection.drive as RoverDrive).sensorDrive; + drive.setThrottle(config.turnThrottle); + + for (final type in targetTypes) { + final detection = collection.video.getDetection( + type, + desiredCamera: desiredCamera, + ); + if (detection != null) { + controller.popState(); + return; + } + } + + drive.spinLeft(); + + if ((collection.imu.heading - _startOrientation).clampHalfAngle().abs() > + 175) { + _rotated180 = true; + } + + if (_rotated180 && + collection.imu.isNear(Orientation(z: _startOrientation))) { + controller.popState(); + } + } + + @override + void exit() { + if (collection.drive is! RoverDrive) { + return; + } + final drive = (collection.drive as RoverDrive).sensorDrive; + drive.stopMotors(); + } +} diff --git a/autonomy/lib/src/video/rover_video.dart b/autonomy/lib/src/video/rover_video.dart index e3363e44..2b2c77a1 100644 --- a/autonomy/lib/src/video/rover_video.dart +++ b/autonomy/lib/src/video/rover_video.dart @@ -21,20 +21,39 @@ class RoverVideo extends VideoInterface { } @override - DetectedObject? getArucoDetection(int id, {CameraName? desiredCamera}) { + DetectedObjectSnapshot? getDetection( + DetectedObjectType type, { + CameraName? desiredCamera, + int? arucoId, + }) { for (final result in _cachedResults.where( (e) => e.details.name == (desiredCamera ?? e.details.name), )) { for (final object in result.detectedObjects) { - if (object.objectType == DetectedObjectType.ARUCO && - object.arucoTagId == id) { - return object; + if (object.objectType != type) { + continue; } + if (type == DetectedObjectType.ARUCO && + arucoId != null && + object.arucoTagId != arucoId) { + continue; + } + return DetectedObjectSnapshot(object: object, details: result.details); } } return null; } + @override + DetectedObject? getArucoDetection(int id, {CameraName? desiredCamera}) { + return getDetection( + DetectedObjectType.ARUCO, + desiredCamera: desiredCamera, + arucoId: id, + ) + ?.object; + } + @override Future waitForAruco( int id, { diff --git a/autonomy/lib/src/video/video_interface.dart b/autonomy/lib/src/video/video_interface.dart index 4344054d..46d0a853 100644 --- a/autonomy/lib/src/video/video_interface.dart +++ b/autonomy/lib/src/video/video_interface.dart @@ -3,6 +3,13 @@ import "dart:async"; import "package:autonomy/constants.dart"; import "package:autonomy/interfaces.dart"; +class DetectedObjectSnapshot { + final DetectedObject object; + final CameraDetails details; + + const DetectedObjectSnapshot({required this.object, required this.details}); +} + /// Handles obstacle detection data and ArUco data from video abstract class VideoInterface extends Service with Receiver { final AutonomyInterface collection; @@ -27,6 +34,13 @@ abstract class VideoInterface extends Service with Receiver { void updateFrame(VideoData result); + DetectedObjectSnapshot? getDetection( + DetectedObjectType type, { + CameraName? desiredCamera, + int? arucoId, + }) => + null; + DetectedObject? getArucoDetection(int id, {CameraName? desiredCamera}) => null; diff --git a/burt_network/lib/src/generated/autonomy.pbenum.dart b/burt_network/lib/src/generated/autonomy.pbenum.dart index 37517594..e43940f5 100644 --- a/burt_network/lib/src/generated/autonomy.pbenum.dart +++ b/burt_network/lib/src/generated/autonomy.pbenum.dart @@ -60,16 +60,25 @@ class AutonomyTask extends $pb.ProtobufEnum { AutonomyTask._(2, _omitEnumNames ? '' : 'VISUAL_MARKER'); static const AutonomyTask BETWEEN_GATES = AutonomyTask._(3, _omitEnumNames ? '' : 'BETWEEN_GATES'); + static const AutonomyTask HAMMER_TARGET = + AutonomyTask._(4, _omitEnumNames ? '' : 'HAMMER_TARGET'); + static const AutonomyTask ROCK_HAMMER_TARGET = + AutonomyTask._(5, _omitEnumNames ? '' : 'ROCK_HAMMER_TARGET'); + static const AutonomyTask BOTTLE_TARGET = + AutonomyTask._(6, _omitEnumNames ? '' : 'BOTTLE_TARGET'); static const $core.List values = [ AUTONOMY_TASK_UNDEFINED, GPS_ONLY, VISUAL_MARKER, BETWEEN_GATES, + HAMMER_TARGET, + ROCK_HAMMER_TARGET, + BOTTLE_TARGET, ]; static final $core.List _byValue = - $pb.ProtobufEnum.$_initByValueList(values, 3); + $pb.ProtobufEnum.$_initByValueList(values, 6); static AutonomyTask? valueOf($core.int value) => value < 0 || value >= _byValue.length ? null : _byValue[value]; diff --git a/burt_network/lib/src/generated/autonomy.pbjson.dart b/burt_network/lib/src/generated/autonomy.pbjson.dart index 641c8cb6..d9cbd8ed 100644 --- a/burt_network/lib/src/generated/autonomy.pbjson.dart +++ b/burt_network/lib/src/generated/autonomy.pbjson.dart @@ -44,13 +44,17 @@ const AutonomyTask$json = { {'1': 'GPS_ONLY', '2': 1}, {'1': 'VISUAL_MARKER', '2': 2}, {'1': 'BETWEEN_GATES', '2': 3}, + {'1': 'HAMMER_TARGET', '2': 4}, + {'1': 'ROCK_HAMMER_TARGET', '2': 5}, + {'1': 'BOTTLE_TARGET', '2': 6}, ], }; /// Descriptor for `AutonomyTask`. Decode as a `google.protobuf.EnumDescriptorProto`. final $typed_data.Uint8List autonomyTaskDescriptor = $convert.base64Decode( 'CgxBdXRvbm9teVRhc2sSGwoXQVVUT05PTVlfVEFTS19VTkRFRklORUQQABIMCghHUFNfT05MWR' - 'ABEhEKDVZJU1VBTF9NQVJLRVIQAhIRCg1CRVRXRUVOX0dBVEVTEAM='); + 'ABEhEKDVZJU1VBTF9NQVJLRVIQAhIRCg1CRVRXRUVOX0dBVEVTEAMSEQoNSEFNTUVSX1RBUkdF' + 'VBAEEhYKElJPQ0tfSEFNTUVSX1RBUkdFVBAFEhEKDUJPVFRMRV9UQVJHRVQQBg=='); @$core.Deprecated('Use autonomyDataDescriptor instead') const AutonomyData$json = { diff --git a/burt_network/lib/src/generated/vision.pbenum.dart b/burt_network/lib/src/generated/vision.pbenum.dart index 4e27f90f..1a50687c 100644 --- a/burt_network/lib/src/generated/vision.pbenum.dart +++ b/burt_network/lib/src/generated/vision.pbenum.dart @@ -23,16 +23,19 @@ class DetectedObjectType extends $pb.ProtobufEnum { DetectedObjectType._(2, _omitEnumNames ? '' : 'HAMMER'); static const DetectedObjectType BOTTLE = DetectedObjectType._(3, _omitEnumNames ? '' : 'BOTTLE'); + static const DetectedObjectType ROCK_HAMMER = + DetectedObjectType._(4, _omitEnumNames ? '' : 'ROCK_HAMMER'); static const $core.List values = [ DETECTION_TYPE_UNDEFINED, ARUCO, HAMMER, BOTTLE, + ROCK_HAMMER, ]; static final $core.List _byValue = - $pb.ProtobufEnum.$_initByValueList(values, 3); + $pb.ProtobufEnum.$_initByValueList(values, 4); static DetectedObjectType? valueOf($core.int value) => value < 0 || value >= _byValue.length ? null : _byValue[value]; diff --git a/burt_network/lib/src/generated/vision.pbjson.dart b/burt_network/lib/src/generated/vision.pbjson.dart index 62b88933..1d2ca60c 100644 --- a/burt_network/lib/src/generated/vision.pbjson.dart +++ b/burt_network/lib/src/generated/vision.pbjson.dart @@ -23,13 +23,14 @@ const DetectedObjectType$json = { {'1': 'ARUCO', '2': 1}, {'1': 'HAMMER', '2': 2}, {'1': 'BOTTLE', '2': 3}, + {'1': 'ROCK_HAMMER', '2': 4}, ], }; /// Descriptor for `DetectedObjectType`. Decode as a `google.protobuf.EnumDescriptorProto`. final $typed_data.Uint8List detectedObjectTypeDescriptor = $convert.base64Decode( 'ChJEZXRlY3RlZE9iamVjdFR5cGUSHAoYREVURUNUSU9OX1RZUEVfVU5ERUZJTkVEEAASCQoFQV' - 'JVQ08QARIKCgZIQU1NRVIQAhIKCgZCT1RUTEUQAw=='); + 'JVQ08QARIKCgZIQU1NRVIQAhIKCgZCT1RUTEUQAxIPCgtST0NLX0hBTU1FUhAE'); @$core.Deprecated('Use pnpResultDescriptor instead') const PnpResult$json = { diff --git a/vision/lib/analyzer.py b/vision/lib/analyzer.py index 50d02bdb..7fcf9eae 100644 --- a/vision/lib/analyzer.py +++ b/vision/lib/analyzer.py @@ -2,6 +2,8 @@ from ultralytics import YOLO import torch +from lib.network.src.generated import vision_pb2 + class ImageAnalyzer: def __init__(self, load_from="trained-model.pt"): @@ -15,32 +17,71 @@ def __init__(self, load_from="trained-model.pt"): self.model = YOLO(load_from, task="detect") def has_mallet(self, frame: cv2.Mat, confidence=0.80) -> tuple[bool, float]: - results = self.model.predict(frame, stream=True, verbose=False) - for result in results: - json = result.summary() - if not json: - return False, 0.0 - object = json[0] - obj_confidence = object["confidence"] - is_mallet = ("mallet" in object["name"].lower()) and obj_confidence > confidence - if is_mallet: - return True, obj_confidence - else: - return False, obj_confidence - else: - return False, 0.0 + annotated, detections = self.analyze_frame(frame, confidence=confidence) + for detection in detections: + if detection.object_type == vision_pb2.HAMMER: + return True, confidence + return False, 0.0 def annotateImage(self, frame: cv2.Mat, confidence=0.80) -> cv2.Mat: - results = self.model.predict(frame, stream=True, verbose=False) - for result in results: - json = result.summary() - if not json: - return frame - object = json[0] - is_mallet = object["name"] == "mallet" and object["confidence"] > confidence - if is_mallet: - return result.plot() - return frame + annotated, _ = self.analyze_frame(frame, confidence=confidence) + return annotated + + def analyze_frame(self, frame: cv2.Mat, confidence=0.80): + results = self.model.predict(frame, verbose=False) + if not results: + return frame, [] + + result = results[0] + detections = [] + + boxes = getattr(result, "boxes", None) + if boxes is None or boxes.xyxy is None: + return frame, [] + + names = self.model.names + height, width = frame.shape[:2] + + for idx in range(len(boxes)): + conf = float(boxes.conf[idx]) + if conf < confidence: + continue + + cls_id = int(boxes.cls[idx]) + label = str(names.get(cls_id, "")).lower() + object_type = self._map_label_to_object_type(label) + if object_type is None: + continue + + x1, y1, x2, y2 = boxes.xyxy[idx].tolist() + center_x = int((x1 + x2) / 2.0) + center_y = int((y1 + y2) / 2.0) + area = max(0.0, (x2 - x1) * (y2 - y1)) + + x_position = 0.0 + if width > 0: + x_position = (center_x / width) * 2.0 - 1.0 + + detection = vision_pb2.DetectedObject( + object_type=object_type, + x_position=x_position, + relative_size=(area / (width * height)) if width > 0 and height > 0 else 0.0, + center_x=center_x, + center_y=center_y, + ) + detections.append(detection) + + annotated = result.plot() if hasattr(result, "plot") else frame + return annotated, detections + + def _map_label_to_object_type(self, label: str): + if "hammer" in label and "rock" in label: + return vision_pb2.ROCK_HAMMER + if "hammer" in label or "mallet" in label: + return vision_pb2.HAMMER + if "bottle" in label: + return vision_pb2.BOTTLE + return None def getFrames(self, cam, show=False): ret, frame = cam.read() diff --git a/vision/lib/analyzer_server.py b/vision/lib/analyzer_server.py index 0a2ac1ca..c0db2d7c 100644 --- a/vision/lib/analyzer_server.py +++ b/vision/lib/analyzer_server.py @@ -35,7 +35,7 @@ def handle_frame(self, data): if self.show == "before": cv2.imshow("Rover Vision Server (before annotation)", matrix) - frame = self.analyzer.annotateImage(matrix) + frame, detections = self.analyzer.analyze_frame(matrix) if self.show == "after": cv2.imshow("Rover Vision Server (after annotation)", frame) @@ -45,7 +45,11 @@ def handle_frame(self, data): ) if not success: return - data = VideoData(frame=bytes(jpg), details=data.details) + data = VideoData( + frame=bytes(jpg), + details=data.details, + detected_objects=detections, + ) jpg_size = len(jpg) if jpg_size > self.max_packet_size: diff --git a/vision/lib/network b/vision/lib/network index d717de8f..91a1550b 160000 --- a/vision/lib/network +++ b/vision/lib/network @@ -1 +1 @@ -Subproject commit d717de8f16deb08058b962b3d04f7fce41004edf +Subproject commit 91a1550b9acf985548385e4f1ad05fbf2a873eae From cd13f10c09ab11a902cd580a9a15c84b68d0209e Mon Sep 17 00:00:00 2001 From: logan Date: Sat, 14 Feb 2026 19:11:52 -0500 Subject: [PATCH 2/6] Update submodule vision/lib/network to latest vision-task --- autonomy/lib/constants.dart | 2 +- .../rover_states/spin_for_object.dart | 1 - autonomy/lib/src/video/rover_video.dart | 15 +++++++-------- vision/lib/analyzer.py | 2 +- 4 files changed, 9 insertions(+), 11 deletions(-) diff --git a/autonomy/lib/constants.dart b/autonomy/lib/constants.dart index 7abbb962..186b6c46 100644 --- a/autonomy/lib/constants.dart +++ b/autonomy/lib/constants.dart @@ -52,7 +52,7 @@ class Constants { static const CameraName arucoDetectionCamera = CameraName.ROVER_FRONT; /// How close (in degrees) the detected object yaw must be to center - static const double visionCenterYawEpsilon = 3.0; + static const double visionCenterYawEpsilon = 3; /// How close (in pixels) the detected object center must be to frame center static const int visionCenterPixelEpsilon = 20; diff --git a/autonomy/lib/src/state_machine/rover_states/spin_for_object.dart b/autonomy/lib/src/state_machine/rover_states/spin_for_object.dart index 24b3fca2..697ce4c6 100644 --- a/autonomy/lib/src/state_machine/rover_states/spin_for_object.dart +++ b/autonomy/lib/src/state_machine/rover_states/spin_for_object.dart @@ -1,4 +1,3 @@ -import "package:autonomy/constants.dart"; import "package:autonomy/interfaces.dart"; import "package:autonomy/rover.dart"; import "package:autonomy/src/drive/drive_config.dart"; diff --git a/autonomy/lib/src/video/rover_video.dart b/autonomy/lib/src/video/rover_video.dart index 2b2c77a1..5006ede8 100644 --- a/autonomy/lib/src/video/rover_video.dart +++ b/autonomy/lib/src/video/rover_video.dart @@ -45,14 +45,13 @@ class RoverVideo extends VideoInterface { } @override - DetectedObject? getArucoDetection(int id, {CameraName? desiredCamera}) { - return getDetection( - DetectedObjectType.ARUCO, - desiredCamera: desiredCamera, - arucoId: id, - ) - ?.object; - } + DetectedObject? getArucoDetection(int id, {CameraName? desiredCamera}) => + getDetection( + DetectedObjectType.ARUCO, + desiredCamera: desiredCamera, + arucoId: id, + ) + ?.object; @override Future waitForAruco( diff --git a/vision/lib/analyzer.py b/vision/lib/analyzer.py index 7fcf9eae..16ba784b 100644 --- a/vision/lib/analyzer.py +++ b/vision/lib/analyzer.py @@ -17,7 +17,7 @@ def __init__(self, load_from="trained-model.pt"): self.model = YOLO(load_from, task="detect") def has_mallet(self, frame: cv2.Mat, confidence=0.80) -> tuple[bool, float]: - annotated, detections = self.analyze_frame(frame, confidence=confidence) + _, detections = self.analyze_frame(frame, confidence=confidence) for detection in detections: if detection.object_type == vision_pb2.HAMMER: return True, confidence From e05bd290181ccb0dd0033d33c34b698add551c9a Mon Sep 17 00:00:00 2001 From: lregueiferos <124931111+lregueiferos@users.noreply.github.com> Date: Tue, 24 Feb 2026 17:49:23 -0500 Subject: [PATCH 3/6] Improve firmware serial handshake retries and timeout handling --- burt_network/lib/src/serial/firmware.dart | 38 +++++++++++++++++++++-- 1 file changed, 35 insertions(+), 3 deletions(-) diff --git a/burt_network/lib/src/serial/firmware.dart b/burt_network/lib/src/serial/firmware.dart index 2bd7d078..1f44e3c8 100644 --- a/burt_network/lib/src/serial/firmware.dart +++ b/burt_network/lib/src/serial/firmware.dart @@ -13,7 +13,8 @@ class BurtFirmwareSerial extends Service { /// The interval to read serial data at. static const readInterval = Duration(milliseconds: 10); /// How long it should take for a firmware device to respond to a handshake. - static const handshakeDelay = Duration(milliseconds: 200); + static const handshakeDelay = Duration(milliseconds: 1200); + static const handshakeTimeout = Duration(milliseconds: 1500); /// The reset code to send to a firmware device. static final resetCode = Uint8List.fromList([0, 0, 0, 0]); @@ -67,7 +68,18 @@ class BurtFirmwareSerial extends Service { // Execute the handshake if (!(await _reset() || await _reset())) logger.warning("The Teensy on port $port failed to reset"); - if (!await _sendHandshake()) { + + // Retry handshake up to 3 times with brief backoff + var handshakeSuccess = false; + for (var attempt = 0; attempt < 3; attempt++) { + if (await _sendHandshake()) { + handshakeSuccess = true; + break; + } + await Future.delayed(const Duration(milliseconds: 200)); + } + + if (!handshakeSuccess) { logger.warning("Could not connect to Teensy", body: "Device on port $port failed the handshake"); return false; } @@ -82,12 +94,32 @@ class BurtFirmwareSerial extends Service { logger.debug("Sending handshake to port $port..."); final handshake = Connect(sender: Device.SUBSYSTEMS, receiver: Device.FIRMWARE); _serial.write(handshake.writeToBuffer()); + + // Wait for MCU boot, then poll for response await Future.delayed(handshakeDelay); - final response = _serial.readBytes(4); + + final deadline = DateTime.now().add(handshakeTimeout); + final response = []; + + while (DateTime.now().isBefore(deadline) && response.length < 4) { + final chunk = _serial.readBytes(4 - response.length); + if (chunk.isNotEmpty) { + response.addAll(chunk); + } else { + await Future.delayed(const Duration(milliseconds: 20)); + } + } + if (response.isEmpty) { logger.trace("Device did not respond"); return false; } + + if (response.length < 4) { + logger.trace("Device responded with incomplete handshake: $response"); + return false; + } + try { final message = Connect.fromBuffer(response); logger.trace("Device responded with: ${message.toProto3Json()}"); From 47216f50d8c178dba87f01aaf93a695c22a73261 Mon Sep 17 00:00:00 2001 From: selester11 Date: Mon, 30 Mar 2026 23:27:32 -0400 Subject: [PATCH 4/6] Add lawnmower search pattern fallback to ArUco task --- autonomy/lib/constants.dart | 7 +++ .../src/orchestrator/rover_orchestrator.dart | 57 ++++++++++++++++++- 2 files changed, 61 insertions(+), 3 deletions(-) diff --git a/autonomy/lib/constants.dart b/autonomy/lib/constants.dart index 186b6c46..dc147542 100644 --- a/autonomy/lib/constants.dart +++ b/autonomy/lib/constants.dart @@ -51,6 +51,13 @@ class Constants { /// 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; diff --git a/autonomy/lib/src/orchestrator/rover_orchestrator.dart b/autonomy/lib/src/orchestrator/rover_orchestrator.dart index 00df6e9e..fa0d03a5 100644 --- a/autonomy/lib/src/orchestrator/rover_orchestrator.dart +++ b/autonomy/lib/src/orchestrator/rover_orchestrator.dart @@ -200,6 +200,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 generateLawnmowerWaypoints(GpsCoordinates center) { + final waypoints = []; + 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; @@ -293,7 +319,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, ); @@ -302,12 +328,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; From 11f6c9f76a50108a534bb822bbbf352739bd6120 Mon Sep 17 00:00:00 2001 From: preston Date: Sun, 12 Apr 2026 11:58:39 -0400 Subject: [PATCH 5/6] Add lawnmower vision stuff --- autonomy/lib/constants.dart | 3 + autonomy/lib/src/drive/drive_interface.dart | 6 + autonomy/lib/src/drive/rover_drive.dart | 6 + autonomy/lib/src/drive/sensor_drive.dart | 30 +++++ .../src/orchestrator/rover_orchestrator.dart | 124 +++++++++--------- .../rover_states/spin_for_object.dart | 70 ---------- burt_network/Protobuf | 2 +- vision/lib/network | 2 +- 8 files changed, 108 insertions(+), 135 deletions(-) delete mode 100644 autonomy/lib/src/state_machine/rover_states/spin_for_object.dart diff --git a/autonomy/lib/constants.dart b/autonomy/lib/constants.dart index dc147542..a9a60470 100644 --- a/autonomy/lib/constants.dart +++ b/autonomy/lib/constants.dart @@ -48,6 +48,9 @@ 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; diff --git a/autonomy/lib/src/drive/drive_interface.dart b/autonomy/lib/src/drive/drive_interface.dart index c87ca3ba..dce47df3 100644 --- a/autonomy/lib/src/drive/drive_interface.dart +++ b/autonomy/lib/src/drive/drive_interface.dart @@ -139,4 +139,10 @@ abstract class DriveInterface extends Service { /// Drive forward to approach an Aruco tag Future approachAruco() async {} + + /// Spin to face a detected object, returns whether or not it was found + Future spinForObject( + List types, { + CameraName? desiredCamera, + }) async => false; } diff --git a/autonomy/lib/src/drive/rover_drive.dart b/autonomy/lib/src/drive/rover_drive.dart index 91064713..fa5538e5 100644 --- a/autonomy/lib/src/drive/rover_drive.dart +++ b/autonomy/lib/src/drive/rover_drive.dart @@ -83,6 +83,12 @@ class RoverDrive extends DriveInterface { @override Future approachAruco() => sensorDrive.approachAruco(); + @override + Future spinForObject( + List types, { + CameraName? desiredCamera, + }) => sensorDrive.spinForObject(types, desiredCamera: desiredCamera); + @override Future faceOrientation(Orientation orientation) async { if (useImu) { diff --git a/autonomy/lib/src/drive/sensor_drive.dart b/autonomy/lib/src/drive/sensor_drive.dart index 3b5302ff..66dda33e 100644 --- a/autonomy/lib/src/drive/sensor_drive.dart +++ b/autonomy/lib/src/drive/sensor_drive.dart @@ -175,6 +175,36 @@ class SensorDrive extends DriveInterface with RoverDriveCommands { return foundAruco; } + @override + Future spinForObject( + List 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 approachAruco() async { // const sizeThreshold = 0.2; diff --git a/autonomy/lib/src/orchestrator/rover_orchestrator.dart b/autonomy/lib/src/orchestrator/rover_orchestrator.dart index fa0d03a5..f085ab1e 100644 --- a/autonomy/lib/src/orchestrator/rover_orchestrator.dart +++ b/autonomy/lib/src/orchestrator/rover_orchestrator.dart @@ -4,7 +4,6 @@ import "dart:math"; import "package:autonomy/constants.dart"; import "package:autonomy/interfaces.dart"; import "package:autonomy/src/state_machine/rover_states/approach_visible_object.dart"; -import "package:autonomy/src/state_machine/rover_states/spin_for_object.dart"; import "package:coordinate_converter/coordinate_converter.dart"; @@ -480,6 +479,7 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { @override Future handleHammerTask(AutonomyCommand command) async { await _handleVisualObjectTask( + command: command, targetTypes: [ DetectedObjectType.HAMMER, DetectedObjectType.ROCK_HAMMER, @@ -491,92 +491,90 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { @override Future handleBottleTask(AutonomyCommand command) async { await _handleVisualObjectTask( + command: command, targetTypes: [DetectedObjectType.BOTTLE], label: "bottle", ); } Future _handleVisualObjectTask({ + required AutonomyCommand command, required List targetTypes, required String label, }) async { collection.drive.setLedStrip(ProtoColor.RED); - var searchFailed = false; - - final objectTaskSequence = SequenceState( - controller, - steps: [ - FunctionalState( - controller, - onEnter: (controller) { - currentState = AutonomyState.SEARCHING; - controller.popState(); - }, - ), - SpinForObject( - controller, - collection: collection, - targetTypes: targetTypes, - desiredCamera: Constants.arucoDetectionCamera, - ), - FunctionalState( - controller, - onEnter: (controller) { - final detection = _firstTargetDetection(targetTypes); - if (detection != null) { - currentState = AutonomyState.APPROACHING; - controller.popState(); - return; - } - currentState = AutonomyState.NO_SOLUTION; - searchFailed = true; - controller.popState(); - }, - ), - ApproachVisibleObject( - controller, - collection: collection, - targetTypes: targetTypes, - desiredCamera: Constants.arucoDetectionCamera, - ), - FunctionalState( - controller, - onEnter: (controller) { - collection.logger.info("Reached target $label"); - currentState = AutonomyState.AT_DESTINATION; - collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); - controller.popState(); - }, - ), - ], + 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); - controller.pushState(objectTaskSequence); + 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; + await calculateAndFollowPath( + waypoint, + abortOnError: false, + alternateEndCondition: () => _firstTargetDetection(targetTypes) != null, + ); + 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) { - collection.logger.warning( - "Execution timer running while command is null", - body: "Canceling timer", - ); - onCommandEnd(); - timer.cancel(); - return; - } - - if (searchFailed) { - collection.logger.error("Failed to find target $label"); - currentState = AutonomyState.NO_SOLUTION; onCommandEnd(); timer.cancel(); return; } if (!controller.hasState()) { - currentState = AutonomyState.NO_SOLUTION; + collection.logger.info("Reached target $label"); + collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); + currentState = AutonomyState.AT_DESTINATION; onCommandEnd(); timer.cancel(); return; diff --git a/autonomy/lib/src/state_machine/rover_states/spin_for_object.dart b/autonomy/lib/src/state_machine/rover_states/spin_for_object.dart deleted file mode 100644 index 697ce4c6..00000000 --- a/autonomy/lib/src/state_machine/rover_states/spin_for_object.dart +++ /dev/null @@ -1,70 +0,0 @@ -import "package:autonomy/interfaces.dart"; -import "package:autonomy/rover.dart"; -import "package:autonomy/src/drive/drive_config.dart"; - -class SpinForObject extends RoverState { - final AutonomyInterface collection; - final List targetTypes; - final CameraName? desiredCamera; - - DriveConfig get config => collection.drive.config; - - double _startOrientation = 0; - bool _rotated180 = false; - - SpinForObject( - super.controller, { - required this.collection, - required this.targetTypes, - this.desiredCamera, - }); - - @override - void enter() { - _startOrientation = collection.imu.heading; - _rotated180 = false; - } - - @override - void update() { - if (collection.drive is! RoverDrive) { - controller.popState(); - return; - } - - final drive = (collection.drive as RoverDrive).sensorDrive; - drive.setThrottle(config.turnThrottle); - - for (final type in targetTypes) { - final detection = collection.video.getDetection( - type, - desiredCamera: desiredCamera, - ); - if (detection != null) { - controller.popState(); - return; - } - } - - drive.spinLeft(); - - if ((collection.imu.heading - _startOrientation).clampHalfAngle().abs() > - 175) { - _rotated180 = true; - } - - if (_rotated180 && - collection.imu.isNear(Orientation(z: _startOrientation))) { - controller.popState(); - } - } - - @override - void exit() { - if (collection.drive is! RoverDrive) { - return; - } - final drive = (collection.drive as RoverDrive).sensorDrive; - drive.stopMotors(); - } -} diff --git a/burt_network/Protobuf b/burt_network/Protobuf index 7d44b26c..9e74c092 160000 --- a/burt_network/Protobuf +++ b/burt_network/Protobuf @@ -1 +1 @@ -Subproject commit 7d44b26cd0d591c45fcfd0dbcaea7d718b59aeb7 +Subproject commit 9e74c092879fbb89d483899bb044123831f1f169 diff --git a/vision/lib/network b/vision/lib/network index 91a1550b..d717de8f 160000 --- a/vision/lib/network +++ b/vision/lib/network @@ -1 +1 @@ -Subproject commit 91a1550b9acf985548385e4f1ad05fbf2a873eae +Subproject commit d717de8f16deb08058b962b3d04f7fce41004edf From be4e57d5662f2a6ae1402380cbbfcc8414fbc2a2 Mon Sep 17 00:00:00 2001 From: preston Date: Sun, 12 Apr 2026 12:28:09 -0400 Subject: [PATCH 6/6] Spin on failed waypoint --- autonomy/lib/src/orchestrator/rover_orchestrator.dart | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/autonomy/lib/src/orchestrator/rover_orchestrator.dart b/autonomy/lib/src/orchestrator/rover_orchestrator.dart index f085ab1e..7c4e466a 100644 --- a/autonomy/lib/src/orchestrator/rover_orchestrator.dart +++ b/autonomy/lib/src/orchestrator/rover_orchestrator.dart @@ -533,13 +533,21 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { final waypoints = generateLawnmowerWaypoints(collection.gps.coordinates); for (final waypoint in waypoints) { if (currentCommand == null) return; - await calculateAndFollowPath( + 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; + } } }