/// <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";
}
