void Processstuff(byte[] message)
{
BinaryReader br = new BinaryReader(new MemoryStream(message));
double maxr = 0;
VelodynePacket vpacket = new VelodynePacket();
vpacket.blocks = new List<VelodyneBlock> (12);
//first read 12 100 byte blocks
for (int i = 0; i < 12; i++)
{
VelodyneBlock block = new VelodyneBlock();
block.block = (BlockHeaderID)br.ReadUInt16();
block.rotation = (((double)(br.ReadUInt16())) / 100.0);
if (block.rotation > maxr) maxr = block.rotation;
block.ranges = new List<double>(32);
block.intensity = new List<int>(32);
//now read 32 lasers
for (int j = 0; j < 32; j++)
{
double range = ((double)(br.ReadUInt16())) * .002;
int intensity = (int)br.ReadByte();
block.ranges.Add(range);
block.intensity.Add(intensity);
}
vpacket.blocks.Add(block);
}
//now read the status
vpacket.spincount = (br.ReadUInt16());
int val1 = br.ReadByte();
int val2 = br.ReadByte();
char[] status = br.ReadChars(4);
foreach (char c in status)
vpacket.info += c + "";
//Console.WriteLine("Got Scan! Length: " + message.Length + " Status: " + val1.ToString() + val2.ToString() + vpacket.info);
if (br.BaseStream.Position != br.BaseStream.Length)
{
Console.WriteLine ("Did not reach end of velodyne packet: Pos: " + br.BaseStream.Position + "Len: " + br.BaseStream.Length);
}
//now make it xyz
if (scan.points == null)
scan.points = new List<VelodyneXYZPoint>();
for (int i=0; i<vpacket.blocks.Count; i++) //each block
{
for (int j=0; j<vpacket.blocks[i].ranges.Count; j++) //each laser
{
double range = vpacket.blocks[i].ranges[j];
//j is laser number
if (range < 1.0) continue;
VelodyneXYZPoint pt = new VelodyneXYZPoint (j,vpacket.blocks[i].block,range,vpacket.blocks[i].rotation,vpacket.blocks[i].intensity[j]);
scan.points.Add (pt);
}
}
scancount++;
Console.WriteLine("spincount: " + vpacket.spincount);
if (scancount >= 400)
//if (lastMaxR>maxr) // we've cycled
//if (vpacket.spincount != lastSpinCount)
{
if (GotPacket != null)
GotPacket(this, new VelodyneEventArgs(scan));
scan = new VelodyneXYZScan();
scancount = 0;
}
lastMaxR = maxr;
lastSpinCount = vpacket.spincount;
}
public VelodyneEventArgs(VelodyneXYZScan scan)
{
this.scan = scan;
}
public VelodyneEventArgs(VelodyneXYZScan scan)
{
this.scan = scan;
}
void Processstuff(byte[] message)
{
BinaryReader br = new BinaryReader(new MemoryStream(message));
double maxr = 0;
VelodynePacket vpacket = new VelodynePacket();
vpacket.blocks = new List <VelodyneBlock> (12);
//first read 12 100 byte blocks
for (int i = 0; i < 12; i++)
{
VelodyneBlock block = new VelodyneBlock();
block.block = (BlockHeaderID)br.ReadUInt16();
block.rotation = (((double)(br.ReadUInt16())) / 100.0);
if (block.rotation > maxr)
{
maxr = block.rotation;
}
block.ranges = new List <double>(32);
block.intensity = new List <int>(32);
//now read 32 lasers
for (int j = 0; j < 32; j++)
{
double range = ((double)(br.ReadUInt16())) * .002;
int intensity = (int)br.ReadByte();
block.ranges.Add(range);
block.intensity.Add(intensity);
}
vpacket.blocks.Add(block);
}
//now read the status
vpacket.spincount = (br.ReadUInt16());
int val1 = br.ReadByte();
int val2 = br.ReadByte();
char[] status = br.ReadChars(4);
foreach (char c in status)
{
vpacket.info += c + "";
}
//Console.WriteLine("Got Scan! Length: " + message.Length + " Status: " + val1.ToString() + val2.ToString() + vpacket.info);
if (br.BaseStream.Position != br.BaseStream.Length)
{
Console.WriteLine("Did not reach end of velodyne packet: Pos: " + br.BaseStream.Position + "Len: " + br.BaseStream.Length);
}
//now make it xyz
if (scan.points == null)
{
scan.points = new List <VelodyneXYZPoint>();
}
for (int i = 0; i < vpacket.blocks.Count; i++) //each block
{
for (int j = 0; j < vpacket.blocks[i].ranges.Count; j++) //each laser
{
double range = vpacket.blocks[i].ranges[j];
//j is laser number
if (range < 1.0)
{
continue;
}
VelodyneXYZPoint pt = new VelodyneXYZPoint(j, vpacket.blocks[i].block, range, vpacket.blocks[i].rotation, vpacket.blocks[i].intensity[j]);
scan.points.Add(pt);
}
}
scancount++;
Console.WriteLine("spincount: " + vpacket.spincount);
if (scancount >= 400)
//if (lastMaxR>maxr) // we've cycled
//if (vpacket.spincount != lastSpinCount)
{
if (GotPacket != null)
{
GotPacket(this, new VelodyneEventArgs(scan));
}
scan = new VelodyneXYZScan();
scancount = 0;
}
lastMaxR = maxr;
lastSpinCount = vpacket.spincount;
}
