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 VelodynePacket ReadNext()
{
if (pointReader.BaseStream.Position >= pointReader.BaseStream.Length)
{
return(new VelodyneEndOfFile());
}
long packetTimestampTicks = pointReader.ReadInt64();
DateTime baseTime = new DateTime(packetTimestampTicks);
baseTime = baseTime.ToUniversalTime();
DateTime dateTopOfTheHour = new DateTime(baseTime.Year, baseTime.Month, baseTime.Day, baseTime.Hour, 0, 0);
/*if (nextPacket == null)
* {
* nextPacket = pointReader.ReadBytes(1248);
* }*/
byte[] packet = pointReader.ReadBytes(1248);
VelodynePointPacket pointPacket = (VelodynePointPacket)PacketInterpreter.ReadRecord(packet, dateTopOfTheHour);
pointPacket.PacketTimestamp = new DateTime(packetTimestampTicks);
//nextPacket = pointReader.ReadBytes(1248);
return(pointPacket);
}
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;
}
}
/// <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);
}
// ref: http://velodynelidar.com/docs/manuals/VLP-16%20User%20Manual%20and%20Programming%20Guide%2063-9243%20Rev%20A.pdf
public override VelodynePacket ReadRecord(byte[] packet, DateTime?baseTime = null)
{
if (packet.Length == 554)
{
return(ReadRecordNMEA(packet));
}
else if (packet.Length == 1248)
{
DateTime?dateTopOfTheHour = null;
if (baseTime != null)
{
dateTopOfTheHour = new DateTime(baseTime.Value.Year, baseTime.Value.Month, baseTime.Value.Day, baseTime.Value.Hour, 0, 0);
}
//byte[] packet = reader.ReadBytes(1248);
VelodynePointPacket pointPacket = new VelodynePointPacket();
// Get timestamp and factory bytes
int l = packet.Length - 6; // this is the end of the pack!
double ts = BitConverter.ToUInt32(new byte[] { packet[l], packet[l + 1], packet[l + 2], packet[l + 3] }, 0) / 1000000.00;
factoryByte1 = packet[l + 4];
factoryByte2 = packet[l + 5];
if (ConvertFactroyByteToString(factoryByte2) != "VLP-16")
{
throw new Exception("Packet seems to come from other than VLP-16 sensor!");
}
if ((factoryByte1 == 0x38) && (this.ReturnMode == ReturnMode.StrongestReturnOnly))
{
throw new ArgumentException("Asked for StrongestReturnOnly but the factory byte indicates that the sensor was set to last return mode!");
}
if ((factoryByte1 == 0x37) && (this.ReturnMode == ReturnMode.LastReturnOnly))
{
throw new ArgumentException("Asked for StrongestReturnOnly but the factory byte indicates that the sensor was set to last return mode!");
}
// settigns for all returns
int i = 42; // header bytes
int k_iter = 2;
int j_iter = 16 * 3;
double[] firingAngle = firingAngleVLP16; // angles for vertical firing sequence
// Data packet
for (int datai = 0; datai < 12; datai++)
{
if ((packet[i] == 0xFF) && (packet[i + 1] == 0xEE))
{
byte lower_azimuth = packet[i + 2];
byte upper_azimuth = packet[i + 3];
double azimuth = BitConverter.ToUInt16(new byte[] { lower_azimuth, upper_azimuth }, 0) / 100.00;
//Console.WriteLine(i + " Azimuth: " + azimuth);
i = i + 4;
ReturnType returnType = datai % 2 == 0 ? ReturnType.StrongestReturn : ReturnType.LastReturn;
if (factoryByte1 == 0x39) // sensor in dual return, but we need only strongest or last returns
{
// skip all odd data block for strongest return
if ((ReturnMode == ReturnMode.StrongestReturnOnly) && (returnType == ReturnType.LastReturn))
{
i += j_iter * k_iter;
continue;
}
else if ((ReturnMode == ReturnMode.LastReturnOnly) && (returnType == ReturnType.StrongestReturn)) // skip all even data block for strongest return
{
i += j_iter * k_iter;
continue;
}
}
else if (factoryByte1 == 0x37) // strongest return
{
returnType = ReturnType.StrongestReturn;
}
else if (factoryByte1 == 0x38) // last return
{
returnType = ReturnType.LastReturn;
}
for (int k = 0; k < k_iter; k++)
{
for (int j = 0; j < j_iter; j = j + 3)
{
byte lower_distance = packet[i + j];
byte upper_distance = packet[i + j + 1];
double distance = (double)BitConverter.ToUInt16(new byte[] { lower_distance, upper_distance }, 0) * 2.0 / 1000.00;
byte intensity = packet[i + j + 2];
double omega = firingAngle[j / 3];
double omega_rad = omega / 180.0 * Math.PI;
double azimuth_rad = azimuth / 180.0 * Math.PI;
// VLP-16
double x = Math.Cos(azimuth_rad) * Math.Sin(omega_rad) * distance;
double y = Math.Cos(azimuth_rad) * Math.Cos(omega_rad) * distance;
double z = Math.Sin(azimuth_rad) * distance;
VelodynePoint pt = new VelodynePoint(x, y, z, distance, azimuth_rad, omega_rad, intensity, returnType);
pt.InternalTime = ts;
if (dateTopOfTheHour != null)
{
pt.Timestamp = dateTopOfTheHour.Value.AddSeconds(pt.InternalTime);
}
pointPacket.Points.Add(pt);
//if (factoryByte1 != 0x39)
//if (distance != 0)
//if (azimuth == 318.78)
//if (k == 0)
{
/*Console.WriteLine("Time (past the hour): " + ts + " s " + ts / 60 + "min \nFactory bytes: " + factoryByte1.ToString("X") + ": " + ConvertFactroyByteToString(factoryByte1) + ", "
+ factoryByte2.ToString("X") + ": " + ConvertFactroyByteToString(factoryByte2));
+ Console.WriteLine("Azimuth: " + azimuth + " Omega: " + omega + " Distance: " + distance);*/
//Console.WriteLine(datai + " " + k + " " + (j / 3) + " Azimuth: " + azimuth.ToString("0.00000") + " Omega: " + omega + " Distance: " + distance + " " + returnType + " " + ConvertFactroyByteToString(factoryByte1));
}
}
i += j_iter;
}
}
else
{
throw new Exception("Error in the point data packet structure!");
}
}
pointPacket.PacketTimestamp = pointPacket.Points[0].Timestamp;
return(pointPacket);
}
return(null);
}
// Ref: http://velodynelidar.com/docs/manuals/VLP-16%20User%20Manual%20and%20Programming%20Guide%2063-9243%20Rev%20A.pdf
public override VelodynePacket ReadRecord(byte[] packet, DateTime?baseTime = null)
{
if (packet.Length == 554)
{
return(ReadRecordNMEA(packet));
}
else if (packet.Length == 1248)
{
// Get timestamp and factory bytes
int l = packet.Length - 6; // this is the end of the pack!
double ts = BitConverter.ToUInt32(new byte[] { packet[l], packet[l + 1], packet[l + 2], packet[l + 3] }, 0) / 1000000.00;
factoryByte1 = packet[l + 4];
factoryByte2 = packet[l + 5];
if (ConvertFactroyByteToString(factoryByte2) != "HDL-32E")
{
throw new Exception("Packet seems to come from other then HDL sensor!");
}
//byte[] packet = reader.ReadBytes(1248);
VelodynePointPacket pointPacket = new VelodynePointPacket();
// Data packet
int i = 42;
for (int datai = 0; datai < 12; datai++)
{
if ((packet[i] == 0xFF) && (packet[i + 1] == 0xEE))
{
//i += 0;
byte lower_azimuth = packet[i + 2];
byte upper_azimuth = packet[i + 3];
double azimuth = BitConverter.ToUInt16(new byte[] { lower_azimuth, upper_azimuth }, 0) / 100.00;
i = i + 4;
int k_iter = 2;
int j_iter = 16 * 3;
int i_jump = 16;
double[] firingAngle = firingAngleHDL32;
k_iter = 1;
j_iter = 32 * 3;
i_jump = 32;
firingAngle = firingAngleHDL32;
for (int k = 0; k < k_iter; k++)
{
for (int j = 0; j < j_iter; j = j + 3)
{
byte lower_distance = packet[i + j];
byte upper_distance = packet[i + j + 1];
double distance = (double)BitConverter.ToUInt16(new byte[] { lower_distance, upper_distance }, 0) * 2.0 / 1000.00;
byte intensity = packet[i + j + 2];
double omega = firingAngle[j / 3];
double omega_rad = omega / 180.0 * Math.PI;
double azimuth_rad = azimuth / 180.0 * Math.PI;
double x = Math.Cos(omega_rad) * Math.Sin(azimuth_rad) * distance;
double y = Math.Cos(omega_rad) * Math.Cos(azimuth_rad) * distance;
double z = Math.Sin(omega_rad) * distance;
VelodynePoint pt = new VelodynePoint(x, y, z, distance, azimuth_rad, omega_rad, intensity, ReturnType.StrongestReturn);
pt.InternalTime = ts;
if (baseTime != null)
{
pt.Timestamp = baseTime.Value.AddSeconds(pt.InternalTime);
}
pointPacket.Points.Add(pt);
//if (ConvertFactroyByteToString(factoryByte2) == "HDL-32E")
//{
// Console.WriteLine("Time (past the hour): " + ts + " s " + ts / 60 + "min \nFactory bytes: " + factoryByte1.ToString("X") + ": " + ConvertFactroyByteToString(factoryByte1) + ", "
// + factoryByte2.ToString("X") + ": " + ConvertFactroyByteToString(factoryByte2));
//Console.WriteLine("Azimuth: " + azimuth + " Omega: " + omega + " Distance: " + distance);
//}
//Console.WriteLine(pt.intensity);
}
i = i + i_jump * 3;
}
}
else
{
throw new Exception("Error in the point data packet structure!");
}
}
pointPacket.PacketTimestamp = pointPacket.Points[0].Timestamp;
return(pointPacket);
}
return(null);
}
