IEnumerator UpdateSensorValue() { List <long> distances = new List <long>(); long time_stamp = 0; for (;;) { string receive_data = read_line(this._networkStream); if (!SCIP_Reader.MD(receive_data, ref time_stamp, ref distances)) { this.outInputField.text += receive_data + "\n"; Debug.Log(receive_data); break; } if (distances.Count == 0) { Debug.Log("distances.Count == 0"); this.outInputField.text += receive_data + "\n"; Debug.Log(receive_data); continue; } // データ表示部分 //TODO:かっこよくしたい。円表示 for (int j = 0; j < distances.Count; j++) { //this.outInputField.text += "\ntime stamp: " + time_stamp.ToString() + " distance[" + j + "] : " + distances[j].ToString() + "\n"; Debug.Log("time stamp: " + time_stamp.ToString() + " distance[" + j + "] : " + distances[j].ToString()); } // yield return(new WaitForSeconds(0.1f));//0.1秒後に処理 } }
private bool portDataReceived() { if (config.IsClosing || !IsOpen) { return(false); } config.IsReading = true; config.port.DiscardInBuffer(); string receiveData = config.port.ReadLine(); config.Receive = new List <long>(); if (!SCIP_Reader.MD(receiveData, ref config.TimeStamp, ref config.Receive)) { Console.WriteLine(receiveData); return(false); } if (config.Receive.Count == 0) { Console.WriteLine(receiveData); return(false); } config.Receive.RemoveRange(config.CutED, config.Receive.Count - config.CutED); config.Receive.RemoveRange(0, config.CutBG); config.IsReading = false; return(true); }
/// <summary> /// 激光数据获取部分,list a中储存获取的数据,在激光工作的时候一直调用这个模块。 /// </summary> /// <param name="q"></param> /// <param name="o"></param> static void Start(int q, int o) { List <long> distances = new List <long>(); long time_stamp = 0; int z = 0; for (int i = 1; i < d_times + 1; i++) { string receive_data = read_line(stream); if (!SCIP_Reader.MD(receive_data, ref time_stamp, ref distances))//数据驱动 { Console.WriteLine(receive_data); break; } if (distances.Count == 0) { Console.WriteLine(receive_data); continue; } z++; if (z == 16) { using (StreamWriter fsb = new StreamWriter(".\\laser_data\\" + q + "_R50mm全部数据" + ".txt", true)) { for (int j = 180; j < 901; j++) //180-901 { fsb.WriteLine(" distance[" + j.ToString() + "] : " + j.ToString() + " " + distances[j].ToString() + " " + o.ToString()); a.Add(j); a.Add(distances[i]); a.Add(o); } } } } //return a; }
void timer_Elapsed(object sender, System.Timers.ElapsedEventArgs e) { if (isClosing) { return; } isReading = true; // 接收数据 string receiveData = basePort.ReadLine(); // 清除缓存 basePort.DiscardInBuffer(); lock (distance) { if (!SCIP_Reader.MD(receiveData, ref timeStamp, ref distance)) { //Console.WriteLine(receiveData); return; } } isReading = false; if (processDel != null) { processDel(getDistance()); } }
public void run() { double per = 360.0 / 1024; //double diff = 60 - per * 44; distances = new List <long>(); distances_raw = new List <long>(); degrees = new List <double>(); long time_stamp = 0; string receive_data = urg.ReadLine(); if (!SCIP_Reader.MD(receive_data, ref time_stamp, ref distances_raw)) { Console.WriteLine(receive_data); return; } // show distance data for (int j = 0; j < distances_raw.Count; j++) { //https://sourceforge.net/p/urgnetwork/wiki/scip_capture_jp/ double degree = (j - 384) * per; distances.Add(distances_raw[j]); degrees.Add(degree); //Console.WriteLine("time stamp: " + time_stamp.ToString() + " distance[" + j.ToString() + "] : "+ distances[j].ToString()); //Console.WriteLine("degree[ " + j + "] :" + degree.ToString() + " distance[" + j.ToString() + "] : " + distances[j].ToString()); } return; }
private void Process(object sender, DoWorkEventArgs e) { while (!lidarBW.CancellationPending) { urg.Write(SCIP_Writer.SCIP2()); urg.ReadLine();//ignore echo urg.Write(SCIP_Writer.MD(start_step, end_step)); urg.ReadLine(); //ignore echo string sData = urg.ReadLine(); //Raw Scanner data long timeStamp = 0; for (int i = 0; i < 10; ++i) { string receive_data = urg.ReadLine(); if (!SCIP_Reader.MD(receive_data, ref timeStamp, ref distances)) { break; } if (distances.Count == 0) { continue; } if (distances != null) { //try barrel detection with a still image UpdateDelegate(distances); } } } }
private void WorkerHandler() { urg.Write(SCIP_Writer.MD(START, END)); urg.ReadLine(); long time_stamp = 0; while (startFlag) { string receive_data = urg.ReadLine(); if (!SCIP_Reader.MD(receive_data, ref time_stamp, ref distances)) { Console.WriteLine(receive_data); break; } if (distances.Count == 0) { Console.WriteLine(receive_data); continue; } //TODO: process the data here // show distance data //for (int i = 0; i < 10; i++) //{ // //Console.WriteLine("time stamp: " + time_stamp.ToString() + " distance[384] : " + distances[384].ToString()); // Console.Write("{0} ", distances[i].ToString()); //} //Console.WriteLine(""); //Console.WriteLine("{0} ", distances[44].ToString()); Thread.Sleep(10); } }
void timer_Elapsed(object sender, System.Timers.ElapsedEventArgs e) { // 尝试关闭或者已经关闭则返回 if (isClosing || !urgPort.isOpen) { return; } try { isReading = true; // 接收数据 string receiveData = basePort.ReadLine();// basePort.ReadExisting(); // 清除缓存 basePort.DiscardInBuffer(); lock (distance) { SCIP_Reader.MD(receiveData, ref timeStamp, ref distance); } // 执行处理数据程序 if (processDel != null) { processDel(getDis()); } isReading = false; } catch { } if (isClosing) { timer.Enabled = false; } }
private void Receive_data() { while (tcpconnect) { try { write(stream, SCIP_Writer.SCIP2()); read_line(stream); // ignore echo back write(stream, SCIP_Writer.MD(start_step, end_step, 1, 0, 0)); read_line(stream); // ignore echo back List <long> distances = new List <long>(); long time_stamp = 0; for (int i = 0; i < GET_NUM; ++i) { string receive_data = read_line(stream); if (!SCIP_Reader.MD(receive_data, ref time_stamp, ref distances)) { //Debug.Log("<color=blue>Receive Data: </color>" + receive_data); break; } if (distances.Count == 0) { //Debug.Log("<color=red>Receive Data: </color>" + receive_data); continue; } // show distance data for (int k = 0; k < distances.Count; k++) { if (showDebugLog) { Debug.Log("k: " + k + " distance: " + distances[k] / 10 + "cm"); } value = (int)distances[k] / 10; if ((int)distances[k] < range) { Debug.Log("<color=teal>Get distance: </color>" + distances[k] / 10 + "cm"); tcpconnect = false; write(stream, SCIP_Writer.QT()); // stop measurement mode read_line(stream); // ignore echo back stream.Close(); urg.Close(); Debug.Log("<color=green>Sensor close.</color>"); break; } } } } catch (System.Exception e) { Debug.Log(e); } //Debug.Log("<color=yellow>sleep</color>"); Thread.Sleep(10); } }
private void Receive_data() { while (tcpconnect) { try { write(stream, SCIP_Writer.SCIP2()); read_line(stream); // ignore echo back write(stream, SCIP_Writer.MD(start_step, end_step, 1, 0, 0)); read_line(stream); // ignore echo back List <long> distances = new List <long>(); long time_stamp = 0; for (int i = 0; i < GET_NUM; ++i) { string receive_data = read_line(stream); if (!SCIP_Reader.MD(receive_data, ref time_stamp, ref distances)) { //Debug.Log(string.Format("<color=blue>Receive Data: {0}</color>", receive_data)); break; } if (distances.Count == 0) { //Debug.Log(string.Format("<color=red>Receive Data: {0}</color>" ,receive_data)); continue; } // show distance data for (int k = 0; k < distances.Count; k++) { if (showDebugLog) { Debug.Log("k: " + k + " distance: " + distances[k] / 10 + "cm"); } value = (int)distances[k] / 10; //StopTcpConnect(distances, i); } } } catch (System.Exception e) { Debug.Log(e); } //Debug.Log("<color=yellow>sleep</color>"); Thread.Sleep(10); } }
//接收数据 private bool getUrgData() { string receiveData = port.ReadLine(); if (!SCIP_Reader.MD(receiveData, ref timeStamp, ref distance)) { Console.WriteLine(receiveData); return(false); } if (distance.Count == 0) { Console.WriteLine(receiveData); return(false); } return(true); }
private void HandleClientComm(object obj) { try { using (TcpClient client = (TcpClient)obj) { using (NetworkStream stream = client.GetStream()) { // NetworkStream clientStream = client.GetStream(); while (true) { long time_stamp = 0; string receive_data = read_line(stream); // messageQueue.Enqueue( receive_data ); string cmd = GetCommand(receive_data); if (cmd == GetCMDString(CMD.MD)) { distances.Clear(); SCIP_Reader.MD(receive_data, ref time_stamp, ref distances); if (onReadMD != null) { onReadMD.Invoke(distances); } } else if (cmd == GetCMDString(CMD.ME)) { distances.Clear(); strengths.Clear(); SCIP_Reader.ME(receive_data, ref time_stamp, ref distances, ref strengths); if (onReadME != null) { onReadME.Invoke(distances, strengths); } } else { Debug.Log(">>" + receive_data); } } // client.Close(); } } } catch (System.Exception ex) { Debug.LogWarning("error: " + ex); } }
private bool portDataReceived() { urgport.DiscardInBuffer(); string receiveData = urgport.ReadLine(); if (!SCIP_Reader.MD(receiveData, ref urgData.TimeStamp, ref urgData.distance)) { Console.WriteLine(receiveData); return(false); } if (urgData.distance.Count == 0) { Console.WriteLine(receiveData); return(false); } urgData.distance.RemoveRange(0, 44); urgData.distance.RemoveRange(673, urgData.distance.Count - 673); return(true); }
private void getDataWork() { var time_stamp = 0L; while (_isRun) { var receive_data = read_line(_stream); SCIP_Reader.MD(receive_data, ref time_stamp, ref _read_data); if (_read_data.Count == 0) { continue; } lock (_lockobj) { _read_data.CopyTo(_distance); GetObjs(); } } }
private static bool portDataReceived() { urgport.DiscardInBuffer(); string receiveData = urgport.ReadLine(); receData = new List <long>(); if (!SCIP_Reader.MD(receiveData, ref TH_data.TimeStamp, ref receData)) { Console.WriteLine(receiveData); return(false); } if (receData.Count == 0) { Console.WriteLine(receiveData); return(false); } receData.RemoveRange(portConfig.CutED, receData.Count - portConfig.CutED); receData.RemoveRange(0, portConfig.CutBG); return(true); }
private void WorkerHandler() { urg.Write(SCIP_Writer.SCIP2()); //set protocol //Thread.Sleep(10); string line1 = urg.ReadLine(); Thread.Sleep(20); urg.Write(SCIP_Writer.MD(START, END)); //set sending message //Thread.Sleep(10); string line2 = urg.ReadLine(); Thread.Sleep(20); long time_stamp = 0; //datapoint = new DataPoint[sample_number]; angle_increase = (float)sample_number / angle_range; for (int i = 0; i <= 60; i++) //from 1 -> 360 degrees. 0 is invalid data { DataArray[i] = -1; //store the lidar data into this array, and i correspond to the angle } for (int i = 300; i <= 360; i++) //from 1 -> 360 degrees. 0 is invalid data { DataArray[i] = -1; //store the lidar data into this array, and i correspond to the angle } while (startFlag) { string receive_data = urg.ReadLine(); if (!SCIP_Reader.MD(receive_data, ref time_stamp, ref distances)) //data stored in distances { Console.WriteLine(receive_data); continue; } if (distances.Count == 0) { Console.WriteLine(receive_data); continue; } //TODO: process the data here //for (int i = 0; i < datapoint.Length; i++) //{ // datapoint[i].Degree = i * angle_increase ; // datapoint[i].Distance = (int)distances[i] ; //} // urg_angle --> distances[x] x = 341/120 * urg_angle + 385 \-120 |0 /120 // urg_angle --> lidar_angle urg_angle = 180 - lidar_angle /60 |0 \300 int temp_x = 0; for (int lidar_angle = 61; lidar_angle < 299; lidar_angle++) //61 and 299 for convenient { temp_x = 681 * (180 - lidar_angle) / 240 + 341; //convert the raw data distances[i] to DataArray[x] if (distances[temp_x] * distances[temp_x + 1] > 0) { if (distances[temp_x - 1] > 0) { DataArray[lidar_angle] = ((int)distances[temp_x - 1] + (int)distances[temp_x] + (int)distances[temp_x + 1]) / 3; } else { DataArray[lidar_angle] = ((int)distances[temp_x] + (int)distances[temp_x + 1]) / 2; } } else { DataArray[lidar_angle] = 0; } //DataArray[lidar_angle] = ((int)distances[temp_x - 1] + (int)distances[temp_x] + (int)distances[temp_x + 1]) / 3; if (DataArray[lidar_angle] > 0 && DataArray[lidar_angle] < 20) { DataArray[lidar_angle] = 0; //see small value as noise } } Thread.Sleep(10); } }
private void bt_Click_Start(object sender, RoutedEventArgs e) { int start_step = 540 - (int)(slider1.Value / 0.25); int end_step = 540 + (int)(slider2.Value / 0.25); int GET_NUM = int.Parse(ScanNum.Text.Trim()); SimplePlotModel = new PlotModel(); //线条 var lineSerial = new LineSeries() { Title = "距离" }; //data.Clear(); data = new System.Collections.Generic.List <Data>(); try { string ip = IPAddr.Text.Trim(); int port = int.Parse(Port.Text.Trim()); urg = new TcpClient(); urg.Connect(ip, port); stream = urg.GetStream(); write(stream, SCIP_Writer.SCIP2()); // 切换到SCIP2.0状态 read_line(stream); // ignore echo back 读一行,并抛弃返回值 write(stream, SCIP_Writer.MD(start_step, end_step)); //写入初始配置参数 read_line(stream); // ignore echo back 读一行,并抛弃返回值 List <long> distances = new List <long>(); long time_stamp = 0;// 时间戳-毫秒 for (int i = 0; i < GET_NUM; ++i) { //每次循环清空图数据 SimplePlotModel.Series.Remove(lineSerial); lineSerial.Points.Clear(); string receive_data = read_line(stream); if (!SCIP_Reader.MD(receive_data, ref time_stamp, ref distances)) { MessageBox.Show("错误数据:" + receive_data); break;// 此时收到的数据开头非MD且二位非00或99,则数据出错,输出数据到屏幕,并结束循环 } if (distances.Count == 0) { continue;// 同上,此时没有接收到距离数据,输出到屏幕,继续下次循环 } //垂直距离 double[] dist = new double[distances.Count]; //水平距离 double[] Xdist = new double[distances.Count]; double L_mid = -1; // 一次测量中的数据 for (int j = 0; j < distances.Count; j++) { //获取所有店的垂直距离 double d = Data.getDist(start_step, j, distances[j]); data.Add(new Data(j, time_stamp, d)); lineSerial.Points.Add(new DataPoint(j, d)); //单独将垂直距离存到一个数组中 dist[j] = d; Xdist[j] = Data.getXDist(start_step, j, distances[j]); //获取中心点直线距离 if (j == (540 - start_step)) { L_mid = d; } } if (L_mid != -1) { PointF p = Data.getXY(L_mid, start_step, dist, Xdist); PointX.Text = p.X.ToString("F2"); PointY.Text = p.Y.ToString("F2"); } listViewData.Items.Refresh(); SimplePlotModel.Series.Add(lineSerial); } } catch (Exception ex) { MessageBox.Show(ex.Message + "\n" + ex.StackTrace); } }
private void parseHokuyuData() { // const int GET_NUM = 10; const int start_step = 0; const int end_step = scan_width; try { SerialPort urg = new SerialPort(hPort, 115200); urg.NewLine = "\n\n"; urg.Open(); urg.Write(SCIP_Writer.SCIP2()); urg.ReadLine(); // ignore echo back urg.Write(SCIP_Writer.MD(start_step, end_step)); urg.ReadLine(); // ignore echo back List <long> distances = new List <long>(); long time_stamp = 0; //for (int i = 0; i < GET_NUM; ++i) while (_continue) { string receive_data = urg.ReadLine(); if (!SCIP_Reader.MD(receive_data, ref time_stamp, ref distances)) { //Console.WriteLine(receive_data); break; } if (distances.Count == 0) { //Console.WriteLine(receive_data); continue; } // show distance data // Console.WriteLine(time_stamp.ToString() +"," + gimbal + "," + up_lidar+","+parse_distance(distances)); //caliberate distances //calib_h_distance distances.ForEach(delegate(long element) { element = element + (long)calib_h_distance; }); using (StreamWriter sw = File.AppendText(path)) { sw.AutoFlush = true; long length = sw.BaseStream.Length; //will give expected output long KB = length / 1024; long MB = KB / 1024; filesize = String.Format("{0} MB", MB); // Console.WriteLine("{0} Distance values", distances.Count()); TimeSpan t = (DateTime.UtcNow - new DateTime(1970, 1, 1)); if (_write_file) { sw.WriteLine(t.TotalSeconds.ToString() + "," + gimbal + "," + yaw + "," + up_lidar + "," + down_lidar + "," + parse_distance(distances)); } last_distances = distances; } } urg.Write(SCIP_Writer.QT()); // stop measurement mode urg.ReadLine(); // ignore echo back urg.Close(); } catch (Exception ex) { Console.WriteLine("Hokuyu Error"); Console.WriteLine(ex.Message); } }