/// <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);
}
}
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>();
}
/// <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";
}
void SonarDataHandler(SonarData sonarData)
{
//Tracer.Trace("ProximityBoardCcrServiceCommander::SonarDataHandler()");
int packetLength = sonarData.angles.Count; // typically 26 packets, each covering 7 degrees; SortedList<int,int> angles
if (packetLength != 26)
{
Tracer.Error("ProximityBoardCcrServiceCommander::SonarDataHandler() not a standard sonar sweep measurement, angles.Count=" + packetLength + " (expected 26) -- ignored");
}
else
{
_pbCommanderDataEventsPort.Post(sonarData);
}
}
private async void SonarTopicOnRosMessage(object sender, RosMessageEventArgs e)
{
var echoes = e.Json["msg"]["echoes"];
var sonarData = new SonarData
{
FrontLeftSide = echoes[9].Value <int>(),
FrontLeft = echoes[8].Value <int>(),
FrontCenter = echoes[7].Value <int>(),
FrontRight = echoes[6].Value <int>(),
FrontRightSide = echoes[5].Value <int>(),
BackLeftSide = echoes[0].Value <int>(),
BackLeft = echoes[1].Value <int>(),
BackCenter = echoes[2].Value <int>(),
BackRight = echoes[3].Value <int>(),
BackRightSide = echoes[4].Value <int>()
};
// Send the sonar data to all clients connected to the sonar hub
await _sonarHubContext.Clients.All.InvokeAsync("sonarUpdate", sonarData);
}
private void ResetSweepCollector()
{
lock (_sonarDatas)
{
_sonarDatas.Clear();
}
lock (_directionDatas)
{
_directionDatas.Clear();
}
lock (_accelDatas)
{
_accelDatas.Clear();
}
lock (_proxiDatas)
{
_proxiDatas.Clear();
}
lock (_parkingSensorsDatas)
{
_parkingSensorsDatas.Clear();
}
_badCount = 0;
timestampLastReading = 0L;
timestampLastSweepReading = 0L;
sonarData = new SonarData();
board = new ProximityBoard();
inTestSamples = TEST_SAMPLES_COUNT;
}
public void Reset()
{
_sonarData = new SonarData();
}
// =================================================================================================================
#region DataReadyPort handlers
/// <summary>
/// This handler is activated by empty Recv messages coming to internal DataReadyPort.
/// The data comes here from pmFrameCompleteHandler
/// This is our opportunity to purge stale data from the queues.
/// The data posted to Responses port is then picked up by ProximityBoardCcrServiceCommander:SonarDataHandler() and others,
/// to be later passed to TrackRoamerBrickProximityBoard.
/// </summary>
/// <param name="recv"></param>
private void DataHandler(Recv recv)
{
/*
* if(_sonarDatas.Count > 1 || _directionDatas.Count > 1 || _accelDatas.Count > 1 || _proxDatas.Count > 1 || _parkingSensorsDatas.Count > 1)
* {
* // for debugging, log if queues get more than 1 piece of each data type:
* Tracer.Trace("ProximityBoardManager: DataHandler() sonars: " + _sonarDatas.Count + " compass: "******" accels: " + _accelDatas.Count + " proxs: " + _proxDatas.Count + " parking: " + _parkingSensorsDatas.Count);
* }
*/
if (_sonarDatas.Count > 0)
{
SonarData sonarData = null;
lock (_sonarDatas)
{
while (_sonarDatas.Count > 0)
{
sonarData = _sonarDatas.Last();
_sonarDatas.Clear();
}
}
if (sonarData != null)
{
MeasurementsPort.Post(sonarData);
}
}
if (_directionDatas.Count > 0)
{
DirectionData directionData = null;
lock (_directionDatas)
{
while (_directionDatas.Count > 0)
{
directionData = _directionDatas.Last();
_directionDatas.Clear();
}
}
if (directionData != null)
{
MeasurementsPort.Post(directionData);
}
}
if (_accelDatas.Count > 0)
{
AccelerometerData accelerometerData = null;
lock (_accelDatas)
{
while (_accelDatas.Count > 0)
{
accelerometerData = _accelDatas.Last();
_accelDatas.Clear();
}
}
if (accelerometerData != null)
{
MeasurementsPort.Post(accelerometerData);
}
}
if (_proxiDatas.Count > 0)
{
ProximityData proximityData = null;
lock (_proxiDatas)
{
while (_proxiDatas.Count > 0)
{
proximityData = _proxiDatas.Last();
_proxiDatas.Clear();
}
}
if (proximityData != null)
{
MeasurementsPort.Post(proximityData);
}
}
if (_parkingSensorsDatas.Count > 0)
{
ParkingSensorData psiData = null;
lock (_parkingSensorsDatas)
{
while (_parkingSensorsDatas.Count > 0)
{
psiData = _parkingSensorsDatas.Last();
_parkingSensorsDatas.Clear();
}
}
if (psiData != null)
{
MeasurementsPort.Post(psiData);
}
}
//Tracer.Trace("AnalogData: current=" + AnalogValue1 + " prev=" + AnalogValue1Prev);
// and last but not least - post AnalogData - only if any of the values changed:
if (AnalogValue1 != AnalogValue1Prev)
{
AnalogValue1Prev = AnalogValue1;
AnalogData analogData = new AnalogData()
{
analogValue1 = AnalogValue1,
TimeStamp = DateTime.Now.Ticks
};
MeasurementsPort.Post(analogData);
}
}
public bool setSonarRayReading(int angleRaw1, double rangeMeters1, int angleRaw2, double rangeMeters2, long timestamp)
{
bool haveData = false;
//Tracer.Trace("setReading() sonar: " + angleRaw1 + " " + rangeMeters1 + " " + angleRaw2 + " " + rangeMeters2);
if (inTestSamples > 0)
{
inTestSamples--;
// for a while just try figuring out what comes in - sweep angle ranges for both sides
if (inTestSamples > TEST_SAMPLES_COUNT - 10) // first few frames are garbled
{
return(haveData);
}
board.registerAnglesRaw(angleRaw1, angleRaw2);
if (inTestSamples == 0) // last count
{
board.finishPrerun();
Tracer.Trace(board.ToString());
}
return(haveData);
}
if (angleRaw1 == board.rawAngle1_Min || angleRaw1 == board.rawAngle1_Max)
{
RangeReading rr1 = board.angleRawToSonarAngleCombo(1, angleRaw1, rangeMeters1, timestamp);
RangeReading rr2 = board.angleRawToSonarAngleCombo(2, angleRaw2, rangeMeters2, timestamp);
sonarData.addRangeReading(rr1, rr2, timestamp);
if (sonarData.angles.Count == 26)
{
lock (_sonarDatas)
{
_sonarDatas.Enqueue(sonarData);
}
haveData = true; // allow DataPort.Post(new Recv()) to happen, with data already enqueued in the ...Datas
_badCount = 0;
TimeSpan sinceLastReading = new TimeSpan(timestamp - timestampLastReading);
TimeSpan sinceLastSweepReading = new TimeSpan(timestamp - timestampLastSweepReading);
//Tracer.Trace("SONAR PACKET READY -- angles: " + sonarData.angles.Count + " packets: " + _sonarDatas.Count
// + (new DateTime(timestamp)).ToString()
// + String.Format(" {0:0.000} s/ray {1:0.000} s/sweep", sinceLastReading.TotalSeconds, sinceLastSweepReading.TotalSeconds));
timestampLastSweepReading = timestamp;
}
else
{
_badCount++;
Tracer.Trace(string.Format("BAD SONAR PACKET -- angles: {0} angleRaw1: {1} - #{2} since last good packet", sonarData.angles.Count, angleRaw1, _badCount));
}
sonarData = new SonarData(); // prepare for the next one
}
timestampLastReading = timestamp;
RangeReading rrr1 = board.angleRawToSonarAngleCombo(1, angleRaw1, rangeMeters1, timestamp);
RangeReading rrr2 = board.angleRawToSonarAngleCombo(2, angleRaw2, rangeMeters2, timestamp);
sonarData.addRangeReading(rrr1, rrr2, timestamp);
return(haveData);
}
