sick_obstacleDetection/Test/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;
        //通信参数
        private static 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";
        //防撞参数
        public int maxDataCount { get; set; }//最大保留历史数据个数
        public int left_range { get; set; } = 385;//325;//231;//271;//386;//366;
        public int right_range { get; set; } = 425;//485;//581;//541;//426;//446;
        private int ground_truth_height = 1700;//2200;//2300;//标准高度
        private int min_num_point_of_hinder = 30;//20;//判定为障碍物的最少点的数目
        private int min_obstacle_height = 500;//最小障碍物高度
        private int max_axis_Y = 2000;//2250;//画图时Y轴最大量程
        private List<string> originDatalist;//原始数据数组
        private List<int> validNum;//811个整型值
        private List<List<double>> validNumBuffer;//保存历史点云数据
        private List<List<double>> validNumBufferKalman;//保存经过kalman滤波的历史点云数据
        private List<double> listy;
        private List<double> listx;
        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<string>();
            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;
        }
        /// <summary>
        /// 启动雷达处理数据
        /// </summary>
        public void work()
        {
            Task.Factory.StartNew(()=>
            {
                while (initialized)
                {
                    try
                    {
                        if (working)
                        {
                        //开辟线程存储雷达原始数据字符串
                            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(str);
                                }
                                if (originDatalist.Count != 0)
                                {
                                    for (int i = 0; i < originDatalist.Count; i++)
                                    {
                                        string ss = trimDataStr(originDatalist[i], "sRA LMDscandata 1 ", " not defined 0 0 0");
                                        string[] data = ss.Split(' ');
                                        string number = data[22];
                                        int length = 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 = Convert.ToInt32(data[23 + j], 16);
                                            }
                                            catch { temp = ground_truth_height; Console.WriteLine("数据转换异常"); }
                                            validNum.Add(temp);
                                        }
                                        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;
                                        }
                                    }
                                }
                            }
                            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="ctrl"></param>
        public void Pause(bool ctrl)
        {
            working = !ctrl;
        }
        /// <summary>
        /// 停止工作
        /// </summary>
        public void Stop()
        {
            working = false;
            flag = sendCmd(endCmd);
            if (stream != null)
            {
                stream.Close();
            }
            if (tcpClient != null)
            {
                tcpClient.Close();
            }
        }

        /// <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="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;
        }

    }
}