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);
}
