public static void onData(KObject data)
{
GoDataSet dataSet = (GoDataSet)data;
for (UInt32 i = 0; i < dataSet.Count; i++)
{
GoDataMsg dataObj = (GoDataMsg)dataSet.Get(i);
switch (dataObj.MessageType)
{
case GoDataMessageType.Stamp:
{
GoStampMsg stampMsg = (GoStampMsg)dataObj;
for (UInt32 j = 0; j < stampMsg.Count; j++)
{
GoStamp stamp = stampMsg.Get(j);
Console.WriteLine("Frame Index = {0}", stamp.FrameIndex);
Console.WriteLine("Time Stamp = {0}", stamp.Timestamp);
Console.WriteLine("Encoder Value = {0}", stamp.Encoder);
}
}
break;
}
}
// Refer to example ReceiveRange, ReceiveProfile, ReceiveMeasurement and ReceiveWholePart on how to receive data
Console.WriteLine(Environment.NewLine);
}
public void AsyncReceiveData(KObject data)
{
GoDataSet dataset = (GoDataSet)data;
for (UInt32 i = 0; i < dataset.Count; i++)
{
GoDataMsg dataObj = (GoDataMsg)dataset.Get(i);
switch (dataObj.MessageType)
{
case GoDataMessageType.Stamp:
{
GoStampMsg stampMsg = (GoStampMsg)dataObj;
for (UInt32 j = 0; j < stampMsg.Count; j++)
{
GoStamp stamp = stampMsg.Get(j);
getStamp.FrameIndex = stamp.FrameIndex;
getStamp.Timestamp = stamp.Timestamp;
getStamp.Encoder = stamp.Encoder;
get_stamp_FrameIndex.Add(stamp.FrameIndex);
get_stamp_TimeStamp.Add(stamp.Timestamp);
get_stamp_EncoderValue.Add(stamp.Encoder);
}
dataReceivedFlag.Stamp = 1;
}
break;
case GoDataMessageType.Measurement:
{
GoMeasurementMsg measurementMsg = (GoMeasurementMsg)dataObj;
for (UInt32 k = 0; k < measurementMsg.Count; k++)
{
GoMeasurementData measurementData = measurementMsg.Get(k);
getMeasurement.Id = measurementMsg.Id;
getMeasurement.value = measurementData.Value;
getMeasurement.decison = measurementData.Decision;
get_measurementMsg_Id.Add(measurementMsg.Id);
get_measurementMsg_Value.Add(measurementData.Value);
get_measurementMsg_Decison.Add(measurementData.Decision);
}
dataReceivedFlag.Measurement = 1;
}
break;
case GoDataMessageType.Alignment:
{
GoAlignMsg dataItem = (GoAlignMsg)dataSet.Get(i);
if (dataItem.Status == KStatus.Ok)
{
calibrationSuccessfulBl = true;
}
else
{
calibrationSuccessfulBl = false;
}
}
break;
}
}
}
ProfilePoint[] TransProfileData(KObject rawData)
{
ProfilePoint[] profileBuffer = null;
GoDataSet _dataSource = new GoDataSet();
_dataSource = (GoDataSet)rawData;
_dataContext = new DataContext();
ProfileShape _profileShape = new ProfileShape();
for (UInt32 i = 0; i < _dataSource.Count; i++)
{
GoDataMsg dataObj = (GoDataMsg)_dataSource.Get(i);
switch (dataObj.MessageType)
{
case GoDataMessageType.ResampledProfile:
{
GoResampledProfileMsg profileMsg = (GoResampledProfileMsg)dataObj;
_dataContext.xResolution = (double)profileMsg.XResolution / 1000000;
_dataContext.zResolution = (double)profileMsg.ZResolution / 1000000;
_dataContext.xOffset = (double)profileMsg.XOffset / 1000;
_dataContext.zOffset = (double)profileMsg.ZOffset / 1000;
_profileShape.width = profileMsg.Width;
_profileShape.size = profileMsg.Size;
_syncContext.Post(delegate
{
xRes.Content = _dataContext.xResolution.ToString();
zRes.Content = _dataContext.zResolution.ToString();
}, null);
//generate csv file for point data save
short[] points = new short[_profileShape.width];
profileBuffer = new ProfilePoint[_profileShape.width];
IntPtr pointsPtr = profileMsg.Data;
Marshal.Copy(pointsPtr, points, 0, points.Length);
for (UInt32 arrayIndex = 0; arrayIndex < _profileShape.width; ++arrayIndex)
{
if (points[arrayIndex] != -32768)
{
profileBuffer[arrayIndex].x = _dataContext.xOffset + _dataContext.xResolution * arrayIndex;
profileBuffer[arrayIndex].z = _dataContext.zOffset + _dataContext.zResolution * points[arrayIndex];
}
else
{
profileBuffer[arrayIndex].x = _dataContext.xOffset + _dataContext.xResolution * arrayIndex;
profileBuffer[arrayIndex].z = activeArea.Top;
}
}
}
break;
default: break;
}
}
return(profileBuffer);
}
public void onData(KObject data)
{
GoDataSet dataSet = (GoDataSet)data;
//var dataWithID = new Dictionary<string, string>();
for (UInt32 i = 0; i < dataSet.Count; i++)
{
GoDataMsg dataObj = (GoDataMsg)dataSet.Get(i);
switch (dataObj.MessageType)
{
case GoDataMessageType.Stamp:
{
GoStampMsg stampMsg = (GoStampMsg)dataObj;
for (UInt32 j = 0; j < stampMsg.Count; j++)
{
GoStamp stamp = stampMsg.Get(j);
//Console.WriteLine("Frame Index = {0}", stamp.FrameIndex);
//Console.WriteLine("Time Stamp = {0}", stamp.Timestamp);
//Console.WriteLine("Encoder Value = {0}", stamp.Encoder);
}
}
break;
case GoDataMessageType.Measurement:
{
GoMeasurementMsg measurementMsg = (GoMeasurementMsg)dataObj;
for (UInt32 k = 0; k < measurementMsg.Count; ++k)
{
GoMeasurementData measurementData = measurementMsg.Get(k);
//Console.WriteLine("ID: {0}", measurementMsg.Id);
//Console.WriteLine("Value: {0}", measurementData.Value);
//Console.WriteLine("Decision: {0}", measurementData.Decision);
//dataWithID.Add(measurementMsg.Id.ToString(), measurementData.Value.ToString());
}
}
break;
}
}
Publish(new LMIData()); //TODO Data Shell
}
private void RawDataResolver(KObject data)
{
GoDataSet dataSet = (GoDataSet)data;
for (UInt32 i = 0; i < dataSet.Count; i++)
{
GoDataMsg dataObj = (GoDataMsg)dataSet.Get(i);
switch (dataObj.MessageType)
{
case GoDataMessageType.UniformSurface:
{
GoUniformSurfaceMsg surfaceMsg = (GoUniformSurfaceMsg)dataObj;
double xResolution = (double)surfaceMsg.XResolution / 1000000;
double zResolution = (double)surfaceMsg.ZResolution / 1000000;
double xOffset = (double)surfaceMsg.XOffset / 1000;
double zOffset = (double)surfaceMsg.ZOffset / 1000;
long width = surfaceMsg.Width;
long height = surfaceMsg.Length;
long bufferSize = width * height;
IntPtr bufferPointer = surfaceMsg.Data;
short[] ranges = new short[bufferSize];
Marshal.Copy(bufferPointer, ranges, 0, ranges.Length);
}
break;
case GoDataMessageType.SurfaceIntensity:
{
GoSurfaceIntensityMsg surfaceMsg = (GoSurfaceIntensityMsg)dataObj;
long width = surfaceMsg.Width;
long height = surfaceMsg.Length;
long bufferSize = width * height;
IntPtr bufferPointeri = surfaceMsg.Data;
byte[] ranges = new byte[bufferSize];
Marshal.Copy(bufferPointeri, ranges, 0, ranges.Length);
}
break;
}
}
}
private void OnDataReceived(KObject data)
{
//system.Stop();
_dataSource = (GoDataSet)data;
for (UInt32 i = 0; i < _dataSource.Count; i++)
{
GoDataMsg dataObj = (GoDataMsg)_dataSource.Get(i);
switch (dataObj.MessageType)
{
case GoDataMessageType.Stamp:
{
GoStampMsg stampMsg = (GoStampMsg)dataObj;
for (UInt32 j = 0; j < stampMsg.Count; j++)
{
GoStamp stamp = stampMsg.Get(j);
}
}
break;
case GoDataMessageType.Surface:
{
GoSurfaceMsg surface = (GoSurfaceMsg)dataObj;
long size = surface.Size;
}
break;
case GoDataMessageType.Profile:
{
}
break;
}
}
// Dispose required to prevent memory leak.
_dataSource.Dispose();
}
private void OnDataReceived(KObject data)
{
//处理速度
//On data Receive
_syncContext.Post(new SendOrPostCallback(delegate { updateMsg("DataCount:" + count.ToString()); }), null);
_syncContext.Post(new SendOrPostCallback(delegate
{
updateMsg("Data received" + timeStamp() + " DataCount:" + count.ToString());
if (count == 51)
{
updateMsg("Data Stream will end");
}
count++;
}), null);
//get profile data
_dataSource = (GoDataSet)data;
for (UInt32 i = 0; i < _dataSource.Count; i++)
{
GoDataMsg dataObj = (GoDataMsg)_dataSource.Get(i);
switch (dataObj.MessageType)
{
case GoDataMessageType.ResampledProfile:
{
GoResampledProfileMsg profileMsg = (GoResampledProfileMsg)dataObj;
_dataContext.xResolution = (double)profileMsg.XResolution / 1000000;
_dataContext.zResolution = (double)profileMsg.ZResolution / 1000000;
_dataContext.xOffset = (double)profileMsg.XOffset / 1000;
_dataContext.zOffset = (double)profileMsg.ZOffset / 1000;
_profileShape.width = profileMsg.Width;
_profileShape.size = profileMsg.Size;
//generate csv file for point data save
sb.Clear();
if (!File.Exists(filePath))
{
sb.Append("Index,");
for (int k = 0; k < _profileShape.width; k++)
{
sb.Append(k.ToString() + ",");
}
File.AppendAllText(filePath, sb.ToString());
//updateMsg("Create file sucessfully");
sb.Clear();
}
short[] points = new short[_profileShape.width];
ProfilePoint[] profileBuffer = new ProfilePoint[_profileShape.width];
IntPtr pointsPtr = profileMsg.Data;
Marshal.Copy(pointsPtr, points, 0, points.Length);
for (UInt32 arrayIndex = 0; arrayIndex < _profileShape.width; ++arrayIndex)
{
if (points[arrayIndex] != -32768)
{
profileBuffer[arrayIndex].x = Resolution2Value(_dataContext.xResolution, _dataContext.xOffset, arrayIndex);
profileBuffer[arrayIndex].z = Resolution2Value(_dataContext.zResolution, _dataContext.zOffset, points[arrayIndex]);
}
else
{
profileBuffer[arrayIndex].x = Resolution2Value(_dataContext.xResolution, _dataContext.xOffset, arrayIndex);
profileBuffer[arrayIndex].z = -32768;
}
//Get Value save to file
sb.Append(index.ToString() + ",");
index++;
sb.Append("x:" + profileBuffer[arrayIndex].x.ToString() + "#" + "z:" + profileBuffer[arrayIndex].z.ToString() + ",");
}
sb.Append(Environment.NewLine);
// write to file
File.AppendAllText(filePath, sb.ToString());
}
break;
default: break;
}
}
}
private void ResolveRawData(KObject kData)
{
GoDataSet dataSet = (GoDataSet)kData;
ushort[] zValue = new ushort[0];
TopBottomSurface topBottomSurface = new TopBottomSurface(SurfaceType.TopBottom);
for (UInt32 i = 0; i < dataSet.Count; i++)
{
GoDataMsg dataObj = (GoDataMsg)dataSet.Get(i);
switch (dataObj.MessageType)
{
#region SurfaceMsg
case GoDataMessageType.Surface:
GocatorContext mContext = new GocatorContext();
GoUniformSurfaceMsg surfaceMsg = (GoUniformSurfaceMsg)dataObj;
long width = surfaceMsg.Width;
long height = surfaceMsg.Length;
long bufferSize = width * height;
mContext.XResolution = (double)surfaceMsg.XResolution / 1000000;
mContext.ZResolution = (double)surfaceMsg.ZResolution / 1000000;
mContext.XOffset = (double)surfaceMsg.XOffset / 1000;
mContext.ZOffset = (double)surfaceMsg.ZOffset / 1000 - mContext.ZResolution * 32768;
mContext.YResolution = (double)surfaceMsg.YResolution / 1000000;
mContext.Width = (int)width;
mContext.Height = (int)height;
IntPtr bufferPointer = surfaceMsg.Data;
short[] ranges = new short[bufferSize];
zValue = new ushort[bufferSize];
Marshal.Copy(bufferPointer, ranges, 0, ranges.Length);
Parallel.For(0, bufferSize, (index) =>
{
zValue[index] = (ushort)(ranges[index] + 32768);
});
if (surfaceMsg.Source.Value == 0)
{
topBottomSurface.TopSurfaceData = zValue;
mContextTop = mContext;
}
else if (surfaceMsg.Source.Value == 1)
{
topBottomSurface.BottomSurfaceData = zValue;
mContextBottom = mContext;
}
break;
#endregion
case GoDataMessageType.SurfaceIntensity:
GoSurfaceIntensityMsg goSurfaceIntensityMsg = (GoSurfaceIntensityMsg)dataObj;
long widthIntensity = goSurfaceIntensityMsg.Width;
long heightIntensity = goSurfaceIntensityMsg.Length;
long bufferSizeIntensity = widthIntensity * heightIntensity;
IntPtr bufferPointeri = goSurfaceIntensityMsg.Data;
byte[] rangesIntensity = new byte[bufferSizeIntensity];
Marshal.Copy(bufferPointeri, rangesIntensity, 0, rangesIntensity.Length);
if (goSurfaceIntensityMsg.Source.Value == 0)
{
topBottomSurface.TopSurfaceIntensityData = rangesIntensity;
}
else if (goSurfaceIntensityMsg.Source.Value == 1)
{
topBottomSurface.BottomSurfaceIntensityData = rangesIntensity;
}
break;
}
}
mResult.Add(topBottomSurface);
DeviceStatusEvent?.Invoke(this, $"Finished {100 * (mResult.Count) / BufferSize * 1.0} %");
}
static int Main(string[] args)
{
try
{
KApiLib.Construct();
GoSdkLib.Construct();
GoSystem system = new GoSystem();
GoSensor sensor;
KIpAddress ipAddress = KIpAddress.Parse(Constants.SENSOR_IP);
GoDataSet dataSet = new GoDataSet();
sensor = system.FindSensorByIpAddress(ipAddress);
sensor.Connect();
system.EnableData(true);
system.Start();
// refer to SetupMeasurement.cs for setting up of the measurement tools
dataSet = system.ReceiveData(30000000);
for (UInt32 i = 0; i < dataSet.Count; i++)
{
GoDataMsg dataObj = (GoDataMsg)dataSet.Get(i);
switch (dataObj.MessageType)
{
case GoDataMessageType.Stamp:
{
GoStampMsg stampMsg = (GoStampMsg)dataObj;
for (UInt32 j = 0; j < stampMsg.Count; j++)
{
GoStamp stamp = stampMsg.Get(j);
Console.WriteLine("Frame Index = {0}", stamp.FrameIndex);
Console.WriteLine("Time Stamp = {0}", stamp.Timestamp);
Console.WriteLine("Encoder Value = {0}", stamp.Encoder);
}
}
break;
case GoDataMessageType.Measurement:
{
GoMeasurementMsg measurementMsg = (GoMeasurementMsg)dataObj;
for (UInt32 k = 0; k < measurementMsg.Count; ++k)
{
GoMeasurementData measurementData = measurementMsg.Get(k);
Console.WriteLine("ID: {0}", measurementMsg.Id);
Console.WriteLine("Value: {0}", measurementData.Value);
Console.WriteLine("Decision: {0}", measurementData.Decision);
}
}
break;
}
}
system.Stop();
}
catch (KException ex)
{
Console.WriteLine("Error: {0}", ex.Status);
}
// wait for ESC key
Console.WriteLine("\nPress ENTER to continue");
do
{
System.Threading.Thread.Sleep(100);
} while (Console.Read() != (int)ConsoleKey.Enter);
return(1);
}
void ProfileProcessingFunc(KObject data, int dataQuene)
{
GoDataSet _dataSource = new GoDataSet();
_dataSource = (GoDataSet)data;
DataContext _dataContext = new DataContext();
ProfileShape _profileShape = new ProfileShape();
StringBuilder sb = new StringBuilder();
StringBuilder rawSb = new StringBuilder();
for (UInt32 i = 0; i < _dataSource.Count; i++)
{
GoDataMsg dataObj = (GoDataMsg)_dataSource.Get(i);
switch (dataObj.MessageType)
{
case GoDataMessageType.ResampledProfile:
{
GoResampledProfileMsg profileMsg = (GoResampledProfileMsg)dataObj;
_dataContext.xResolution = (double)profileMsg.XResolution / 1000000;
_dataContext.zResolution = (double)profileMsg.ZResolution / 1000000;
_dataContext.xOffset = (double)profileMsg.XOffset / 1000;
_dataContext.zOffset = (double)profileMsg.ZOffset / 1000;
_profileShape.width = profileMsg.Width;
_profileShape.size = profileMsg.Size;
//generate csv file for point data save
short[] points = new short[_profileShape.width];
ProfilePoint[] profileBuffer = new ProfilePoint[_profileShape.width];
IntPtr pointsPtr = profileMsg.Data;
Marshal.Copy(pointsPtr, points, 0, points.Length);
for (UInt32 arrayIndex = 0; arrayIndex < _profileShape.width; ++arrayIndex)
{
if (points[arrayIndex] != -32768)
{
profileBuffer[arrayIndex].x = _dataContext.xOffset + _dataContext.xResolution * arrayIndex;
profileBuffer[arrayIndex].z = _dataContext.zOffset + _dataContext.zResolution * points[arrayIndex];
}
else
{
profileBuffer[arrayIndex].x = _dataContext.xOffset + _dataContext.xResolution * arrayIndex;
profileBuffer[arrayIndex].z = -32768;
}
zValueList.Add(profileBuffer[arrayIndex].z);
}
sb.Clear();
if (!File.Exists(filePath))
{
sb.Append("Index,");
for (int k = 0; k < profileBuffer.Length; k++)
{
sb.Append(profileBuffer[k].x.ToString() + ",");
}
sb.Append(Environment.NewLine);
File.AppendAllText(filePath, sb.ToString());
//updateMsg("Create file sucessfully");
sb.Clear();
}
sb.Append(dataQuene.ToString() + ",");
//Get Value save to file
for (int k = 0; k < profileBuffer.Length; k++)
{
sb.Append(profileBuffer[k].z.ToString() + ",");
}
sb.Append(Environment.NewLine);
// write to file
File.AppendAllText(filePath, sb.ToString());
sb.Clear();
}
break;
default: break;
}
}
}
public List <List <PosXYZ> > GetResult()
{
if (FrameworkExtenion.IsSimulate)
{
return(new List <List <PosXYZ> >()
{
new List <PosXYZ>()
{
new PosXYZ(1, 1, 1), new PosXYZ(1, 1, 1), new PosXYZ(1, 1, 1)
},
new List <PosXYZ>()
{
new PosXYZ(2, 2, 2), new PosXYZ(2, 2, 2), new PosXYZ(2, 2, 2)
}
});
}
try
{
//goOutput = sensor.Output.GetEthernetAt(0);
//goOutput.ClearAllSources();
var dataSet = system.ReceiveData(3000000);
system.Stop();
system.EnableData(false);
//parse dataSet
for (int i = 0; i < dataSet.Count; i++)
{
GoDataMsg dataObj = (GoDataMsg)dataSet.Get(i);
switch (dataObj.MessageType)
{
case GoDataMessageType.Measurement:
{
//GoMeasurementMsg measurementMsg = (GoMeasurementMsg)dataObj;
//if (measurementMsg != null)
//{
// var output = new List<double[]>();
// for (var k = 0; k < measurementMsg.Count; ++k)
// {
// GoMeasurementData measurementData = measurementMsg.Get(k);
// var data = new double[3];
// data[0] = measurementMsg.Id;
// data[1] = measurementData.Value;
// data[2] = measurementData.Decision;
// output.Add(data);
// }
// return output;
//}
}
break;
case GoDataMessageType.Generic:
{
GoGenericMsg genericMsg = dataObj as GoGenericMsg;
if (genericMsg != null)
{
byte[] buffer = new byte[genericMsg.BufferSize];
Marshal.Copy(genericMsg.BufferData, buffer, 0, (int)genericMsg.BufferSize);
var gridRawData = FlatnessParser.Parse(buffer);
var output = FlatnessParser.GetGridOutput(gridRawData);
if (FlatnessParser.GridParseMethod == GridParseMethod.RigidAlignAndOrderByX)
{
if (DisplayResultEvent != null)
{
var dataList = new List <PosXYZ>();
foreach (var d in output)
{
dataList.AddRange(d);
}
DisplayResultEvent.Invoke(FlatnessParser.CreateDisplay(dataList));
}
}
else
{
if (DisplayResultEvent != null)
{
DisplayResultEvent.Invoke(FlatnessParser.CreateDisplay(gridRawData));
}
}
return(output);
}
}
break;
}
}
}
catch (Exception ex)
{
LastError = $"GetResultError: {ex.Message}";
}
return(null);
}
public static void takephoto(string name2)
{
try
{
KApiLib.Construct();
GoSdkLib.Construct();
GoSystem system = new GoSystem();
GoSensor sensor;
KIpAddress ipAddress = KIpAddress.Parse(Constants.SENSOR_IP);
GoDataSet dataSet = new GoDataSet();
sensor = system.FindSensorByIpAddress(ipAddress);
sensor.Connect();
GoSetup setup = sensor.Setup;
setup.ScanMode = GoMode.Surface;
system.EnableData(true);
system.Start();
Console.WriteLine("Waiting for Whole Part Data...");
dataSet = system.ReceiveData(30000000);
//DataContext context = new DataContext();
for (UInt32 i = 0; i < dataSet.Count; i++)
{
GoDataMsg dataObj = (GoDataMsg)dataSet.Get(i);
switch (dataObj.MessageType)
{
case GoDataMessageType.Stamp:
{
GoStampMsg stampMsg = (GoStampMsg)dataObj;
for (UInt32 j = 0; j < stampMsg.Count; j++)
{
GoStamp stamp = stampMsg.Get(j);
Console.WriteLine("Frame Index = {0}", stamp.FrameIndex);
Console.WriteLine("Time Stamp = {0}", stamp.Timestamp);
Console.WriteLine("Encoder Value = {0}", stamp.Encoder);
}
}
break;
case GoDataMessageType.UniformSurface:
{
GoUniformSurfaceMsg goSurfaceMsg = (GoUniformSurfaceMsg)dataObj; // 定义变量gosurfacemsg,类型gosurfacemsg
long length = goSurfaceMsg.Length; //surface长度
long width = goSurfaceMsg.Width; //surface宽度
long bufferSize = width * length;
double XResolution = goSurfaceMsg.XResolution / 1000000.0; //surface 数据X方向分辨率为nm,转为mm
double YResolution = goSurfaceMsg.YResolution / 1000000.0; //surface 数据Y方向分辨率为nm,转为mm
double ZResolution = goSurfaceMsg.ZResolution / 1000000.0; //surface 数据Z方向分辨率为nm,转为mm
double XOffset = goSurfaceMsg.XOffset / 1000.0; //接收到surface数据X方向补偿单位um，转mm
double YOffset = goSurfaceMsg.YOffset / 1000.0; //接收到surface数据Y方向补偿单位um，转mm
double ZOffset = goSurfaceMsg.ZOffset / 1000.0; //接收到surface数据Z方向补偿单位um，转mm
IntPtr bufferPointer = goSurfaceMsg.Data;
int rowIdx, colIdx;
SurfacePoints[] surfacePointCloud = new SurfacePoints[bufferSize];
short[] ranges = new short[bufferSize];
Marshal.Copy(bufferPointer, ranges, 0, ranges.Length);
FileStream fs;
string path = string.Format("D:\\Grasp\\Grasp\\EyeToHandData\\{0}.txt", name2);
//if (!File.Exists(path))
//{
fs = new FileStream(path, FileMode.Create, FileAccess.Write);
//else
//{
// fs = new FileStream(path, FileMode.Append, FileAccess.Write);
//}
StreamWriter sr = new StreamWriter(fs);
for (rowIdx = 0; rowIdx < length; rowIdx++) //row is in Y direction
{
for (colIdx = 0; colIdx < width; colIdx++) //col is in X direction
{
surfacePointCloud[rowIdx * width + colIdx].x = colIdx * XResolution + XOffset; //客户需要的点云数据X值
surfacePointCloud[rowIdx * width + colIdx].y = rowIdx * YResolution + YOffset; //客户需要的点云数据Y值
surfacePointCloud[rowIdx * width + colIdx].z = ranges[rowIdx * width + colIdx] * ZResolution + ZOffset; //客户需要的点云数据Z值
sr.WriteLine(surfacePointCloud[rowIdx * width + colIdx].x + "," + surfacePointCloud[rowIdx * width + colIdx].y + "," + surfacePointCloud[rowIdx * width + colIdx].z); //开始写入值
}
}
sr.Write("end");
//ushort[] ZValues = new ushort[ranges.Length];
//for (int k = 0; k < ranges.Length; k++)
//{
// ZValues[k] = (ushort)(ranges[k] - short.MinValue);
//}
//for (UInt32 k = 0; k < bufferSize; k++)
//{
// sr.WriteLine(surfacePointCloud[k].x.ToString() + "," + surfacePointCloud[k].y.ToString() + "," + surfacePointCloud[k].z.ToString());//开始写入值
//}
sr.Close();
fs.Close();
}
break;
case GoDataMessageType.SurfaceIntensity:
{
GoSurfaceIntensityMsg surfaceMsg = (GoSurfaceIntensityMsg)dataObj;
long width = surfaceMsg.Width;
long height = surfaceMsg.Length;
long bufferSize = width * height;
IntPtr bufferPointeri = surfaceMsg.Data;
Console.WriteLine("Whole Part Intensity Image received:");
Console.WriteLine(" Buffer width: {0}", width);
Console.WriteLine(" Buffer height: {0}", height);
byte[] ranges = new byte[bufferSize];
Marshal.Copy(bufferPointeri, ranges, 0, ranges.Length);
}
break;
}
}
system.Stop();
}
catch (KException ex)
{
Console.WriteLine("Error: {0}", ex.Status);
}
// wait for ESC key
//Console.WriteLine("\nPress ENTER to continue");
//do
//{
// System.Threading.Thread.Sleep(100);
//} while (Console.Read() != (int)ConsoleKey.Enter);
}
public void ReceiveData(KObject data)
{
// 데이터 들어오면 카메라 시스템 정지
var pDocument = DeepSight.CDocument.GetDocument;
//if( CDefine.enumTrigger.TRIGGER_OFF == pDocument.GetTrigger( ( int )CDefine.enumCamera.CAMERA_1 ) ) return;
m_objCameraSystem.Stop();
CImageData3D objImageData3D = new CImageData3D();
// 일단 해상도를 임의로 지정하자
GoDataSet dataSet = ( GoDataSet )data;
pDocument.SetUpdateLog(CDefine.enumLogType.LOG_PROCESS, "GOCATOR RECEIVE DATA : " + dataSet.Count.ToString());
for (UInt32 i = 0; i < dataSet.Count; i++)
{
GoDataMsg dataObj = ( GoDataMsg )dataSet.Get(i);
switch (dataObj.MessageType)
{
case GoDataMessageType.Stamp: {
GoStampMsg stampMsg = ( GoStampMsg )dataObj;
for (UInt32 j = 0; j < stampMsg.Count; j++)
{
GoStamp stamp = stampMsg.Get(j);
Console.WriteLine("Frame Index = {0}", stamp.FrameIndex);
Console.WriteLine("Time Stamp = {0}", stamp.Timestamp);
Console.WriteLine("Encoder Value = {0}", stamp.Encoder);
}
}
break;
case GoDataMessageType.UniformSurface: {
#pragma warning disable CS0618 // 형식 또는 멤버는 사용되지 않습니다.
GoSurfaceMsg surfaceMsg = ( GoSurfaceMsg )dataObj;
#pragma warning restore CS0618 // 형식 또는 멤버는 사용되지 않습니다.
objImageData3D.iOffsetZ = surfaceMsg.ZOffset;
objImageData3D.iResolutionX = surfaceMsg.XResolution; // * DEF_3D_DATA_MULTIPLE;
objImageData3D.iResolutionY = surfaceMsg.YResolution;
objImageData3D.iResolutionZ = surfaceMsg.ZResolution;
int lWidth = objImageData3D.iWidth = ( int )surfaceMsg.Width;
int lHeight = objImageData3D.iHeight = ( int )surfaceMsg.Length;
int bufferSize = lWidth * lHeight;
IntPtr bufferPointer = surfaceMsg.Data;
Console.WriteLine("Whole Part Height Map received:");
Console.WriteLine(" Buffer width: {0}", lWidth);
Console.WriteLine(" Buffer Height: {0}", lHeight);
objImageData3D.objHeightDataOrigin = new short[bufferSize];
Marshal.Copy(bufferPointer, objImageData3D.objHeightDataOrigin, 0, objImageData3D.objHeightDataOrigin.Length);
// SetDataToCsv("d:\\test.csv", objImageData3D.objHeightDataOrigin);
}
break;
case GoDataMessageType.SurfaceIntensity: {
GoSurfaceIntensityMsg surfaceMsg = ( GoSurfaceIntensityMsg )dataObj;
long width = surfaceMsg.Width;
long length = surfaceMsg.Length;
long bufferSize = width * length;
IntPtr bufferPointeri = surfaceMsg.Data;
Console.WriteLine("Whole Part Intensity Image received:");
Console.WriteLine(" Buffer width: {0}", width);
Console.WriteLine(" Buffer length: {0}", length);
objImageData3D.objIntensityDataOrigin = new byte[bufferSize];
Marshal.Copy(bufferPointeri, objImageData3D.objIntensityDataOrigin, 0, objImageData3D.objIntensityDataOrigin.Length);
// objImageData3D.objBitmapIntensity = CopyDataToBitmap( ( int )length, ( int )width, objImageData3D.objIntensityDataOrigin );
// objImageData3D.objBitmapIntensity.Save( "d:\\Intensity.bmp" );
}
break;
default:
break;
}
}
if (null != m_objCallback)
{
CImageData objData = new CImageData();
objData.bGrabComplete = true;
objData.bitmapImage = objImageData3D.objBitmapIntensity;
objData.objCameraData3D = objImageData3D;
m_objCallback(objData);
}
iGCCollectCount++;
if (1 < iGCCollectCount)
{
GC.Collect();
iGCCollectCount = 0;
}
}
static int Main(string[] args)
{
try
{
KApiLib.Construct();
GoSdkLib.Construct();
GoSystem system = new GoSystem();
GoSensor sensor;
KIpAddress ipAddress = KIpAddress.Parse(Constants.SENSOR_IP);
GoDataSet dataSet = new GoDataSet();
sensor = system.FindSensorByIpAddress(ipAddress);
sensor.Connect();
system.EnableData(true);
system.Start();
dataSet = system.ReceiveData(30000000);
DataContext context = new DataContext();
for (UInt32 i = 0; i < dataSet.Count; i++)
{
GoDataMsg dataObj = (GoDataMsg)dataSet.Get(i);
switch (dataObj.MessageType)
{
case GoDataMessageType.Stamp:
{
GoStampMsg stampMsg = (GoStampMsg)dataObj;
for (UInt32 j = 0; j < stampMsg.Count; j++)
{
GoStamp stamp = stampMsg.Get(j);
Console.WriteLine("Frame Index = {0}", stamp.FrameIndex);
Console.WriteLine("Time Stamp = {0}", stamp.Timestamp);
Console.WriteLine("Encoder Value = {0}", stamp.Encoder);
}
}
break;
case GoDataMessageType.UniformProfile:
{
GoResampledProfileMsg profileMsg = (GoResampledProfileMsg)dataObj;
Console.WriteLine(" Resampled Profile Message batch count: {0}", profileMsg.Count);
for (UInt32 k = 0; k < profileMsg.Count; ++k)
{
int validPointCount = 0;
int profilePointCount = profileMsg.Width;
Console.WriteLine(" Item[{0}]: Profile data ({1} points)", k, profileMsg.Width);
context.xResolution = (double)profileMsg.XResolution / 1000000;
context.zResolution = (double)profileMsg.ZResolution / 1000000;
context.xOffset = (double)profileMsg.XOffset / 1000;
context.zOffset = (double)profileMsg.ZOffset / 1000;
short[] points = new short[profilePointCount];
ProfilePoint[] profileBuffer = new ProfilePoint[profilePointCount];
IntPtr pointsPtr = profileMsg.Data;
Marshal.Copy(pointsPtr, points, 0, points.Length);
for (UInt32 arrayIndex = 0; arrayIndex < profilePointCount; ++arrayIndex)
{
if (points[arrayIndex] != -32768)
{
profileBuffer[arrayIndex].x = context.xOffset + context.xResolution * arrayIndex;
profileBuffer[arrayIndex].z = context.zOffset + context.zResolution * points[arrayIndex];
validPointCount++;
}
else
{
profileBuffer[arrayIndex].x = context.xOffset + context.xResolution * arrayIndex;
profileBuffer[arrayIndex].z = -32768;
}
}
Console.WriteLine("Received {0} Range Points", profilePointCount);
Console.WriteLine("Valid Points {0}", validPointCount);
}
}
break;
case GoDataMessageType.ProfilePointCloud:
{
GoProfileMsg profileMsg = (GoProfileMsg)dataObj;
Console.WriteLine(" Profile Message batch count: {0}", profileMsg.Count);
for (UInt32 k = 0; k < profileMsg.Count; ++k)
{
int validPointCount = 0;
long profilePointCount = profileMsg.Width;
Console.WriteLine(" Item[{0}]: Profile data ({1} points)", i, profileMsg.Width);
context.xResolution = profileMsg.XResolution / 1000000;
context.zResolution = profileMsg.ZResolution / 1000000;
context.xOffset = profileMsg.XOffset / 1000;
context.zOffset = profileMsg.ZOffset / 1000;
GoPoints[] points = new GoPoints[profilePointCount];
ProfilePoint[] profileBuffer = new ProfilePoint[profilePointCount];
int structSize = Marshal.SizeOf(typeof(GoPoints));
IntPtr pointsPtr = profileMsg.Data;
for (UInt32 array = 0; array < profilePointCount; ++array)
{
IntPtr incPtr = new IntPtr(pointsPtr.ToInt64() + array * structSize);
points[array] = (GoPoints)Marshal.PtrToStructure(incPtr, typeof(GoPoints));
}
for (UInt32 arrayIndex = 0; arrayIndex < profilePointCount; ++arrayIndex)
{
if (points[arrayIndex].x != -32768)
{
profileBuffer[arrayIndex].x = context.xOffset + context.xResolution * points[arrayIndex].x;
profileBuffer[arrayIndex].z = context.xOffset + context.xResolution * points[arrayIndex].y;
validPointCount++;
}
else
{
profileBuffer[arrayIndex].x = -32768;
profileBuffer[arrayIndex].z = -32768;
}
}
Console.WriteLine("Received {0} Range Points", profilePointCount);
Console.WriteLine("Valid Points {0}", validPointCount);
}
}
break;
case GoDataMessageType.ProfileIntensity:
{
GoProfileIntensityMsg profileMsg = (GoProfileIntensityMsg)dataObj;
Console.WriteLine(" Profile Intensity Message batch count: {0}", profileMsg.Count);
for (UInt32 k = 0; k < profileMsg.Count; ++k)
{
byte[] intensity = new byte[profileMsg.Width];
IntPtr intensityPtr = profileMsg.Data;
Marshal.Copy(intensityPtr, intensity, 0, intensity.Length);
}
}
break;
}
}
system.Stop();
}
catch (KException ex)
{
Console.WriteLine("Error: {0}", ex.Status);
}
// wait for ESC key
Console.WriteLine("\nPress ENTER to continue");
do
{
System.Threading.Thread.Sleep(100);
} while (Console.Read() != (int)ConsoleKey.Enter);
return(1);
}
/// <summary>
/// 监听回调函数
/// </summary>
/// <param name="data"></param>
public void onData(KObject data)
{
if (File.Exists(strFileSave))
{
File.Delete(strFileSave);
}
bool length_incr = true;
lst_ImagSur.Clear();
GoDataSet dataSet = (GoDataSet)data;
DataContext context = new DataContext();
for (UInt32 i = 0; i < dataSet.Count; ++i)
{
GoDataMsg dataObj = (GoDataMsg)dataSet.Get(i);
switch (dataObj.MessageType)
{
case GoDataMessageType.Stamp:
{
Console.WriteLine("GoDataMessageType.Stamp");
//GoStampMsg stampMsg = (GoStampMsg)dataObj;
//for (UInt32 j = 0; j < stampMsg.Count; j++)
//{
// GoStamp stamp = stampMsg.Get(j);
// Console.WriteLine("Frame Index = {0}", stamp.FrameIndex);
// Console.WriteLine("Time Stamp = {0}", stamp.Timestamp);
// Console.WriteLine("Encoder Value = {0}", stamp.Encoder);
//}
}
break;
case GoDataMessageType.UniformSurface: //case GoDataMessageType.Surface:
{
Console.WriteLine("GoDataMessageType.UniformSurface");
GoUniformSurfaceMsg surfaceMsg = (GoUniformSurfaceMsg)dataObj;
long width = surfaceMsg.Width;
long height = surfaceMsg.Length;
long bufferSize = width * height;
IntPtr bufferPointer = surfaceMsg.Data;
short[] ranges = new short[bufferSize];
Marshal.Copy(bufferPointer, ranges, 0, ranges.Length);
context.xResolution = (double)surfaceMsg.XResolution / 1000000.0;
context.zResolution = (double)surfaceMsg.ZResolution / 1000000.0;
context.yResolution = (double)surfaceMsg.YResolution / 1000000.0;
context.yOffset = (double)surfaceMsg.YOffset / 1000.0;
context.xOffset = (double)surfaceMsg.XOffset / 1000.0;
context.zOffset = (double)surfaceMsg.ZOffset / 1000.0;
double phy_x;
double phy_y;
double phy_z;
FileStream fs = new FileStream(strFileSave, FileMode.Create);
StreamWriter sw = new StreamWriter(fs);
for (int m = 0; m < height; m++)
{
for (int j = 0; j < width; j++)
{
phy_z = ranges[m * width + j] * context.zResolution + context.zOffset;
if (true) // /*phy_z > 20*/ 这个过滤阈值根据实际情况选取
{
phy_x = j * context.xResolution + context.xOffset;
phy_y = m * context.yResolution + context.yOffset;
string strWrite = string.Format("{0} {1} {2}", phy_x, phy_y, phy_z);
sw.WriteLine(strWrite);
lst_ImagSur.Add(new Vector4((float)phy_x, (float)phy_y, (float)phy_z, 1.0f));
}
}
}
sw.Flush();
//关闭流
sw.Close();
fs.Close();
}
break;
case GoDataMessageType.Measurement:
{
Console.WriteLine("GoDataMessageType.Measurement");
//GoMeasurementMsg measurementMsg = (GoMeasurementMsg)dataObj;
//for (UInt32 k = 0; k < measurementMsg.Count; ++k)
//{
// GoMeasurementData measurementData = measurementMsg.Get(k);
// mPoint.X = measurementData.Value;
// mPoint.Y = measurementData.Value;
// mPoint.Z = measurementData.Value;
//}
}
break;
case GoDataMessageType.ProfilePointCloud: //case GoDataMessageType.Profile:
{
Console.WriteLine("GoDataMessageType.Profile");
//StreamWriter write = new StreamWriter(strFileSave, true);
//GoProfilePointCloudMsg profileMsg = (GoProfilePointCloudMsg)dataObj;
//Console.WriteLine(" Profile Message batch count: {0}", profileMsg.Count);
//for (UInt32 k = 0; k < profileMsg.Count; ++k)
//{
// //int validPointCount = 0;
// long profilePointCount = profileMsg.Width;
// Console.WriteLine(" Item[{0}]: Profile data ({1} points)", i, profileMsg.Width);
// context.xResolution = (profileMsg.XResolution / 1000000.0);
// context.zResolution = profileMsg.ZResolution / 1000000.0;
// context.xOffset = profileMsg.XOffset / 1000.0;
// context.zOffset = profileMsg.ZOffset / 1000.0;
// GoPoints[] points = new GoPoints[profilePointCount];
// //point[] point111 = new point[profilePointCount];
// ProfilePoint[] profileBuffer = new ProfilePoint[profilePointCount];
// int structSize = Marshal.SizeOf(typeof(GoPoints));
// IntPtr pointsPtr = profileMsg.Data;
// for (UInt32 array = 0; array < profilePointCount; ++array)
// {
// IntPtr incPtr = new IntPtr(pointsPtr.ToInt64() + array * structSize);
// points[array] = (GoPoints)Marshal.PtrToStructure(incPtr, typeof(GoPoints));
// double real_x = (context.xOffset + context.xResolution * points[array].x);
// double real_z = (context.zOffset + context.zResolution * points[array].y);
// if (length_incr == true)
// {
// length += 1;//ReadIniSettings.ReadIni.objIniValue.iniScanner.step;
// length_incr = false;
// }
// if (/*(real_z > 15 && real_x > -500.0 && real_z < 400*/true)
// //if (real_z > 5.0 && real_z < 25 && length >100 && length < 2000)
// {
// write.WriteLine(real_x + " " + length + " " + real_z);
// }
// }
// write.Flush();
//}
//write.Close();
}
break;
case GoDataMessageType.ProfileIntensity:
{
Console.WriteLine("GoDataMessageType.ProfileIntensity");
//GoProfileIntensityMsg profileMsg = (GoProfileIntensityMsg)dataObj;
//Console.WriteLine(" Profile Intensity Message batch count: {0}", profileMsg.Count);
//for (UInt32 k = 0; k < profileMsg.Count; ++k)
//{
// byte[] intensity = new byte[profileMsg.Width];
// IntPtr intensityPtr = profileMsg.Data;
// Marshal.Copy(intensityPtr, intensity, 0, intensity.Length);
//}
}
break;
case GoDataMessageType.Alignment:
Console.WriteLine("GoDataMessageType.Alignment");
break;
case GoDataMessageType.BoundingBoxMatch:
Console.WriteLine("GoDataMessageType.BoundingBoxMatch");
break;
case GoDataMessageType.EdgeMatch:
Console.WriteLine("GoDataMessageType.EdgeMatch");
break;
case GoDataMessageType.EllipseMatch:
Console.WriteLine("GoDataMessageType.EllipseMatch");
break;
case GoDataMessageType.Event:
Console.WriteLine("GoDataMessageType.Event");
break;
case GoDataMessageType.ExposureCal:
Console.WriteLine("GoDataMessageType.ExposureCal");
break;
case GoDataMessageType.Generic:
Console.WriteLine("GoDataMessageType.Generic");
break;
case GoDataMessageType.Health:
Console.WriteLine("GoDataMessageType.Health");
break;
case GoDataMessageType.Range:
Console.WriteLine("GoDataMessageType.Range");
break;
case GoDataMessageType.RangeIntensity:
Console.WriteLine("GoDataMessageType.RangeIntensity");
break;
case GoDataMessageType.ResampledProfile: //case GoDataMessageType.UniformProfile:
Console.WriteLine("GoDataMessageType.ResampledProfile");
break;
case GoDataMessageType.Section:
Console.WriteLine("GoDataMessageType.Section");
break;
case GoDataMessageType.SectionIntensity:
Console.WriteLine("GoDataMessageType.SectionIntensity");
break;
case GoDataMessageType.SurfaceIntensity:
Console.WriteLine("GoDataMessageType.SurfaceIntensity");
break;
case GoDataMessageType.SurfacePointCloud:
Console.WriteLine("GoDataMessageType.SurfacePointCloud");
break;
case GoDataMessageType.Tracheid:
Console.WriteLine("GoDataMessageType.Tracheid");
break;
case GoDataMessageType.Unknown:
Console.WriteLine("GoDataMessageType.Unknown");
break;
case GoDataMessageType.Video:
Console.WriteLine("GoDataMessageType.Video");
break;
}
}
dataSet.Dispose();
if (ActGetBit != null && lst_ImagSur.Count > 0)
{
int imgWidth, imgHeight;
int rowNum = lst_ImagSur.Count;
double[] xVal = new double[rowNum];
double[] yVal = new double[rowNum];
ushort[] zVal = new ushort[rowNum];
for (int m = 0; m < rowNum; m++)
{
xVal[m] = Math.Round(lst_ImagSur[m].x / pixelPitch);
yVal[m] = Math.Round(lst_ImagSur[m].y / pixelPitch);
zVal[m] = (ushort)lst_ImagSur[m].z;
}
ArrayList list_x = new ArrayList(xVal);
list_x.Sort();
if (list_x.Count == 0)
{
double minVal_x = 0; // Convert.ToInt32(list[0]);
double maxVal_x = 0;
ActGetBit(null);
}
else
{
double minVal_x = Convert.ToDouble(list_x[0]); // Convert.ToInt32(list[0]);
double maxVal_x = Convert.ToDouble(list_x[list_x.Count - 1]);
ArrayList list_y = new ArrayList(yVal);
list_y.Sort();
double minVal_y = Convert.ToDouble(list_y[0]); // Convert.ToInt32(list[0]);
double maxVal_y = Convert.ToDouble(list_y[list_y.Count - 1]);
ArrayList list_z = new ArrayList(zVal);
list_z.Sort();
double minVal_z = Convert.ToDouble(list_z[0]); // Convert.ToInt32(list[0]);
double maxVal_z = Convert.ToDouble(list_z[list_z.Count - 1]);
imgHeight = Convert.ToInt32(Math.Round(maxVal_x - minVal_x)) + 1;
imgWidth = Convert.ToInt32(Math.Round(maxVal_y - minVal_y)) + 1;
double scale = (double)imgHeight / imgWidth;
double[,] imgTemp = new double[imgHeight, imgWidth];
for (int m = 0; m < rowNum; m++)
{
xVal[m] = xVal[m] - minVal_x;
yVal[m] = yVal[m] - minVal_y;
}
for (int n = 0; n < rowNum; n++)
{
imgTemp[Convert.ToInt32(xVal[n]), Convert.ToInt32(yVal[n])] = zVal[n];
}
// 将imgTemp转成一维图像
ushort[] ZValues = new ushort[imgHeight * imgWidth];
for (int i = 0; i < ZValues.Length; i++)
{
ZValues[i] = (ushort)imgTemp[i / imgWidth, i % imgWidth];
}
ColorMaps MapColor = new ColorMaps();
ushort minValue = 0, maxValue = 0;
MapColor.FindMinMaxForColor(ZValues, (UInt32)(imgHeight * imgWidth), 0, ref minValue, ref maxValue);
Color[] colors = new Color[imgHeight * imgWidth];
MapColor.ToColors(ZValues, minValue, maxValue, 0, ref colors, (UInt32)(imgHeight * imgWidth));
Bitmap Bimage = new Bitmap(imgWidth, imgHeight);
BitmapData bmdata = Bimage.LockBits(new Rectangle(0, 0, imgWidth, imgHeight), ImageLockMode.ReadWrite, PixelFormat.Format24bppRgb);
//IntPtr intptr = bmdata.Scan0;
byte[] Pixlemaps = new byte[bmdata.Stride * bmdata.Height];
// int offset = bmdata.Stride - bmdata.Width * 3;
unsafe
{
byte *pp = (byte *)(void *)bmdata.Scan0;
for (int k = 0; k < bmdata.Height; k++)
{
for (int m = 0; m < bmdata.Width; m++)
{
pp[0] = (byte)(colors[k * bmdata.Width + m].R);
pp[1] = (byte)(colors[k * bmdata.Width + m].G);
pp[2] = (byte)(colors[k * bmdata.Width + m].B);
pp += 3;
}
pp += bmdata.Stride - bmdata.Width * 3;
}
}
Bimage.UnlockBits(bmdata);
Bitmap aBM2 = Bimage.Clone(new RectangleF(0, 0, Bimage.Width, Bimage.Height), PixelFormat.Format24bppRgb);
ActGetBit(aBM2);
//pbxImage.Width = 150;
//pbxImage.Height = Convert.ToInt32(pbxImage.Width * scale);
//Bitmap imgShow_scale = new Bitmap(aBM2, pbxImage.Width, pbxImage.Height);
//pbxImage.Image = imgShow_scale;
}
}
}
public void onData(KObject data)
{
bDataSaved = false;
double yPoint = 1;
string strWrite = "";
bool length_incr = true;
GoDataSet dataSet = (GoDataSet)data;
DataContext context = new DataContext();
for (UInt32 i = 0; i < dataSet.Count; i++)
{
GoDataMsg dataObj = (GoDataMsg)dataSet.Get(i);
switch (dataObj.MessageType)
{
case GoDataMessageType.Stamp:
{
GoStampMsg stampMsg = (GoStampMsg)dataObj;
for (UInt32 j = 0; j < stampMsg.Count; j++)
{
GoStamp stamp = stampMsg.Get(j);
//Console.WriteLine("Frame Index = {0}", stamp.FrameIndex);
//Console.WriteLine("Time Stamp = {0}", stamp.Timestamp);
//Console.WriteLine("Encoder Value = {0}", stamp.Encoder);
}
}
break;
case GoDataMessageType.Surface: // Surface
{
GoSurfaceMsg surfaceMsg = (GoSurfaceMsg)dataObj;
long width = surfaceMsg.Width;
long height = surfaceMsg.Length;
long bufferSize = width * height;
IntPtr bufferPointer = surfaceMsg.Data;
short[] ranges = new short[bufferSize];
Marshal.Copy(bufferPointer, ranges, 0, ranges.Length);
context.xResolution = (double)surfaceMsg.XResolution / 1000000.0;
context.zResolution = (double)surfaceMsg.ZResolution / 1000000.0;
context.yResolution = (double)surfaceMsg.YResolution / 1000000.0;
context.yOffset = (double)surfaceMsg.YOffset / 1000.0;
context.xOffset = (double)surfaceMsg.XOffset / 1000.0;
context.zOffset = (double)surfaceMsg.ZOffset / 1000.0;
double phy_x;
double phy_y;
double phy_z;
FileStream fs = new FileStream(strFileSave, FileMode.Create);
StreamWriter sw = new StreamWriter(fs);
for (int m = 0; m < height; m++)
{
for (int j = 0; j < width; j++)
{
phy_z = ranges[m * width + j] * context.zResolution + context.zOffset;
if (/*phy_z > 20*/ true) //这个过滤阈值根据实际情况选取
{
phy_x = j * context.xResolution + context.xOffset;
phy_y = m * context.yResolution + context.yOffset;
strWrite = string.Format("{0} {1} {2}", phy_x, phy_y, phy_z);
sw.WriteLine(strWrite);
}
}
}
sw.Flush();
//关闭流
sw.Close();
fs.Close();
}
break;
case GoDataMessageType.Profile:
{
StreamWriter write = new StreamWriter(strFileSave, true);
GoProfileMsg profileMsg = (GoProfileMsg)dataObj;
Console.WriteLine(" Profile Message batch count: {0}", profileMsg.Count);
for (UInt32 k = 0; k < profileMsg.Count; ++k)
{
int validPointCount = 0;
long profilePointCount = profileMsg.Width;
Console.WriteLine(" Item[{0}]: Profile data ({1} points)", i, profileMsg.Width);
context.xResolution = (profileMsg.XResolution / 1000000.0);
context.zResolution = profileMsg.ZResolution / 1000000.0;
context.xOffset = profileMsg.XOffset / 1000.0;
context.zOffset = profileMsg.ZOffset / 1000.0;
GoPoints[] points = new GoPoints[profilePointCount];
point[] point111 = new point[profilePointCount];
ProfilePoint[] profileBuffer = new ProfilePoint[profilePointCount];
int structSize = Marshal.SizeOf(typeof(GoPoints));
IntPtr pointsPtr = profileMsg.Data;
for (UInt32 array = 0; array < profilePointCount; ++array)
{
IntPtr incPtr = new IntPtr(pointsPtr.ToInt64() + array * structSize);
points[array] = (GoPoints)Marshal.PtrToStructure(incPtr, typeof(GoPoints));
double real_x = (context.xOffset + context.xResolution * points[array].x);
double real_z = (context.zOffset + context.zResolution * points[array].y);
if (length_incr == true)
{
length += 1; //ReadIniSettings.ReadIni.objIniValue.iniScanner.step;
length_incr = false;
}
if (real_z > 60 && real_z < 800)
{
write.WriteLine(real_x + " " + length + " " + real_z);
}
}
write.Flush();
}
write.Close();
}
break;
case GoDataMessageType.ProfileIntensity:
{
GoProfileIntensityMsg profileMsg = (GoProfileIntensityMsg)dataObj;
Console.WriteLine(" Profile Intensity Message batch count: {0}", profileMsg.Count);
for (UInt32 k = 0; k < profileMsg.Count; ++k)
{
byte[] intensity = new byte[profileMsg.Width];
IntPtr intensityPtr = profileMsg.Data;
Marshal.Copy(intensityPtr, intensity, 0, intensity.Length);
}
}
break;
case GoDataMessageType.Measurement: // Measurement
{
GoMeasurementMsg measurementMsg = (GoMeasurementMsg)dataObj;
}
break;
}
}
bDataSaved = true;
}
/// <summary>
/// Get Point Cloud from Gocator
/// </summary>
/// <returns></returns>
private void Point3DGenerator(KObject data)
{
GoDataSet goDataSource = (GoDataSet)data;
PointCloud pc = new PointCloud();
long count = goDataSource.Count;
for (int i = 0; i < count; i++)
{
GoDataMsg dataMsg = (GoDataMsg)goDataSource.Get(i);
switch (dataMsg.MessageType)
{
case GoDataMessageType.Surface:
GoSurfaceMsg goSurfaceMsg = (GoSurfaceMsg)(dataMsg);
pc.Width = (int)goSurfaceMsg.Width;
pc.Height = (int)goSurfaceMsg.Length;
pc.XResolution = (float)goSurfaceMsg.XResolution / 1000000;
pc.YResolution = (float)goSurfaceMsg.YResolution / 1000000;
pc.ZResolution = (float)goSurfaceMsg.ZResolution / 1000000;
pc.XOffset = (float)goSurfaceMsg.XOffset / 1000;
pc.ZOffset = (float)goSurfaceMsg.ZOffset / 1000;
pc.YOffset = (float)goSurfaceMsg.YOffset / 1000;
GoSetup goSetup = _sensor.Setup;
GoRole goRole = new GoRole();
pc.ZStart = (float)goSetup.GetActiveAreaZ(goRole);
pc.XStart = (float)goSetup.GetActiveAreaX(goRole);
pc.ZRange = (float)goSetup.GetActiveAreaHeight(goRole);
///
//generate csv file for point data save
///
pc.ProfileList = new List <List <Point3D> >();
//int pointsCount = 0;
pc.Point3DArray = new Point3D[pc.Width * pc.Height];
int pointsCount = 0;
for (int j = 0; j < pc.Height; j++)
{
List <Point3D> profileList = new List <Point3D>();
for (int k = 0; k < pc.Width; k++)
{
Point3D tempPoint = new Point3D();
tempPoint.X = pc.XOffset + k * pc.XResolution;
tempPoint.Y = pc.YOffset + j * pc.YResolution;
//tempPoint.Z = (goSurfaceMsg.Get(j, k) == -32768) ? 0 : (pc.ZOffset + goSurfaceMsg.Get(j, k) * pc.ZResolution);
//No Z Start
//tempPoint.X = k * pc.XResolution;
//tempPoint.Y = j * pc.YResolution;
tempPoint.Z = (goSurfaceMsg.Get(j, k) == -32768) ? 0 : (pc.ZOffset + goSurfaceMsg.Get(j, k) * pc.ZResolution - pc.ZStart);
profileList.Add(tempPoint);
pc.Point3DArray[pointsCount] = tempPoint;
pointsCount++;
}
pc.ProfileList.Add(profileList);
}
//data.Destroy();
dataMsg.Destroy();
goSurfaceMsg.Destroy();
SerializePointCloud(pc);
break;
default:
break;
}
}
}
