static int Main(string[] args)
{
kStatus status;
IntPtr api = IntPtr.Zero;
IntPtr system = IntPtr.Zero;
IntPtr sensor = IntPtr.Zero;
IntPtr buddy = IntPtr.Zero;
IntPtr dataset = IntPtr.Zero;
IntPtr dataObj = IntPtr.Zero;
IntPtr stampMsg = IntPtr.Zero;
IntPtr profileMsg = IntPtr.Zero;
IntPtr stampPtr = IntPtr.Zero;
GoStamp stamp;
IntPtr addrPtr = IntPtr.Zero;
IntPtr intensityPtr = IntPtr.Zero;
address addr = new address();
address addr_buddy = new address();
IntPtr addrPtr_buddy = IntPtr.Zero;
DataContext context = new DataContext();
if ((status = (kStatus)GoSdk_Construct(ref api)) != kStatus.kOK)
{
Console.WriteLine("GoSdk_Construct Error:{0}", (int)status);
return((int)status);
}
if ((status = (kStatus)GoSystem_Construct(ref system, IntPtr.Zero)) != kStatus.kOK)
{
Console.WriteLine("GoSystem_Construct Error:{0}", (int)status);
return((int)status);
}
addrPtr = Marshal.AllocHGlobal(Marshal.SizeOf(addr));
Marshal.StructureToPtr(addr, addrPtr, false);
if ((status = (kStatus)kIpAddress_Parse(addrPtr, Constants.SENSOR_IP)) != kStatus.kOK)
{
Console.WriteLine("kIpAddress_Parse Error:{0}", (int)status);
return((int)status);
}
if ((status = (kStatus)GoSystem_FindSensorByIpAddress(system, addrPtr, ref sensor)) != kStatus.kOK)
{
Console.WriteLine("GoSystem_FindSensorByIpAddress Error:{0}", (int)status);
return((int)status);
}
//NOTE: GoSystem_Connect() can be used on Gocator buddy systems already configured, the GoSystem() will be able to
//recognize the buddy sensor as a paired buddy and not attempt to obtain a separate connection from the budfdy sensor.
//In this example, we are trying to pair two sensors as buddies, hence the need to use GoSensor_Connect() to retrieve sensor
if ((status = (kStatus)GoSensor_Connect(sensor)) != kStatus.kOK)
{
Console.WriteLine("GoSensor_Connect Error:{0}", (int)status);
return((int)status);
}
if (!GoSensor_HasBuddy(sensor))
{
addrPtr_buddy = Marshal.AllocHGlobal(Marshal.SizeOf(addr_buddy));
Marshal.StructureToPtr(addr_buddy, addrPtr_buddy, false);
if ((status = (kStatus)kIpAddress_Parse(addrPtr_buddy, Constants.BUDDY_IP)) != kStatus.kOK)
{
Console.WriteLine("kIpAddress_Parse Error:{0}", (int)status);
return((int)status);
}
if ((status = (kStatus)GoSystem_FindSensorByIpAddress(system, addrPtr_buddy, ref buddy)) != kStatus.kOK)
{
Console.WriteLine("GoSystem_FindSensorByIpAddress Error:{0}", (int)status);
return((int)status);
}
if ((status = (kStatus)GoSensor_Connect(buddy)) != kStatus.kOK)
{
Console.WriteLine("GoSensor_Connect Error:{0}", (int)status);
return((int)status);
}
if ((status = (kStatus)GoSensor_AddBuddy(sensor, buddy)) != kStatus.kOK)
{
Console.WriteLine("GoSensor_AddBuddy Error:{0}", (int)status);
return((int)status);
}
Console.WriteLine("Buddy sensor assigned.");
}
if ((status = (kStatus)GoSystem_EnableData(system, true)) != kStatus.kOK)
{
Console.WriteLine("GoSystem_EnableData Error:{0}", (int)status);
return((int)status);
}
if ((status = (kStatus)GoSystem_Start(system)) != kStatus.kOK)
{
Console.WriteLine(" GoSystem_Start Error:{0}", (int)status);
return((int)status);
}
// read data from sensor
if ((status = (kStatus)GoSystem_ReceiveData(system, ref dataset, 2000000)) == kStatus.kOK)
{
// each result can have multiple data items
// loop through all items in result message
for (UInt32 i = 0; i < GoDataSet_Count(dataset); ++i)
{
dataObj = GoDataSet_At(dataset, i);
switch (GoDataMsg_Type(dataObj))
{
// retrieve GoStamp message
case GoDataMessageTypes.GO_DATA_MESSAGE_TYPE_STAMP:
{
stampMsg = dataObj;
for (UInt32 j = 0; j < GoStampMsg_Count(stampMsg); j++)
{
stampPtr = GoStampMsg_At(stampMsg, j);
stamp = (GoStamp)Marshal.PtrToStructure(stampPtr, typeof(GoStamp));
Console.WriteLine("Frame Index = {0}", stamp.frameIndex);
Console.WriteLine("Time Stamp = {0}", stamp.timestamp);
Console.WriteLine("Encoder Value = {0}", stamp.encoder);
}
}
break;
// retreieve resampled profile data
case GoDataMessageTypes.GO_DATA_MESSAGE_TYPE_RESAMPLED_PROFILE:
{
profileMsg = dataObj;
Console.WriteLine(" Resampled Profile Message batch count: {0}", GoResampledProfileMsg_Count(profileMsg));
for (UInt32 k = 0; k < GoResampledProfileMsg_Count(profileMsg); ++k)
{
int validPointCount = 0;
UInt32 profilePointCount = GoResampledProfileMsg_Width(profileMsg);
Console.WriteLine(" Item[{0}]: Profile data ({1} points)", k, GoResampledProfileMsg_Width(profileMsg));
context.xResolution = GoResampledProfileMsg_XResolution(profileMsg) / 1000000;
context.zResolution = GoResampledProfileMsg_ZResolution(profileMsg) / 1000000;
context.xOffset = GoResampledProfileMsg_XOffset(profileMsg) / 1000;
context.zOffset = GoResampledProfileMsg_ZOffset(profileMsg) / 1000;
short[] points = new short[profilePointCount];
ProfilePoint[] profileBuffer = new ProfilePoint[profilePointCount];
IntPtr pointsPtr = GoResampledProfileMsg_At(profileMsg, k);
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 GoDataMessageTypes.GO_DATA_MESSAGE_TYPE_PROFILE:
{
profileMsg = dataObj;
Console.WriteLine(" Profile Message batch count: {0}", GoProfileMsg_Count(profileMsg));
for (UInt32 k = 0; k < GoProfileMsg_Count(profileMsg); ++k)
{
int validPointCount = 0;
UInt32 profilePointCount = GoProfileMsg_Width(profileMsg);
Console.WriteLine(" Item[{0}]: Profile data ({1} points)", i, GoProfileMsg_Width(profileMsg));
context.xResolution = GoProfileMsg_XResolution(profileMsg) / 1000000;
context.zResolution = GoProfileMsg_ZResolution(profileMsg) / 1000000;
context.xOffset = GoProfileMsg_XOffset(profileMsg) / 1000;
context.zOffset = GoProfileMsg_ZOffset(profileMsg) / 1000;
GoPoints[] points = new GoPoints[profilePointCount];
ProfilePoint[] profileBuffer = new ProfilePoint[profilePointCount];
int structSize = Marshal.SizeOf(typeof(GoPoints));
IntPtr pointsPtr = GoProfileMsg_At(profileMsg, k);
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 GoDataMessageTypes.GO_DATA_MESSAGE_TYPE_PROFILE_INTENSITY:
{
profileMsg = dataObj;
Console.WriteLine(" Profile Intensity Message batch count: {0}", GoProfileIntensityMsg_Count(profileMsg));
for (UInt32 k = 0; k < GoProfileIntensityMsg_Count(profileMsg); ++k)
{
byte[] intensity = new byte[GoProfileIntensityMsg_Width(profileMsg)];
intensityPtr = GoProfileIntensityMsg_At(profileMsg, k);
Marshal.Copy(intensityPtr, intensity, 0, intensity.Length);
}
}
break;
}
}
}
if ((status = (kStatus)GoSystem_Stop(system)) != kStatus.kOK)
{
Console.Write("GoSystem_Stop Error:{0}", (int)status);
return((int)status);
}
if (GoSensor_HasBuddy(sensor))
{
if ((status = (kStatus)GoSensor_RemoveBuddy(sensor)) != kStatus.kOK)
{
Console.Write("GoSensor_RemoveBuddy Error:{0}", (int)status);
return((int)status);
}
Console.WriteLine("Buddy sensor removed.");
}
// destroy handles
GoDestroy(system);
GoDestroy(api);
// wait for ESC key
Console.WriteLine("\nPress ENTER to continue");
do
{
System.Threading.Thread.Sleep(100);
} while (Console.Read() != (int)ConsoleKey.Enter);
return((int)kStatus.kOK);
}
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);
}
