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! } }