예제 #1
0
        private void Send_Button_Click(object sender, RoutedEventArgs e)
        {
            //if (MessageTextBox.Text.Trim() != string.Empty)
            //{
            //    NetworkStream sendStream = client.GetStream();
            //    byte[] buffer = Encoding.Default.GetBytes(MessageTextBox.Text.Trim());
            //    sendStream.Write(buffer, 0, buffer.Length);
            //    MessageTextBox.Text = string.Empty;
            //}

            if (client != null)
            {
                //要发送的信息
                if (MessageTextBox.Text.Trim() == string.Empty)
                {
                    return;
                }
                string msg = MessageTextBox.Text.Trim();

                byte[] buffer = Encoding.Default.GetBytes(msg);

                NetworkStream sendStream = client.GetStream();
                sendStream.Write(buffer, 0, buffer.Length);

                ComWinTextBox.AppendText("发送给客户端的数据:" + msg + "\n");
                MessageTextBox.Text = string.Empty;
            }
        }
예제 #2
0
        private void Switch_Button_Click(object sender, RoutedEventArgs e) //请求连接
        {
            Button bt = sender as Button;

            if (bt.Content.ToString() == "请求连接")
            {
                if (IPAdressTextBox.Text.Trim() == string.Empty)
                {
                    ComWinTextBox.Dispatcher.Invoke(new showData(ComWinTextBox.AppendText), "请填入服务器IP地址\n");
                    return;
                }
                if (PortTextBox.Text.Trim() == string.Empty)
                {
                    ComWinTextBox.Dispatcher.Invoke(new showData(ComWinTextBox.AppendText), "请填入服务器端口号\n");
                    return;
                }

                try
                {
                    IPAddress ip = IPAddress.Parse(IPAdressTextBox.Text);
                    ComWinTextBox.AppendText("开始连接服务端....\n");
                    client = new TcpClient();
                    client.Connect(ip, int.Parse(PortTextBox.Text));
                    ComWinTextBox.AppendText("已经连接服务端\n");
                    statusInfoTextBlock.Text = "已与服务器建立连接";

                    sendStream = client.GetStream();

                    if ((thread.ThreadState == ThreadState.Aborted))
                    {
                        thread = new Thread(ListenerServer);
                        thread.IsBackground = true;
                    }
                    if (thread.ThreadState == (ThreadState.Unstarted | ThreadState.Background))
                    {
                        thread.Start();
                    }

                    bt.Content = "断开连接";
                }
                catch
                {
                    ComWinTextBox.AppendText("连接服务端失败\n");
                    statusInfoTextBlock.Text = "与服务器建立连接失败";
                }
            }
            else
            {
                thread.Abort();
                //sendStream.Close();
                client.Close();
                client     = null;
                bt.Content = "请求连接";
                ComWinTextBox.AppendText("已与服务器断开连接\n");
                statusInfoTextBlock.Text = "已与服务器断开连接";
            }
        }
예제 #3
0
        private void Send_Button_Click(object sender, RoutedEventArgs e)
        {
            if (client != null)
            {
                //要发送的信息
                if (MessageTextBox.Text.Trim() == string.Empty)
                {
                    return;
                }
                string msg = MessageTextBox.Text.Trim();

                byte[] buffer = Encoding.Default.GetBytes(msg);

                sendStream.Write(buffer, 0, buffer.Length);

                ComWinTextBox.AppendText("发送给服务端的数据:" + msg + "\n");
                MessageTextBox.Text = string.Empty;
            }
            else
            {
                ComWinTextBox.AppendText("未连接服务器,无法发送数据\n");
                statusInfoTextBlock.Text = "未连接服务器,无法发送数据\n";
            }
        }
예제 #4
0
 private void ComWinTextBox_TextChanged(object sender, TextChangedEventArgs e)
 {
     ComWinTextBox.ScrollToEnd(); //当通信窗口内容有变化时保持滚动条在最下面
 }
