private void ArTagCallback(IArTagDetectionEvent e)
{
if (e.TagId == _tagId)
{
float[] matrix = e.ArTagPose.HomogeneousMatrix;
//_skillHelper.LogMessage($"AR tag pose: [{matrix[0]}, {matrix[1]}, {matrix[2]}, {matrix[3]}, {matrix[4]}, {matrix[5]}, {matrix[6]}, {matrix[7]}, {matrix[8]}, {matrix[9]}, {matrix[10]}, {matrix[11]}, {matrix[12]}, {matrix[13]}, {matrix[14]}, {matrix[15]}]");
_currentTagPose = NavigationHelper.ConvertPose(matrix);
//_skillHelper.LogMessage("AR tag pose: " + _currentTagPose.ToString());
}
}
/// <summary>
/// Converts a 16 element 3D pose matrix in Occipital coordinates to a SimplePose
/// </summary>
public static Simple2DPose ConvertPose(float[] pose)
{
var simplePose = new Simple2DPose()
{
X = pose[14],
Y = -pose[12],
Yaw = Math.Asin(pose[2])
};
return(simplePose);
}
private void ChargerPoseMessage(IChargerPoseEvent eventResponse)
{
float[] matrix = eventResponse.Pose.HomogeneousMatrix;
//_skillHelper.LogMessage($"Charger pose: [{matrix[0]}, {matrix[1]}, {matrix[2]}, {matrix[3]}, {matrix[4]}, {matrix[5]}, {matrix[6]}, {matrix[7]}, {matrix[8]}, {matrix[9]}, {matrix[10]}, {matrix[11]}, {matrix[12]}, {matrix[13]}, {matrix[14]}, {matrix[15]}]");
// Occipital coordinates have left-right axis as positive to the right.
// Occipital origin is 0.035mm to the right of Misty.
// So we add 0.035 to the received value to get a left-right value for Misty's body.
// The _charger_y_offset is an optionally manually set offset that may help some Misty's dock more reliably.
matrix[12] += (0.035f + _charger_y_offset);
_chargerPose = NavigationHelper.ConvertPose(matrix);
//_skillHelper.LogMessage("Charger pose: " + _chargerPose.ToString());
}
// Sequentially invoke each command in collection provided.
public async Task <bool> Execute(List <IFollowPathCommand> commands)
{
_abort = false;
bool success = true;
// Disabling for now as we get too many false positives.
_skillHelper.DisableHazardSystem();
foreach (IFollowPathCommand cmd in commands)
{
if (cmd is DriveCommand)
{
success = await _skillHelper.DriveAsync(((DriveCommand)cmd).Meters);
}
else if (cmd is TurnCommand)
{
success = await _skillHelper.TurnAsync(((TurnCommand)cmd).Degrees);
}
else if (cmd is ARTagAlignment)
{
ARTagAlignment arCmd = (ARTagAlignment)cmd;
var goalTagPose = new Simple2DPose()
{
X = arCmd.X,
Y = arCmd.Y,
Yaw = arCmd.Yaw.Radians()
};
success = await _arTagAligner.AlignToArTag(arCmd.Dictionary, arCmd.Size, arCmd.TagId, goalTagPose);
}
else if (cmd is DelegateCommand)
{
success = await((DelegateCommand)cmd).Delegate.Invoke(((DelegateCommand)cmd).Argument);
}
else if (cmd is DockCommand)
{
success = await _docker.DockAsync();
}
if (_abort || !success)
{
break;
}
}
_skillHelper.EnableHazardSystem();
return(success);
}
/// <summary>
/// Returns the location of object 1 in object 2's coordinate system given the location of object 2 in object 1's coordinate system.
/// </summary>
public static Point SwapCoordinateSystems(Simple2DPose object2Pose)
{
// Translate the coordinate system.
var object1PoseTranslated = new Point()
{
X = -object2Pose.X,
Y = -object2Pose.Y
};
// Rotate the coordinate system.
// https://en.wikipedia.org/wiki/Rotation_matrix
double rotation = Math.PI - object2Pose.Yaw;
var object1Pose = new Point()
{
X = object1PoseTranslated.X * Math.Cos(rotation) - object1PoseTranslated.Y * Math.Sin(rotation),
Y = object1PoseTranslated.X * Math.Sin(rotation) + object1PoseTranslated.Y * Math.Cos(rotation)
};
return(object1Pose);
}
private async Task <bool> ExecuteDockAsync(bool lastTry)
{
// Find the charger. It should be right in front of us.
// Depending upon how far away from the charger we are we may need a different head pitch.
// Start by assuming that we're fairly close to the charger which means we need to look more downward to see the charger.
double headPitch = 35;
await _skillHelper.MoveHeadAsync(headPitch, 0, 0);
if (!await FindChargerAsync())
{
headPitch = 15;
await _skillHelper.MoveHeadAsync(headPitch, 0, 0);
if (!await FindChargerAsync())
{
_skillHelper.MistySpeak("I can't find the charger.");
await Task.Delay(3000);
return(false);
}
}
_skillHelper.MistySpeak("I see the charger.");
_skillHelper.LogMessage($"Charger is located at [x, y, yaw] : [{_chargerPose.X:f3}, {_chargerPose.Y:f3}, {_chargerPose.Yaw.Degrees():f1}].");
int retries = 0;
while (_chargerPose.X - IDEAL_ALIGNMENT_DISTANCE > 0.04 || IDEAL_ALIGNMENT_DISTANCE - _chargerPose.X > 0.02 ||
Math.Abs(_chargerPose.Y) > 0.01 || Math.Abs(_chargerPose.Yaw.Degrees()) > 2.5)
{
var goalPose = new Simple2DPose()
{
X = IDEAL_ALIGNMENT_DISTANCE,
Y = 0,
Yaw = 0
};
MoveSequence moveSequence = NavigationHelper.CalculateMoveSequence(_chargerPose, goalPose);
_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.");
if (!await _skillHelper.TurnAsync(moveSequence.Turn1.Degrees()))
{
return(false);
}
if (_abort)
{
return(false);
}
if (!await _skillHelper.DriveAsync(moveSequence.DriveDistance, true))
{
return(false);
}
if (_abort)
{
return(false);
}
if (!await _skillHelper.TurnAsync(moveSequence.Turn2.Degrees()))
{
return(false);
}
if (_abort)
{
return(false);
}
if (headPitch != 35)
{
// In case we had to lift the head to originally find the charger.
headPitch = 35;
await _skillHelper.MoveHeadAsync(headPitch, 0, 0);
}
await Task.Delay(1000);
if (!await CanSeeChargerAsync(1))
{
_skillHelper.LogMessage("Can no longer see the charger.");
_skillHelper.MistySpeak("Uh oh. I can't see the charger any more.");
return(false);
}
_skillHelper.LogMessage($"Charger now located at [x, y, yaw] : [{_chargerPose.X:f3}, {_chargerPose.Y:f3}, {_chargerPose.Yaw.Degrees():f1}]. " +
$"Error : [{(_chargerPose.X - goalPose.X):f3}, {(_chargerPose.Y - goalPose.Y):f3}, {(_chargerPose.Yaw.Degrees() - goalPose.Yaw.Degrees()):f1}].");
if (retries++ > 5)
{
_skillHelper.LogMessage("Failed to line up with the charger.");
_skillHelper.MistySpeak("I can't seem to line up right.");
await Task.Delay(5000);
return(false);
}
}
_skillHelper.MistySpeak("I should be all lined up now. Going to turn around and back on to the charger.");
// Turn around.
// Note that we do 2 x 91 degrees. Misty tends to turn slightly less than requested and any turn where the absolute
// value is over 180 degrees will get converted to a <180 turn in the other direction.
_skillHelper.LogMessage("Turning 180 degrees to face away from charger.");
if (!await _skillHelper.TurnAsync(91))
{
return(false);
}
if (_abort)
{
return(false);
}
if (!await _skillHelper.TurnAsync(91))
{
return(false);
}
if (_abort)
{
return(false);
}
// Back on to charger.
_skillHelper.LogMessage($"Driving {IDEAL_ALIGNMENT_DISTANCE:f3} meters to back on to charger.");
if (!await _skillHelper.DriveAsync(-IDEAL_ALIGNMENT_DISTANCE - 0.1, true))
{
return(false);
}
if (_abort)
{
return(false);
}
// Move forward slightly.
_misty.DriveHeading(0, 0.01, 1000, false, OnResponse);
// It can take several seconds for the charging indicator to update...
_charging = false;
DateTime start = DateTime.Now;
while (!_charging && DateTime.Now.Subtract(start).TotalSeconds < 8)
{
await Task.Delay(250);
}
// If charging then the battery current is a positive number about 0.4 Amps
if (_charging)
{
_skillHelper.MistySpeak("Ahh. I feel the power.");
await Task.Delay(2000);
}
else
{
if (!lastTry)
{
_skillHelper.MistySpeak("Hmm. I don't seem to be charging. I'm going to drive forward and try docking again.");
_skillHelper.LogMessage("Did not detect that we're charging. Driving forward and trying again.");
await _skillHelper.DriveAsync(IDEAL_ALIGNMENT_DISTANCE + 0.1);
if (_abort)
{
return(false);
}
await _skillHelper.TurnAsync(180);
return(false);
}
}
return(true);
}
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);
}
/// <summary>
/// Given the pose of a target from two different locations, determine the MoveSequence required to move
/// from position1 to position2.
/// </summary>
public static MoveSequence CalculateMoveSequence(Simple2DPose targetFromPosition1, Simple2DPose targetFromPosition2)
{
System.Diagnostics.Debug.WriteLine("Target from position1: " + targetFromPosition1.ToString());
System.Diagnostics.Debug.WriteLine("Target from position2: " + targetFromPosition2.ToString());
// Get the coordinates of both positions in the target's coordinate system.
Point position1FromTarget = SwapCoordinateSystems(targetFromPosition1);
Point position2FromTarget = SwapCoordinateSystems(targetFromPosition2);
System.Diagnostics.Debug.WriteLine($"Position 1 from target: [{position1FromTarget.X:f3}, {position1FromTarget.Y:f3}]");
System.Diagnostics.Debug.WriteLine($"Position 2 from target: [{position2FromTarget.X:f3}, {position2FromTarget.Y:f3}]");
// Get the movement needed from position 1 to position 2 in the target's coordinate system.
var moveInTargetCoordinates = new Point()
{
X = position2FromTarget.X - position1FromTarget.X,
Y = position2FromTarget.Y - position1FromTarget.Y
};
System.Diagnostics.Debug.WriteLine($"Movement needed in target coordinate system: [{moveInTargetCoordinates.X:f3}, {moveInTargetCoordinates.Y:f3}]");
// Rotate the movement back to position 1's coordinate system.
double rotation = Math.PI + targetFromPosition1.Yaw;
var moveInPosition1Coordinates = new Point()
{
X = moveInTargetCoordinates.X * Math.Cos(rotation) - moveInTargetCoordinates.Y * Math.Sin(rotation),
Y = moveInTargetCoordinates.X * Math.Sin(rotation) + moveInTargetCoordinates.Y * Math.Cos(rotation)
};
System.Diagnostics.Debug.WriteLine($"Movement needed in position 1's coordinate system: [{moveInPosition1Coordinates.X:f3}, {moveInPosition1Coordinates.Y:f3}]");
// Get distance and bearing from position 1 to position 2 in position 1's coordinate system.
double distanceFromPosition1ToPosition2 = Math.Sqrt(Math.Pow(moveInPosition1Coordinates.X, 2) + Math.Pow(moveInPosition1Coordinates.Y, 2));
double bearingFromPosition1ToPosition2;
if (moveInPosition1Coordinates.X == 0)
{
if (moveInPosition1Coordinates.Y > 0)
{
// Moving directly to the left
bearingFromPosition1ToPosition2 = Math.PI / 2.0;
}
else
{
// Moving directly to the right
bearingFromPosition1ToPosition2 = -Math.PI / 2.0;
}
}
else if (moveInPosition1Coordinates.X > 0)
{
// Moving towards target
bearingFromPosition1ToPosition2 = Math.Atan(moveInPosition1Coordinates.Y / moveInPosition1Coordinates.X);
}
else
{
// Moving away from target
bearingFromPosition1ToPosition2 = Math.PI + Math.Atan(moveInPosition1Coordinates.Y / moveInPosition1Coordinates.X);
}
bearingFromPosition1ToPosition2 = NormalizeTurn(bearingFromPosition1ToPosition2);
System.Diagnostics.Debug.WriteLine($"Bearing from position 1 to position 2: {bearingFromPosition1ToPosition2.Degrees():f0}");
// Determine the final turn needed to face the target after moving to position 2.
var targetFromPosition2InPosition1Coordinates = new Point()
{
X = targetFromPosition1.X - moveInPosition1Coordinates.X,
Y = targetFromPosition1.Y - moveInPosition1Coordinates.Y
};
targetFromPosition2InPosition1Coordinates.X = targetFromPosition2InPosition1Coordinates.X == 0 ? 0.00001 : targetFromPosition2InPosition1Coordinates.X;
double faceTarget = -bearingFromPosition1ToPosition2 + Math.Atan(targetFromPosition2InPosition1Coordinates.Y / targetFromPosition2InPosition1Coordinates.X);
faceTarget = NormalizeTurn(faceTarget);
// Adjust face target turn so we end with the desired orientation.
targetFromPosition2.X = targetFromPosition2.X == 0 ? 0.00001 : targetFromPosition2.X;
double finalTurn = faceTarget - Math.Atan(targetFromPosition2.Y / targetFromPosition2.X);
finalTurn = NormalizeTurn(finalTurn);
return(new MoveSequence()
{
Turn1 = bearingFromPosition1ToPosition2,
DriveDistance = distanceFromPosition1ToPosition2,
Turn2 = finalTurn
});
}
