From 296823611b3ce268d32590950494353ad257646b Mon Sep 17 00:00:00 2001 From: Gold87 <91761103+Gold872@users.noreply.github.com> Date: Fri, 7 Feb 2025 16:09:17 -0500 Subject: [PATCH 1/6] Added aruco tracking --- lib/constants.dart | 10 +- lib/src/detector/detector_interface.dart | 1 - lib/src/detector/network_detector.dart | 3 - lib/src/detector/rover_detector.dart | 4 - lib/src/detector/sim_detector.dart | 3 - lib/src/drive/drive_interface.dart | 2 +- lib/src/drive/rover_drive.dart | 5 +- lib/src/drive/sensor_drive.dart | 41 ++++--- lib/src/drive/sim_drive.dart | 3 + lib/src/orchestrator/rover_orchestrator.dart | 113 +++++++++++++++---- lib/src/video/rover_video.dart | 68 +++++++++-- lib/src/video/sim_video.dart | 2 +- lib/src/video/video_interface.dart | 13 ++- pubspec.yaml | 1 + 14 files changed, 201 insertions(+), 68 deletions(-) diff --git a/lib/constants.dart b/lib/constants.dart index 03e23ad..ce6fbdf 100644 --- a/lib/constants.dart +++ b/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: 10); + + /// The camera that should be used to detect Aruco tags + static const CameraName arucoDetectionCamera = CameraName.ROVER_FRONT; } diff --git a/lib/src/detector/detector_interface.dart b/lib/src/detector/detector_interface.dart index c42a694..8d5909e 100644 --- a/lib/src/detector/detector_interface.dart +++ b/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(); } diff --git a/lib/src/detector/network_detector.dart b/lib/src/detector/network_detector.dart index c91934d..37e7796 100644 --- a/lib/src/detector/network_detector.dart +++ b/lib/src/detector/network_detector.dart @@ -6,9 +6,6 @@ class NetworkDetector extends DetectorInterface { NetworkDetector({required super.collection}); - @override - bool canSeeAruco() => false; - @override Future dispose() async {} diff --git a/lib/src/detector/rover_detector.dart b/lib/src/detector/rover_detector.dart index 3ed0161..a7d7e16 100644 --- a/lib/src/detector/rover_detector.dart +++ b/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 init() async => true; diff --git a/lib/src/detector/sim_detector.dart b/lib/src/detector/sim_detector.dart index dfac538..4b8e57a 100644 --- a/lib/src/detector/sim_detector.dart +++ b/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] } diff --git a/lib/src/drive/drive_interface.dart b/lib/src/drive/drive_interface.dart index 8d250cf..267ad07 100644 --- a/lib/src/drive/drive_interface.dart +++ b/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 spinForAruco() async => false; + Future spinForAruco(int arucoId, {CameraName? desiredCamera}) async => false; /// Drive forward to approach an Aruco tag Future approachAruco() async { } diff --git a/lib/src/drive/rover_drive.dart b/lib/src/drive/rover_drive.dart index c43a5fb..70c9cec 100644 --- a/lib/src/drive/rover_drive.dart +++ b/lib/src/drive/rover_drive.dart @@ -77,7 +77,10 @@ class RoverDrive extends DriveInterface { } @override - Future spinForAruco() => sensorDrive.spinForAruco(); + Future spinForAruco( + int arucoId, { + CameraName? desiredCamera, + }) => sensorDrive.spinForAruco(arucoId, desiredCamera: desiredCamera); @override Future approachAruco() => sensorDrive.approachAruco(); diff --git a/lib/src/drive/sensor_drive.dart b/lib/src/drive/sensor_drive.dart index 7a9654f..44b6534 100644 --- a/lib/src/drive/sensor_drive.dart +++ b/lib/src/drive/sensor_drive.dart @@ -97,27 +97,38 @@ class SensorDrive extends DriveInterface with RoverDriveCommands { } @override - Future spinForAruco() async { + Future 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 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(); } } diff --git a/lib/src/drive/sim_drive.dart b/lib/src/drive/sim_drive.dart index a9f0760..67c2450 100644 --- a/lib/src/drive/sim_drive.dart +++ b/lib/src/drive/sim_drive.dart @@ -36,6 +36,9 @@ class DriveSimulator extends DriveInterface { return true; } + @override + Future spinForAruco(int arucoId, {CameraName? desiredCamera}) async => true; + @override Future stop() async { collection.logger.debug("Stopping"); diff --git a/lib/src/orchestrator/rover_orchestrator.dart b/lib/src/orchestrator/rover_orchestrator.dart index f3f417f..bd4b7a5 100644 --- a/lib/src/orchestrator/rover_orchestrator.dart +++ b/lib/src/orchestrator/rover_orchestrator.dart @@ -35,33 +35,27 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { @override Message getMessage() => statusMessage; - @override - Future 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 calculateAndFollowPath(GpsCoordinates goal, {bool abortOnError = true}) async { await collection.drive.resolveOrientation(); collection.detector.findObstacles(); - while (!collection.gps.coordinates.isNear(destination)) { + while (!collection.gps.coordinates.isNear(goal)) { // 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()); @@ -104,7 +98,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 +110,25 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { } } } + return true; + } + + @override + Future 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,18 +140,72 @@ 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(); - collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); - currentState = AutonomyState.AT_DESTINATION; + 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()}", + ); + + final distanceToTag = detectedAruco.bestPnpResult.cameraToTarget.translation.z.abs() - 0.5; // 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)) { + 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; + } else { + collection.logger.error("Could not spin towards ArUco tag"); + currentCommand = null; } } diff --git a/lib/src/video/rover_video.dart b/lib/src/video/rover_video.dart index b9442d2..1170108 100644 --- a/lib/src/video/rover_video.dart +++ b/lib/src/video/rover_video.dart @@ -1,31 +1,77 @@ import "dart:async"; +import "package:autonomy/constants.dart"; import "package:autonomy/interfaces.dart"; +import "package:collection/collection.dart"; class RoverVideo extends VideoInterface { + final List _cachedResults = []; + RoverVideo({required super.collection}); @override Future init() async { collection.server.messages.onMessage( - name: VideoData().messageName, - constructor: VideoData.fromBuffer, + name: VisionResult().messageName, + constructor: VisionResult.fromBuffer, callback: updateFrame, ); return true; } @override - Future dispose() async { } + Future dispose() async {} + + @override + void updateFrame(VisionResult result) { + hasValue = true; + if (result.objects.isEmpty) return; + + _cachedResults.removeWhere((e) => e.name == result.name); + + _cachedResults.add(result); + } + + @override + DetectedObject? getArucoDetection(int id, {CameraName? desiredCamera}) { + for (final result in _cachedResults.where((e) => e.name == (desiredCamera ?? e.name))) { + for (final object in result.objects) { + if (object.arucoTagId == id) { + return object; + } + } + } + return null; + } @override - void updateFrame(VideoData newData) { - data = newData; - // if (data.arucoDetected == BoolState.YES) { - // flag = true; - // Timer(const Duration(seconds: 3), () => flag = false); - // collection.logger.info("Is ArUco detected: ${data.arucoDetected}"); - // } - hasValue = true; + Future waitForAruco( + int id, { + CameraName? desiredCamera, + Duration timeout = Constants.arucoSearchTimeout, + }) async { + final completer = Completer(); + + late final StreamSubscription resultSubscription; + + resultSubscription = collection.server.messages.onMessage( + name: VisionResult().messageName, + constructor: VisionResult.fromBuffer, + callback: (result) async { + if (result.name != (desiredCamera ?? result.name)) return; + final object = result.objects.firstWhereOrNull((e) => e.arucoTagId == id); + if (object != null) { + await resultSubscription.cancel(); + completer.complete(object); + } + }, + ); + + try { + return await completer.future.timeout(timeout); + } on TimeoutException { + await resultSubscription.cancel(); + return null; + } } } diff --git a/lib/src/video/sim_video.dart b/lib/src/video/sim_video.dart index c7d5f1c..a2eb73d 100644 --- a/lib/src/video/sim_video.dart +++ b/lib/src/video/sim_video.dart @@ -17,5 +17,5 @@ class VideoSimulator extends VideoInterface { Uint16List depthFrame = Uint16List.fromList([]); @override - void updateFrame(VideoData newData) { } + void updateFrame(VisionResult result) {} } diff --git a/lib/src/video/video_interface.dart b/lib/src/video/video_interface.dart index 77ea2ad..4731f97 100644 --- a/lib/src/video/video_interface.dart +++ b/lib/src/video/video_interface.dart @@ -1,3 +1,4 @@ +import "package:autonomy/constants.dart"; import "package:autonomy/interfaces.dart"; /// Handles obstacle detection data and ArUco data from video @@ -7,10 +8,14 @@ abstract class VideoInterface extends Service with Receiver { final AutonomyInterface collection; VideoInterface({required this.collection}); - VideoData data = VideoData(); + void updateFrame(VisionResult result); - void updateFrame(VideoData newData); + DetectedObject? getArucoDetection(int id, {CameraName? desiredCamera}) => null; - double get arucoSize => 0; // data.arucoSize; - double get arucoPosition => 0; // data.arucoPosition; + Future waitForAruco( + int id, { + CameraName? desiredCamera, + Duration timeout = Constants.arucoSearchTimeout, + }) => + Future.value(); } diff --git a/pubspec.yaml b/pubspec.yaml index 9dfa7b7..7c5058d 100644 --- a/pubspec.yaml +++ b/pubspec.yaml @@ -12,6 +12,7 @@ dependencies: burt_network: ^2.3.1 a_star: ^3.0.0 meta: ^1.11.0 + collection: ^1.19.1 dev_dependencies: test: ^1.21.0 From 2f8becfb555d827fb947378432e9faf164dce2de Mon Sep 17 00:00:00 2001 From: Gold87 <91761103+Gold872@users.noreply.github.com> Date: Sun, 9 Feb 2025 15:40:56 -0500 Subject: [PATCH 2/6] Update protobuf messages --- lib/src/video/rover_video.dart | 39 ++++++++++++------------------ lib/src/video/sim_video.dart | 11 ++------- lib/src/video/video_interface.dart | 26 ++++++++++++++++---- 3 files changed, 38 insertions(+), 38 deletions(-) diff --git a/lib/src/video/rover_video.dart b/lib/src/video/rover_video.dart index 1170108..b0d7167 100644 --- a/lib/src/video/rover_video.dart +++ b/lib/src/video/rover_video.dart @@ -5,37 +5,27 @@ import "package:autonomy/interfaces.dart"; import "package:collection/collection.dart"; class RoverVideo extends VideoInterface { - final List _cachedResults = []; + final List _cachedResults = []; RoverVideo({required super.collection}); @override - Future init() async { - collection.server.messages.onMessage( - name: VisionResult().messageName, - constructor: VisionResult.fromBuffer, - callback: updateFrame, - ); - return true; - } - - @override - Future dispose() async {} - - @override - void updateFrame(VisionResult result) { + void updateFrame(VideoData result) { hasValue = true; - if (result.objects.isEmpty) return; + if (result.hasFrame()) return; - _cachedResults.removeWhere((e) => e.name == result.name); + _cachedResults.removeWhere((e) => e.details.name == result.details.name); + if (result.detectedObjects.isEmpty) return; _cachedResults.add(result); } @override DetectedObject? getArucoDetection(int id, {CameraName? desiredCamera}) { - for (final result in _cachedResults.where((e) => e.name == (desiredCamera ?? e.name))) { - for (final object in result.objects) { + for (final result in _cachedResults.where( + (e) => e.details.name == (desiredCamera ?? e.details.name), + )) { + for (final object in result.detectedObjects) { if (object.arucoTagId == id) { return object; } @@ -52,14 +42,15 @@ class RoverVideo extends VideoInterface { }) async { final completer = Completer(); - late final StreamSubscription resultSubscription; + late final StreamSubscription resultSubscription; resultSubscription = collection.server.messages.onMessage( - name: VisionResult().messageName, - constructor: VisionResult.fromBuffer, + name: VideoData().messageName, + constructor: VideoData.fromBuffer, callback: (result) async { - if (result.name != (desiredCamera ?? result.name)) return; - final object = result.objects.firstWhereOrNull((e) => e.arucoTagId == id); + if (result.hasFrame()) return; + if (result.details.name != (desiredCamera ?? result.details.name)) return; + final object = result.detectedObjects.firstWhereOrNull((e) => e.arucoTagId == id); if (object != null) { await resultSubscription.cancel(); completer.complete(object); diff --git a/lib/src/video/sim_video.dart b/lib/src/video/sim_video.dart index a2eb73d..ca6b17b 100644 --- a/lib/src/video/sim_video.dart +++ b/lib/src/video/sim_video.dart @@ -1,5 +1,3 @@ -import "dart:typed_data"; - import "package:autonomy/interfaces.dart"; class VideoSimulator extends VideoInterface { @@ -8,14 +6,9 @@ class VideoSimulator extends VideoInterface { @override Future init() async { hasValue = true; - return true; + return super.init(); } @override - Future dispose() async => depthFrame = Uint16List.fromList([]); - - Uint16List depthFrame = Uint16List.fromList([]); - - @override - void updateFrame(VisionResult result) {} + void updateFrame(VideoData result) {} } diff --git a/lib/src/video/video_interface.dart b/lib/src/video/video_interface.dart index 4731f97..a9d095d 100644 --- a/lib/src/video/video_interface.dart +++ b/lib/src/video/video_interface.dart @@ -1,14 +1,31 @@ +import "dart:async"; + import "package:autonomy/constants.dart"; import "package:autonomy/interfaces.dart"; /// Handles obstacle detection data and ArUco data from video abstract class VideoInterface extends Service with Receiver { - bool flag = false; - final AutonomyInterface collection; + StreamSubscription? _dataSubscription; + VideoInterface({required this.collection}); - void updateFrame(VisionResult result); + @override + Future init() async { + _dataSubscription = collection.server.messages.onMessage( + name: VideoData().messageName, + constructor: VideoData.fromBuffer, + callback: updateFrame, + ); + return true; + } + + @override + Future dispose() async { + await _dataSubscription?.cancel(); + } + + void updateFrame(VideoData result); DetectedObject? getArucoDetection(int id, {CameraName? desiredCamera}) => null; @@ -16,6 +33,5 @@ abstract class VideoInterface extends Service with Receiver { int id, { CameraName? desiredCamera, Duration timeout = Constants.arucoSearchTimeout, - }) => - Future.value(); + }) => Future.value(); } From d79f7900726722b274b6023e3cc5711762c3b094 Mon Sep 17 00:00:00 2001 From: Binghamton Rover Date: Wed, 12 Feb 2025 21:48:51 -0500 Subject: [PATCH 3/6] Testing on pi --- bin/autonomy.dart | 2 ++ lib/constants.dart | 4 ++-- lib/src/orchestrator/rover_orchestrator.dart | 6 +++--- lib/src/video/rover_video.dart | 2 +- 4 files changed, 8 insertions(+), 6 deletions(-) diff --git a/bin/autonomy.dart b/bin/autonomy.dart index 31bd0b6..5bf6d3d 100644 --- a/bin/autonomy.dart +++ b/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(); } diff --git a/lib/constants.dart b/lib/constants.dart index ce6fbdf..cbfcb1b 100644 --- a/lib/constants.dart +++ b/lib/constants.dart @@ -28,8 +28,8 @@ class Constants { 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: 10); + static const Duration arucoSearchTimeout = Duration(seconds: 20); /// The camera that should be used to detect Aruco tags - static const CameraName arucoDetectionCamera = CameraName.ROVER_FRONT; + static const CameraName arucoDetectionCamera = CameraName.AUTONOMY_DEPTH; } diff --git a/lib/src/orchestrator/rover_orchestrator.dart b/lib/src/orchestrator/rover_orchestrator.dart index bd4b7a5..ffa7b46 100644 --- a/lib/src/orchestrator/rover_orchestrator.dart +++ b/lib/src/orchestrator/rover_orchestrator.dart @@ -183,7 +183,7 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { body: "Detection: ${detectedAruco.toProto3Json()}", ); - final distanceToTag = detectedAruco.bestPnpResult.cameraToTarget.translation.z.abs() - 0.5; // Don't drive *into* the tag + final distanceToTag = detectedAruco.bestPnpResult.cameraToTarget.translation.z.abs() - 1; // Don't drive *into* the tag if (distanceToTag < 1) { // well that was easy @@ -192,8 +192,8 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { return; } - final relativeX = distanceToTag * sin((collection.imu.heading - detectedAruco.yaw) * pi / 180); - final relativeY = distanceToTag * cos((collection.imu.heading - detectedAruco.yaw) * pi / 180); + final relativeX = distanceToTag * sin((collection.imu.heading - detectedAruco.yaw - 90) * pi / 180); + final relativeY = distanceToTag * cos((collection.imu.heading - detectedAruco.yaw - 90) * pi / 180); final destinationCoordinates = (collection.gps.coordinates.inMeters + (lat: relativeY, long: relativeX)).toGps(); diff --git a/lib/src/video/rover_video.dart b/lib/src/video/rover_video.dart index b0d7167..491342d 100644 --- a/lib/src/video/rover_video.dart +++ b/lib/src/video/rover_video.dart @@ -12,7 +12,7 @@ class RoverVideo extends VideoInterface { @override void updateFrame(VideoData result) { hasValue = true; - if (result.hasFrame()) return; + if (result.hasFrame() && result.frame.isNotEmpty) return; _cachedResults.removeWhere((e) => e.details.name == result.details.name); if (result.detectedObjects.isEmpty) return; From 5a7ecc75f483e65c18134f99ef4465ded2cbc46c Mon Sep 17 00:00:00 2001 From: Gold87 <91761103+Gold872@users.noreply.github.com> Date: Thu, 13 Feb 2025 10:42:16 -0500 Subject: [PATCH 4/6] Fixed distance and angle errors --- lib/constants.dart | 4 ++-- lib/src/orchestrator/rover_orchestrator.dart | 14 +++++++++++--- lib/src/video/rover_video.dart | 9 ++++++--- 3 files changed, 19 insertions(+), 8 deletions(-) diff --git a/lib/constants.dart b/lib/constants.dart index cbfcb1b..f93986e 100644 --- a/lib/constants.dart +++ b/lib/constants.dart @@ -28,8 +28,8 @@ class Constants { 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); + static const Duration arucoSearchTimeout = Duration(seconds: 25); /// The camera that should be used to detect Aruco tags - static const CameraName arucoDetectionCamera = CameraName.AUTONOMY_DEPTH; + static const CameraName arucoDetectionCamera = CameraName.ROVER_FRONT; } diff --git a/lib/src/orchestrator/rover_orchestrator.dart b/lib/src/orchestrator/rover_orchestrator.dart index ffa7b46..f49f0b6 100644 --- a/lib/src/orchestrator/rover_orchestrator.dart +++ b/lib/src/orchestrator/rover_orchestrator.dart @@ -183,7 +183,15 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { body: "Detection: ${detectedAruco.toProto3Json()}", ); - final distanceToTag = detectedAruco.bestPnpResult.cameraToTarget.translation.z.abs() - 1; // Don't drive *into* the tag + // 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 @@ -192,8 +200,8 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { return; } - final relativeX = distanceToTag * sin((collection.imu.heading - detectedAruco.yaw - 90) * pi / 180); - final relativeY = distanceToTag * cos((collection.imu.heading - detectedAruco.yaw - 90) * pi / 180); + 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(); diff --git a/lib/src/video/rover_video.dart b/lib/src/video/rover_video.dart index 491342d..3698d82 100644 --- a/lib/src/video/rover_video.dart +++ b/lib/src/video/rover_video.dart @@ -26,7 +26,8 @@ class RoverVideo extends VideoInterface { (e) => e.details.name == (desiredCamera ?? e.details.name), )) { for (final object in result.detectedObjects) { - if (object.arucoTagId == id) { + if (object.objectType == DetectedObjectType.ARUCO && + object.arucoTagId == id) { return object; } } @@ -48,9 +49,11 @@ class RoverVideo extends VideoInterface { name: VideoData().messageName, constructor: VideoData.fromBuffer, callback: (result) async { - if (result.hasFrame()) return; + if (result.hasFrame() && result.frame.isNotEmpty) return; if (result.details.name != (desiredCamera ?? result.details.name)) return; - final object = result.detectedObjects.firstWhereOrNull((e) => e.arucoTagId == id); + final object = result.detectedObjects.firstWhereOrNull( + (e) => e.objectType == DetectedObjectType.ARUCO && e.arucoTagId == id, + ); if (object != null) { await resultSubscription.cancel(); completer.complete(object); From ab40fdde50296e942db72e643956ca649c7fbe98 Mon Sep 17 00:00:00 2001 From: Binghamton Rover Date: Thu, 13 Feb 2025 21:40:20 -0500 Subject: [PATCH 5/6] Works on rover! --- lib/constants.dart | 2 +- lib/src/drive/drive_config.dart | 2 +- lib/src/orchestrator/rover_orchestrator.dart | 56 ++++++++++++++++++-- 3 files changed, 55 insertions(+), 5 deletions(-) diff --git a/lib/constants.dart b/lib/constants.dart index f93986e..a712176 100644 --- a/lib/constants.dart +++ b/lib/constants.dart @@ -28,7 +28,7 @@ class Constants { 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: 25); + static const Duration arucoSearchTimeout = Duration(seconds: 20); /// The camera that should be used to detect Aruco tags static const CameraName arucoDetectionCamera = CameraName.ROVER_FRONT; diff --git a/lib/src/drive/drive_config.dart b/lib/src/drive/drive_config.dart index eed787e..d5f4419 100644 --- a/lib/src/drive/drive_config.dart +++ b/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", ); diff --git a/lib/src/orchestrator/rover_orchestrator.dart b/lib/src/orchestrator/rover_orchestrator.dart index f49f0b6..65d508b 100644 --- a/lib/src/orchestrator/rover_orchestrator.dart +++ b/lib/src/orchestrator/rover_orchestrator.dart @@ -35,10 +35,14 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { @override Message getMessage() => statusMessage; - Future calculateAndFollowPath(GpsCoordinates goal, {bool abortOnError = true}) async { + Future calculateAndFollowPath( + GpsCoordinates goal, { + bool abortOnError = true, + bool Function()? alternateEndCondition, + }) async { await collection.drive.resolveOrientation(); collection.detector.findObstacles(); - while (!collection.gps.coordinates.isNear(goal)) { + while (!collection.gps.coordinates.isNear(goal) && !(alternateEndCondition?.call() ?? false)) { // Calculate a path collection.logger.debug("Finding a path"); currentState = AutonomyState.PATHING; @@ -64,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) { @@ -205,7 +213,49 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { final destinationCoordinates = (collection.gps.coordinates.inMeters + (lat: relativeY, long: relativeX)).toGps(); - if (await calculateAndFollowPath(destinationCoordinates, abortOnError: false)) { + 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 < 0.75; + }, + )) { + detectedAruco = collection.video.getArucoDetection( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ); + if (detectedAruco == null) { + await collection.drive.spinForAruco( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ); + } + + detectedAruco = collection.video.getArucoDetection( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ); + + if (detectedAruco != null) { + await collection.drive.faceOrientation( + Orientation( + z: collection.imu.heading - detectedAruco!.yaw, + ), + ); + } + collection.logger.info("Successfully reached within ${Constants.maxErrorMeters} meters of the Aruco tag"); collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); currentState = AutonomyState.AT_DESTINATION; From 227c309a94023bf5f409397735c87bb6a9b561af Mon Sep 17 00:00:00 2001 From: Gold87 <91761103+Gold872@users.noreply.github.com> Date: Fri, 14 Feb 2025 09:41:49 -0500 Subject: [PATCH 6/6] More log messages for aruco re-searching --- lib/src/orchestrator/rover_orchestrator.dart | 60 +++++++++++--------- 1 file changed, 32 insertions(+), 28 deletions(-) diff --git a/lib/src/orchestrator/rover_orchestrator.dart b/lib/src/orchestrator/rover_orchestrator.dart index 65d508b..bd1cb50 100644 --- a/lib/src/orchestrator/rover_orchestrator.dart +++ b/lib/src/orchestrator/rover_orchestrator.dart @@ -213,7 +213,7 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { final destinationCoordinates = (collection.gps.coordinates.inMeters + (lat: relativeY, long: relativeX)).toGps(); - if (await calculateAndFollowPath( + if (!await calculateAndFollowPath( destinationCoordinates, abortOnError: false, alternateEndCondition: () { @@ -229,42 +229,46 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { pow(cameraToTag.translation.z, 2) + pow(cameraToTag.translation.x, 2), ); - return distanceToTag < 0.75; + return distanceToTag < 1; }, )) { - detectedAruco = collection.video.getArucoDetection( + 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, ); - if (detectedAruco == null) { - await collection.drive.spinForAruco( - command.arucoId, - desiredCamera: Constants.arucoDetectionCamera, - ); - } + } - detectedAruco = collection.video.getArucoDetection( - 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, + ), ); - - if (detectedAruco != null) { - await collection.drive.faceOrientation( - Orientation( - z: collection.imu.heading - detectedAruco!.yaw, - ), - ); - } - - collection.logger.info("Successfully reached within ${Constants.maxErrorMeters} meters of the Aruco tag"); - collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); - currentState = AutonomyState.AT_DESTINATION; + } else { + collection.logger.warning("Could not find Aruco after following path"); } - currentCommand = null; - } else { - collection.logger.error("Could not spin towards ArUco tag"); - currentCommand = null; + + 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