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);
        }
Exemplo n.º 5
0
        // 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);
        }