/// <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> /// Trigger an ObstacleEvent. /// </summary> /// <param name="e">ObstacleEventArgs</param> protected virtual void OnObstacleEvent(ObstacleEventArgs e) { EventHandler <ObstacleEventArgs> handler = ObstacleEvent; if (handler != null) { // BeginInvoke here? use same thread or spawn new thread? handler(this, e); } }