private void NotifyNavigationEnded(bool isSuccessful)
{
if (OnNavigationStrategyEnded != null)
{
if (step == 1)
{
step++;
//navigate based on door detection finished
//now using sonar to make sure we can safely go throught the door
//could go back to step 1 if it's neccesary
if (_sonarDetector != null)
{
_sonarDetector.OnDistanceBigChangedDetected += SonarDetector_OnDistanceChanged;
_sonarDetector.OnObstacleTooClosed += SonarDetector_OnObstacleTooClosed;
}
if (!isSuccessful) //fail to approaching door change to manaul mode
{
NavigationStrategyEndedEventArgs args = new NavigationStrategyEndedEventArgs(isSuccessful, step);
}
}
else if (step == 2)
{
NavigationStrategyEndedEventArgs args = new NavigationStrategyEndedEventArgs(isSuccessful, step);
OnNavigationStrategyEnded(this, args);
}
}
}
void DoorOrientationNavigator_OnNavigationStrategyEnded(object sender, NavigationStrategyEndedEventArgs eventArgs)
{
// If the door navigation wasn't able to successfully finish
if (!eventArgs.IsComplete)
{
// If we encounter an error, return to user control
CurrentDriveType = DriveTypeStatus.UserDriven;
return;
}
DoorOrientationNavigator.OnNavigationStrategyEnded -= DoorOrientationNavigator_OnNavigationStrategyEnded;
//DoorOrientationNavigator = null;
CurrentDriveType = DriveTypeStatus.RoutDriven;
}