public SonarDataDssSerializable(SonarData sonarData) { TimeStamp = new DateTime(sonarData.TimeStamp); Count = sonarData.angles.Count; RawAngles = sonarData.angles.Keys.ToArray<int>(); RangeMeters = (from v in sonarData.angles.Values select v.rangeMeters).ToArray<double>(); }
public void Reset() { _sonarData = new SonarData(); }
/// <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> /// Handle new measurement data from the Proximity Board. /// </summary> /// <param name="measurement">Measurement Data</param> void SonarMeasurementHandler(SonarData measurement) { //Tracer.Trace("TrackRoamerBrickProximityBoardService::SonarMeasurementHandler()"); try { _state.LastSampleTimestamp = new DateTime(measurement.TimeStamp); _state.MostRecentSonar = new SonarDataDssSerializable(measurement); _state.LinkState = "receiving Sonar Data"; // // Inform subscribed services that the state has changed. // _submgrPort.Post(new submgr.Submit(_state, DsspActions.ReplaceRequest)); UpdateSonarData usd = new UpdateSonarData(); usd.Body = _state.MostRecentSonar; //Tracer.Trace(" ========= sending UpdateSonarData notification =================="); base.SendNotification(_submgrPort, usd); } catch (Exception e) { _state.LinkState = "Error while receiving Sonar Data"; LogError(e); } }