public async Task <bool> StartTrackingAsync(string mapName)
{
_skillHelper.LogMessage($"Attempting to track within map {mapName}.");
_abort = false;
// We don't get the newly loaded map back from Occ software until we start and stop tracking. So...
_skillHelper.LogMessage("Loading map and starting tracking.");
_misty.SetCurrentSlamMap(mapName, OnResponse);
await Task.Delay(1000);
_misty.StartTracking(OnResponse);
await _skillHelper.MoveHeadAsync(_headPitchOffset, _headRollOffset, _headYawOffset);
_misty.RegisterSlamStatusEvent(SlamStatusCallback, 0, true, "MapDockSlamStatusEvent", null, OnResponse);
_misty.RegisterSelfStateEvent(SelfStateCallback, 250, true, "MapDockSelfStateEvent", OnResponse);
// There is a current defect where switching maps does not fully take effect until we start tracking
// and stop tracking. So we need to start, stop, and re-start.
_misty.StopTracking(OnResponse);
await Task.Delay(4000);
_misty.StartTracking(OnResponse);
await Task.Delay(4000);
_misty.GetMap(OnResponse);
await Task.Delay(4000);
if (_slamStatus == null || _slamStatus.SensorStatus != MistyRobotics.Common.Types.SlamSensorMode.Streaming)
{
_skillHelper.LogMessage("Failed to start tracking.");
Cleanup();
return(false);
}
_skillHelper.LogMessage("Checking for pose.");
int count = 0;
while (_slamStatus.RunMode != MistyRobotics.Common.Types.SlamRunningMode.Tracking && count++ < 40)
{
await _skillHelper.TurnAsync(10);
_misty.GetSlamStatus(OnResponse);
}
if (_slamStatus.RunMode == MistyRobotics.Common.Types.SlamRunningMode.Tracking)
{
_skillHelper.LogMessage($"Pose acquired. Map cell is [{_mapCell.X},{_mapCell.Y}]. Map yaw is {_mapYaw:f2}.");
_tracking = true;
}
else
{
_skillHelper.LogMessage("Unable to obtain pose.");
Cleanup();
_tracking = false;
}
return(_tracking);
}
private async Task <bool> ExecuteDockAsync()
{
_charging = false;
// Set head position.
await _skillHelper.MoveHeadAsync(_headPitchOffset, _headRollOffset, _headYawOffset);
if (_abort)
{
return(false);
}
// Find the charger.
if (!await FindChargerAsync())
{
return(false);
}
// Get Misty aligned with charger based upon charger pose X and Euler angle.
int retries = 0;
while (Math.Abs(_chargerPose.X) > ALIGNED_X || Math.Abs(_chargerPose.EulerYaw) > ALIGNED_EULER_YAW)
{
if (retries++ > ALIGN_MAX_RETRIES)
{
_skillHelper.LogMessage("Failed to align with charger.");
return(false);
}
_skillHelper.LogMessage($"Charger position is [{_chargerPose.X:f3}, {_chargerPose.Y:f3}, {_chargerPose.Z:f3}] meters. Euler yaw is {_chargerPose.EulerYaw:f3} degrees.");
await FaceChargerAsync(ALIGNED_X);
if (_abort)
{
return(false);
}
if (Math.Abs(_chargerPose.X) > ALIGNED_X || Math.Abs(_chargerPose.EulerYaw) > ALIGNED_EULER_YAW)
{
await AlignWithChargerAsync();
if (Math.Abs(_chargerPose.Z - IDEAL_ALIGNMENT_DISTANCE) > .1)
{
await _skillHelper.DriveAsync(_chargerPose.Z - IDEAL_ALIGNMENT_DISTANCE);
}
}
}
_skillHelper.LogMessage($"Charger position is [{_chargerPose.X:f3}, {_chargerPose.Y:f3}, {_chargerPose.Z:f3}] meters. Euler yaw is {_chargerPose.EulerYaw:f3} degrees.");
// Backup and drive straight forward to check alignment.
double chargerDistance = _chargerPose.Z;
var offsets = new List <double>();
await _skillHelper.DriveAsync(-1.0, true);
await Task.Delay(1000);
offsets.Add(_chargerPose.X);
for (int i = 0; i < 6; i++)
{
await _skillHelper.DriveAsync(.25, true);
await Task.Delay(1000);
offsets.Add(_chargerPose.X);
}
foreach (var o in offsets)
{
_skillHelper.LogMessage(o.ToString("f3"));
}
double slope = (offsets[0] - offsets.Last()) / 1.5;
double estimateFinalOffset = offsets.Last() - slope * 0.5;
double offsetAngle = -Math.Asin(estimateFinalOffset / 0.4) * 180.0 / Math.PI;
_skillHelper.LogMessage($"Estimate final offset {estimateFinalOffset:f3} meters and {offsetAngle} degrees.");
if (Math.Abs(offsetAngle) > FINAL_OFFSET_ANGLE_MAX)
{
_skillHelper.LogMessage("Offset angle is too large. Backing up and retrying dock process.");
await _skillHelper.DriveAsync(-IDEAL_ALIGNMENT_DISTANCE + 0.25);
return(false);
}
await _skillHelper.TurnAsync(offsetAngle);
// Turn around.
_skillHelper.LogMessage("Turning 180 degrees to face away from charger.");
await _skillHelper.TurnAsync(-180);
if (_abort)
{
return(false);
}
// Back on to charger.
_skillHelper.LogMessage($"Driving {chargerDistance - 0.5 + CHARGER_DOCK_OVERSHOOT:f3} meters to back on to charger.");
await _skillHelper.DriveAsync(-chargerDistance + 0.5 - CHARGER_DOCK_OVERSHOOT, true);
if (_abort)
{
return(false);
}
// Check if we've ended up on top of the wedge.
await Task.Delay(1000);
_skillHelper.LogMessage($"Roll = {_skillHelper.ImuRoll:f3}. Pitch = {_skillHelper.ImuPitch:f3}.");
if (Math.Abs(_skillHelper.ImuRoll) > MISTY_ON_WEDGE_ROLL || Math.Abs(AxisRotation(_initialPitch, _skillHelper.ImuPitch)) > MISTY_ON_WEDGE_PITCH)
{
// We're on the wedge. Drive away from the charger and try again.
_skillHelper.LogMessage($"We appear to have driven on top of the alignment wedge. IMU roll is {_skillHelper.ImuRoll:f3}. IMU pitch is {_skillHelper.ImuPitch:f3}.");
await _skillHelper.DriveAsync(IDEAL_ALIGNMENT_DISTANCE - 0.1);
if (_abort)
{
return(false);
}
await _skillHelper.TurnAsync(180);
_chargerPose = null;
return(false);
}
// Check that we're fully docked: back up a little bit to get more aligned.
_misty.DriveHeading(0, 0.2, 500, true, OnResponse);
// It can take several seconds for the charging indicator to update...
await Task.Delay(7000);
// Check that we're charging.
if (!_charging)
{
_misty.PlayAudio("s_Anger3.wav", 100, OnResponse);
await _skillHelper.DriveAsync(IDEAL_ALIGNMENT_DISTANCE - 0.1);
if (_abort)
{
return(false);
}
await _skillHelper.TurnAsync(180);
_chargerPose = null;
return(false);
}
_misty.PlayAudio("s_Ecstacy.wav", 100, OnResponse);
return(true);
}
private Task <bool> TurnAsync(double degrees)
{
return(_skillHelper.TurnAsync(degrees));
}
