private void ProcessReadings(WifiPacket packet)
{
readingsToProcess.Enqueue (new WifiReading{ timestamp_us = packet.timestamp_us, signal_dbm = packet.signal_dbm });
bool not_in_history, not_yet;
Matrix4x4 robotToGlobal = new Matrix4x4();
while (readingsToProcess.Count > 0)
{
WifiReading reading = readingsToProcess.Peek ();
PositionHistory.PositionSnapshot snapshot= positionHistory.GetPositionSnapshotThreadSafe(reading.timestamp_us, reading.timestamp_us, out not_yet, out not_in_history);
if (not_in_history)
{
print("wifi - ignoring packet (position data not in history) with timestamp " + reading.timestamp_us);
readingsToProcess.Dequeue ();
continue;
}
if (not_yet) //wait for position data to arrive
return;
readingsToProcess.Dequeue();
float height01 = ((float)reading.signal_dbm - (float)signal.minValueDbm) / ((float)signal.maxValueDbm-(float)signal.minValueDbm);
float heightm = Mathf.LerpUnclamped(plot.minHeight, plot.maxHeight, height01);
Vector3 position = new Vector3(wifiPosition.x, heightm, wifiPosition.z);
PositionData pos = snapshot.PositionAt (reading.timestamp_us);
robotToGlobal.SetTRS(pos.position, Quaternion.Euler(0.0f, pos.heading, 0.0f), Vector3.one);
position = robotToGlobal.MultiplyPoint3x4(position);
wifiReadingsCB.Put (position);
}
}
protected override void Awake()
{
base.Awake();
lastPosition = new PositionData{position=transform.parent.position, heading=transform.parent.eulerAngles.y};
initialHeading = lastPosition.heading;
thread_shared_position = lastPosition;
}
protected override void ProcessPacket(DeadReconningPacket packet)
{
//First call - set first udp packet with reference encoder positions
if (lastPacket.timestamp_us == 0)
{
lastPacket.CloneFrom(packet);
return;
}
// UDP doesn't guarantee ordering of packets, if previous odometry is newer ignore the received
if (packet.timestamp_us <= lastPacket.timestamp_us)
{
print("dead-reconning - ignoring out of time packet (previous, now):" + Environment.NewLine + lastPacket.ToString() + Environment.NewLine + packet.ToString());
return;
}
//this is the actual work
lastPosition=EstimatePosition(lastPosition, lastPacket, packet);
lastPacket.CloneFrom(packet);
// Share the new calculated position estimate with Unity thread
lock (deadReconningLock)
{
thread_shared_position=lastPosition;
thread_shared_averaged_packet_time_ms = AveragedPacketTimeMs();
}
positionHistory.PutThreadSafe(lastPosition);
}
private PositionData EstimatePosition(PositionData lastPosition, DeadReconningPacket lastPacket, DeadReconningPacket packet)
{
// Calculate the linear displacement since last packet
float distance_per_encoder_count_mm = Mathf.PI * physics.wheelDiameterMm / physics.encoderCountsPerRotation;
float ldiff = packet.position_left - lastPacket.position_left;
float rdiff = packet.position_right - lastPacket.position_right;
float displacement_m = (ldiff + rdiff) * distance_per_encoder_count_mm / 2.0f / Constants.MM_IN_M;
if (physics.reverseMotorPolarity)
displacement_m = -displacement_m;
// Calculate the average heading from previous and current packet
float angle_start_deg = lastPacket.HeadingInDegrees;
float angle_end_deg = packet.HeadingInDegrees;
float angle_difference_deg = angle_end_deg - angle_start_deg;
// The tricky case when we cross 0 or -180/180 in packets has to be handled separately
if (Mathf.Abs(angle_difference_deg) > 180.0f)
angle_difference_deg = angle_difference_deg - Mathf.Sign(angle_difference_deg) * 360.0f;
float average_heading_rad = (angle_start_deg + initialHeading + angle_difference_deg / 2.0f) * Constants.DEG2RAD;
// Finally update the position and heading
lastPosition.timestamp = packet.timestamp_us;
lastPosition.position = new Vector3(lastPosition.position.x + displacement_m * Mathf.Sin(average_heading_rad), lastPosition.position.y, lastPosition.position.z + displacement_m * Mathf.Cos(average_heading_rad));
lastPosition.heading = angle_end_deg + initialHeading;
return lastPosition;
}
void Update ()
{
lock (deadReconningLock)
{
actualPosition = thread_shared_position;
averagedPacketTimeMs = thread_shared_averaged_packet_time_ms;
}
transform.parent.transform.position=actualPosition.position;
transform.parent.transform.rotation=Quaternion.Euler(0.0f, actualPosition.heading, 0.0f);
}
public void PutThreadSafe(PositionData pos)
{
lock (syncRoot)
{
if (history.Size == 0)
{
history.Put(pos);
return;
}
// if we get some UDP packets not in chronological order, ignore them
if (history.PeekNewest().timestamp >= pos.timestamp)
return;
history.Put(pos);
}
}
private bool TranslateReadingsToGlobalReferenceFrame(ref int from,ref int len, ulong t_from, ulong t_to)
{
bool not_in_history, not_yet;
if (threadInternal.pending && t_from != threadInternal.t_from)
AddPendingDataToProcess(ref from, ref len, ref t_from);
PositionHistory.PositionSnapshot snapshot= positionHistory.GetPositionSnapshotThreadSafe(t_from, t_to, out not_yet, out not_in_history);
if (not_in_history)
{
print("Laser - ignoring packet (position data not in history) with timestamp " + t_from);
threadInternal.pending = false;
return false;
}
if (not_yet)
{
threadInternal.SetPending (from, len, t_from, t_to);
return false;
}
threadInternal.pending = false;
Matrix4x4 robotToGlobal = new Matrix4x4();
Vector3 scale = Vector3.one;
PositionData pos=new PositionData();
ulong[] timestamps = threadInternal.timestamps;
Vector3[] readings = threadInternal.readings;
for (int i = from, ind; i < from+len; ++i)
{
ind = i % 360;
pos = snapshot.PositionAt(timestamps[ind]);
robotToGlobal.SetTRS(pos.position, Quaternion.Euler(0.0f, pos.heading, 0.0f), scale);
readings[ind]=robotToGlobal.MultiplyPoint3x4(readings[ind]);
}
return true;
}
public PositionData PositionAt(ulong timestamp)
{
try
{
if (timestamp < data[0].timestamp || timestamp > data[data.Length - 1].timestamp)
throw new System.ArgumentOutOfRangeException("timestamp not in snapshot");
PositionData pos = new PositionData{ timestamp = timestamp };
int index=System.Array.BinarySearch<PositionData>(data, pos);
if (index >= 0)
return data[index];
int after = ~index;
int before = after - 1;
//now interpolate position
float at=(float)(timestamp-data[before].timestamp)/(float)(data[after].timestamp-data[before].timestamp);
pos.position=data[before].position + at * (data[after].position - data[before].position);
//now interpolate heading
float angle_difference = data[after].heading - data[before].heading;
//if we are wrapping around -180/180 it's the small angle, not the big one
if (Mathf.Abs(angle_difference) > 180.0f)
angle_difference = angle_difference - Mathf.Sign(angle_difference) * 360.0f;
pos.heading = data[before].heading + at * angle_difference;
return pos;
}
catch(System.Exception) {
//the whole try catch can be removed, but recall when this was needed
return new PositionData{heading=0,position=new Vector3(data[0].timestamp, timestamp, data[data.Length-1].timestamp), timestamp=timestamp}; //DUMMY!
}
}