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);
}
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;
}
}
}
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);
}
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;
}
}
}
