private static void ExitFromInvalidArgrument(String errorMessage)
{
// Invalid command line argument - write to error output, debug output, and base station
Console.Error.Write(errorMessage + "\n");
System.Diagnostics.Debug.Write(errorMessage + "\n");
StatusHandler.SendDebugAIPacket(Status.AIS_FATAL_ERROR, "Error: " + errorMessage);
// Exit with Windows ERROR_BAD_ARGUMENTS system error code
Environment.Exit(160);
}
/// <summary>
/// Get the next StateType. This is used to check if the state must be switched by Autonomous Service.
/// </summary>
/// <returns>StateType - the next StateType</returns>
public StateType GetNextStateType()
{
if (this.switchToGPS)
{
StatusHandler.SendDebugAIPacket(Status.AIS_SWITCH_TO_GPS, "Drive state switch: Vision to GPS.");
return(StateType.GPSState);
}
return(StateType.VisionState);
}
/// <summary>
/// Main execution function for AutonomousService.
/// </summary>
public void Execute(Object source, ElapsedEventArgs e)
{
// Don't execute if existing execution is not complete
if (!Monitor.TryEnter(executeLock))
{
return;
}
// TODO debugging - delete
//Console.WriteLine("The Elapsed event was raised at {0:HH:mm:ss.fff}", e.SignalTime);
// this is for when we're running out of time and just roll back to only gps and hope for the best
if (!this.panic && (DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - this.startTimeStamp.ToUnixTimeMilliseconds() > this.fuckItGoForItCountDown))
{
StatusHandler.SendDebugAIPacket(Status.AIS_LOG, "Starting hail mary");
this.panic = true;
this.driveContext.HandleFinalCountDown();
}
// If detected an obstacle within the last 5 seconds, continue straight to clear obstacle
if (IsLastObstacleWithinInterval(OBSTACLE_WATCHDOG_MILLIS))
{
StatusHandler.SendSimpleAIPacket(Status.AIS_OUT_WATCHDOG);
Console.WriteLine("Watchdog");
// If more than 0.5 seconds have passed since last event, it's safe to start issuing drive
// commands - otherwise race condition may occur when continually detecting an obstacle
if (!IsLastObstacleWithinInterval(CLEAR_OBSTACLE_DELAY_MILLIS))
{
this.driveContext.Drive(DriveCommand.Straight(Speed.CLEAR_OBSTACLE));
}
return;
}
// Get DriveCommand from current drive state, issue DriveCommand
DriveCommand driveCommand = this.driveContext.FindNextDriveCommand();
this.driveContext.Drive(driveCommand);
// If state change is required, change state
StateType nextState = this.driveContext.GetNextStateType();
if (this.driveContext.IsStateChangeRequired(nextState))
{
Console.WriteLine("Switching from state: " + this.driveContext.StateType.ToString() + " to: " + nextState.ToString());
this.driveContext.ChangeState(nextState);
}
Monitor.Exit(executeLock);
}
void HandleSocketReceive(IAsyncResult result)
{
IPEndPoint recvAddr = new IPEndPoint(IPAddress.Loopback, 0);
byte[] data = rocks_lrf_socket.EndReceive(result, ref recvAddr);
this.rocks_lrf_socket.BeginReceive(HandleSocketReceive, null);
List <byte> list = new List <byte>();
for (int i = 0; i < data.Length; i++)
{
list.Add(data[i]);
}
if (!this.handshake)
{
if (list.Count == 4)
{
if (list[0] == 0xDE && list[1] == 0xAD && list[2] == 0xBE && list[3] == 0xEF)
{
IPEndPoint sendAddr = new IPEndPoint(IPAddress.Loopback, 10000);
byte[] lrf_min_angle = BitConverter.GetBytes(DriveContext.LRF_MIN_ANGLE);
byte[] lrf_max_angle = BitConverter.GetBytes(DriveContext.LRF_MAX_ANGLE);
byte[] region_separation_distance = BitConverter.GetBytes(REGION_SEPARATION_DISTANCE);
byte[] rdp_threshold = BitConverter.GetBytes(RDP_THRESHOLD);
List <byte> buffer = new List <byte>();
buffer.Add(0xDE);
buffer.Add(0xAD);
buffer.AddRange(lrf_min_angle);
buffer.AddRange(lrf_max_angle);
buffer.AddRange(region_separation_distance);
buffer.AddRange(rdp_threshold);
buffer.Add(0xBE);
buffer.Add(0xEF);
this.rocks_lrf_socket.Send(buffer.ToArray(), buffer.Count, sendAddr);
this.handshake = true;
StatusHandler.SendDebugAIPacket(Status.AIS_LOG, "Received LRF handshare");
}
}
return;
}
this.plot = Plot.Deserialize(list);
}
/// <summary>
/// Get the next StateType. This is used to check if the state must be switched by Autonomous Service.
/// </summary>
/// <returns>StateType - the next StateType</returns>
public StateType GetNextStateType()
{
// Avoid trashing
if (this.averagingQueue.Count < 5)
{
return(StateType.GPSState);
}
// Get average distance to avoid erroneous switching due to noise
double averageDistance = 0.0;
foreach (double distanceItr in this.averagingQueue)
{
averageDistance += distanceItr;
}
averageDistance = averageDistance / 5;
// When to be switch from GPSDriveState to VisionDriveState
if (averageDistance <= GATE_PROXIMITY)
{
if (GATE_PROXIMITY == DriveContext.HAIL_MARY_GATE_PROXIMITY)
{
StatusHandler.SendSimpleAIPacket(Status.AIS_FOUND_GATE);
Console.WriteLine("Hail mary is within required distance - halting ");
this.isTaskComplete = true;
}
else
{
// Send log back to base station
StatusHandler.SendDebugAIPacket(Status.AIS_SWITCH_TO_VISION, "Drive state switch: GPS to Vision.");
Console.WriteLine("WITHIN PROXIMITY | ");
}
return(StateType.VisionState);
}
return(StateType.GPSState);
}
/// <summary>
/// Detect if an ObstacleEvent must be raised according to the LRF data being read. Trigger
/// ObstacleEvent if an obstacle exists within the maximum allowable distance threshold.
/// </summary>
public void DetectObstacleEvent(Object source, ElapsedEventArgs e)
{
// Sanity check
if (this.plot == null)
{
return;
}
// See if any obstacle within maximum allowed distance
bool obstacleDetected = false;
foreach (Region region in this.plot.Regions)
{
foreach (Coordinate coordinate in region.ReducedCoordinates)
{
if (coordinate.R < OBSTACLE_DETECTION_DISTANCE)
{
if (Math.Abs(coordinate.X) < DriveContext.ASCENT_WIDTH / 2)
{
obstacleDetected = true;
break;
}
}
}
if (obstacleDetected)
{
break;
}
}
// If obstacle detected, trigger event
if (obstacleDetected)
{
StatusHandler.SendDebugAIPacket(Status.AIS_OBS_DETECT, "Obstacle detected");
OnObstacleEvent(new ObstacleEventArgs(this.plot));
}
}
private const long OBSTACLE_INTERVAL_MILLIS = 100; //TODO change
static void Main(string[] args)
{
Console.SetWindowSize(110, 25);
int lrfPort = 11000;
StateType initialStateType = StateType.GPSState;
IPAddress destinationIP = IPAddress.Loopback;
bool lrfTest = false;
bool gateGPSTest = false;
GPS gate = null;
List <float> latitude = null;
List <float> longitude = null;
int fuckItGoForItCountDown = 480_000;
for (int i = 0; i < args.Length; i++)
{
String curr = args[i];
switch (curr)
{
case "-l":
{
// LRF
if (!Int32.TryParse(args[++i], out lrfPort))
{
ExitFromInvalidArgrument("Invalid integer for StateType parsing: " + args[i]);
}
break;
}
case "-d":
{
// StateType
int res = 0;
// If valid int
if (!Int32.TryParse(args[++i], out res))
{
ExitFromInvalidArgrument("Invalid integer for StateType parsing: " + args[i]);
}
// If valid StateType from int
try
{
initialStateType = StateTypeHelper.FromInteger(res);
}
catch (Exception e)
{
ExitFromInvalidArgrument(e.Message);
}
break;
}
case "-g":
{
// Gazebo address
// If valid IP
try
{
destinationIP = IPAddress.Parse(args[++i]);
}
catch (Exception)
{
ExitFromInvalidArgrument("Invalid IP address for Gazebo testing: " + args[i]);
}
break;
}
case "-t":
{
// LRF test mode
lrfTest = true;
break;
}
case "-lat":
{
try
{
latitude = new List <float>();
latitude.Add(float.Parse(args[++i])); // Degrees
latitude.Add(float.Parse(args[++i])); // Minutes
latitude.Add(float.Parse(args[++i])); // Seconds
}
catch (Exception)
{
ExitFromInvalidArgrument("Invalid latitude for GPS");
}
break;
}
case "-long":
{
try
{
longitude = new List <float>();
longitude.Add(float.Parse(args[++i])); // Degrees
longitude.Add(float.Parse(args[++i])); // Minutes
longitude.Add(float.Parse(args[++i])); // Seconds
}
catch (Exception)
{
ExitFromInvalidArgrument("Invalid longitude for GPS");
}
break;
}
case "-nogps":
{
// GPS test mode
gateGPSTest = true;
break;
}
case "-h":
{
if (!Int32.TryParse(args[++i], out fuckItGoForItCountDown))
{
ExitFromInvalidArgrument("Invalid integer for StateType parsing: " + args[i]);
}
break;
}
default:
{
// Invalid command line argument
ExitFromInvalidArgrument("Unrecognized command line argument: " + curr);
break;
}
}
}
// Initialize AscentPacketHandler
AscentPacketHandler.Initialize(destinationIP);
if (gateGPSTest)
{
// Test mode
gate = new GPS(0, 0, 0, 0, 0, 0);
}
else if (longitude != null || latitude != null)
{
if (longitude != null || latitude != null)
{
// Specified as args
gate = new GPS(latitude.ElementAt(0), latitude.ElementAt(1), latitude.ElementAt(2),
longitude.ElementAt(0), longitude.ElementAt(1), longitude.ElementAt(2));
}
else
{
// Not both specified, throw error
ExitFromInvalidArgrument("Specifying latitude/longitude as a command line arg requires both to be specified");
}
}
else
{
// Spin, wait for gate GPS from ROCKS (Base Station GUI)
while (!AscentPacketHandler.ReceivedGate)
{
Console.Write("\rWaiting for gate GPS from base station");
}
gate = AscentPacketHandler.Gate;
}
// Create AutonomousService
AutonomousService autonomousService = new AutonomousService(initialStateType, gate, lrfPort, fuckItGoForItCountDown, lrfTest);
StatusHandler.SendDebugAIPacket(Status.AIS_LOG, "Autonomous Service initialized");
// Create Timer for Execute function
Timer executeTimer = new Timer(EXECUTE_INTERVAL_MILLIS);
executeTimer.AutoReset = true;
executeTimer.Elapsed += autonomousService.Execute;
executeTimer.Enabled = true;
/*
* // Create timer for ObstacleEvent, tied in to autonomousService.DetectObstacleEvent()
* Timer obstacleTimer = new Timer(OBSTACLE_INTERVAL_MILLIS);
* obstacleTimer.AutoReset = true;
* obstacleTimer.Elapsed += autonomousService.DetectObstacleEvent;
* obstacleTimer.Enabled = true;
*/
// Execute while not receive an ACK from base station and DriveContext is not complete
while (!autonomousService.IsComplete())
{
}
StatusHandler.SendDebugAIPacket(Status.AIS_LOG, "Received ACK from Base Station and drive is complete. Exiting AI-ROCKS..");
Console.Write("WE DONE BITCHES");
}
private DriveCommand DriveNoBallDetected(TennisBall ball)
{
// Sanity check
if (ball != null)
{
return(null);
}
Console.Write("Ball not detected | ");
// Clear verification queue if it has values
this.verificationQueue.Clear();
GPS ascent = AscentPacketHandler.GPSData;
double distanceToGate = ascent.GetDistanceTo(this.gate);
// Kick back to GPS
if (distanceToGate > DISTANCE_SWITCH_TO_GPS) // 6 meters
{
Console.WriteLine("Distance: " + distanceToGate + ". Switch to GPS");
switchToGPS = true;
return(DriveCommand.Straight(Speed.HALT));
}
// Turn to face heading, drive toward it
if (distanceToGate > DISTANCE_USE_HEADING) // 3 meters
{
Console.WriteLine("Distance: " + distanceToGate + ". Turning toward heading to drive towrad it");
short ascentHeading = AscentPacketHandler.Compass;
double headingToGate = ascent.GetHeadingTo(this.gate);
Console.Write("currCompass: "******" | headingToGoal: " + headingToGate + " | distance: " + distanceToGate + " | ");
// Aligned with heading. Start going straight
if (IMU.IsHeadingWithinThreshold(ascentHeading, headingToGate, Scan.HEADING_THRESHOLD))
{
return(DriveCommand.Straight(Speed.VISION));
}
// Turn toward gate heading angle
if (IMU.IsHeadingWithinThreshold(ascentHeading, (headingToGate + 90) % 360, 90))
{
return(DriveCommand.LeftTurn(Speed.VISION_SCAN));
}
else
{
return(DriveCommand.RightTurn(Speed.VISION_SCAN));
}
// Probably would work, kept as reference
/*
* double lowBound = headingToGate;
* double highBound = (headingToGate + 180) % 360;
*
* if (lowBound < highBound)
* {
* if (lowBound < ascentHeading && ascentHeading < highBound)
* {
* return DriveCommand.LeftTurn(DriveCommand.SPEED_VISION_SCAN);
* }
* else
* {
* return DriveCommand.RightTurn(DriveCommand.SPEED_VISION_SCAN);
* }
* }
* else
* {
* if (!(highBound < ascentHeading && ascentHeading < lowBound))
* {
* return DriveCommand.LeftTurn(DriveCommand.SPEED_VISION_SCAN);
* }
* else
* {
* return DriveCommand.RightTurn(DriveCommand.SPEED_VISION_SCAN);
* }
* }
*/
}
// If scanning, complete scan
if (this.scan != null)
{
Console.WriteLine("Scanning... Distance: " + distanceToGate);
if (!this.scan.IsComplete())
{
return(scan.FindNextDriveCommand());
}
else
{
this.completedScans++;
// Clear scan, will rescan below
this.scan = null;
}
}
switch (this.completedScans)
{
case 0:
{
// Initialize the first scan
if (distanceToGate > DISTANCE_CLOSE_RANGE) // 2 meters
{
// Turn toward heading. Scan, use heading as reference
StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: Using heading as reference. Distance: " + Math.Round(distanceToGate, 2));
Console.WriteLine("Scan: Using heading as reference. Distance: " + Math.Round(distanceToGate, 2));
this.scan = new Scan(this.gate, true);
}
else
{
// Scan
StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: Not using heading as reference. Distance: " + Math.Round(distanceToGate, 2));
Console.WriteLine("Scan: Not using heading as reference. Distance: " + Math.Round(distanceToGate, 2));
this.scan = new Scan(this.gate, false);
}
break;
}
case 1:
{
// Align toward heading, drive for 10ish seconds,
StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: 1st 10s scan toward heading. Distance: " + Math.Round(distanceToGate, 2));
Console.WriteLine("Scan: Distance: " + Math.Round(distanceToGate, 2) + ". Scanning (using heading). Driving 5m away...");
this.scan = new Scan(this.gate, SCAN_DRIVE_STRAIGHT_DURATION_MILLIS);
break;
}
/*
* case 2:
* {
* // Broaden HSV values, scan
* StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: Broadening HSV values. Distance: " + Math.Round(distanceToGate, 2));
* Console.WriteLine("Scan: Broadening HSV values. Distance: " + Math.Round(distanceToGate, 2));
*
* this.camera.BroadenHsvValues();
*
* this.scan = new Scan(this.gate, false);
*
* break;
* }
*/
case 2:
{
// Align toward heading (again), drive for 5ish seconds,
StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: 2nd 5m scan toward heading. Distance: " + Math.Round(distanceToGate, 2));
Console.WriteLine("Scan: 2nd 5m scan toward heading. Distance: " + Math.Round(distanceToGate, 2));
this.scan = new Scan(this.gate, SCAN_DRIVE_STRAIGHT_DURATION_MILLIS);
break;
}
case 3:
{
// We've already run 1 scan, 1 5m scan, 1 broaden HSV, 1 more 5m scan, so drive straight to kick back to GPS and do it over again
return(DriveCommand.Straight(Speed.VISION));
}
default:
{
break;
}
}
return(scan.FindNextDriveCommand());
}
/// <summary>
/// For all DetectedBalls in queue, verify whether the cumulative DetectedBalls in the this queue accurately detect a tennis ball,
/// according to specified parameters. This uses average timestamp (within specified time), average camera and GPS distances
/// (distance to ball), and percentage of correct camera and GPS distances to verify that a TennisBall is accurately detected.
/// </summary>
/// <param name="distancePercentageThreshold">Percentage of DetectedBalls that must be correct (for distance).</param>
/// <param name="timestampThreshold">DetectedBalls must average to be within this timestamp threshold.</param>
/// <param name="cameraDistanceThreshold">Distance threshold for distance calculated by camera.</param>
/// <param name="gpsDistanceThreshold">Distance threshold for distance calculated by GPS.</param>
/// <returns>bool - If the current queue's DetectedBalls represent a valid, detected ball.</returns>
public bool VerifyBallDetection(Double distancePercentageThreshold, Double timestampThreshold, Double cameraDistanceThreshold, Double gpsDistanceThreshold)
{
Queue <DetectedBall> detectedBallsCopy = new Queue <DetectedBall>(this.detectedBalls);
// Require a full queue (capacity-number of detected balls) to even consider being a success
if (detectedBallsCopy.Count < this.capacity)
{
return(false);
}
// TODO other parameters to be checking on:
// Radius? Get average and standard deviation to be sure
// Location? Make sure we're not hopping around the frame, erroneously detecting something
// Check for valid ball detection by averaging
long averageTimestamp = 0;
Double averageCameraDistance = 0.0;
Double averageGPSDistance = 0.0;
int countCameraDistanceOutsideThreshold = 0;
int countGPSDistanceOutsideThreshold = 0;
// Find averages
foreach (DetectedBall detectedBall in detectedBallsCopy)
{
averageTimestamp += detectedBall.TimestampMillis;
averageCameraDistance += detectedBall.CameraDistance;
if (detectedBall.CameraDistance > cameraDistanceThreshold)
{
countCameraDistanceOutsideThreshold++;
}
averageGPSDistance += detectedBall.GPSDistance;
if (detectedBall.GPSDistance > gpsDistanceThreshold)
{
countGPSDistanceOutsideThreshold++;
}
}
averageTimestamp = averageTimestamp / this.capacity;
averageCameraDistance = averageCameraDistance / this.capacity;
averageGPSDistance = averageGPSDistance / this.capacity;
// Timestamp checking
if (averageTimestamp <= timestampThreshold)
{
StatusHandler.SendDebugAIPacket(Status.AIS_VERIFY_FAIL_TIMESTAMP, "Verification: Timestamp average failure");
Console.Write(" Not verified - timestamp | ");
return(false);
}
// Camera distance checking
Double cameraDistancePercentageInThreshold = 1 - ((double)countCameraDistanceOutsideThreshold / this.capacity);
if (cameraDistancePercentageInThreshold < distancePercentageThreshold)
{
StatusHandler.SendDebugAIPacket(Status.AIS_VERIFY_FAIL_CAM_PERCENT, "Verification: Camera distance percentage failure");
Console.Write(" Not verified - camera distance percentage | ");
return(false);
}
if (averageCameraDistance > cameraDistanceThreshold)
{
StatusHandler.SendDebugAIPacket(Status.AIS_VERIFY_FAIL_CAM_AVG, "Verification: Camera distance average failure");
Console.Write(" Not verified - average camera distance | ");
return(false);
}
// GPS distance checking
Double gpsDistancePercentageInThreshold = 1 - ((double)countGPSDistanceOutsideThreshold / this.capacity);
if (gpsDistancePercentageInThreshold < distancePercentageThreshold)
{
StatusHandler.SendDebugAIPacket(Status.AIS_VERIFY_FAIL_GPS_PERCENT, "Verification: GPS distance percentage failure");
Console.Write(" Not verified - GPS distance percentage | ");
return(false);
}
if (averageCameraDistance > gpsDistanceThreshold)
{
StatusHandler.SendDebugAIPacket(Status.AIS_VERIFY_FAIL_GPS_AVG, "Verification: GPS distance average failure");
Console.Write(" Not verified - GPS distance | ");
return(false);
}
StatusHandler.SendDebugAIPacket(Status.AIS_VERIFY_SUCCESS, "Verification: Success");
Console.Write(" Verified: averageTimestamp: {0}, averageCameraDistance: {1}, averageGPSDistance: {2} | ", averageTimestamp, averageCameraDistance, averageGPSDistance);
return(true);
}
