diff --git a/autonomy/lib/constants.dart b/autonomy/lib/constants.dart index b6e121fc..6383d9cb 100644 --- a/autonomy/lib/constants.dart +++ b/autonomy/lib/constants.dart @@ -13,7 +13,7 @@ class Constants { static const gpsError = 0.00003; /// The maximum error or "tolerance" for reaching the end goal - static const double maxErrorMeters = 1; + static const double maxErrorMeters = 1.5; /// The closest distance the pathfinding algorithm will allow /// the rover to go near an obstacle @@ -30,11 +30,11 @@ class Constants { static const double replanErrorMeters = 3; /// The IMU angle tolerance for a turn during autonomy - static const double turnEpsilon = 3; + static const double turnEpsilon = 1; /// The IMU angle tolerance when turning to re-correct to the /// proper orientation before driving forward - static const double driveRealignmentEpsilon = 5; + static const double driveRealignmentEpsilon = 3; /// The maximum time to spend waiting for the drive to reach a desired GPS coordinate. /// @@ -46,7 +46,7 @@ class Constants { ); /// The maximum time to spend searching for an aruco tag - static const Duration arucoSearchTimeout = Duration(seconds: 20); + static const Duration arucoSearchTimeout = Duration(seconds: 30); /// 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..2c7acea0 100644 --- a/autonomy/lib/src/drive/drive_interface.dart +++ b/autonomy/lib/src/drive/drive_interface.dart @@ -76,6 +76,9 @@ abstract class DriveInterface extends Service { StateInterface turnStateState(AutonomyAStarState state) => faceOrientationState(state.orientation.orientation); + /// State to spin towards the specified aruco tag + StateInterface spinForArucoState(int arucoId, {CameraName? desiredCamera}); + /// Stop the rover Future stop(); @@ -131,6 +134,8 @@ abstract class DriveInterface extends Service { blink: blink ? BoolState.YES : BoolState.NO, ); sendCommand(command); + sendCommand(command); + sendCommand(command); } /// Spin to face an Aruco tag, returns whether or not it was able to face the tag diff --git a/autonomy/lib/src/drive/rover_drive.dart b/autonomy/lib/src/drive/rover_drive.dart index 91064713..48048775 100644 --- a/autonomy/lib/src/drive/rover_drive.dart +++ b/autonomy/lib/src/drive/rover_drive.dart @@ -142,4 +142,16 @@ class RoverDrive extends DriveInterface { return simDrive.faceOrientationState(orientation); } } + + @override + StateInterface spinForArucoState(int arucoId, {CameraName? desiredCamera}) { + if (useImu) { + return sensorDrive.spinForArucoState( + arucoId, + desiredCamera: desiredCamera, + ); + } else { + return simDrive.spinForArucoState(arucoId, desiredCamera: desiredCamera); + } + } } diff --git a/autonomy/lib/src/drive/sensor_drive.dart b/autonomy/lib/src/drive/sensor_drive.dart index 3b5302ff..c79a8f05 100644 --- a/autonomy/lib/src/drive/sensor_drive.dart +++ b/autonomy/lib/src/drive/sensor_drive.dart @@ -1,4 +1,5 @@ import "package:autonomy/autonomy.dart"; +import "package:autonomy/src/state_machine/rover_states/spin_for_aruco.dart"; import "drive_commands.dart"; @@ -42,6 +43,16 @@ class SensorDrive extends DriveInterface with RoverDriveCommands { drive: this, ); + @override + StateInterface spinForArucoState(int arucoId, {CameraName? desiredCamera}) => + SpinForAruco( + controller, + drive: this, + arucoId: arucoId, + desiredCamera: desiredCamera, + collection: collection, + ); + @override Future stop() async { stopMotors(); diff --git a/autonomy/lib/src/drive/sim_drive.dart b/autonomy/lib/src/drive/sim_drive.dart index 3626b495..2d542f84 100644 --- a/autonomy/lib/src/drive/sim_drive.dart +++ b/autonomy/lib/src/drive/sim_drive.dart @@ -54,6 +54,13 @@ class DriveSimulator extends DriveInterface { goalOrientation: orientation, ); + @override + StateInterface spinForArucoState(int arucoId, {CameraName? desiredCamera}) => + FunctionalState( + controller, + onEnter: (controller) => controller.popState(), + ); + @override Future init() async => true; diff --git a/autonomy/lib/src/drive/timed_drive.dart b/autonomy/lib/src/drive/timed_drive.dart index 131ff8ab..4c5546cb 100644 --- a/autonomy/lib/src/drive/timed_drive.dart +++ b/autonomy/lib/src/drive/timed_drive.dart @@ -52,6 +52,10 @@ class TimedDrive extends DriveInterface with RoverDriveCommands { ); } + @override + StateInterface spinForArucoState(int arucoId, {CameraName? desiredCamera}) => + throw UnsupportedError("Cannot spin for aruco using TimedDrive"); + @override StateInterface driveForwardState(GpsCoordinates coordinates) => _TimedOperationState( diff --git a/autonomy/lib/src/orchestrator/rover_orchestrator.dart b/autonomy/lib/src/orchestrator/rover_orchestrator.dart index 8e05835d..3c86edd7 100644 --- a/autonomy/lib/src/orchestrator/rover_orchestrator.dart +++ b/autonomy/lib/src/orchestrator/rover_orchestrator.dart @@ -2,6 +2,7 @@ import "dart:math"; import "package:autonomy/constants.dart"; import "package:autonomy/interfaces.dart"; +import "package:autonomy/src/state_machine/rover_states/deferred_state.dart"; import "dart:async"; import "package:coordinate_converter/coordinate_converter.dart"; @@ -273,155 +274,256 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { // Go to GPS coordinates collection.logger.info("Got ArUco Task"); - DetectedObject? detectedAruco; + var driveToDestinationFailed = false; + var arucoSearchFailed = false; - 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( - command.arucoId, - desiredCamera: Constants.arucoDetectionCamera, - ); - detectedAruco = collection.video.getArucoDetection( - command.arucoId, - desiredCamera: Constants.arucoDetectionCamera, - ); + final arucoTaskSequence = SequenceState( + controller, + steps: [ + if (command.destination != + GpsCoordinates(latitude: 0, longitude: 0)) ...[ + PathingState( + controller, + collection: collection, + orchestrator: this, + destination: command.destination, + tolerance: 3, + ), + FunctionalState( + controller, + onEnter: (controller) { + if (!collection.gps.isNear(command.destination, 3.25)) { + // handle error + driveToDestinationFailed = true; + } else { + // starting search + } + controller.popState(); + }, + ), + ], + // Find the aruco tag + FunctionalState( + controller, + onEnter: (controller) { + currentState = AutonomyState.SEARCHING; + controller.popState(); + }, + ), + collection.drive.spinForArucoState( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ), + // Check if tag was found and face it + DeferredState(controller, (controller) { + final detectedAruco = collection.video.getArucoDetection( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ); + if (detectedAruco == null) { + currentState = AutonomyState.NO_SOLUTION; + return FunctionalState( + controller, + onEnter: (controller) { + arucoSearchFailed = true; + controller.popState(); + }, + ); + } + collection.logger.info("Found aruco"); + currentState = AutonomyState.APPROACHING; + return collection.drive.faceOrientationState( + Orientation(z: collection.imu.heading - detectedAruco.yaw), + ); + }), + // Wait 3 seconds for a detection + TimeoutDecorator( + timeout: const Duration(seconds: 3), + onTimeout: (controller) => controller.popState(), + child: FunctionalState( + controller, + onUpdate: (controller) { + if (collection.video.getArucoDetection( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ) != + null) { + controller.popState(); + } + }, + ), + ), + DeferredState(controller, (controller) { + final detectedAruco = collection.video.getArucoDetection( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ); + if (detectedAruco == null || !detectedAruco.hasBestPnpResult()) { + currentState = AutonomyState.NO_SOLUTION; + return FunctionalState( + controller, + onEnter: (controller) { + collection.logger.error( + "Could not find desired Aruco tag after rotating towards it", + ); + currentState = AutonomyState.NO_SOLUTION; + arucoSearchFailed = true; + }, + ); + } + 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 + return FunctionalState( + controller, + onEnter: (controller) { + collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); + currentState = AutonomyState.AT_DESTINATION; + controller.popState(); + }, + ); + } - if (!didSeeAruco || detectedAruco == null) { - collection.logger.error("Could not find desired Aruco tag"); - currentState = AutonomyState.NO_SOLUTION; - currentCommand = null; - return; - } + final relativeX = + -distanceToTag * + sin((collection.imu.heading - detectedAruco.yaw) * pi / 180); + final relativeY = + distanceToTag * + cos((collection.imu.heading - detectedAruco.yaw) * pi / 180); - collection.logger.info("Found aruco"); - currentState = AutonomyState.APPROACHING; - 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), - ); + final destinationCoordinates = + (collection.gps.coordinates.toUTM() + + UTMCoordinates(y: relativeY, x: relativeX, zoneNumber: 1)) + .toGps(); - if (detectedAruco == null || !detectedAruco.hasBestPnpResult()) { - // TODO: handle this condition properly - collection.logger.error( - "Could not find desired Aruco tag after rotating towards it", - ); - currentState = AutonomyState.NO_SOLUTION; - currentCommand = null; - return; - } + return PathingState( + controller, + collection: collection, + orchestrator: this, + destination: destinationCoordinates, + tolerance: 2, + ); + }), - collection.logger.debug( - "Planning path to Aruco ID ${command.arucoId}", - body: "Detection: ${detectedAruco.toProto3Json()}", + // Re-spin towards tag + DeferredState(controller, (controller) { + final detectedAruco = collection.video.getArucoDetection( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ); + collection.logger.info("Arrived at estimated Aruco position"); + if (detectedAruco == null) { + collection.logger.info("Re-spinning to find Aruco"); + // Re-find aruco, and point towards it + return SequenceState( + controller, + steps: [ + collection.drive.spinForArucoState( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ), + DeferredState(controller, (controller) { + final detectedAruco = collection.video.getArucoDetection( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ); + if (detectedAruco != null) { + collection.logger.info("Rotating towards Aruco"); + return collection.drive.faceOrientationState( + Orientation( + z: collection.imu.heading - detectedAruco.yaw, + ), + ); + } else { + return FunctionalState( + controller, + onEnter: (controller) { + collection.logger.warning( + "Could not find Aruco after following path", + ); + controller.popState(); + }, + ); + } + }), + ], + ); + } + return FunctionalState( + controller, + onEnter: (controller) => controller.popState(), + ); + }), + FunctionalState( + controller, + onEnter: (controller) { + collection.logger.info( + "Successfully reached within ${Constants.maxErrorMeters} meters of the Aruco tag", + ); + currentState = AutonomyState.AT_DESTINATION; + collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); + controller.popState(); + // TODO: end command execution, we are done + }, + ), + ], ); - // 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; - currentCommand = null; - return; - } + controller.pushState(arucoTaskSequence); - 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.toUTM() + - UTMCoordinates(y: relativeY, x: relativeX, zoneNumber: 1)) - .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), + executionTimer = PeriodicTimer(const Duration(milliseconds: 10), ( + timer, + ) async { + if (currentCommand == null) { + collection.logger.warning( + "Execution timer running while command is null", + body: "Canceling timer", ); - return distanceToTag < 1; - }, - )) { - collection.logger.error("Could not spin towards ArUco tag"); - currentState = AutonomyState.NO_SOLUTION; - 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, - ); - } + onCommandEnd(); + timer.cancel(); + return; + } - 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"); - } + if (driveToDestinationFailed) { + collection.logger.error("Failed to drive to initial destination"); + onCommandEnd(); + timer.cancel(); + return; + } - collection.logger.info( - "Successfully reached within ${Constants.maxErrorMeters} meters of the Aruco tag", - ); - collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); - currentState = AutonomyState.AT_DESTINATION; + if (arucoSearchFailed) { + collection.logger.error("Failed to find an Aruco tag"); + currentState = AutonomyState.NO_SOLUTION; + onCommandEnd(); + timer.cancel(); + return; + } - currentCommand = null; + if (!controller.hasState()) { + currentState = AutonomyState.NO_SOLUTION; + onCommandEnd(); + timer.cancel(); + return; + } + + controller.update(); + }); } @override diff --git a/autonomy/lib/src/state_machine/rover_states/deferred_state.dart b/autonomy/lib/src/state_machine/rover_states/deferred_state.dart new file mode 100644 index 00000000..dc51e42b --- /dev/null +++ b/autonomy/lib/src/state_machine/rover_states/deferred_state.dart @@ -0,0 +1,12 @@ +import "package:autonomy/autonomy.dart"; + +class DeferredState extends RoverState { + final StateInterface Function(StateController controller) supplier; + + DeferredState(super.controller, this.supplier); + + @override + void enter() { + controller.transitionTo(supplier(controller)); + } +} diff --git a/autonomy/lib/src/state_machine/rover_states/navigation.dart b/autonomy/lib/src/state_machine/rover_states/navigation.dart index c20f81dc..101d54b9 100644 --- a/autonomy/lib/src/state_machine/rover_states/navigation.dart +++ b/autonomy/lib/src/state_machine/rover_states/navigation.dart @@ -1,3 +1,4 @@ +import "dart:async"; import "dart:math"; import "package:autonomy/constants.dart"; @@ -23,6 +24,9 @@ class NavigationState extends RoverState { /// The final destination to navigate to final GpsCoordinates destination; + // The tolerance of how to be to the destination + final double tolerance; + /// Whether or not the state has performed pre-step correction bool hasCorrected = false; @@ -32,6 +36,8 @@ class NavigationState extends RoverState { /// The index of the waypoint being followed int waypointIndex = 0; + Timer? _doneCheckTimer; + /// The current step of the path being followed AutonomyAStarState? get currentPathState { if (orchestrator.currentPath == null || @@ -47,6 +53,7 @@ class NavigationState extends RoverState { required this.collection, required this.orchestrator, required this.destination, + this.tolerance = Constants.maxErrorMeters, }); @override @@ -56,6 +63,11 @@ class NavigationState extends RoverState { hasFollowed = false; orchestrator.currentState = AutonomyState.DRIVING; + + _doneCheckTimer = Timer.periodic( + const Duration(milliseconds: 20), + _checkIfDone, + ); } /// Checks if the rover is oriented properly before driving the [state] @@ -110,6 +122,7 @@ class NavigationState extends RoverState { collection: collection, orchestrator: orchestrator, destination: destination, + tolerance: tolerance, ), ); return true; @@ -130,6 +143,16 @@ class NavigationState extends RoverState { } } + void _checkIfDone([_]) { + if (collection.gps.isNear(destination, tolerance)) { + controller.popUntil(); + controller.popState(); + collection.drive.stop(); + _doneCheckTimer?.cancel(); + _doneCheckTimer = null; + } + } + @override void update() { if (currentPathState == null) { @@ -158,6 +181,7 @@ class NavigationState extends RoverState { collection: collection, orchestrator: orchestrator, destination: destination, + tolerance: tolerance, ), ); return; @@ -172,6 +196,7 @@ class NavigationState extends RoverState { collection: collection, orchestrator: orchestrator, destination: destination, + tolerance: tolerance, ), ); return; @@ -187,5 +212,7 @@ class NavigationState extends RoverState { if (currentPathState != null) { orchestrator.traversed.add(currentPathState!.position); } + _doneCheckTimer?.cancel(); + _doneCheckTimer = null; } } diff --git a/autonomy/lib/src/state_machine/rover_states/pathing.dart b/autonomy/lib/src/state_machine/rover_states/pathing.dart index 81166038..ce50e430 100644 --- a/autonomy/lib/src/state_machine/rover_states/pathing.dart +++ b/autonomy/lib/src/state_machine/rover_states/pathing.dart @@ -15,12 +15,16 @@ class PathingState extends RoverState { /// The destination to plan a path to final GpsCoordinates destination; + // The tolerance of how to be to the destination + final double tolerance; + /// Default constructor for [PathingState] PathingState( super.controller, { required this.collection, required this.orchestrator, required this.destination, + this.tolerance = Constants.maxErrorMeters, }); @override @@ -58,6 +62,7 @@ class PathingState extends RoverState { collection: collection, orchestrator: orchestrator, destination: destination, + tolerance: tolerance, ), ); } diff --git a/autonomy/lib/src/state_machine/rover_states/spin_for_aruco.dart b/autonomy/lib/src/state_machine/rover_states/spin_for_aruco.dart new file mode 100644 index 00000000..56ccd3e9 --- /dev/null +++ b/autonomy/lib/src/state_machine/rover_states/spin_for_aruco.dart @@ -0,0 +1,61 @@ +import "package:autonomy/autonomy.dart"; +import "package:autonomy/src/drive/drive_commands.dart"; +import "package:autonomy/src/drive/drive_config.dart"; + +class SpinForAruco extends RoverState { + final AutonomyInterface collection; + final RoverDriveCommands drive; + + DriveConfig get config => collection.drive.config; + + final int arucoId; + final CameraName? desiredCamera; + + double _startOrientation = 0; + bool _rotated180 = false; + + SpinForAruco( + super.controller, { + required this.drive, + required this.arucoId, + required this.collection, + this.desiredCamera, + }); + + @override + void enter() { + _startOrientation = collection.imu.heading; + _rotated180 = false; + } + + @override + void update() { + drive.setThrottle(0.05); + + if (collection.video.getArucoDetection( + arucoId, + desiredCamera: desiredCamera, + ) != + null) { + controller.popState(); + return; + } + + drive.spinLeft(); + + if ((collection.imu.heading - _startOrientation).clampHalfAngle() > 175) { + _rotated180 = true; + } + + if (_rotated180 && + collection.imu.isNear(Orientation(z: _startOrientation))) { + controller.popState(); + return; + } + } + + @override + void exit() { + drive.stop(); + } +} diff --git a/autonomy/test/network_test.dart b/autonomy/test/network_test.dart index 78912f24..fe71bf5c 100644 --- a/autonomy/test/network_test.dart +++ b/autonomy/test/network_test.dart @@ -139,8 +139,8 @@ void main() => group("[Network]", tags: ["network"], () { expect(subsystems.throttle, 0); expect(subsystems.left, 0); expect(subsystems.right, 0); - expect(simulator.gps.isNear(origin), isTrue); - expect(simulator.gps.isNear(oneMeter), isFalse); + expect(simulator.gps.isNear(origin, 1), isTrue); + expect(simulator.gps.isNear(oneMeter, 1), isFalse); expect(subsystems.throttleFlag, isFalse); final forwardFuture = Future.delayed( @@ -158,8 +158,8 @@ void main() => group("[Network]", tags: ["network"], () { expect(subsystems.throttle, isNot(0)); expect(subsystems.left, isNot(0)); expect(subsystems.right, isNot(0)); - expect(simulator.gps.isNear(origin), isTrue); - expect(simulator.gps.isNear(oneMeter), isFalse); + expect(simulator.gps.isNear(origin, 1), isTrue); + expect(simulator.gps.isNear(oneMeter, 1), isFalse); simulator.orchestrator.controller.update(); simulator.orchestrator.controller.update();