public static Point3D[] Parse3DPoints(SingleScanData single_scan_data)
{
List <Point3D> list_3d_point = new List <Point3D>();
double x_pos = single_scan_data.XPos;
double plane_angle = single_scan_data.PlaneAngle;
double[] scan_data = single_scan_data.ScanData;
double start_scan_angle = double.Parse(ConfigFileManager.ReadSensorStartAngle());
double angle_resolution = double.Parse(ConfigFileManager.ReadSensorAngleResolution());
double plane_angle_rad = plane_angle * Math.PI / 180;
// calculate the start and end angle
double start_spreader_range_angle = Math.Atan((single_scan_data.XPos + ConfigParameters.SENSOR_OFFSET_X
- (single_scan_data.XPos + ConfigParameters.SPREADER_OFFSET_X + ConfigParameters.CONTAINER_WIDTH
+ ConfigParameters.SPREADER_SWING_RANGE)) / single_scan_data.YPos) / Math.PI * 180 + 90;
double end_spreader_range_angle = Math.Atan((single_scan_data.XPos + ConfigParameters.SENSOR_OFFSET_X
- (single_scan_data.XPos + ConfigParameters.SPREADER_OFFSET_X - ConfigParameters.SPREADER_SWING_RANGE)) //rate
/ (single_scan_data.YPos - ConfigParameters.CONTAINER_HEIGHT)) / Math.PI * 180 + 90;
double spreader_angle_range = (end_spreader_range_angle - start_spreader_range_angle);
bool isRemoveSpreader = false;
if (Math.Abs((1 / Math.Tan(plane_angle_rad)) * (single_scan_data.YPos - ConfigParameters.CONTAINER_HEIGHT))
< ConfigParameters.MIDDLE_STACK_CONTAINER_LENGTH / 2 + ConfigParameters.SPREADER_SWING_RANGE)
{
isRemoveSpreader = true;
}
for (int i = 0; i < scan_data.Length; i++)
{
Point3D point = new Point3D();
double beam_angle_deg = start_scan_angle + i * angle_resolution;
// skip if in spreader range
if (beam_angle_deg >= start_spreader_range_angle && beam_angle_deg <= (start_spreader_range_angle + spreader_angle_range) && isRemoveSpreader)
{
continue;
}
double beam_angle_rad = beam_angle_deg * Math.PI / 180;
// change the angle
//plane_angle_rad -= Math.PI;
point.X = scan_data[i] * Math.Cos(beam_angle_rad) + x_pos;
point.Y = scan_data[i] * Math.Sin(beam_angle_rad) * Math.Sin(plane_angle_rad);
point.Z = scan_data[i] * Math.Sin(beam_angle_rad) * Math.Cos(plane_angle_rad);
list_3d_point.Add(point);
}
return(list_3d_point.ToArray());
}
public static double[] ParseStream(byte[] instream)
{
string dataString = "";
double startAngle;
double angleResolution;
int data_count;
#region Process output data and get all parameters
try
{
// convert output from byte to string
dataString = System.Text.Encoding.ASCII.GetString(instream);
}
catch (Exception ex)
{
Console.WriteLine("Error: " + ex.ToString());
return(null);
}
// the sign of mark the end of output string
char etx = (char)3;
// end data output position
int endPst = dataString.IndexOf(etx);
// remove the unusable data string
dataString = dataString.Remove(endPst);
string tmp_ouput_data = dataString;
// split data output and put in an array
string[] data_output_arr = tmp_ouput_data.Split(' ');
#endregion
#region Get the start angle
string start_angle_hex = data_output_arr[23];
int start_angle_raw;
try
{
// convert angle from hex to int
start_angle_raw = Int32.Parse(start_angle_hex, System.Globalization.NumberStyles.HexNumber);
}
catch (Exception ex)
{
Console.WriteLine("Error: " + ex.ToString());
return(null);
}
// Normalizing angle
startAngle = Math.Ceiling((double)(start_angle_raw / 10000.00));
#endregion
#region Get the angle resolution
string angle_resolution_hex = data_output_arr[24];
int angle_resolution_raw;
try
{
angle_resolution_raw = Int32.Parse(angle_resolution_hex, System.Globalization.NumberStyles.HexNumber);
}
catch (Exception ex)
{
Console.WriteLine("Ex: " + ex.ToString());
return(null);
}
angleResolution = (double)(angle_resolution_raw) / 10000.00;
#endregion
#region Get the number of items in measured output
string data_number_hex = data_output_arr[25];
try
{
data_count = Int32.Parse(data_number_hex, System.Globalization.NumberStyles.HexNumber);
}
catch (Exception ex)
{
Console.WriteLine("Error: " + ex.ToString());
return(null);
}
#endregion
#region Get measured output data
int measured_distance;
double[] measuredDataArr = new double[data_count];
// add translate measured data to X and Y and add it to arr
for (int i = 0; i < data_count; i++)
{
try
{
// convert measured number from hex to int
measured_distance = Int32.Parse(data_output_arr[i + 26], System.Globalization.NumberStyles.HexNumber);
}
catch (Exception ex)
{
Console.WriteLine("Error: " + ex.ToString());
return(null);
}
measuredDataArr[i] = measured_distance;
}
#endregion
// check whether all config parameters match with output
if (startAngle != Convert.ToDouble(ConfigFileManager.ReadSensorStartAngle()) || angleResolution != Convert.ToDouble(ConfigFileManager.ReadSensorAngleResolution()))
{
return(null);
}
return(measuredDataArr);
}