예제 #5
0
        public void timer1_cycle(object sender, EventArgs e)
        {
            try
            {
                double[] ptx   = new double[W_H];  //640*480
                double[] pty   = new double[W_H];  //640*480
                double[] ptz   = new double[W_H];  //640*480
                ushort[] depth = new ushort[W_H];  //640*480


                //获取相机内参
                var stream          = (VideoStreamProfile)pProfile.GetStream(Stream.Color);
                var colorIntrinsics = stream.GetIntrinsics();

                //获取深度图像
                var frames = pipeline.WaitForFrames();
                var dframe = frames.DepthFrame;
                dframe.CopyTo(depth);

                //Write_Flag = true;

                //是否开始识别
                if (Write_Flag == true)
                {
                    Write_Flag = false;

                    //获取有效空间范围内的深度并转换为点云
                    int pc_cnt = 0;
                    for (int i = 0; i < W_H; i++)
                    {
                        if (depth[i] > 400 && depth[i] < 6000)
                        {
                            ptx[pc_cnt] = (((i % WIDTH) - colorIntrinsics.ppx) / colorIntrinsics.fx) * depth[i];
                            pty[pc_cnt] = (((i / WIDTH) - colorIntrinsics.ppy) / colorIntrinsics.fy) * depth[i];
                            ptz[pc_cnt] = depth[i];
                            pc_cnt++;
                        }
                    }
                    double[] pcx = new double[pc_cnt];    //640*480
                    double[] pcy = new double[pc_cnt];    //640*480
                    double[] pcz = new double[pc_cnt];    //640*480
                    Array.Copy(ptx, pcx, pc_cnt);
                    Array.Copy(pty, pcy, pc_cnt);
                    Array.Copy(ptz, pcz, pc_cnt);

                    //********************************************************
                    //RANSAC地面提取
                    int    ITER       = 200;
                    double bpa        = 0;
                    double bpb        = 0;
                    double bpc        = 0;
                    int    best_count = 0;
                    Random rd         = new Random(Guid.NewGuid().GetHashCode());
                    var    rd1        = rd.Next(1, pcx.Length);
                    var    rd2        = rd.Next(1, pcx.Length);
                    var    rd3        = rd.Next(1, pcx.Length);
                    for (int i = 0; i < ITER; i++)
                    {
                        rd1 = rd.Next(1, pcx.Length);
                        rd2 = rd.Next(1, pcx.Length);
                        rd3 = rd.Next(1, pcx.Length);
                        //拟合直线方程z=ax+by+c
                        double[] plane = new double[3];
                        double   a     = ((pcz[rd1] - pcz[rd2]) * (pcy[rd2] - pcy[rd3]) - (pcz[rd2] - pcz[rd3]) * (pcy[rd1] - pcy[rd2])) / ((pcx[rd1] - pcx[rd2]) * (pcy[rd2] - pcy[rd3]) - (pcx[rd2] - pcx[rd3]) * (pcy[rd1] - pcy[rd2]));
                        double   b     = ((pcz[rd1] - pcz[rd2]) - a * (pcx[rd1] - pcx[rd2])) / (pcy[rd1] - pcy[rd2]);
                        double   c     = pcz[rd1] - a * pcx[rd1] - b * pcy[rd1];

                        double[] dist = new double[pcx.Length];
                        int      m    = 0;
                        for (int k = 0; k < pcx.Length; k++)
                        {
                            dist[k] = Math.Abs(a * pcx[k] + b * pcy[k] + c - pcz[k]);
                            if (dist[k] < 1.0)
                            {
                                m++;
                            }
                        }
                        if (m > best_count)
                        {
                            best_count = m;
                            bpa        = a;
                            bpb        = b;
                            bpc        = c;
                        }
                    }

                    //将点云分为地面点和其他点
                    double[] o_pcx    = new double[pcx.Length];
                    double[] o_pcy    = new double[pcx.Length];
                    double[] o_pcz    = new double[pcx.Length];
                    int      o_pc_cnt = 0;
                    for (int i = 0; i < pcx.Length; i++)
                    {
                        double dist = bpa * pcx[i] + bpb * pcy[i] + bpc - pcz[i];
                        if (dist < -20 || dist > 20)
                        {
                            o_pcx[o_pc_cnt] = pcx[i];
                            o_pcy[o_pc_cnt] = pcy[i];
                            o_pcz[o_pc_cnt] = pcz[i];
                            o_pc_cnt++;
                        }
                    }

                    double[] on_pcx = new double[o_pc_cnt];
                    double[] on_pcy = new double[o_pc_cnt];
                    double[] on_pcz = new double[o_pc_cnt];
                    Array.Copy(o_pcx, on_pcx, o_pc_cnt);
                    Array.Copy(o_pcy, on_pcy, o_pc_cnt);
                    Array.Copy(o_pcz, on_pcz, o_pc_cnt);

                    //获取新的深度图,不包含地面
                    int[,] new_depth      = new int[WIDTH, HEIGHT];
                    double[,] depth_label = new double[WIDTH, HEIGHT];
                    for (int i = 0; i < on_pcx.Length; i++)
                    {
                        var x = Math.Round((on_pcx[i] / on_pcz[i]) * colorIntrinsics.fx + colorIntrinsics.ppx);
                        var y = Math.Round((on_pcy[i] / on_pcz[i]) * colorIntrinsics.fy + colorIntrinsics.ppy);
                        if (x < WIDTH && y < HEIGHT)
                        {
                            new_depth[(int)x, (int)y]   = 1;
                            depth_label[(int)x, (int)y] = on_pcz[i];
                        }
                    }

                    //连通域标记
                    var cc = CalConnections(new_depth);
                    //获取距离人中心最近的地面物体
                    double[] dist1    = new double[cc.Count];
                    int[]    dist_key = new int[cc.Count];
                    int      dist_cnt = 0;
                    foreach (int key in cc.Keys)
                    {
                        var cc_values = cc[key];
                        foreach (var val in cc_values)
                        {
                            var d = (val.X - 480) * (val.X - 480) + (val.Y - 320) * (val.Y - 320);
                            //var d = val.Y * val.Y + (val.X - 320) * (val.X - 320);
                            if (dist1[dist_cnt] == 0)
                            {
                                dist1[dist_cnt] = d;
                            }
                            if (d < dist1[dist_cnt])
                            {
                                dist1[dist_cnt] = d;
                            }
                        }
                        dist_key[dist_cnt] = key;
                        dist_cnt++;
                    }

                    //获取最近区域
                    int ii    = 0;
                    int index = 0;
                    while (true)
                    {
                        if (dist1[ii] == dist1.Min())
                        {
                            index = ii;
                            break;
                        }
                        ii++;
                    }

                    //将障碍物转换为点云
                    var obs_depth = cc[dist_key[index]];
                    //obsize_cnt.Content = obs_depth.Count.ToString();
                    double[] obs_pcx = new double[obs_depth.Count];
                    double[] obs_pcy = new double[obs_depth.Count];
                    double[] obs_pcz = new double[obs_depth.Count];
                    byte[,] todisp = new byte[WIDTH, HEIGHT];

                    for (int i = 0; i < obs_depth.Count; i++)
                    {
                        todisp[obs_depth[i].Y, obs_depth[i].X] = 255;

                        obs_pcx[i] = ((obs_depth[i].Y - colorIntrinsics.ppx) / colorIntrinsics.fx) * depth_label[obs_depth[i].Y, obs_depth[i].X];
                        obs_pcy[i] = ((obs_depth[i].X - colorIntrinsics.ppy) / colorIntrinsics.fy) * depth_label[obs_depth[i].Y, obs_depth[i].X];
                        obs_pcz[i] = depth_label[obs_depth[i].Y, obs_depth[i].X];
                    }

                    //计算障碍物尺寸、距离等信息
                    double[] size_zs = new double[obs_pcz.Length];
                    for (int i = 0; i < size_zs.Length; i++)
                    {
                        size_zs[i] = Math.Abs((bpa * obs_pcx[i] + bpb * obs_pcy[i] - obs_pcz[i] + bpc) / (Math.Sqrt(bpa * bpa + bpb * bpb + 1)));
                    }
                    double size_z = size_zs.Max();

                    var theta = Math.Acos(1 / Math.Sqrt(bpa * bpa + bpb * bpb + 1));//夹角要加绝对值

                    double[] obs_pcx_pj = new double[obs_pcx.Length];
                    double[] obs_pcy_pj = new double[obs_pcx.Length];
                    double[] obs_pcz_pj = new double[obs_pcx.Length];
                    double[] dist_ys    = new double[obs_pcx.Length];
                    for (int i = 0; i < obs_pcx_pj.Length; i++)
                    {
                        var T = (bpa * obs_pcx[i] + bpb * obs_pcy[i] - obs_pcz[i] + bpc) / (bpa * bpa + bpb * bpb + 1);
                        obs_pcx_pj[i] = obs_pcx[i] - bpa * T;
                        obs_pcy_pj[i] = obs_pcy[i] - bpb * T;
                        obs_pcz_pj[i] = obs_pcz[i] + T;
                        dist_ys[i]    = Math.Abs(obs_pcz_pj[i] * Math.Sin(theta) - obs_pcy_pj[i] * Math.Cos(theta));
                    }

                    var size_x = Math.Abs(obs_pcx_pj.Max() - obs_pcx_pj.Min());
                    var size_y = Math.Abs((obs_pcy_pj.Max() - obs_pcy_pj.Min()) * (-1 / Math.Sqrt(bpa * bpa + bpb * bpb + 1)));
                    var dist_y = dist_ys.Min();
                    //********************************************************

                    obsize_x.Content = size_x.ToString("f2");
                    obsize_y.Content = size_y.ToString("f2");
                    obsize_z.Content = size_z.ToString("f2");
                    obdist_y.Content = dist_y.ToString("f2");

                    byte[] ethbuf = new byte[11];
                    //新增加:将障碍物高度、长度和距离转换为下一步的步长和步高
                    int Dmin    = 80;       //预留脚离障碍物的最小距离
                    int Lmax    = 800;      //机器人允许的最大步长
                    int Hmax    = 500;      //机器人允许的最大步高
                    int DeltaL  = 30;       //测量的最大误差
                    int DeltaH  = 30;       //测量的最大误差
                    int Lnormal = 400;      //正常行走的步长
                    int Hnormal = 100;      //正常行走的步高

                    int nStep          = 0; //接下来应该按正常步长走nStep步
                    int lastStepLength = 0; //走完nStep步后最后一小步的步长
                    int lastStepHeight = 0; //走完nStep步后最后一小步的步高
                    int overStepLength = 0; //跨越的步长
                    int overStepHeight = 0; //跨越的步高

                    nStep = ((int)dist_y - Dmin) / Lnormal;
                    if (((int)dist_y - Dmin) % Lnormal <= 80)
                    {
                        // 若最后一步小于80则使最后一步步长等于80
                        lastStepLength = 80;
                    }
                    else
                    {
                        lastStepLength = ((int)dist_y - Dmin) % Lnormal;
                    }
                    lastStepHeight = Hnormal;

                    if ((int)size_z + DeltaH < 200)
                    {
                        // 若跨越步高小于200则设为200
                        overStepHeight = 200;
                    }
                    else
                    {
                        //overStepHeight = (int)size_z + DeltaH;
                        overStepHeight = 400;
                    }
                    overStepLength = 2 * Dmin + (int)size_y + DeltaL;

                    if (overStepLength > Lmax || overStepHeight > Hmax)  //如果步长或步高有一个超限了,将跨越步长和步高都置0,即不跨越
                    {
                        overStepLength = 0;
                        overStepHeight = 0;
                    }

                    ComWinTextBox.AppendText("Send to Exoskeleton:" + lastStepLength.ToString() + lastStepHeight.ToString() + overStepLength.ToString() + overStepHeight.ToString() + "\n");

                    //jiang
                    ethbuf[0]  = 0xAA;                                       //起始标志
                    ethbuf[1]  = (byte)nStep;
                    ethbuf[2]  = (byte)((lastStepLength >> 8) & 0x000000FF); //取最后一步步长的高8位
                    ethbuf[3]  = (byte)(lastStepLength & 0x000000FF);        //取最后一步步长的低8位
                    ethbuf[4]  = (byte)((lastStepHeight >> 8) & 0x000000FF); //取最后一步步高的高8位
                    ethbuf[5]  = (byte)(lastStepHeight & 0x000000FF);        //取最后一步步高的低8位
                    ethbuf[6]  = (byte)((overStepLength >> 8) & 0x000000FF); //取跨越步长的高8位
                    ethbuf[7]  = (byte)(overStepLength & 0x000000FF);        //取跨越步长的低8位
                    ethbuf[8]  = (byte)((overStepHeight >> 8) & 0x000000FF); //取跨越步高的高8位
                    ethbuf[9]  = (byte)(overStepHeight & 0x000000FF);        //取跨越步高的低8位
                    ethbuf[10] = 0xEE;                                       //结束标志

                    //将ethbuf通过网络发送出去
                    NetworkStream sendStream = client.GetStream(); //获得用于数据传输的流
                    sendStream.Write(ethbuf, 0, ethbuf.Length);    //最终写入流中
                    //string showmsg = Encoding.Default.GetString(ethbuf, 0, ethbuf.Length);
                    //ComWinTextBox.AppendText("发送给客户端数据:" + showmsg + "\n");

                    ////write_point_to_txt(pcx, pcy, pcz, "valid_pc");
                    ////write_point_to_txt_1(bpa, bpb, bpc, "best_plane");

                    obsize_x.Content = size_x.ToString("f2");
                    obsize_y.Content = size_y.ToString("f2");
                    obsize_z.Content = size_z.ToString("f2");
                    obdist_y.Content = dist_y.ToString("f2");

                    var bytes = new byte[HEIGHT * WIDTH];
                    for (int i = 0; i < HEIGHT; i++)
                    {
                        for (int j = 0; j < WIDTH; j++)
                        {
                            bytes[i * WIDTH + j] = todisp[j, i];
                        }
                    }
                    var bs     = BitmapSource.Create(WIDTH, HEIGHT, 300, 300, PixelFormats.Gray8, null, bytes, WIDTH);
                    var imgSrc = bs as ImageSource;
                    imgObs.Source = imgSrc;

                    //write_point_to_txt_2(bytes, "obstacle");
                }

                var colorized_depth = colorizer.Colorize(frames.DepthFrame);
                UploadImage(imgDepth, colorized_depth);
                UploadImage(imgColor, frames.ColorFrame);
            }
            catch (Exception ex)
            {
            }
        }
