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