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 03e23ad..a712176 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: 20); + + /// 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_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/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..bd1cb50 100644 --- a/lib/src/orchestrator/rover_orchestrator.dart +++ b/lib/src/orchestrator/rover_orchestrator.dart @@ -35,33 +35,31 @@ 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, + 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 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 diff --git a/lib/src/video/rover_video.dart b/lib/src/video/rover_video.dart index b9442d2..3698d82 100644 --- a/lib/src/video/rover_video.dart +++ b/lib/src/video/rover_video.dart @@ -1,31 +1,71 @@ 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, - callback: updateFrame, - ); - return true; + void updateFrame(VideoData result) { + hasValue = true; + if (result.hasFrame() && result.frame.isNotEmpty) return; + + _cachedResults.removeWhere((e) => e.details.name == result.details.name); + if (result.detectedObjects.isEmpty) return; + + _cachedResults.add(result); } @override - Future dispose() async { } + DetectedObject? getArucoDetection(int id, {CameraName? desiredCamera}) { + 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; + } + } + } + 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: VideoData().messageName, + constructor: VideoData.fromBuffer, + callback: (result) async { + if (result.hasFrame() && result.frame.isNotEmpty) return; + if (result.details.name != (desiredCamera ?? result.details.name)) return; + final object = result.detectedObjects.firstWhereOrNull( + (e) => e.objectType == DetectedObjectType.ARUCO && 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..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(VideoData newData) { } + void updateFrame(VideoData result) {} } diff --git a/lib/src/video/video_interface.dart b/lib/src/video/video_interface.dart index 77ea2ad..a9d095d 100644 --- a/lib/src/video/video_interface.dart +++ b/lib/src/video/video_interface.dart @@ -1,16 +1,37 @@ +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}); - VideoData data = VideoData(); + @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); - 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 c1f41ab..d32e8ee 100644 --- a/pubspec.yaml +++ b/pubspec.yaml @@ -12,6 +12,7 @@ dependencies: burt_network: ^2.4.0 a_star: ^3.0.0 meta: ^1.11.0 + collection: ^1.19.1 dev_dependencies: test: ^1.21.0