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