sick_obstacleDetection/encTest/SickTimEnc.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Net;
using System.Net.Sockets;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace Test
{
    class SickTimEnc
    {
        //状态指示变量
        /// <summary>
        /// 指示初始化是否成功
        /// </summary>
        public bool initialized { get; set; }
        /// <summary>
        /// 指示是否工作中
        /// </summary>
        public bool working { get; set; }
        /// <summary>
        /// 指示是否检测到障碍物
        /// </summary>
        public bool obstacleDetected { get; set; }
        /// <summary>
        /// 发送指令返回值
        /// </summary>
        private int flag = 0;
        public int dataCount = 0;
        //通信参数
        private readonly object Lock = new object();
        private IPAddress ipa;
        private int port;
        private TcpClient tcpClient;
        private NetworkStream stream;
        private int bufferSize = 16384;
        private const string startCmd = "02 73 52 4E 20 4C 4D 44 73 63 61 6E 64 61 74 61 03";
        private const string endCmd = "02 73 45 4E 20 4C 4D 44 73 63 61 6E 64 61 74 61 20 30 03";
        private static char[] prefix = "sRA LMDscandata 1 ".ToCharArray();
        private static char[] postfix = " not defined 0 0 0".ToCharArray();
        //防撞参数
        public int maxDataCount { get; set; }//最大保留历史数据个数
        private int left_range = 385;//325;//231;//271;//386;//366;
        private int right_range = 425;//485;//581;//541;//426;//446;
        public int ground_truth_height { get; set; } = 2200;//2200;//2300;//标准高度
        public int min_num_point_of_hinder { get; set; } = 30;//20;//判定为障碍物的最少点的数目
        public int min_obstacle_height { get; set; } = 500;//最小障碍物高度
        private List<byte[]> originDatalist;//原始数据数组
        private List<int> validNum;//811个整型值
        private List<List<double>> validNumBuffer;//保存历史点云数据
        private List<List<double>> validNumBufferKalman;//保存经过kalman滤波的历史点云数据
        private List<double> listy;
        public List<double> listx { get; set; }
        public List<double> yFiltered { get; set; }
        public List<double> yFilteredKalman { get; set; }
        //卡尔曼滤波参数
        private List<double> prevData, p, q, r, kGain;

        public SickTimEnc(string ip, int port)
        {
            initialized = false;
            maxDataCount = 3;
            originDatalist = new List<byte[]>();
            validNum = new List<int>();
            validNumBuffer = new List<List<double>>();
            validNumBufferKalman = new List<List<double>>();
            prevData = new List<double>();
            p = new List<double>();
            q = new List<double>();
            r = new List<double>();
            kGain = new List<double>();
            for (int i = 0; i < right_range - left_range; i++)
            {
                prevData.Add(0);
                p.Add(450);
                q.Add(0.0001);
                r.Add(0.005);
                kGain.Add(0);
            }
            listy = new List<double>();
            listx = new List<double>();
            tcpClient = new TcpClient();
            if(!IPAddress.TryParse(ip, out ipa))
            {
                ipa = null;
                return;
            }
            try
            {
                tcpClient.Connect(ipa, port);
                stream = tcpClient.GetStream();
            }
            catch { return; }
            initialized = true;
            working = true;
            work();
        }
        /// <summary>
        /// 暂停工作
        /// </summary>
        /// <param name="ctrl"></param>
        public void Pause()
        {
            working = !working;
        }
        /// <summary>
        /// 停止工作
        /// </summary>
        public void Stop()
        {
            working = false;
            flag = sendCmd(endCmd);
            if (stream != null)
            {
                stream.Close();
            }
            if (tcpClient != null)
            {
                tcpClient.Close();
            }
        }
        /// <summary>
        /// 获取最小角度阈值，范围-45 - 225
        /// </summary>
        /// <param name="type">类型，点编号index或角度degree</param>
        /// <returns></returns>
        public int GetLeftRange(string type)
        {
            if (type == "degree")
                return left_range / 3 - 45;
            else
                return left_range;
        }
        /// <summary>
        /// 获取最大角度阈值，范围-45 - 225
        /// </summary>
        /// <param name="type">类型，点编号index或角度degree</param>
        /// <returns></returns>
        public int GetRightRange(string type)
        {
            if (type == "degree")
                return right_range / 3 - 45;
            else
                return right_range;
        }
        ///// <summary>
        ///// 获取距地面高度
        ///// </summary>
        ///// <returns></returns>
        //public int GetHeight()
        //{
        //    return ground_truth_height;
        //}
        /// <summary>
        /// 设置最小角度阈值
        /// </summary>
        /// <param name="degree"></param>
        public void SetLeftRange(int degree)
        {
            if (working)
            {
                working = false;
                lock (Lock)
                {
                    left_range = (degree + 45) * 3;
                }
                working = true;
            }
            else
            {
                left_range = (degree + 45) * 3;
            }
            initKalman();
        }
        /// <summary>
        /// 设置最大角度阈值
        /// </summary>
        /// <param name="degree"></param>
        public void SetRightRange(int degree)
        {
            if (working)
            {
                working = false;
                lock (Lock)
                {
                    right_range = (degree + 45) * 3;
                }
                working = true;
            }
            else
            {
                right_range = (degree + 45) * 3;
            }
            initKalman();
        }
        ///// <summary>
        ///// 设置距地面高度
        ///// </summary>
        ///// <param name="height"></param>
        //public void SetHeight(int height)
        //{
        //    ground_truth_height = height;
        //}


        /// <summary>
        /// 初始化kalman滤波参数
        /// </summary>
        private void initKalman()
        {
            prevData.Clear();
            p.Clear();
            q.Clear();
            r.Clear();
            kGain.Clear();
            for (int i = 0; i < right_range - left_range; i++)
            {
                prevData.Add(0);
                p.Add(450);
                q.Add(0.0001);
                r.Add(0.005);
                kGain.Add(0);
            }
        }
        /// <summary>
        /// 启动雷达处理数据
        /// </summary>
        private void work()
        {
            Task.Factory.StartNew(()=>
            {
                while (initialized)
                {
                    try
                    {
                        if (working)
                        {
                            System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
                            sw.Start();
                            //开辟线程存储雷达原始数据字符串
                            lock (Lock)
                            {
                                originDatalist.Clear();
                                flag = sendCmd(startCmd);
                                if (flag == 1)
                                {
                                    byte[] buffer = new byte[bufferSize];
                                    stream.Read(buffer, 0, buffer.Length);
                                    //string str = HexStringToASCII(buffer);
                                    originDatalist.Add(buffer);
                                }
                                if (originDatalist.Count != 0)
                                {
                                    //System.Diagnostics.Stopwatch sw1 = new System.Diagnostics.Stopwatch();
                                    //sw1.Start();
                                    List<byte> result = new List<byte>();
                                    for (int i = 0; i < originDatalist.Count; i++)
                                    {
                                        //string ss = trimDataStr(originDatalist[i], "sRA LMDscandata 1 ", " not defined 0 0 0");
                                        bool trimResult = trimData(originDatalist[i], out result);
                                        if (!trimResult)
                                            continue;

                                        //string[] data = ss.Split(' ');
                                        //string number = data[22];
                                        byte[] lengthArr = { 32, 51, 50, 66 };
                                        int index = 0,index2 = 0, length=0;
                                        while(index < 500 && index2 < lengthArr.Length)
                                        {
                                            if (result[index] == lengthArr[index2])
                                                index2++;
                                            index++;
                                        }
                                        if (index2 < lengthArr.Length)
                                            continue;
                                        else
                                            length = 811;
                                        //while (result[index] != ' ')
                                        //{
                                        //    byte currByte = result[index];
                                        //    length = (length << 4) + (currByte<='9'?(currByte-48):(currByte-55));
                                        //    index++;
                                        //}
                                        //result[22]/10 * 16 + result[22] % 10;//Convert.ToInt32(number, 16);
                                        int temp = 0;
                                        //int[] validNum = new int[length];
                                        //string aws = "";
                                        //获取所有距离数值
                                        validNum.Clear();
                                        listy.Clear();
                                        listx.Clear();
                                        
                                        for (int j = 0; j < length; j++)
                                        {
                                            try
                                            {
                                                temp = -1;
                                                //temp = result[23+j] / 10 * 16 + result[23+j] % 10;//Convert.ToInt32(data[23 + j], 16);
                                                while(result[index] == ' ')
                                                {
                                                    index++;
                                                }
                                                while (result[index] != ' ')
                                                {
                                                    byte currByte = result[index];
                                                    temp = ((temp==-1?0:temp) << 4) + (currByte <= '9' ? (currByte - 48) : (currByte - 55));
                                                    index++;
                                                }
                                                if (temp == -1)
                                                    continue;
                                            }
                                            catch { temp = ground_truth_height; Console.WriteLine("数据转换异常"); }
                                            validNum.Add(temp);
                                        }
                                        //Console.WriteLine("valid points: "+validNum.Count);
                                        for (int j = 0; j < length; j++)
                                        {
                                            double x = validNum[j] * Math.Cos((-45 + 0.333 * j) * Math.PI / 180);
                                            double y = validNum[j] * Math.Sin((-45 + 0.333 * j) * Math.PI / 180);
                                            listy.Add(y);
                                            listx.Add(x);
                                        }
                                       

                                        validNumBuffer.Add(listy);
                                        validNumBufferKalman.Add(kalmanFilter(listy));
                                        while (validNumBufferKalman.Count > maxDataCount)
                                        {
                                            validNumBufferKalman.RemoveAt(0);
                                            validNumBuffer.RemoveAt(0);
                                        }
                                        
                                        if (validNumBufferKalman.Count == maxDataCount)
                                        {
                                            bool obstacle = false;
                                            yFiltered = dataFilter(validNumBuffer, out obstacle);
                                            yFilteredKalman = dataFilter(validNumBufferKalman, out obstacle);
                                            obstacleDetected = obstacle;
                                        }
                                        //sw.Stop();
                                        //Console.WriteLine("laserData: " + sw.ElapsedMilliseconds + "ms");
                                        //dataCount++;
                                        //Console.WriteLine("laser:: "+dataCount);
                                    }
                                }
                            }
                            sw.Stop();
                            //Console.WriteLine("laserDataFilter: " + sw.ElapsedMilliseconds + "ms");
                            Thread.Sleep(1);
                        }
                        else
                        {
                            //空闲状态
                            Thread.Sleep(1000);
                        }
                    }
                    catch (Exception e) { Thread.Sleep(500); Console.WriteLine(e.Message); Console.WriteLine(e.StackTrace); }
                    
                }
            });
        }
        /// <summary>
        /// 向雷达发送指令
        /// </summary>
        /// <param name="cmd"></param>
        /// <returns></returns>
        private int sendCmd(string cmd)
        {
            try
            {
                if (cmd.Equals(startCmd))
                {
                    byte[] byteStartCmd = HexStringToBinary(startCmd);
                    string result = HexStringToASCII(byteStartCmd);
                    byte[] b = Encoding.UTF8.GetBytes(result);
                    stream.Write(b, 0, b.Length);
                    return 1;
                }
                if (cmd.Equals(endCmd))
                {
                    byte[] byteStartCmd = HexStringToBinary(endCmd);
                    string result = HexStringToASCII(byteStartCmd);
                    byte[] b = Encoding.UTF8.GetBytes(result);
                    stream.Write(b, 0, b.Length);
                    return 2;
                }
            }
            catch { return 0; }
            return 0;
        }
        /// <summary>
        /// 将一条十六进制字符串转换为ASCII
        /// </summary>
        /// <param name="hexstring">一条十六进制字符串</param>
        /// <returns>返回一条ASCII码</returns>
        private static string HexStringToASCII(byte[] bt)
        {
            //byte[] bt = HexStringToBinary(hexstring);
            string lin = "";
            for (int i = 0; i < bt.Length; i++)
            {
                lin = lin + bt[i] + " ";
            }


            string[] ss = lin.Trim().Split(new char[] { ' ' });
            char[] c = new char[ss.Length];
            int a;
            for (int i = 0; i < c.Length; i++)
            {
                a = Convert.ToInt32(ss[i]);
                c[i] = Convert.ToChar(a);
            }

            string b = new string(c);
            return b;
        }
        /// <summary>
        /// 16进制字符串转换为二进制数组
        /// </summary>
        /// <param name="hexstring">用空格切割字符串</param>
        /// <returns>返回一个二进制字符串</returns>
        private static byte[] HexStringToBinary(string hexstring)
        {

            string[] tmpary = hexstring.Trim().Split(' ');
            byte[] buff = new byte[tmpary.Length];
            for (int i = 0; i < buff.Length; i++)
            {
                buff[i] = Convert.ToByte(tmpary[i], 16);
            }
            return buff;
        }
        /// <summary>
        /// 截取数据段
        /// </summary>
        /// <param name="sourse"></param>
        /// <param name="startstr"></param>
        /// <param name="endstr"></param>
        /// <returns></returns>
        private static string trimDataStr(string sourse, string startstr, string endstr)
        {
            string result = string.Empty;
            int startindex, endindex;
            try
            {
                startindex = sourse.IndexOf(startstr);
                if (startindex == -1)
                {
                    return result;
                }
                string tmpstr = sourse.Substring(startindex + startstr.Length);
                endindex = tmpstr.IndexOf(endstr);
                if (endindex == -1)
                {
                    return result;
                }
                result = tmpstr.Remove(endindex);
            }
            catch (Exception) { }
            return result;
        }
        /// <summary>
        /// 截取数据段
        /// </summary>
        /// <param name="source"></param>
        /// <param name="left"></param>
        /// <param name="right"></param>
        /// <returns></returns>
        private static bool trimData(byte[] source, out List<byte> result)
        {
            int i = 0, j = 0, k=postfix.Length-1;
            int left = 0, right = 0;
            result = new List<byte>();
            //find prefix index
            while (j < prefix.Length && i<source.Length)
            {
                if (source[i] == prefix[j])
                    j++;
                i++;
            }
            if (j >= prefix.Length)
                left = i;
            else
                return false;

            //find postfix index
            i = source.Length - 1;
            while(k>=0 && i >= left)
            {
                if (postfix[k] == source[i])
                    k--;
                i--;
            }
            if (k < 0)
                right = i;
            else
                return false;

            for (int x = left; x <= right; x++)
            {
                result.Add(source[x]);
            }
            return true;
        }

        /// <summary>
        /// 平滑数据，识别障碍物
        /// </summary>
        /// <param name="y"></param>
        /// <param name="isObstacle"></param>
        /// <returns></returns>
        private List<double> dataFilter(List<List<double>> y, out bool isObstacle)
        {
            List<double> y_udt = new List<double>();
            isObstacle = false;
            int hinder_point_count = 0;
            for (int i = left_range; i < right_range; i++)
            {
                int hinder_frame_count = 0;

                for (int j = 0; j < y.Count; j++)
                {
                    if (i != left_range && i != right_range - 1)
                    {
                        y[j][i] = (y[j][i - 1] + y[j][i] + y[j][i + 1]) / 3;//每个点的y为其和相邻两个点和的平均值
                    }
                    double d = Math.Abs(y[j][i] - ground_truth_height);
                    if (d > min_obstacle_height)//300
                        hinder_frame_count = hinder_frame_count + 1;
                }

                if (hinder_frame_count == y.Count)
                    hinder_point_count = hinder_point_count + 1;
                y_udt.Add(y[y.Count - 1][i]);
            }
            if (hinder_point_count >= min_num_point_of_hinder)
                isObstacle = true;
            return y_udt;
        }
        /// <summary>
        /// 卡尔曼滤波
        /// </summary>
        /// <param name="data_in"></param>
        /// <returns></returns>
        private List<double> kalmanFilter(List<double> data_in)
        {
            for (int i = left_range; i < right_range; i++)
            {
                p[i - left_range] = p[i - left_range] + q[i - left_range];
                kGain[i - left_range] = p[i - left_range] / (p[i - left_range] + r[i - left_range]);
                data_in[i] = prevData[i - left_range] + (kGain[i - left_range] * (data_in[i] - prevData[i - left_range]));
                p[i - left_range] = (1 - kGain[i - left_range]) * p[i - left_range];
                prevData[i - left_range] = data_in[i];
            }

            return data_in;
        }

    }
}