public async Task <bool> AlignToArTag(int dictionary, double size, int tagId, Simple2DPose goalTagPose) { _abort = false; _tagId = tagId; _skillHelper.LogMessage($"Starting AR tag alignment. Desired tag pose is [x, y, yaw] {goalTagPose.X:f3}, {goalTagPose.Y:f3}, {goalTagPose.Yaw.Degrees():f0}]."); // Look straight ahead await _skillHelper.MoveHeadAsync(0, 0, 0); // Startup AR tag detection _misty.EnableCameraService(OnResponse); _misty.EnableCameraService(OnResponse); // Calling twice to make sure. TODO: something smarter _misty.StartArTagDetector(dictionary, size, OnResponse); _misty.StartArTagDetector(dictionary, size, OnResponse); // Calling twice to make sure. TODO: something smarter _misty.RegisterArTagDetectionEvent(ArTagCallback, 100, true, "artagevent", OnResponse); // Wait for measurments to start. if (!await WaitForMeasurementAsync(10, 2)) { if (_abort) { return(false); } // Never detected the AR tag. Try a small sweep. _skillHelper.MistySpeak("I can't see the tag. Looking around for it."); _skillHelper.LogMessage("Do not see the tag. Starting sweep."); bool arTagFound = false; await _skillHelper.TurnAsync(20); int maxTurns = 5; while (!arTagFound && --maxTurns >= 0) { arTagFound = await WaitForMeasurementAsync(1, 1); if (_abort) { return(false); } if (!arTagFound) { await _skillHelper.TurnAsync(-10); } } if (!arTagFound) { _skillHelper.MistySpeak("I cannot find the tag."); _skillHelper.LogMessage("Never detected the AR tag."); Cleanup(); return(false); } } _skillHelper.LogMessage($"AR tag located at [x, y, yaw] : [{_currentTagPose.X:f3}, {_currentTagPose.Y:f3}, {_currentTagPose.Yaw.Degrees():f1}]. " + $"Offset : [{(_currentTagPose.X - goalTagPose.X):f3}, {(_currentTagPose.Y - goalTagPose.Y):f3}, {(_currentTagPose.Yaw.Degrees() - goalTagPose.Yaw.Degrees()):f1}]."); if (Math.Abs(_currentTagPose.X - goalTagPose.X) < X_TOLERANCE && Math.Abs(_currentTagPose.Y - goalTagPose.Y) < Y_TOLERANCE && Math.Abs(_currentTagPose.Yaw.Degrees() - goalTagPose.Yaw.Degrees()) < YAW_TOLERANCE) { _skillHelper.MistySpeak("We're already aligned with the tag. No need to move."); _skillHelper.LogMessage("No movement needed to align to tag."); } else { int retries = 0; while (Math.Abs(_currentTagPose.X - goalTagPose.X) > X_TOLERANCE || Math.Abs(_currentTagPose.Y - goalTagPose.Y) > Y_TOLERANCE || Math.Abs(_currentTagPose.Yaw.Degrees() - goalTagPose.Yaw.Degrees()) > YAW_TOLERANCE) { MoveSequence moveSequence = NavigationHelper.CalculateMoveSequence(_currentTagPose, goalTagPose); _skillHelper.LogMessage($"Movement sequence to goal is turn {moveSequence.Turn1.Degrees():f0} degrees, drive {moveSequence.DriveDistance:f3} meters, " + $"and turn {moveSequence.Turn2.Degrees():f0} degrees."); await _skillHelper.TurnAsync(moveSequence.Turn1.Degrees()); if (_abort) { return(false); } await _skillHelper.DriveAsync(moveSequence.DriveDistance, true); if (_abort) { return(false); } await _skillHelper.TurnAsync(moveSequence.Turn2.Degrees()); if (_abort) { return(false); } _skillHelper.LogMessage($"AR tag located at [x, y, yaw] : [{_currentTagPose.X:f3}, {_currentTagPose.Y:f3}, {_currentTagPose.Yaw.Degrees():f1}]. " + $"Offset : [{(_currentTagPose.X - goalTagPose.X):f3}, {(_currentTagPose.Y - goalTagPose.Y):f3}, {(_currentTagPose.Yaw.Degrees() - goalTagPose.Yaw.Degrees()):f1}]."); if (retries++ > 5) { _skillHelper.LogMessage("Failed to line up with the tag."); _skillHelper.MistySpeak("I can't seem to line up right. I give up."); await Task.Delay(5000); Cleanup(); return(false); } } _skillHelper.MistySpeak("Alignment complete."); } Cleanup(); return(true); }