예제 #6
0
        private void showParaTimer_Tick(object sender, EventArgs e)//输出步态参数到相应文本框的委托
        {
            //nStepTextBox.Text = nStep.ToString();
            //normalStepLengthTextBox.Text = normalStepLength.ToString();
            //normalStepHeightTextBox.Text = normalStepHeight.ToString();
            //lastStepLengthTextBox.Text = lastStepLength.ToString();
            //lastStepHeightTextBox.Text = lastStepHeight.ToString();
            //overStepLengthTextBox.Text = overStepLength.ToString();
            //overStepHeightTextBox.Text = overStepHeight.ToString();

            // Demo用显示步长步高
            overStepLengthTextBox.Text = demoStepLength.ToString();
            overStepHeightTextBox.Text = demoStepHeight.ToString();

            gaitTypeTextBox.Text = gaittype;

            //stopWatchStartTextBlock.Text = stopwatch_start.ElapsedMilliseconds.ToString(); // 秒表文本显示【以毫秒为单位】
            //stopWatchTextBlock.Text = stopwatch.ElapsedMilliseconds.ToString(); // 秒表文本显示【以毫秒为单位】

            if (start_flag)
            {
                start_flag     = false;
                gaittype       = "Initial Pattern";
                demoStepLength = 0.2;
                demoStepHeight = 0.09;
                stopwatch_start.Start();
            }
            if (stopwatch_start.ElapsedMilliseconds > START_TIME)
            {
                stopwatch_start.Reset();
                stopwatch_start.Stop();

                IPAddress ip = IPAddress.Parse(IPAdressTextBox.Text);
                client = new TcpClient();
                ComWinTextBox.AppendText("Start connecting to the server...\n");
                client.Connect(ip, int.Parse(PortTextBox.Text));
                ComWinTextBox.AppendText("The server has been connected\n");
                statusInfoTextBlock.Text = "A connection has been established to the server";

                statusBar.Background = new SolidColorBrush(Color.FromArgb(255, 230, 20, 20));
                //statusInfoTextBlock.Text = "可以开始生成步态";

                sendStream = client.GetStream();
                Thread thread = new Thread(ListenerServer);
                thread.Start();

                // 秒表开始
                stopwatch.Start();
            }

            if (visual_count <= GAIT_COUNT)
            {
                if (stopwatch.ElapsedMilliseconds > RESET_TIME)
                {
                    stopwatch.Reset();
                    stopwatch.Start();

                    //if (count == 1)
                    //{
                    //    visual_count = 0;
                    //}
                    //else
                    //{
                    //    visual_count = count - 1;
                    //}
                    //stopWatchTextBlock.Text = stopwatch.ElapsedMilliseconds.ToString(); // 秒表文本显示【以毫秒为单位】

                    //// 向深度相机请求视觉反馈
                    //string msg = "The exoskeleton requests visual feedback from the server";

                    //byte[] buffer = Encoding.Default.GetBytes(msg);
                    ////lock (sendStream)
                    ////{
                    //sendStream.Write(buffer, 0, buffer.Length);
                    ////}
                    //ComWinTextBox.AppendText(msg + "\n");

                    visual_count = count;

                    count = count + 1;
                }
            }

            countTextBox.Text = visual_count.ToString();
        }