コード例 #1
0
 internal static void ThrowIfNotOK(NatNetError result, string message)
 {
     if (result != NatNetError.NatNetError_OK)
     {
         throw new NatNetException(message + " (" + result.ToString() + ")");
     }
 }
コード例 #2
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;
            }

            // 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;
        }
コード例 #3
0
ファイル: Client.cs プロジェクト: chobitsfan/MultiDrone
        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.");
            }
        }
コード例 #4
0
        public void RemoteTrigger(string command)
        {
            ThrowIfDisposed();
            // TODO: MarshalAs...

            NatNetError retval = NatNetLib.NativeMethods.SendMessage(command);

            NatNetException.ThrowIfNotOK(retval, "NatNet_Client RemoteTrigger failed.");
        }
コード例 #5
0
ファイル: Client.cs プロジェクト: chobitsfan/MultiDrone
        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);
        }
コード例 #6
0
ファイル: Client.cs プロジェクト: chobitsfan/MultiDrone
        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);
        }
コード例 #7
0
        public void Disconnect()
        {
            ThrowIfDisposed();

            if (Connected)
            {
                NatNetError retval = NatNetLib.NativeMethods.NatNet_Client_Disconnect(m_clientHandle);
                NatNetException.ThrowIfNotOK(retval, "NatNet_Client_Disconnect failed.");

                Connected = false;
            }
        }
コード例 #8
0
        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.");
        }
コード例 #9
0
ファイル: Client.cs プロジェクト: chobitsfan/MultiDrone
        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]);
        }
コード例 #10
0
ファイル: Client.cs プロジェクト: chobitsfan/MultiDrone
        /// <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;
        }
コード例 #11
0
        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;
        }
コード例 #12
0
        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);
        }
コード例 #13
0
    /// <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);
        }
    }