void m_NatNet_OnFrameReady(NatNetML.FrameOfMocapData data, NatNetML.NatNetClientML client)
{
// [optional] High-resolution frame arrival timing information
Int64 currTime = timer.Value;
if (lastTime != 0)
{
// Get time elapsed in tenths of a millisecond.
Int64 timeElapsedInTicks = currTime - lastTime;
Int64 timeElapseInTenthsOfMilliseconds = (timeElapsedInTicks * 10000) / timer.Frequency;
// uncomment for timing info
//OutputMessage("Frame Delivered: (" + timeElapseInTenthsOfMilliseconds.ToString() + ") FrameTimestamp: " + data.fLatency);
}
// [NatNet] Add the incoming frame of mocap data to our frame queue,
// Note: the frame queue is a shared resource with the UI thread, so lock it while writing
lock (syncLock)
{
// [optional] clear the frame queue before adding a new frame
m_FrameQueue.Clear();
m_FrameQueue.Enqueue(data);
}
lastTime = currTime;
// MessageBox.Show(data.ToString());
}
static void fetchFrameData(NatNetML.FrameOfMocapData data, NatNetML.NatNetClientML client)
{
/* Exception handler for cases where assets are added or removed.
* Data description is re-obtained in the main function so that contents
* in the frame handler is kept minimal. */
if ((data.bTrackingModelsChanged == true || data.nRigidBodies != mRigidBodies.Count || data.nSkeletons != mSkeletons.Count || data.nForcePlates != mForcePlates.Count))
{
mAssetChanged = true;
}
/* Processing and ouputting frame data every 200th frame.
* This conditional statement is included in order to simplify the program output */
if (data.iFrame % 200 == 0)
{
if (data.bRecording == false)
{
Console.WriteLine("Frame #{0} Received:", data.iFrame);
}
else if (data.bRecording == true)
{
Console.WriteLine("[Recording] Frame #{0} Received:", data.iFrame);
}
processFrameData(data);
}
}
static void fetchFrameData(NatNetML.FrameOfMocapData data, NatNetML.NatNetClientML client)
{
if ((data.bTrackingModelsChanged == true || data.nRigidBodies != mRigidBodies.Count))
{
mAssetChanged = true;
}
/* Processing */
if (data.iFrame % 10 == 0)
{
if (data.bRecording == false)
{
Console.WriteLine("Frame #{0} Received:", data.iFrame);
}
else if (data.bRecording == true)
{
Console.WriteLine("[Recording] Frame #{0} Received:", data.iFrame);
}
//Console.WriteLine(index);
index = NatNetClient.processFrameData(data, index);
//Console.WriteLine(index);
if (index > 10)
{
Console.Write("helloworld");
mNatNet.OnFrameReady -= fetchFrameData;
}
}
}
void m_NatNet_OnFrameReady(NatNetML.FrameOfMocapData data, NatNetML.NatNetClientML client)
{
// measure time between frame arrival (inter frame)
m_FramePeriodTimer.Stop();
interframeDuration = m_FramePeriodTimer.Duration();
// measure processing time (intra frame)
m_ProcessingTimer.Start();
// process data
// NOTE! do as little as possible here as we're on the data servicing thread
ProcessFrameOfData(ref data);
// report if we are taking longer than a mocap frame time
// which eventually will back up the network receive buffer and result in frame drop
m_ProcessingTimer.Stop();
double appProcessingTimeMSecs = m_ProcessingTimer.Duration();
double mocapFramePeriodMSecs = (1.0f / m_ServerFramerate) * 1000.0f;
if (appProcessingTimeMSecs > mocapFramePeriodMSecs)
{
//changed for now
Console.WriteLine("Warning : Frame handler taking longer than frame period: " + appProcessingTimeMSecs.ToString("F2"));
}
m_FramePeriodTimer.Start();
}
public void Evaluate(int SpreadMax)
{
if (FInputInit[0])
{
if (m_NatNet != null)
{
m_NatNet.Disconnect();
}
m_NatNet = new NatNetML.NatNetClientML();
NatNetClientML.ConnectParams _params = new NatNetClientML.ConnectParams();
_params.ServerAddress = FInputIPServer[0];
_params.LocalAddress = FInputIPClient[0];
_params.ConnectionType = ConnectionType.Multicast;
returnCode = m_NatNet.Connect(_params);
returnCode = m_NatNet.GetServerDescription(desc);
FOutputStatus.SliceCount = 1;
FOutputStatus[0] = "";
m_NatNet.OnFrameReady += new NatNetML.FrameReadyEventHandler(m_NatNet_OnFrameReady);
//Version info
int[] ver = new int[4];
ver = m_NatNet.NatNetVersion();
FOutputVersion.SliceCount = 1;
FOutputVersion[0] = String.Format("{0}.{1}.{2}.{3}", ver[0], ver[1], ver[2], ver[3]);
FOutputClient.SliceCount = 1;
FOutputClient[0] = m_NatNet;
}
}
/// <summary>
/// Create a new NatNet client, which manages all communication with the NatNet server (e.g. Motive)
/// </summary>
/// <param name="iConnectionType">0 = Multicast, 1 = Unicast</param>
/// <returns></returns>
private int CreateClient(int iConnectionType)
{
// release any previous instance
if (m_NatNet != null)
{
m_NatNet.Uninitialize();
}
// [NatNet] create a new NatNet instance
m_NatNet = new NatNetML.NatNetClientML(iConnectionType);
// [NatNet] set a "Frame Ready" callback function (event handler) handler that will be
// called by NatNet when NatNet receives a frame of data from the server application
m_NatNet.OnFrameReady += new NatNetML.FrameReadyEventHandler(m_NatNet_OnFrameReady);
/*
* // [NatNet] for testing only - event signature format required by some types of .NET applications (e.g. MatLab)
* m_NatNet.OnFrameReady2 += new FrameReadyEventHandler2(m_NatNet_OnFrameReady2);
*/
// [NatNet] print version info
int[] ver = new int[4];
ver = m_NatNet.NatNetVersion();
String strVersion = String.Format("NatNet Version : {0}.{1}.{2}.{3}", ver[0], ver[1], ver[2], ver[3]);
OutputMessage(strVersion);
return(0);
}
static void fetchFrameData(NatNetML.FrameOfMocapData data, NatNetML.NatNetClientML client)
{
if ((data.bTrackingModelsChanged == true || data.nRigidBodies != mRigidBodies.Count))
{
mAssetChanged = true;
}
/* Processing */
int index = 1;
if (data.iFrame % 1 == 0)
{
if (data.bRecording == false)
{
Console.WriteLine("Frame #{0} Received:", data.iFrame);
}
else if (data.bRecording == true)
{
Console.WriteLine("[Recording] Frame #{0} Received:", data.iFrame);
}
index = NatNetClient.processFrameData(data, index);
}
}
// [NatNet] m_NatNet_OnFrameReady will be called when a frame of Mocap
// data has is received from the server application.
//
// Note: This callback is on the network service thread, so it is
// important to return from this function quickly as possible
// to prevent incoming frames of data from buffering up on the
// network socket.
//
// Note: "data" is a reference structure to the current frame of data.
// NatNet re-uses this same instance for each incoming frame, so it should
// not be kept (the values contained in "data" will become replaced after
// this callback function has exited).
void m_NatNet_OnFrameReady(NatNetML.FrameOfMocapData data, NatNetML.NatNetClientML client)
{
lock (syncLock)
{
m_FrameQueue.Clear();
m_FrameQueue.Enqueue(data);
}
}
//Constructor
public DeviceOptiTrackNode() {
//The client argument:
// 0: Multicast
// 1: Unicast
mNatNet = new NatNetClientML(0);
mFrameOfData = new NatNetML.FrameOfMocapData();
m_FrameQueue = new Queue<NatNetML.FrameOfMocapData>();
mDesc = new NatNetML.ServerDescription();
// [NatNet] set a "Frame Ready" callback function (event handler) handler that will be
// called by NatNet when NatNet receives a frame of data from the server application
mNatNet.OnFrameReady += new NatNetML.FrameReadyEventHandler(m_NatNet_OnFrameReady);
//mNatNet.OnFrameReady2 += new FrameReadyEventHandler2(m_NatNet_OnFrameReady2);
}
void m_NatNet_OnFrameReady(NatNetML.FrameOfMocapData data, NatNetML.NatNetClientML client)
{
m_FrameQueue.Clear();
FrameOfMocapData deepCopy = new FrameOfMocapData(data);
m_FrameQueue.Enqueue(deepCopy);
FOutputFrameData.SliceCount = 1;
FOutputFrameData[0] = deepCopy;
FOutputTimestamp.SliceCount = 1;
FOutputTimestamp[0] = deepCopy.fTimestamp;
FOutputFrameCount.SliceCount = 1;
FOutputFrameCount[0] = deepCopy.iFrame;
}
//Constructor
public DeviceOptiTrackNode()
{
//The client argument:
// 0: Multicast
// 1: Unicast
mNatNet = new NatNetClientML(0);
mFrameOfData = new NatNetML.FrameOfMocapData();
m_FrameQueue = new Queue <NatNetML.FrameOfMocapData>();
mDesc = new NatNetML.ServerDescription();
// [NatNet] set a "Frame Ready" callback function (event handler) handler that will be
// called by NatNet when NatNet receives a frame of data from the server application
mNatNet.OnFrameReady += new NatNetML.FrameReadyEventHandler(m_NatNet_OnFrameReady);
//mNatNet.OnFrameReady2 += new FrameReadyEventHandler2(m_NatNet_OnFrameReady2);
}
// [NatNet] m_NatNet_OnFrameReady will be called when a frame of Mocap
// data has is received from the server application.
//
// Note: This callback is on the network service thread, so it is
// important to return from this function quickly as possible
// to prevent incoming frames of data from buffering up on the
// network socket.
//
// Note: "data" is a reference structure to the current frame of data.
// NatNet re-uses this same instance for each incoming frame, so it should
// not be kept (the values contained in "data" will become replaced after
// this callback function has exited).
void m_NatNet_OnFrameReady(NatNetML.FrameOfMocapData data, NatNetML.NatNetClientML client)
{
Int64 currTime = timer.Value;
if (lastTime != 0)
{
// Get time elapsed in tenths of a millisecond.
Int64 timeElapsedInTicks = currTime - lastTime;
Int64 timeElapseInTenthsOfMilliseconds = (timeElapsedInTicks * 10000) / timer.Frequency;
// uncomment for timing info
//OutputMessage("Frame Delivered: (" + timeElapseInTenthsOfMilliseconds.ToString() + ") FrameTimestamp: " + data.fLatency);
}
lock (syncLock)
{
m_FrameQueue.Clear();
m_FrameQueue.Enqueue(data);
}
lastTime = currTime;
}
private static void connectToServer()
{
/* [NatNet] Instantiate the client object */
mNatNet = new NatNetML.NatNetClientML();
/* [NatNet] Checking verions of the NatNet SDK library */
int[] verNatNet = new int[4]; // Saving NatNet SDK version number
verNatNet = mNatNet.NatNetVersion();
Console.WriteLine("NatNet SDK Version: {0}.{1}.{2}.{3}", verNatNet[0], verNatNet[1], verNatNet[2], verNatNet[3]);
/* [NatNet] Connecting to the Server */
Console.WriteLine("\nConnecting...\n\tLocal IP address: {0}\n\tServer IP Address: {1}\n\n", mStrLocalIP, mStrServerIP);
NatNetClientML.ConnectParams connectParams = new NatNetClientML.ConnectParams();
connectParams.ConnectionType = mConnectionType;
connectParams.ServerAddress = mStrServerIP;
connectParams.LocalAddress = mStrLocalIP;
mNatNet.Connect(connectParams);
}
/// <summary>
/// [NatNet] m_NatNet_OnFrameReady will be called when a frame of Mocap
/// data has is received from the server application.
///
/// Note: This callback is on the network service thread, so it is
/// important to return from this function quickly as possible
/// to prevent incoming frames of data from buffering up on the
/// network socket.
///
/// Note: "data" is a reference structure to the current frame of data.
/// NatNet re-uses this same instance for each incoming frame, so it should
/// not be kept (the values contained in "data" will become replaced after
/// this callback function has exited).
/// </summary>
/// <param name="data">The actual frame of mocap data</param>
/// <param name="client">The NatNet client instance</param>
void m_NatNet_OnFrameReady(NatNetML.FrameOfMocapData data, NatNetML.NatNetClientML client)
{
double elapsedIntraMS = 0.0f;
QueryPerfCounter intraTimer = new QueryPerfCounter();
intraTimer.Start();
// check and report frame arrival period (time elapsed since previous frame arrived)
m_FramePeriodTimer.Stop();
double elapsedMS = m_FramePeriodTimer.Duration();
if ((mLastFrame % 100) == 0)
{
OutputMessage("FrameID:" + data.iFrame + " Timestamp: " + data.fTimestamp + " Period:" + elapsedMS);
}
// check and report frame drop
if ((mLastFrame != 0) && ((data.iFrame - mLastFrame) != 1))
{
OutputMessage("Frame Drop: ( ThisFrame: " + data.iFrame.ToString() + " LastFrame: " + mLastFrame.ToString() + " )");
}
// [NatNet] Add the incoming frame of mocap data to our frame queue,
// Note: the frame queue is a shared resource with the UI thread, so lock it while writing
lock (syncLock)
{
// [optional] clear the frame queue before adding a new frame
m_FrameQueue.Clear();
FrameOfMocapData deepCopy = new FrameOfMocapData(data);
m_FrameQueue.Enqueue(deepCopy);
}
intraTimer.Stop();
elapsedIntraMS = intraTimer.Duration();
if (elapsedIntraMS > 5.0f)
{
OutputMessage("Warning : Frame handler taking too long: " + elapsedIntraMS.ToString("F2"));
}
mLastFrame = data.iFrame;
m_FramePeriodTimer.Start();
}
private int CreateClient(int iConnectionType)
{
if (m_NatNet != null)
{
m_NatNet.Uninitialize();
}
m_NatNet = new NatNetML.NatNetClientML(iConnectionType);
// [NatNet] set a "Frame Ready" callback function (event handler) handler that will be
// called by NatNet when NatNet receives a frame of data from the server application
m_NatNet.OnFrameReady += new NatNetML.FrameReadyEventHandler(m_NatNet_OnFrameReady);
// [NatNet] print version info
int[] ver = new int[4];
ver = m_NatNet.NatNetVersion();
String strVersion = String.Format("NatNet Version : {0}.{1}.{2}.{3}", ver[0], ver[1], ver[2], ver[3]);
OutputMessage(strVersion);
return(0);
}
/// <summary>
/// [NatNet] m_NatNet_OnFrameReady will be called when a frame of Mocap
/// data has is received from the server application.
///
/// Note: This callback is on the network service thread, so it is
/// important to return from this function quickly as possible
/// to prevent incoming frames of data from buffering up on the
/// network socket.
///
/// Note: "data" is a reference structure to the current frame of data.
/// NatNet re-uses this same instance for each incoming frame, so it should
/// not be kept (the values contained in "data" will become replaced after
/// this callback function has exited).
/// </summary>
/// <param name="data">The actual frame of mocap data</param>
/// <param name="client">The NatNet client instance</param>
void m_NatNet_OnFrameReady(NatNetML.FrameOfMocapData data, NatNetML.NatNetClientML client)
{
double elapsedIntraMS = 0.0f;
QueryPerfCounter intraTimer = new QueryPerfCounter();
intraTimer.Start();
// detect and report and 'measured' frame drop (as measured by client)
m_FramePeriodTimer.Stop();
double elapsedMS = m_FramePeriodTimer.Duration();
ProcessFrameOfData(ref data);
// report if we are taking too long, which blocks packet receiving, which if long enough would result in socket buffer drop
intraTimer.Stop();
elapsedIntraMS = intraTimer.Duration();
if (elapsedIntraMS > 5.0f)
{
OutputMessage("Warning : Frame handler taking too long: " + elapsedIntraMS.ToString("F2"));
}
m_FramePeriodTimer.Start();
}
/// <summary>
/// [NatNet] parseFrameData will be called when a frame of Mocap
/// data has is received from the server application.
///
/// Note: This callback is on the network service thread, so it is
/// important to return from this function quickly as possible
/// to prevent incoming frames of data from buffering up on the
/// network socket.
///
/// Note: "data" is a reference structure to the current frame of data.
/// NatNet re-uses this same instance for each incoming frame, so it should
/// not be kept (the values contained in "data" will become replaced after
/// this callback function has exited).
/// </summary>
/// <param name="data">The actual frame of mocap data</param>
/// <param name="client">The NatNet client instance</param>
static void fetchFrameData(NatNetML.FrameOfMocapData data, NatNetML.NatNetClientML client)
{
/* Exception handler for cases where assets are added or removed.
* Data description is re-obtained in the main function so that contents
* in the frame handler is kept minimal. */
if ((data.bTrackingModelsChanged == true || data.nRigidBodies != mRigidBodies.Count || data.nSkeletons != mSkeletons.Count || data.nForcePlates != mForcePlates.Count))
{
mAssetChanged = true;
}
/* Processing and ouputting frame data every 200th frame.
* This conditional statement is included in order to simplify the program output */
//if(data.iFrame % 12 == 0) //camera 120 fps, but ardupilot limit data rate to 70ms
//for parrot bebop2, I can feed it with 14Hz. But for skyviper v2450, 5hz is max. Faster data seems overload its CPU
{
//if (data.bRecording == false)
// Console.WriteLine("Frame #{0} Received:", data.iFrame);
//else if (data.bRecording == true)
// Console.WriteLine("[Recording] Frame #{0} Received:", data.iFrame);
processFrameData(data);
}
}
/// <summary>
/// Create a new NatNet client, which manages all communication with the NatNet server (e.g. Motive)
/// </summary>
/// <param name="iConnectionType">0 = Multicast, 1 = Unicast</param>
/// <returns></returns>
private static int CreateClient(int iConnectionType)
{
// release any previous instance
if (m_NatNet != null)
{
m_NatNet.Uninitialize();
}
// [NatNet] create a new NatNet instance
m_NatNet = new NatNetML.NatNetClientML(iConnectionType);
// [NatNet] set a "Frame Ready" callback function (event handler) handler that will be
// called by NatNet when NatNet receives a frame of data from the server application
m_NatNet.OnFrameReady += new NatNetML.FrameReadyEventHandler(m_NatNet_OnFrameReady);
// [NatNet] print version info
int[] ver = new int[4];
ver = m_NatNet.NatNetVersion();
String strVersion = String.Format("NatNet Version : {0}.{1}.{2}.{3}", ver[0], ver[1], ver[2], ver[3]);
Console.WriteLine(strVersion);
return 0;
}
// [NatNet] m_NatNet_OnFrameReady will be called when a frame of Mocap data has is received
// from the server application.
void m_NatNet_OnFrameReady(NatNetML.FrameOfMocapData data, NatNetML.NatNetClientML client)
{
m_FrameQueue.Enqueue(data);
}
static void connectToServer()
{
/* [NatNet] Instantiate the client object */
m_NatNet = new NatNetML.NatNetClientML(iConnectionType);
/* [NatNet] Checking verions of the NatNet SDK library */
int[] verNatNet = new int[4]; // Saving NatNet SDK version number
verNatNet = m_NatNet.NatNetVersion();
Console.WriteLine("NatNet SDK Version: {0}.{1}.{2}.{3}", verNatNet[0], verNatNet[1], verNatNet[2], verNatNet[3]);
/* [NatNet] Connecting to the Server */
Console.WriteLine("\nConnecting...\n\tLocal IP address: {0}\n\tServer IP Address: {1}\n\n", strLocalIP, strServerIP);
m_NatNet.Initialize(strLocalIP, strServerIP);
}
private int CreateClient(int iConnectionType)
{
if(m_NatNet != null)
{
m_NatNet.Uninitialize();
}
m_NatNet = new NatNetML.NatNetClientML(iConnectionType);
// [NatNet] set a "Frame Ready" callback function (event handler) handler that will be
// called by NatNet when NatNet receives a frame of data from the server application
m_NatNet.OnFrameReady += new NatNetML.FrameReadyEventHandler(m_NatNet_OnFrameReady);
/*
// [NatNet] for testing only - event signature format required by some types of .NET applications (e.g. MatLab)
m_NatNet.OnFrameReady2 += new FrameReadyEventHandler2(m_NatNet_OnFrameReady2);
*/
// [NatNet] print version info
int[] ver = new int[4];
ver = m_NatNet.NatNetVersion();
String strVersion = String.Format("NatNet Version : {0}.{1}.{2}.{3}", ver[0], ver[1],ver[2],ver[3]);
OutputMessage(strVersion);
return 0;
}
