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);
}
}
