void ProximityMeasurementHandler(ProximityData measurement)
{
//Tracer.Trace("TrackRoamerBrickProximityBoardService::ProximityMeasurementHandler()");
try
{
_state.LastSampleTimestamp = new DateTime(measurement.TimeStamp);
_state.MostRecentProximity = new ProximityDataDssSerializable(measurement);
_state.LinkState = "receiving Proximity Data";
//
// Inform subscribed services that the state has changed.
//
_submgrPort.Post(new submgr.Submit(_state, DsspActions.ReplaceRequest));
UpdateProximityData usd = new UpdateProximityData();
usd.Body = _state.MostRecentProximity;
base.SendNotification <UpdateProximityData>(_submgrPort, usd);
}
catch (Exception e)
{
_state.LinkState = "Error while receiving Proximity Data";
LogError(e);
}
}
/// <summary>
/// Scan for obstructions in a cone
/// </summary>
private IEnumerator Scan()
{
while (scanning)
{
float proximity = maxRange;
// roll
for (float r = 0; r <= 180f; r += 10f)
{
// yaw
for (float y = -Fov; y < Fov; y += 6f)
{
Quaternion rotation;
Vector3 direction;
rotation = Quaternion.AngleAxis(y, transform.up);
direction = rotation * transform.forward;
rotation = Quaternion.AngleAxis(r, transform.forward);
direction = rotation * direction;
float cast = Cast(direction);
// if this cast is shorter than returnData
if (cast < proximity)
{
proximity = cast;
}
}
}
_data = new ProximityData(transform.forward * proximity, proximity < maxRange);
_callback(_data);
yield return(new WaitForSeconds(updateDt));
}
}
public virtual void OnCurrentValueChanged(DependencyPropertyChangedEventArgs e)
{
ProximityData newValue = (ProximityData)e.NewValue;
beamsSt[0].ScaleX = mToScale(newValue.mffr);
beamsSt[1].ScaleX = mToScale(newValue.mfr);
beamsSt[2].ScaleX = mToScale(newValue.mbr);
beamsSt[3].ScaleX = mToScale(newValue.mbbr);
beamsSt[4].ScaleX = mToScale(newValue.mbbl);
beamsSt[5].ScaleX = mToScale(newValue.mbl);
beamsSt[6].ScaleX = mToScale(newValue.mfl);
beamsSt[7].ScaleX = mToScale(newValue.mffl);
}
public ProximityDataDssSerializable(ProximityData proximityData)
{
TimeStamp = new DateTime(proximityData.TimeStamp);
// clockwise starting from rear right side:
mbr = arrangeProximityReading(proximityData.mbr, 0);
mbbr = arrangeProximityReading(proximityData.mbbr, 1);
mbbl = arrangeProximityReading(proximityData.mbbl, 2);
mbl = arrangeProximityReading(proximityData.mbl, 3);
mfl = arrangeProximityReading(proximityData.mfl, 4);
mffl = arrangeProximityReading(proximityData.mffl, 5);
mffr = arrangeProximityReading(proximityData.mffr, 6);
mfr = arrangeProximityReading(proximityData.mfr, 7);
}
private unsafe void PacketReceived(Packet.Header *header)
{
switch (header->Type)
{
case (byte)CustomPackets.Type.OrientationData:
{
Orientation = *((CustomPackets.OrientationDataPacket *)header);
DebugTools.Print($"DATA: {Orientation}");
break;
}
case (byte)CustomPackets.Type.ProximityData:
{
ProximityData data = *((CustomPackets.ProximityDataPacket *)header);
DebugTools.Print($"DATA: {data}");
if (data.ID < 0 || data.ID >= ProximityContent.Length)
{
DebugTools.Print($"Invalid sensor index ID: {data.ID}");
return;
}
int count = ProximityContent[data.ID].Count;
int cap = m_ContentCapacity - 1;
for (; count-- > cap;)
{
ProximityContent[data.ID].RemoveAt(0);
}
ProximityContent[data.ID].Add(data);
break;
}
default:
{
DebugTools.Print($"Unknown packet type: {header->Type}");
break;
}
}
}
/// <summary>
/// Sensor data callback passes proximity data to INavigation
/// </summary>
/// <param name="data">Data.</param>
public void ReceiveSensorData(ProximityData data)
{
if (_navigation == null)
{
return;
}
// data is recieved here by any enabled sensor in world space
// transformed into robot local space if necessary
// and transmitted to INavigation
if (_navigation.spaceRelativeTo == Space.Self)
{
_navigation.Proximity(
Vector3.zero,
transform.InverseTransformDirection(data.direction),
data.obstructed);
}
else
{
_navigation.Proximity(
transform.position,
transform.position + data.direction,
data.obstructed);
}
}
private void pmFrameCompleteHandler_UI(object sender, AsyncInputFrameArgs aira)
{
long timestamp = aira.timestamp;
// have raw data displayed in a text box:
StringBuilder frameValue = new StringBuilder();
frameValue.AppendFormat("{0} POT: {1}\r\n", aira.dPos1Mks, aira.sensorsState.potValue);
frameValue.AppendFormat("{0}\r\n", aira.dPos2Mks);
frameValue.AppendFormat("{0} ", aira.dPing1DistanceM * 1000.0d); // print distance in milimeters
frameValue.AppendFormat("{0}\r\n", aira.fromPingScanStop);
frameValue.AppendFormat("{0}\r\n", aira.dPing2DistanceM * 1000.0d);
// expect 7 on "not in sight", 3 on "far", 1 on "medium" and 0 on "close":
frameValue.AppendFormat("{0} ", aira.sensorsState.irbE1);
frameValue.AppendFormat("{0} ", aira.sensorsState.irbE2);
frameValue.AppendFormat("{0} ", aira.sensorsState.irbE3);
frameValue.AppendFormat("{0} ", aira.sensorsState.irbE4);
frameValue.AppendFormat("{0} ", aira.sensorsState.irbO1);
frameValue.AppendFormat("{0} ", aira.sensorsState.irbO2);
frameValue.AppendFormat("{0} ", aira.sensorsState.irbO3);
frameValue.AppendFormat("{0}\r\n", aira.sensorsState.irbO4);
frameValue.AppendFormat("{0} : {1:0.0} * {2:0.0} * {3:0.0} * {4:0.0} \r\n", aira.sensorsState.parkingSensorsCount,
aira.sensorsState.parkingSensorMetersLB, aira.sensorsState.parkingSensorMetersLF, aira.sensorsState.parkingSensorMetersRF, aira.sensorsState.parkingSensorMetersRB);
/*
* Tracer.Trace(string.Format("=== xpsi: {0} : {1} {2} {3} {4} : {5} {6} {7} {8}", aira.sensorsState.parkingSensorsCount,
* aira.sensorsState.parkingSensors[0], aira.sensorsState.parkingSensors[1], aira.sensorsState.parkingSensors[2], aira.sensorsState.parkingSensors[3],
* aira.sensorsState.parkingSensors[4], aira.sensorsState.parkingSensors[5], aira.sensorsState.parkingSensors[6], aira.sensorsState.parkingSensors[7]));
*
* Tracer.Trace(string.Format("=== bpsi: {0} : {1} {2} {3} {4} : {5} {6} {7} {8}", aira.sensorsState.parkingSensorsCount,
* Convert.ToString(aira.sensorsState.parkingSensors[0],2), Convert.ToString(aira.sensorsState.parkingSensors[1],2), Convert.ToString(aira.sensorsState.parkingSensors[2],2), Convert.ToString(aira.sensorsState.parkingSensors[3],2),
* Convert.ToString(aira.sensorsState.parkingSensors[4],2), Convert.ToString(aira.sensorsState.parkingSensors[5],2), Convert.ToString(aira.sensorsState.parkingSensors[6],2), Convert.ToString(aira.sensorsState.parkingSensors[7],2)));
*/
//frameValue.AppendFormat("{0} {1} {2} {3}\r\n", aira.sensorsState.parkingSensors[4], aira.sensorsState.parkingSensors[5], aira.sensorsState.parkingSensors[6], aira.sensorsState.parkingSensors[7]);
pmValuesLabel.Content = frameValue.ToString();
// Proximity sensors display their data in dedicated control:
lastProxData = new ProximityData()
{
TimeStamp = timestamp
};
lastProxData.setProximityData(aira.sensorsState.irbE1, aira.sensorsState.irbE2, aira.sensorsState.irbE3, aira.sensorsState.irbE4,
aira.sensorsState.irbO1, aira.sensorsState.irbO2, aira.sensorsState.irbO3, aira.sensorsState.irbO4);
lastProxDataChanged = true;
// accelerometer has its own control for displaying data:
lastAccData = new AccelerometerDataWpf()
{
TimeStamp = timestamp
};
lastAccData.setAccelerometerData(aira.sensorsState.accelX, aira.sensorsState.accelY, aira.sensorsState.accelZ);
lastAccDataChanged = true;
// compass has its own control for displaying data:
if (aira.sensorsState.compassHeading >= 0.0d && aira.sensorsState.compassHeading <= 359.999999d && (lastDirData == null || Math.Abs(lastDirData.heading - aira.sensorsState.compassHeading) > 2.0d))
{
lastDirData = new DirectionData()
{
heading = aira.sensorsState.compassHeading, TimeStamp = timestamp
};
lastDirDataChanged = true;
}
else if ((bool)compassCheckBox.IsChecked && (lastCompassSlider != compassSlider.Value || timestamp > lastCompassSliderTimestamp + 20000000L))
{
// simulated direction, controlled by a slider:
lastCompassSliderTimestamp = timestamp;
lastCompassSlider = compassSlider.Value;
double hdg = compassSlider.Value; // -180 ... 180
if (hdg < 0)
{
hdg += 360.0d; // 0...360
}
lastDirData = new DirectionData()
{
heading = hdg, TimeStamp = timestamp
};
lastDirDataChanged = true;
}
if (aira.sensorsState.parkingSensorsValid)
{
lastPsiData = new ParkingSensorData()
{
parkingSensorMetersLB = aira.sensorsState.parkingSensorMetersLB,
parkingSensorMetersLF = aira.sensorsState.parkingSensorMetersLF,
parkingSensorMetersRB = aira.sensorsState.parkingSensorMetersRB,
parkingSensorMetersRF = aira.sensorsState.parkingSensorMetersRF,
TimeStamp = timestamp
};
lastPsiDataChanged = true;
}
// frames that are marked "" feed the sonar sweep view controls. They are updated in real time:
if (aira.fromPingScanStop)
{
int angleRaw1 = (int)aira.dPos1Mks;
double distM1 = aira.dPing1DistanceM;
int angleRaw2 = (int)aira.dPos2Mks;
double distM2 = aira.dPing2DistanceM;
if (inTestSamples > 0)
{
// prerun - for a while just try figuring out what comes in - sweep angle ranges for both sides
inTestSamples--;
if (inTestSamples < TEST_SAMPLES_COUNT - 10) // first few frames are garbled
{
board.registerAnglesRaw(angleRaw1, angleRaw2);
if (inTestSamples == 0) // last count
{
board.finishPrerun();
Tracer.Trace(board.ToString());
}
}
}
else
{
// real sonar data is coming, and we finished prerun and know how many beams per sweep we are dealing with
lastRangeReadingSet = new RangeReadingSet()
{
left = board.angleRawToSonarAngle(new RangeReading(angleRaw1, distM1, timestamp)), right = board.angleRawToSonarAngle(new RangeReading(angleRaw2, distM2, timestamp))
};
lastRangeReadingSetChanged = true;
lastRangeReadingSet.combo1 = board.angleRawToSonarAngleCombo(1, angleRaw1, distM1, timestamp);
lastRangeReadingSet.combo2 = board.angleRawToSonarAngleCombo(2, angleRaw2, distM2, timestamp);
lastRangeReadingSet.hasCombo = true;
if (angleRaw1 == board.rawAngle1_Min || angleRaw1 == board.rawAngle1_Max)
{
lastRangeReadingSet.sweepFrameReady = true;
}
}
}
// sanity check:
if (lastRangeReadingSetChanged && lastRangeReadingSet.sweepFrameReady && lastDirData == null)
{
// we probably don't have compass connected - have a fake dir data for now.
lastDirData = new DirectionData()
{
heading = 0.0d, TimeStamp = timestamp
};
lastDirDataChanged = true;
}
// update all controls with collected data:
setSonarViewControls(timestamp);
updateMapperVicinity(timestamp, sweepViewControlCombo.sonarData);
lastRangeReadingSetChanged = false;
setViewControls(timestamp);
}
// =================================================================================================================
#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);
}
}
// =================================================================================================================
#region Processing Data Frame from LibPicSensors.ProximityModule - pmFrameCompleteHandler()
// This will be called whenever the data is read from the board:
private void pmFrameCompleteHandler(object sender, AsyncInputFrameArgs aira)
{
/*
* Tracer.Trace("Async frame arrived. " + now);
*
* StringBuilder frameValue = new StringBuilder();
*
* frameValue.AppendFormat("ProximityBoardManager: frame arrived: {0} ", aira.dPos1Mks);
* frameValue.AppendFormat("{0} ", aira.dPos2Mks);
* frameValue.AppendFormat("{0} ", aira.dPing1DistanceMm);
* frameValue.AppendFormat("{0}", aira.dPing2DistanceMm);
*
* Tracer.Trace(frameValue.ToString());
*/
bool haveData = false;
// IR Proximity sensors data:
ProximityData proxData = new ProximityData()
{
TimeStamp = aira.timestamp
};
proxData.setProximityData(aira.sensorsState.irbE1, aira.sensorsState.irbE2, aira.sensorsState.irbE3, aira.sensorsState.irbE4,
aira.sensorsState.irbO1, aira.sensorsState.irbO2, aira.sensorsState.irbO3, aira.sensorsState.irbO4);
lock (_proxiDatas)
{
_proxiDatas.Enqueue(proxData);
}
haveData = true; // allow DataPort.Post(new Recv()) to happen, with data already enqueued in the ...Datas
// accelerometer data:
AccelerometerData accData = new AccelerometerData()
{
TimeStamp = aira.timestamp
};
accData.setAccelerometerData(aira.sensorsState.accelX, aira.sensorsState.accelY, aira.sensorsState.accelZ);
lock (_accelDatas)
{
_accelDatas.Enqueue(accData);
}
haveData = true; // allow DataPort.Post(new Recv()) to happen, with data already enqueued in the ...Datas
// compass data:
DirectionData dirData = null;
if (aira.sensorsState.compassHeading >= 0.0d && aira.sensorsState.compassHeading <= 359.999999d)
{
dirData = new DirectionData()
{
heading = aira.sensorsState.compassHeading, TimeStamp = aira.timestamp
};
lock (_directionDatas)
{
_directionDatas.Enqueue(dirData);
}
haveData = true; // allow DataPort.Post(new Recv()) to happen, with data already enqueued in the ...Datas
}
ParkingSensorData psiData = null;
if (aira.sensorsState.parkingSensorsValid)
{
psiData = new ParkingSensorData()
{
TimeStamp = aira.timestamp
};
psiData.setParkingSensorData(aira.sensorsState);
lock (_parkingSensorsDatas)
{
_parkingSensorsDatas.Enqueue(psiData);
}
haveData = true; // allow DataPort.Post(new Recv()) to happen, with data already enqueued in the ...Datas
}
// frames that are marked "fromPingScanStop" feed the sonar sweep view controls:
if (aira.fromPingScanStop)
{
int angleRaw1 = (int)aira.dPos1Mks;
double distM1 = aira.dPing1DistanceM;
//Tracer.Trace(String.Format("angleRaw1 = {0} us", angleRaw1));
//Tracer.Trace(String.Format("distMm1 = {0} mm", distMm1));
int angleRaw2 = (int)aira.dPos2Mks;
double distM2 = aira.dPing2DistanceM;
//Tracer.Trace(String.Format("angleRaw2 = {0} us", angleRaw2));
//Tracer.Trace(String.Format("distMm2 = {0} mm", distMm2));
haveData = haveData && setSonarRayReading(angleRaw1, distM1, angleRaw2, distM2, aira.timestamp);
}
if (haveData)
{
AnalogValue1 = aira.sensorsState.analogValue1;
// just post internally an empty signal message, the receiver - DataHandler(Recv recv) should examine the ...Datas and forward data to Responses port
DataReadyPort.Post(new Recv());
}
}
void ProximityDataHandler(ProximityData proximityData)
{
//Tracer.Trace("ProximityBoardCcrServiceCommander::ProximityDataHandler()");
_pbCommanderDataEventsPort.Post(proximityData);
}
