//打开串口
public override bool OpenPort(string portName, string portBundrate)
{
if (!UrgState)
{
UrgState = true;
int bundrate = int.Parse(portBundrate);
port = new SerialPort(portName, bundrate);
port.NewLine = "\n\n";
try
{
port.Open();
}
catch (Exception)
{
UrgState = false;
return(false);
}
port.Write(SCIP_Writer.SCIP2());
port.ReadLine();
port.Write(SCIP_Writer.MD(startStep, endStep));
port.ReadLine();
return(true);
}
return(false);
}
/// <summary>
/// 关闭激光雷达串口
/// </summary>
/// <returns>返回关闭激光雷达是否成功</returns>
public bool closePort()
{
// 串口已经关闭返回成功
if (!urgPort.isOpen || urgPort == null)
{
return(true);
}
try {
isClosing = true;
// 等待读取串口程序完成
while (isReading)
{
System.Windows.Forms.Application.DoEvents();
}
// 停止读取数据命令
// timer.Enabled = false;
// 发送停止命令并清除缓存
basePort.Write(SCIP_Writer.QT());
basePort.DiscardInBuffer();
//basePort.ReadLine();
// 尝试关闭串口
basePort.Close();
}
catch {
return(false);
}
return(true);
}
public bool closePort()
{
if (config.port == null || !config.port.IsOpen)
{
return(true);
}
config.IsClosing = true;
while (config.IsReading)
{
;
}
try
{
config.port.Write(SCIP_Writer.QT());
config.port.ReadLine();
config.port.Close();
config.IsClosing = false;
return(true);
}
catch
{
config.IsClosing = false;
return(false);
}
}
public void StartButton()
{
Debug.Log("開始");
const int start_step = 0;
const int end_step = 1080;
try {
string ip_address = this.ipAddressInputField.text;
int port_number = int.Parse(this.portInputField.text);
this.outInputField.text += ip_address + " : " + port_number + "\n";
Debug.Log(ip_address + " : " + port_number);
this._urgTCPClient.Connect(ip_address, port_number);
this._networkStream = this._urgTCPClient.GetStream();
write(this._networkStream, SCIP_Writer.SCIP2());
read_line(this._networkStream); // ignore echo back
write(this._networkStream, SCIP_Writer.MD(start_step, end_step));
read_line(this._networkStream); // ignore echo back
this._sensorCoroutine = StartCoroutine(UpdateSensorValue());
} catch (Exception ex) {
Debug.Log(ex.Message);
Debug.Log(ex.StackTrace);
} finally {
Debug.Log("Finish");
//Console.ReadKey();
}
}
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);
}
}
/// <summary>
/// 打开串口方法
/// </summary>
/// <returns>串口打开是否成功</returns>
public bool openPort()
{
// 串口打开返回成功
basePort.NewLine = "\n\n";
if (basePort.IsOpen)
{
isClosing = false;
timer.Enabled = true;
return(true);
}
// 尝试打开串口
try {
basePort.Open();
}
catch {
return(false);
}
// 写入读取数据命令
basePort.Write(SCIP_Writer.SCIP2());
basePort.ReadLine();
basePort.Write(SCIP_Writer.MD(startStep, endStep));
basePort.ReadLine();
basePort.DataReceived += basePort_DataReceived;
isClosing = false;
timer.Enabled = true;
return(true);
}
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);
}
}
}
}
/// <summary>
/// 关闭激光雷达串口
/// </summary>
/// <returns>返回关闭激光雷达是否成功</returns>
public bool closePort()
{
// 串口已经关闭返回成功
if (!basePort.IsOpen)
{
return(true);
}
isClosing = true;
Console.WriteLine("Here");
// 写入停止读取数据命令
basePort.DataReceived -= basePort_DataReceived;
timer.Enabled = false;
// 100ms后关闭激光雷达串口
while (isReading)
{
System.Windows.Forms.Application.DoEvents();
}
basePort.Write(SCIP_Writer.QT());
basePort.ReadLine();
// 尝试关闭串口
try {
basePort.Close();
}
catch {
return(false);
}
return(true);
}
public void OpenStream(string ip_address, int start_step, int end_step)
{
const int port_number = 10940;
_start_step = start_step;
_end_step = end_step;
_urg = new TcpClient();
_urg.SendBufferSize = 0;
_urg.ReceiveBufferSize = 0;
_urg.Connect(ip_address, port_number);
_stream = _urg.GetStream();
write(_stream, SCIP_Writer.PP());
readSpec(read_line(_stream));
write(_stream, SCIP_Writer.SCIP2());
read_line(_stream); // ignore echo back
write(_stream, SCIP_Writer.MD(_start_step, _end_step));
read_line(_stream); // ignore echo back
_lockobj = new object();
_read_data = new List <long>();
_distance = new long[_amax + 1];
_calib_distance = new long[_amax + 1]; // store calibration data
_filtered_distance = new long[_amax + 1];
_objs = new List <Vector4>();
_isRun = true;
_thread = new Thread(new ThreadStart(getDataWork));
_thread.Start();
}
/// <summary>
/// 打开串口方法
/// </summary>
/// <returns>串口打开是否成功</returns>
public bool openPort()
{
if (urgPort == null)
{
return(false);
}
try {
// 串口打开返回成功
basePort.NewLine = "\n\n";
if (urgPort.isOpen)
{
isClosing = false;
timer.Enabled = true;
return(true);
}
// 尝试打开串口
basePort.Open();
// 写入读取数据命令
basePort.Write(SCIP_Writer.SCIP2());
basePort.ReadLine();
basePort.Write(SCIP_Writer.MD(startStep, endStep));
basePort.ReadLine();
isClosing = false;
timer.Enabled = true;
}
catch {
return(false);
}
return(true);
}
public bool Close()
{
if (!urgport.IsOpen)
{
return(true);
}
portState.IsClosing = true;
// 等待读取完毕
// while (portState.IsReading) { System.Windows.Forms.Application.DoEvents(); }
// 关闭
try
{
urgport.Write(SCIP_Writer.QT());
urgport.ReadLine();
urgport.Close();
portState.IsClosing = false;
return(true);
}
catch
{
portState.IsClosing = false;
return(false);
}
}
static void unlink()
{
write(stream, SCIP_Writer.QT()); // stop measurement mode
read_line(stream); // ignore echo back
stream.Close();
urg.Close();
}
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);
}
}
public void StopSensor()
{
StopCoroutine(this._sensorCoroutine);
write(this._networkStream, SCIP_Writer.QT()); // stop measurement mode
read_line(this._networkStream); // ignore echo back
this._networkStream.Close();
this._urgTCPClient.Close();
}
public void CloseStream()
{
_isRun = false;
_thread.Join();
write(_stream, SCIP_Writer.QT()); // stop measurement mode
read_line(_stream); // ignore echo back
_stream.Close();
_urg.Close();
}
private void bt_Click_End(object sender, RoutedEventArgs e)
{
try
{
write(stream, SCIP_Writer.QT()); // stop measurement mode 关闭激光，禁用测量状态
read_line(stream); // ignore echo back
stream.Close();
urg.Close();
}
catch (Exception ex)
{
MessageBox.Show(ex.Message + "\n" + ex.StackTrace);
}
}
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 void StopTcpConnect(List <long> k, int i)
{
List <long> dis = new List <long>();
dis = k;
if ((int)dis[i] < range)
{
Debug.Log(string.Format("<color=teal>Get distance: {0}cm</color>", dis[i] / 10));
tcpconnect = false;
ipconfig = 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>");
}
}
private string SearchComPort() //check every valid port and return a valid port name
{
string[] comNames = SerialPort.GetPortNames();
if (comNames.Length == 0)
{
return(null);
}
for (int i = 0; i < comNames.Length; i++)
{
try
{
if (comNames[i].ToUpper().StartsWith("COM"))
{
using (var port = new SerialPort(comNames[i], BUADRATE))
{
port.NewLine = "\n\n";
port.WriteTimeout = 10;
port.Open();
port.Write(SCIP_Writer.SCIP2());
Thread.Sleep(50);
var bufferCount = port.BytesToRead;
if (bufferCount > 3)
{
string line = port.ReadExisting();
if (line.Contains("0Ee"))
{
return(comNames[i]);
}
}
port.Close();
}
}
}
catch (Exception ex)
{
Console.WriteLine(ex.Message);
}
}
return(null);
}
public bool StopService()
{
try
{
urg.Write(SCIP_Writer.QT());
urg.ReadLine();
this.startFlag = false;
Thread.Sleep(10);
this.sThread.Abort();
this.sThread = null;
return(true);
}
catch (Exception ex)
{
Console.WriteLine(ex);
return(false);
}
}
public bool Close()
{
if (!urgport.IsOpen)
{
return(true);
}
try
{
urgport.Write(SCIP_Writer.QT());
urgport.ReadLine();
urgport.Close();
return(true);
}
catch
{
return(false);
}
}
// static List<List<double>> data = new List<List<double>>();//动态二维数组
/// <summary>
/// 给下位机arduino发“2”代表激光板和arduino之间的通信自检，发“3”代表功能自检，需要运动电机
/// </summary>
static void link()
{
/*--------------bulid laser tcp/ip connect----------------*/
string ip_address;
int port_number;
//get_connect_information(out ip_address, out port_number);
ip_address = "192.168.0.10";
port_number = 10940;
urg = new TcpClient();
urg.Connect(ip_address, port_number);
Console.WriteLine("linking\n");
stream = urg.GetStream();
write(stream, SCIP_Writer.SCIP2());
read_line(stream); // ignore echo back
write(stream, SCIP_Writer.MD(start_step, end_step));
read_line(stream); // ignore echo back
Console.WriteLine("link to radar successfully!\n");
}
//关闭串口
public bool ClosePort()
{
if (UrgState)
{
UrgState = false;
port.Write(SCIP_Writer.QT());
port.ReadLine();
try
{
port.Close();
}
catch (Exception)
{
UrgState = true;
return(false);
}
return(true);
}
return(false);
}
//////////////////////////////////////// public method ////////////////////////////////////////////////
public bool Open(string portName, string baudRate)
{
if (IsOpen)
{
return(true);
}
try
{
Initial_PortState();
urgport = new SerialPort(portName, int.Parse(baudRate));
urgport.NewLine = "\n\n";
urgport.Open();
urgport.Write(SCIP_Writer.SCIP2());
urgport.ReadLine();
urgport.Write(SCIP_Writer.MD(urgData.start, urgData.end));
urgport.ReadLine();
return(true);
}
catch { return(false); }
}
////////////////////////////////////////// public attribute ////////////////////////////////////////////////
public bool Open(bool CreatePort = false)
{
// 建立串口
if (IsOpen)
{
return(true);
}
if (CreatePort)
{
urgport = new SerialPort(TH_data.PortName, TH_data.BaudRate);
}
try
{
// 初始化线程
Initial_TH_urg();
// 打开串口
urgport.NewLine = "\n\n";
urgport.Open();
urgport.Write(SCIP_Writer.SCIP2());
urgport.ReadLine();
urgport.Write(SCIP_Writer.MD(portConfig.ReceiveBG, portConfig.ReceiveED));
urgport.ReadLine();
// 打开线程
TH_data.TH_cmd_abort = true;
while (TH_urg != null && TH_urg.ThreadState == System.Threading.ThreadState.Running)
{
;
}
TH_data.TH_cmd_abort = false;
TH_urg.Start();
return(true);
}
catch { System.Windows.Forms.MessageBox.Show("URG Open Error"); return(false); }
}
public urgClass(string port_name, int baudrate)
{
const int start_step = 0;
const int end_step = 780;
urg = new SerialPort(port_name, baudrate);
urg.NewLine = "\n\n";
try
{
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
}
catch (Exception ex)
{
Console.WriteLine(ex.Message);
Console.WriteLine("接続に失敗しました");
urg.Close();
}
}
public bool openPort()
{
// 等待串口关闭
while (config.IsClosing)
{
;
}
// 串口已经打开，重新开启线程
if (config.port != null && config.port.IsOpen)
{
return(true);
}
// 获取串口配置
string portName = InfoManager.portIF.UrgPortName;//DataArea.InfoModel.Data[(int)FileInfo.paramE.UrgPortName];
int baudRate = InfoManager.portIF.UrgPortRate;;
//try { baudRate = int.Parse(DataArea.InfoModel.Data[(int)FileInfo.paramE.UrgPortRate]); }
//catch { return false; }
config.port = new SerialPort(portName, baudRate);
// 尝试打开串口
try
{
config.port.NewLine = "\n\n";
config.port.Open();
config.port.Write(SCIP_Writer.SCIP2());
config.port.ReadLine();
config.port.Write(SCIP_Writer.MD(config.ReceiveBG, config.ReceiveED));
config.port.ReadLine();
}
catch { return(false); }
// 重新开启线程
return(Start_TH_urg());
}
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);
}
}
