/// <summary>
/// read MD command
/// </summary>
/// <param name="get_command">received command</param>
/// <param name="time_stamp">timestamp data</param>
/// <param name="distances">distance data</param>
/// <returns>is successful</returns>
public static bool MD(string get_command, ref long time_stamp, ref List <long> distances)
{
distances.Clear();
string[] split_command = get_command.Split(new char[] { '\n' }, StringSplitOptions.RemoveEmptyEntries);
if (!split_command[0].StartsWith("MD"))
{
return(false);
}
if (split_command[1].StartsWith("00"))
{
return(true);
}
else if (split_command[1].StartsWith("99"))
{
time_stamp = SCIP_Reader.decode(split_command[2], 4);
distance_data(split_command, 3, ref distances);
return(true);
}
else
{
return(false);
}
}
/// <summary>
/// read distance data
/// </summary>
/// <param name="lines"></param>
/// <param name="start_line"></param>
/// <returns></returns>
public static bool distance_data(string[] lines, int start_line, ref List <long> distances)
{
StringBuilder sb = new StringBuilder();
for (int i = start_line; i < lines.Length; ++i)
{
sb.Append(lines[i].Substring(0, lines[i].Length - 1));
}
return(SCIP_Reader.decode_array(sb.ToString(), 3, ref distances));
}
public static void TestMode()
{
const int GET_NUM = 10;
const int start_step = 0;
const int end_step = 1080;
try
{
string ip_address = LRF_IP_ADDR;
int port_number = LRF_PORT;
//get_connect_information(out ip_address, out port_number);
TcpClient urg = new TcpClient();
urg.Connect(ip_address, port_number);
NetworkStream 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
List <double> distances = new List <double>();
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))
{
Console.WriteLine(receive_data);
break;
}
if (distances.Count == 0)
{
Console.WriteLine(receive_data);
continue;
}
// show distance data
Console.WriteLine("time stamp: " + time_stamp.ToString() + " distance[100] : " + distances[100].ToString());
}
write(stream, SCIP_Writer.QT()); // stop measurement mode
read_line(stream); // ignore echo back
stream.Close();
urg.Close();
}
catch (Exception ex)
{
Console.WriteLine(ex.Message);
Console.WriteLine(ex.StackTrace);
}
finally
{
Console.WriteLine("Press any key.");
//Console.ReadKey();
}
}
/// <summary>
/// LANからデータ取得
/// </summary>
/// <returns></returns>
public double[] getScanDataIP()
{
const int start_step = 0;
const int end_step = 1080;
try
{
write(stream, SCIP_Writer.MD(start_step, end_step, 4, 1)); // 270=度のParticleFilterの仕様にあわせる
//read_line(stream); // ignore echo back
List <double> distances = new List <double>();
long time_stamp = 0;
bool retryFlg = true;
while (retryFlg)
{
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("not DistanceData :" + receive_data);
continue;
}
else
{
retryFlg = false;
}
// show distance data
//Console.WriteLine("time stamp: " + time_stamp.ToString() + " distance[100] : " + distances[100].ToString());
}
return(distances.ToArray());
}
catch
{
return(new double[0]);
}
}
/// <summary>
/// スケール値セット
/// </summary>
/// <param name="val"></param>
public static void setScale(double val)
{
LRF_Scale = val;
SCIP_Reader.SetScale(val);
URGLogRead.SetScale(val);
}
