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; } }
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 = "已与服务器断开连接"; } }
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"; } }
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) { } }
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(); }