private void ReadDistanceData()
{
var distanceRecord = new DistanceRecord();
string timestamp = transport.ReadLine();
string actualTimestamp = timestamp.Substring(0, timestamp.Length - 1);
var timestampBytes = Encoding.GetEncoding("UTF-8").GetBytes(actualTimestamp);
if (timestampBytes.Length == 4)
{
int[] timestampTmp = new int[1];
DecodeMulti(timestampBytes, timestampTmp, 4);
distanceRecord.Timestamp = timestampTmp[0];
}
string data = "";
while (true)
{
string line = transport.ReadLine();
//Debug.Log(string.Format("read: {0}", line.Length));
if (line.Length == 0)
{
// last
break;
}
// last character is checksum
string actualData = line.Substring(0, line.Length - 1);
data += actualData;
}
// Debug.Log(data);
var dataBytes = Encoding.GetEncoding("UTF-8").GetBytes(data);
DecodeMulti(dataBytes, distances, 3);
distanceRecord.RawDistances = distances;
var detectedLocations = new List <DetectedLocation>();
for (var i = 0; i < distances.Length; i++)
{
detectedLocations.Add(new DetectedLocation(i, stepAngleDegrees * i + offsetDegrees, distances[i]));
}
foreach (var filter in locationFilters)
{
detectedLocations = filter.Filter(detectedLocations);
}
// pass a copy of list since the original list is not thread safe
var copy = new List <DetectedLocation>();
copy.AddRange(detectedLocations.Select(i => (DetectedLocation)i.Clone()));
distanceRecord.FilteredResults = copy;
if (clusterExtraction != null)
{
distanceRecord.ClusteredIndices = clusterExtraction.ExtractClusters(copy);
}
OnDistanceReceived?.Invoke(distanceRecord);
}
void Urg_OnDistanceReceived(DistanceRecord data)
{
Debug.LogFormat("distance received: SCIP timestamp={0} unity timer={1}", data.Timestamp, stopwatch.ElapsedMilliseconds);
Debug.LogFormat("cluster count: {0}", data.ClusteredIndices.Count);
this.rawDistances = data.RawDistances;
this.locations = data.FilteredResults;
this.clusterIndices = data.ClusteredIndices;
}
