public List <VelodynePoint> ReadNextFrame()
{
List <VelodynePoint> points = new List <VelodynePoint>();
bool isGo = true;
while (isGo)
{
VelodynePacket packet = ReadNext();
if (packet is VelodynePacket)
{
VelodynePointPacket pointPacket = packet as VelodynePointPacket;
foreach (VelodynePoint pt in pointPacket.Points)
{
points.Add(pt);
if ((lastPoint != null) && ((pt.Hz - lastPoint.Hz) < 0))
{
isGo = false;
break;
}
}
lastPoint = pointPacket.Points.Last();
}
}
return(points);
}
public void AnalysOffset()
{
DateTime prevGps = DateTime.MaxValue;
int k = 0;
foreach (IndexData idx in Indeces)
{
if (idx.Nmea.GPSTime != prevGps)
{
Seek(idx);
VelodynePacket packet = ReadNext();
VelodynePointPacket pointPacket = packet as VelodynePointPacket;
DateTime? ptTs = pointPacket.Points[0].Timestamp;
if (ptTs != null)
{
TimeSpan dt = new TimeSpan(ptTs.Value.TimeOfDay.Ticks - idx.Nmea.GPSTime.TimeOfDay.Ticks);
//if (dt.Minutes < 1)
//{
//if (k++ > 5000) return;
if (dt.TotalSeconds > 0.1)
{
Console.WriteLine(dt.TotalSeconds.ToString("0.000") + " " + idx.InternalTimeStamp.ToString("HH:mm:ss.fff") + " " + idx.Nmea.GPSTime.ToString("HH:mm:ss.fff") + " " + Math.Floor(pointPacket.Points[0].InternalTime / 60.0).ToString("0")
+ " " + (pointPacket.Points[0].InternalTime - Math.Floor(pointPacket.Points[0].InternalTime / 60.0) * 60).ToString("0.000"));
}
//}
}
}
prevGps = idx.Nmea.GPSTime;
//prevGps = DateTime.MaxValue;
//if (k++ > 5000) return;
}
}
static void Main(string[] args)
{
//String pcapFile = @"F:\CAR\2017_08_28_2\Velodynes\VHDL\2017-08-28-16-01-45_Velodyne-HDL-32-Data.pcap";
//String pcapFile = @"E:\CAR\2017_10_13\Velodynes\VWHITE\2017-10-13-12-11-18_Velodyne-VLP-16-Data.pcap";
//String pcapFile = @"E:\CAR\2017_10_13\Velodynes\VRED\2017-10-13-12-03-19_Velodyne-VLP-16-Data.pcap";
String pcapFile = @"E:\CAR\2017_10_13\Velodynes\VGREEN\2017-10-13-12-04-11_Velodyne-VLP-16-Data.pcap";
String indexFile = VelodyneConverter.GetDefaultIndexFile(pcapFile);
String pointFile = VelodyneConverter.GetDefaultPointFile(pcapFile);
/*String indexFile = @"E:\CAR\2017_08_28_2\Velodynes\VRED\2017-08-28-15-59-48_Velodyne-VLP-16-Data.pcap.idx";
* String pointFile = @"E:\CAR\2017_08_28_2\Velodynes\VRED\2017-08-28-15-59-48_Velodyne-VLP-16-Data.pcap.bin";
* VelodyneReader veloReader = new VelodyneReader(VelodyneReader.SensorType.VLP16, indexFile, pointFile);*/
/*VelodyneConverter converter = VelodyneConverter.Create(pcapFile);
* converter.ProgressReport += Converter_ProgressReport;
* converter.Convert();
* Console.WriteLine("Conversion is done!");
* return;*/
Console.WriteLine("Done.");
List <VelodynePoint> pts = new List <VelodynePoint>();
using (VelodyneReader veloReader = VelodyneReader.Open(VelodyneSensorType.VLP16, ReturnMode.LastReturnOnly, indexFile, pointFile))
{
//veloReader.AnalysOffset();
IndexData idx = veloReader.FindIndexByTime(new DateTime(2017, 10, 13, 16, 45, 04, 0), VelodyneReader.SearchType.FLOOR);
veloReader.SeekByTime(new DateTime(2017, 10, 13, 16, 52, 04, 0));
Console.WriteLine(idx.PacketTimeStamp.ToString("yyyy-MM-dd HH:mm:ss") + " NMEA: " + idx.InternalTimeStamp.ToString("yyyy-MM-dd HH:mm:ss") + " Pos: " + idx.Position + " NMEA: " + idx.Nmea.NmeaString);
for (int k = 0; k < 1000; k++)
{
VelodynePacket packet = veloReader.ReadNext();
if (packet is VelodynePointPacket)
{
VelodynePointPacket pointPacket = packet as VelodynePointPacket;
}
}
Console.WriteLine("done.");
Console.ReadKey();
Simple3dViewerWnd viewer = new Simple3dViewerWnd(veloReader);
viewer.ShowDialog();
}
Console.ReadKey();
}
/// <summary>
/// Seek to the indicated type
/// </summary>
/// <param name="timeStamp">Timestamp to seek</param>
/// <param name="searchType"></param>
/// <returns>Time difference between the found and requested times</returns>
public double SeekByTime(DateTime timeStamp, SearchType searchType = SearchType.FLOOR)
{
IndexData idx = FindIndexByTime(timeStamp, SearchType.FLOOR);
Seek(idx);
// now read records until find the best point record
VelodynePacket packet = ReadNext();
double it = 0;
VelodynePointPacket pointPacket = null;
while ((!(packet is VelodyneEndOfFile)))
{
pointPacket = packet as VelodynePointPacket;
if (pointPacket.Points[0].Timestamp.Value.Ticks > timeStamp.Ticks)
{
break;
}
packet = ReadNext();
it = pointPacket.Points[0].InternalTime;
}
return(new TimeSpan(pointPacket.Points[0].Timestamp.Value.Ticks - timeStamp.Ticks).TotalSeconds);
}
