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;
        }
示例#2
0
        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;
        }