/// <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(); } }