/// <summary>
/// Reads the data from the LIDAR, formats it and triggers an event
/// </summary>
/// <returns></returns>
private async Task Read()
{
if (readings == null)
{
readings = new List <Hit>();
}
try
{
byte[] readout = await ReadData(5, readingToken.Token);
Hit currentReading = FormatData(readout);
// Trigger event and clear current readings on new scan
if (currentReading.NewScan)
{
OnScanCompleted(readings.ToArray());
readings.Clear();
}
// Only add reading if no error
if (!currentReading.Error)
{
readings.Add(currentReading);
}
else
{
Debug.WriteLine("Read Error -> " + currentReading.ToString());
await ClearReadBuffer();
}
}
catch (Exception ex)
{
Debug.WriteLine(ex.Message);
}
}
public void ProcessReadings(ref Hit[] readings, long timestamp)
{
// Only process one reading at a time
lock (processingLock)
{
// This group of readings are older than the last group processed, so skip
if (timestamp < LastProcessingTime)
{
return;
}
//Debug.WriteLine($"Processing {readings.Length} points, {timestamp}...");
var newRec = LidarRecommendation.Move;
double adjustAngle = 0;
var leftPoints = new List <Hit>();
var rightPoints = new List <Hit>();
// Do processing
foreach (var reading in readings)
{
// Skip bad reading
if (reading.Quality == 0)
{
continue;
}
// Convert to Cartesian coordiantes
var point = reading.CartesianPoint;
// Check if we need to stop because we're about to hit something
if (IsInStopZone(reading.Angle, reading.Distance))
{
// Set recommendation to stop so the robot doesn't keep trying to move
newRec = LidarRecommendation.Stop;
// Update the last processing time
LastProcessingTime = timestamp;
OnAnalysisChanged(newRec, adjustAngle);
return;
}
// Check if we need to adjust trajectory
if (IsInAdjustZone(reading.Angle, reading.Distance))
{
if (point.X < 0)
{
leftPoints.Add(reading);
}
else
{
rightPoints.Add(reading);
}
}
}
// Check for obstacle
if (leftPoints.Count > MIN_ADJUST_POINT_COUNT || rightPoints.Count > MIN_ADJUST_POINT_COUNT)
{
// Find the point that is closest to the robot
var leftMinPoint = new Hit()
{
Distance = int.MaxValue
};
var rightMinPoint = new Hit()
{
Distance = int.MaxValue
};
foreach (var point in leftPoints)
{
if (point.Distance < leftMinPoint.Distance)
{
leftMinPoint = point;
}
}
foreach (var point in rightPoints)
{
if (point.Distance < rightMinPoint.Distance)
{
rightMinPoint = point;
}
}
// Check to see which side is closer to object
if (leftMinPoint.Distance < rightMinPoint.Distance)
{
newRec = LidarRecommendation.AdjustObstacle;
adjustAngle = 90;
}
else if (leftMinPoint.Distance > rightMinPoint.Distance)
{
newRec = LidarRecommendation.AdjustObstacle;
adjustAngle = -90;
}
else
{
newRec = LidarRecommendation.Stop;
}
}
// Update the last processing time
LastProcessingTime = timestamp;
OnAnalysisChanged(newRec, adjustAngle);
}
}