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);
}
else if (cmd == GetCMDString(CMD.ME))
{
distances.Clear();
strengths.Clear();
SCIP_Reader.ME(receive_data, ref time_stamp, ref distances, ref strengths);
}
else
{
Debug.Log(">>" + receive_data);
}
}
// client.Close();
}
}
} catch (System.Exception ex) {
Debug.LogWarning("error: " + ex);
}
}
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 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);
}
}
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);
}
}
