Example #1
0
        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);
        }
Example #2
0
 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;
 }