/// <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);
}
/// <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 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);
}
}
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);
}
}
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 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>");
}
}
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);
}
}
//关闭串口
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);
}
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);
}
}
