/// <summary>
/// Function subscribed to AutonomousService's ObstacleEvent to handle an ObstacleEvent when it's triggered.
///
/// This function gets the Plot representing the detected obstacles and uses the current DriveState's
/// FindBestGap() to determine the best gap to drive toward. It then issues the corresponding DriveCommand.
/// </summary>
/// <param name="sender"></param>
/// <param name="e">ObstacleEventArgs that contains the Plot representing the detected obstacles.</param>
public void HandleObstacleEvent(Object sender, ObstacleEventArgs e)
{
DriveCommand driveCommand;
Plot obstacles = e.Data;
// Find the best gap
Line bestGap = this.driveState.FindBestGap(obstacles);
// If bestGap exists, drive toward it's midpoint. Otherwise, turn right
if (bestGap != null)
{
Coordinate midpoint = bestGap.FindMidpoint();
double angle = midpoint.Theta;
driveCommand = new DriveCommand(angle, Speed.CLEAR_OBSTACLE);
}
else
{
driveCommand = DriveCommand.RightTurn(Speed.CLEAR_OBSTACLE);
}
// Send driveCommand
lock (this.sendDriveCommandLock)
{
DriveHandler.SendDriveCommand(driveCommand);
this.lastObstacleDetected = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
}
}
/// <summary>
/// Issue the specified DriveCommand to ROCKS through AscentPacketHandler.
///
/// If the SendDriveCommandLock cannot be obtained, do not send the DriveCommand. TODO why? Shore this up
/// </summary>
/// <param name="driveCommand">The DriveCommand to be executed.</param>
public void Drive(DriveCommand driveCommand)
{
// Obtain lock
bool isLocked = false;
Monitor.TryEnter(this.sendDriveCommandLock, ref isLocked);
if (isLocked)
{
// Send DriveCommand to AscentPacketHandler
DriveHandler.SendDriveCommand(driveCommand);
// Release lock
Monitor.Exit(this.sendDriveCommandLock);
}
}