/// <summary>
/// update map with the detected objects
/// </summary>
/// <param name="timestamp"></param>
private void updateMapperVicinity(long timestamp, SonarData p)
{
if (lastRangeReadingSetChanged && lastRangeReadingSet.sweepFrameReady && p.angles.Keys.Count == 26)
{
RangeReading rrFirst = p.getLatestReadingAt(p.angles.Keys.First());
RangeReading rrLast = p.getLatestReadingAt(p.angles.Keys.Last());
double sweepAngle = rrFirst.angleDegrees - rrLast.angleDegrees;
double forwardAngle = sweepAngle / 2.0d;
foreach (int angle in p.angles.Keys)
{
RangeReading rr = p.getLatestReadingAt(angle);
double rangeMeters = rr.rangeMeters + robotCornerDistanceMeters; // sensor is on the corner, adjust for that
double relBearing = rr.angleDegrees - forwardAngle;
GeoPosition pos1 = (GeoPosition)mapperVicinity.robotPosition.Clone();
pos1.translate(new Direction()
{
heading = mapperVicinity.robotDirection.heading, bearingRelative = relBearing
}, new Distance(rangeMeters));
DetectedObstacle dobst1 = new DetectedObstacle(pos1)
{
geoPosition = pos1,
firstSeen = timestamp,
lastSeen = timestamp,
color = Colors.Red,
detectorType = DetectorType.SONAR_SCANNING,
objectType = DetectedObjectType.Obstacle
};
mapperVicinity.Add(dobst1);
}
// make sure we have the latest heading info in the mapper, and map positions have been computed:
Direction dir = new Direction()
{
heading = lastDirData.heading, bearing = mapperVicinity.robotDirection.bearing, TimeStamp = timestamp
};
mapperVicinity.robotDirection = dir; // will call mapperVicinity.computeMapPositions();
updateMapWindow();
}
mapperTraceLabel.Content = "" + mapperVicinity.detectedObjects.Count + " objects";
}
/// <summary>
/// using previously accumulated SonarData, draws the boundaries of the scanned area.
/// </summary>
private void drawScannedArea()
{
if (numRays >= 2)
{
StringBuilder psb = new StringBuilder();
// the "Path" coordinates of the center of ray rotation:
int centerX = (int)Math.Round(ScannedArea.ActualWidth / 2.0d);
int centerY = (int)ScannedArea.ActualHeight;
psb.AppendFormat("M{0},{1} ", centerX, centerY);
foreach (int angleRaw in _sonarData.angles.Keys)
{
RangeReading reading = _sonarData.getLatestReadingAt(angleRaw);
if (reading != null)
{
// 0 = full range, centerY = zero range
double lineLength = reading.rangeMeters * ScannedArea.ActualHeight / rangeMaxValueM;
double angleDegrees = reading.angleDegrees - (angleMaxValue - angleMinValue) / 2.0d;
double angleRads = angleDegrees * Math.PI / 180.0d;
int x1 = (int)Math.Round(lineLength * Math.Sin(angleRads)) + centerX;
int y1 = -(int)Math.Round(lineLength * Math.Cos(angleRads)) + centerY;
// Tracer.Trace("angle: " + angleDegrees + "/" + angleRads + " x=" + x1 + " y=" + y1);
psb.AppendFormat("L{0},{1} ", x1, y1);
}
}
psb.Append("z");
// Tracer.Trace(psb.ToString());
PathFigureCollectionConverter fcvt = new PathFigureCollectionConverter();
PathFigureCollection figures = (PathFigureCollection)fcvt.ConvertFromString(psb.ToString()); //"M200,200 L100,10 L100,10 L50,100 z");
ScannedArea.Data = new PathGeometry(figures);
drawReferenceCircles();
}
}