internal static void ThrowIfNotOK(NatNetError result, string message)
{
if (result != NatNetError.NatNetError_OK)
{
throw new NatNetException(message + " (" + result.ToString() + ")");
}
}
/// <summary>
/// Called by both the <see cref="IDisposable.Dispose()"/> override,
/// as well as the finalizer, to do the actual cleanup work.
/// </summary>
/// <param name="disposing">
/// True if <see cref="Dispose()"/> was called explicitly. False if
/// running as part of the finalizer. If false, do not attempt to
/// reference other managed objects, since they may have already been
/// finalized themselves.
/// </param>
protected virtual void Dispose(bool disposing)
{
if (m_disposed)
{
return;
}
// Disconnect if necessary.
if (Connected)
{
NatNetError disconnectResult = NatNetLib.NativeMethods.NatNet_Client_Disconnect(m_clientHandle);
if (disconnectResult != NatNetError.NatNetError_OK)
{
System.Diagnostics.Debug.WriteLine("NatNet_Client_Disconnect returned " + disconnectResult.ToString() + ".");
}
Connected = false;
}
// Now destroy the native client.
NatNetError destroyResult = NatNetLib.NativeMethods.NatNet_Client_Destroy(m_clientHandle);
if (destroyResult != NatNetError.NatNetError_OK)
{
System.Diagnostics.Debug.WriteLine("NatNet_Client_Destroy returned " + destroyResult.ToString() + ".");
}
m_clientHandle = IntPtr.Zero;
m_disposed = true;
}
public NatNetServerDiscovery(IEnumerable <string> knownServerAddresses = null)
{
m_nativeCallbackHandler = ServerDiscoveredNativeThunk;
if (knownServerAddresses == null)
{
NatNetError result = NatNetLib.NativeMethods.NatNet_CreateAsyncServerDiscovery(out m_discoveryHandle, m_nativeCallbackHandler);
NatNetException.ThrowIfNotOK(result, "NatNet_CreateAsyncServerDiscovery failed.");
if (m_discoveryHandle == IntPtr.Zero)
{
throw new NatNetException("NatNet_CreateAsyncServerDiscovery returned null handle.");
}
}
else
{
NatNetError result = NatNetLib.NativeMethods.NatNet_CreateAsyncServerDiscovery(out m_discoveryHandle, m_nativeCallbackHandler, IntPtr.Zero, false);
NatNetException.ThrowIfNotOK(result, "NatNet_CreateAsyncServerDiscovery failed.");
foreach (string serverAddress in knownServerAddresses)
{
result = NatNetLib.NativeMethods.NatNet_AddDirectServerToAsyncDiscovery(m_discoveryHandle, serverAddress);
NatNetException.ThrowIfNotOK(result, "NatNet_AddDirectServerToAsyncDiscovery failed.");
}
result = NatNetLib.NativeMethods.NatNet_StartAsyncDiscovery(m_discoveryHandle);
NatNetException.ThrowIfNotOK(result, "NatNet_StartAsyncDiscovery failed.");
}
}
public void RemoteTrigger(string command)
{
ThrowIfDisposed();
// TODO: MarshalAs...
NatNetError retval = NatNetLib.NativeMethods.SendMessage(command);
NatNetException.ThrowIfNotOK(retval, "NatNet_Client RemoteTrigger failed.");
}
public bool RequestCommand(string request, Int32 timeoutMs = 1000, Int32 numAttempts = 1)
{
ThrowIfDisposed();
IntPtr responsePtr;
Int32 responseLen;
NatNetError result = NatNetLib.NativeMethods.NatNet_Client_Request(m_clientHandle, request, out responsePtr, out responseLen, timeoutMs, numAttempts);
return(result == NatNetError.NatNetError_OK);
}
public NatNetError Request(string request, out IntPtr pResponse, out Int32 pResponseLenBytes, Int32 timeoutMs = 1000, Int32 numAttempts = 1)
{
ThrowIfDisposed();
NatNetError retval = NatNetLib.NativeMethods.NatNet_Client_Request(m_clientHandle, request, out pResponse, out pResponseLenBytes, timeoutMs, numAttempts);
NatNetException.ThrowIfNotOK(retval, "NatNet_Client_Request failed.");
return(retval);
}
public void Disconnect()
{
ThrowIfDisposed();
if (Connected)
{
NatNetError retval = NatNetLib.NativeMethods.NatNet_Client_Disconnect(m_clientHandle);
NatNetException.ThrowIfNotOK(retval, "NatNet_Client_Disconnect failed.");
Connected = false;
}
}
public NatNetClient(NatNetConnectionType connectionType)
{
NatNetError retval = NatNetLib.NativeMethods.NatNet_Client_Create(out m_clientHandle, connectionType);
NatNetException.ThrowIfNotOK(retval, "NatNet_Client_Create failed.");
if (m_clientHandle == IntPtr.Zero)
{
throw new NatNetException("NatNet_Client_Create returned null handle.");
}
// This ensures the reverse P/Invoke delegate passed to the native code stays alive.
m_nativeFrameReceivedHandler = FrameReceivedNativeThunk;
retval = NatNetLib.NativeMethods.NatNet_Client_SetFrameReceivedCallback(m_clientHandle, m_nativeFrameReceivedHandler);
NatNetException.ThrowIfNotOK(retval, "NatNet_Client_SetFrameReceivedCallback failed.");
}
public Int32 RequestInt32(string request, Int32 timeoutMs = 1000, Int32 numAttempts = 1)
{
ThrowIfDisposed();
IntPtr responsePtr;
Int32 responseLen;
NatNetError result = NatNetLib.NativeMethods.NatNet_Client_Request(m_clientHandle, request, out responsePtr, out responseLen, timeoutMs, numAttempts);
NatNetException.ThrowIfNotOK(result, "NatNet_Client_Request failed.");
if (responseLen != Marshal.SizeOf(typeof(Int32)))
{
throw new NatNetException("Response has incorrect length");
}
Int32[] responseArray = { Int32.MinValue };
Marshal.Copy(responsePtr, responseArray, 0, 1);
return(responseArray[0]);
}
/// <summary>
/// Called by both the <see cref="IDisposable.Dispose()"/> override,
/// as well as the finalizer, to do the actual cleanup work.
/// </summary>
/// <param name="disposing">
/// True if <see cref="Dispose()"/> was called explicitly. False if
/// running as part of the finalizer. If false, do not attempt to
/// reference other managed objects, since they may have already been
/// finalized themselves.
/// </param>
protected virtual void Dispose(bool disposing)
{
if (m_disposed)
{
return;
}
// Now destroy the native discovery handle.
NatNetError destroyResult = NatNetLib.NativeMethods.NatNet_FreeAsyncServerDiscovery(m_discoveryHandle);
if (destroyResult != NatNetError.NatNetError_OK)
{
System.Diagnostics.Debug.WriteLine("NatNet_FreeAsyncServerDiscovery returned " + destroyResult.ToString() + ".");
}
m_discoveryHandle = IntPtr.Zero;
m_disposed = true;
}
public void Connect(IPAddress localAddress,
IPAddress serverAddress,
UInt16 serverCommandPort = NatNetConstants.DefaultCommandPort,
UInt16 serverDataPort = NatNetConstants.DefaultDataPort,
IPAddress multicastAddress = null)
{
ThrowIfDisposed();
sNatNetClientConnectParams initParams = new sNatNetClientConnectParams {
LocalAddress = localAddress.ToString(),
ServerAddress = serverAddress.ToString(),
ServerCommandPort = serverCommandPort,
ServerDataPort = serverDataPort,
MulticastAddress = multicastAddress == null ? null : multicastAddress.ToString()
};
NatNetError retval = NatNetLib.NativeMethods.NatNet_Client_Connect(m_clientHandle, ref initParams);
NatNetException.ThrowIfNotOK(retval, "NatNet_Client_Connect failed.");
Connected = true;
}
public DataDescriptions GetDataDescriptions()
{
ThrowIfDisposed();
IntPtr pDataDescriptions;
NatNetError retval = NatNetLib.NativeMethods.NatNet_Client_GetDataDescriptionList(m_clientHandle, out pDataDescriptions);
NatNetException.ThrowIfNotOK(retval, "NatNet_Client_GetDataDescriptions failed.");
sDataDescriptions dataDescriptions = (sDataDescriptions)Marshal.PtrToStructure(pDataDescriptions, typeof(sDataDescriptions));
// Do a quick first pass to determine the required capacity for the returned lists.
Int32 numMarkerSetDescs = 0;
Int32 numRigidBodyDescs = 0;
Int32 numSkeletonDescs = 0;
Int32 numForcePlateDescs = 0;
for (Int32 i = 0; i < dataDescriptions.DataDescriptionCount; ++i)
{
sDataDescription desc = dataDescriptions.DataDescriptions[i];
switch (desc.DescriptionType)
{
case (Int32)NatNetDataDescriptionType.NatNetDataDescriptionType_MarkerSet:
++numMarkerSetDescs;
break;
case (Int32)NatNetDataDescriptionType.NatNetDataDescriptionType_RigidBody:
++numRigidBodyDescs;
break;
case (Int32)NatNetDataDescriptionType.NatNetDataDescriptionType_Skeleton:
++numSkeletonDescs;
break;
case (Int32)NatNetDataDescriptionType.NatNetDataDescriptionType_ForcePlate:
++numForcePlateDescs;
break;
}
}
// Allocate the lists to be returned based on our counts.
DataDescriptions retDescriptions = new DataDescriptions {
MarkerSetDescriptions = new List <sMarkerSetDescription>(numMarkerSetDescs),
RigidBodyDescriptions = new List <sRigidBodyDescription>(numRigidBodyDescs),
SkeletonDescriptions = new List <sSkeletonDescription>(numSkeletonDescs),
ForcePlateDescriptions = new List <sForcePlateDescription>(numForcePlateDescs),
};
// Now populate the lists.
for (Int32 i = 0; i < dataDescriptions.DataDescriptionCount; ++i)
{
sDataDescription desc = dataDescriptions.DataDescriptions[i];
switch (desc.DescriptionType)
{
case (Int32)NatNetDataDescriptionType.NatNetDataDescriptionType_MarkerSet:
sMarkerSetDescription markerSetDesc = (sMarkerSetDescription)Marshal.PtrToStructure(desc.Description, typeof(sMarkerSetDescription));
retDescriptions.MarkerSetDescriptions.Add(markerSetDesc);
break;
case (Int32)NatNetDataDescriptionType.NatNetDataDescriptionType_RigidBody:
sRigidBodyDescription rigidBodyDesc = (sRigidBodyDescription)Marshal.PtrToStructure(desc.Description, typeof(sRigidBodyDescription));
retDescriptions.RigidBodyDescriptions.Add(rigidBodyDesc);
break;
case (Int32)NatNetDataDescriptionType.NatNetDataDescriptionType_Skeleton:
sSkeletonDescription skeletonDesc = (sSkeletonDescription)Marshal.PtrToStructure(desc.Description, typeof(sSkeletonDescription));
retDescriptions.SkeletonDescriptions.Add(skeletonDesc);
break;
case (Int32)NatNetDataDescriptionType.NatNetDataDescriptionType_ForcePlate:
sForcePlateDescription forcePlateDesc = (sForcePlateDescription)Marshal.PtrToStructure(desc.Description, typeof(sForcePlateDescription));
retDescriptions.ForcePlateDescriptions.Add(forcePlateDesc);
break;
}
}
return(retDescriptions);
}
/// <summary>
/// Event handler for NatNet frame delivery. Updates our simplified state representations.
/// NOTE: This executes in the context of the NatNetLib network service thread!
/// </summary>
/// <remarks>
/// Because the <see cref="sFrameOfMocapData"/> type is expensive to marshal, we instead utilize the
/// <see cref="NatNetClient.NativeFrameReceivedEventArgs.NativeFramePointer"/>, treating it as as opaque, and
/// passing it to some helper "accessor" functions to retrieve the subset of data we care about, using only
/// blittable types which do not cause any garbage to be allocated.
/// </remarks>
/// <param name="sender"></param>
/// <param name="eventArgs"></param>
private void OnNatNetFrameReceived(object sender, NatNetClient.NativeFrameReceivedEventArgs eventArgs)
{
// In the event of contention, drop the frame being delivered and return immediately.
// We don't want to stall NatNetLib's internal network service thread.
if (!Monitor.TryEnter(m_frameDataUpdateLock))
{
return;
}
try
{
// Update health markers.
m_receivedFrameSinceConnect = true;
Interlocked.Exchange(ref m_lastFrameDeliveryTimestamp.m_ticks, OptitrackHiResTimer.Now().m_ticks);
// Process received frame.
IntPtr pFrame = eventArgs.NativeFramePointer;
NatNetError result = NatNetError.NatNetError_OK;
// Update rigid bodies.
Int32 frameRbCount;
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetRigidBodyCount(pFrame, out frameRbCount);
NatNetException.ThrowIfNotOK(result, "NatNet_Frame_GetRigidBodyCount failed.");
for (int rbIdx = 0; rbIdx < frameRbCount; ++rbIdx)
{
sRigidBodyData rbData = new sRigidBodyData();
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetRigidBody(pFrame, rbIdx, out rbData);
NatNetException.ThrowIfNotOK(result, "NatNet_Frame_GetRigidBody failed.");
bool bTrackedThisFrame = (rbData.Params & 0x01) != 0;
if (bTrackedThisFrame == false)
{
continue;
}
// Ensure we have a state corresponding to this rigid body ID.
OptitrackRigidBodyState rbState = GetOrCreateRigidBodyState(rbData.Id);
rbState.DeliveryTimestamp = OptitrackHiResTimer.Now();
// Flip coordinate handedness from right to left by inverting X and W.
rbState.Pose.Position = new Vector3(-rbData.X, rbData.Y, rbData.Z);
rbState.Pose.Orientation = new Quaternion(-rbData.QX, rbData.QY, rbData.QZ, -rbData.QW);
}
// Update skeletons.
Int32 frameSkeletonCount;
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetSkeletonCount(pFrame, out frameSkeletonCount);
NatNetException.ThrowIfNotOK(result, "NatNet_Frame_GetSkeletonCount failed.");
for (int skelIdx = 0; skelIdx < frameSkeletonCount; ++skelIdx)
{
Int32 skeletonId;
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_Skeleton_GetId(pFrame, skelIdx, out skeletonId);
NatNetException.ThrowIfNotOK(result, "NatNet_Frame_Skeleton_GetId failed.");
// Ensure we have a state corresponding to this skeleton ID.
OptitrackSkeletonState skelState = GetOrCreateSkeletonState(skeletonId);
// Enumerate this skeleton's bone rigid bodies.
Int32 skelRbCount;
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_Skeleton_GetRigidBodyCount(pFrame, skelIdx, out skelRbCount);
NatNetException.ThrowIfNotOK(result, "NatNet_Frame_Skeleton_GetRigidBodyCount failed.");
for (int boneIdx = 0; boneIdx < skelRbCount; ++boneIdx)
{
sRigidBodyData boneData = new sRigidBodyData();
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_Skeleton_GetRigidBody(pFrame, skelIdx, boneIdx, out boneData);
NatNetException.ThrowIfNotOK(result, "NatNet_Frame_Skeleton_GetRigidBody failed.");
// In the context of frame data (unlike in the definition data), this ID value is a
// packed composite of both the asset/entity (skeleton) ID and member (bone) ID.
Int32 boneSkelId, boneId;
NaturalPoint.NatNetLib.NativeMethods.NatNet_DecodeID(boneData.Id, out boneSkelId, out boneId);
// TODO: Could pre-populate this map when the definitions are retrieved.
// Should never allocate after the first frame, at least.
if (skelState.BonePoses.ContainsKey(boneId) == false)
{
skelState.BonePoses[boneId] = new OptitrackPose();
}
// Flip coordinate handedness from right to left by inverting X and W.
skelState.BonePoses[boneId].Position = new Vector3(-boneData.X, boneData.Y, boneData.Z);
skelState.BonePoses[boneId].Orientation = new Quaternion(-boneData.QX, boneData.QY, boneData.QZ, -boneData.QW);
}
}
// Update markers
Int32 MarkerCount;
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetLabeledMarkerCount(pFrame, out MarkerCount);
NatNetException.ThrowIfNotOK(result, "NatNet_Frame_GetSkeletonCount failed.");
m_latestMarkerStates.Clear();
for (int markerIdx = 0; markerIdx < MarkerCount; ++markerIdx)
{
sMarker marker = new sMarker();
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetLabeledMarker(pFrame, markerIdx, out marker);
NatNetException.ThrowIfNotOK(result, "NatNet_Frame_GetLabeledMarker failed.");
// Flip coordinate handedness
Vector3 markerPos = new Vector3(-marker.X, marker.Y, marker.Z);
OptitrackMarkerState markerState = GetOrCreateMarkerState(marker.Id);
markerState.Position = markerPos;
markerState.Size = marker.Size;
markerState.Labeled = (marker.Params & 0x10) == 0;
markerState.Id = marker.Id;
}
}
catch (Exception ex)
{
Debug.LogError(GetType().FullName + ": OnNatNetFrameReceived encountered an exception.", this);
Debug.LogException(ex, this);
}
finally
{
Monitor.Exit(m_frameDataUpdateLock);
}
}
