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> StartDockDetectorAsync()
{
_chargeDetectorRunning = false;
_misty.RegisterSlamStatusEvent(SlamStatusCallback, 0, true, "ChargerDockSlamStatus", null, OnResponse);
_skillHelper.LogMessage("Start dock detector.");
_misty.StartLocatingDockingStation(5000, 5000, OnResponse);
await Task.Delay(5000);
int retries = 0;
while (retries++ < 3 && !_chargeDetectorRunning)
{
string s = string.Empty;
foreach (var status in _slamStatus.StatusList)
{
s += status + "|";
}
if (_slamStatus == null)
{
_skillHelper.LogMessage($"Dock detector did not start. SLAM sensor status is null. Restarting SLAM service.");
}
else
{
_skillHelper.LogMessage($"Dock detector did not start. SLAM sensor status is {_slamStatus.SensorStatus}. SLAM status list is {s}. Restarting SLAM service.");
}
_misty.DisableSlamService(OnResponse);
await Task.Delay(5000);
_misty.EnableSlamService(OnResponse);
await Task.Delay(7000);
_skillHelper.LogMessage("Start dock detector.");
_misty.StartLocatingDockingStation(5000, 5000, OnResponse);
await Task.Delay(5000);
}
_misty.UnregisterEvent("ChargerDockSlamStatus", OnResponse);
if (_slamStatus == null || (_slamStatus.Status & DockingStationDetectorEnabled) == 0)
{
return(false);
}
else
{
return(true);
}
}
/// <summary>
/// Encapsulates starting charger detection with retries.
/// </summary>
private async Task <bool> StartChargerDetectionAsync()
{
_chargeDetectorRunning = false;
_misty.RegisterSlamStatusEvent(SlamStatusCallback, 0, true, "ChargerDockSlamStatus", null, OnResponse);
_skillHelper.LogMessage("Starting charger detector.");
_misty.StartLocatingDockingStation(5000, 5000, true, OnResponse);
await Task.Delay(5000);
int retries = 0;
while (retries++ < 3 && !_chargeDetectorRunning && !_abort)
{
if (_slamStatus == null)
{
_skillHelper.LogMessage($"Charger detection did not start. Occipital sensor status is null. Restarting Occipital service.");
}
else
{
string s = string.Empty;
foreach (var status in _slamStatus.StatusList)
{
s += status + "|";
}
_skillHelper.LogMessage($"Charger detection did not start. Occipital sensor status is {_slamStatus.SensorStatus}. Occipital status list is {s}. Restarting Occipital service.");
}
_misty.DisableSlamService(OnResponse);
await Task.Delay(5000);
_misty.EnableSlamService(OnResponse);
if (_abort)
{
break;
}
await Task.Delay(8000);
if (_abort)
{
break;
}
_misty.StartLocatingDockingStation(5000, 5000, true, OnResponse);
await Task.Delay(5000);
}
_misty.UnregisterEvent("ChargerDockSlamStatus", OnResponse);
//_misty.SetSlamIrExposureAndGain(0.01, 2.0, OnResponse);
if (!_chargeDetectorRunning)
{
_skillHelper.LogMessage("Failed to start the charger detector.");
_skillHelper.MistySpeak("I could not start the charger detector.");
return(false);
}
else
{
return(true);
}
}