Exemple #1
0
    System.Collections.IEnumerator CheckConnectionHealth()
    {
        const float kHealthCheckIntervalSeconds  = 1.0f;
        const float kRecentFrameThresholdSeconds = 5.0f;

        // The lifespan of these variables is tied to the lifespan of a single connection session.
        // The coroutine is stopped on disconnect and restarted on connect.
        YieldInstruction checkIntervalYield = new WaitForSeconds(kHealthCheckIntervalSeconds);

        OptitrackHiResTimer.Timestamp connectionInitiatedTimestamp = OptitrackHiResTimer.Now();
        OptitrackHiResTimer.Timestamp lastFrameReceivedTimestamp;
        bool wasReceivingFrames      = false;
        bool warnedPendingFirstFrame = false;

        while (true)
        {
            yield return(checkIntervalYield);

            if (m_receivedFrameSinceConnect == false)
            {
                // Still waiting for first frame. Warn exactly once if this takes too long.
                if (connectionInitiatedTimestamp.AgeSeconds > kRecentFrameThresholdSeconds)
                {
                    if (warnedPendingFirstFrame == false)
                    {
                        Debug.LogWarning(GetType().FullName + ": No frames received from the server yet. Verify your connection settings are correct and that the server is streaming.", this);
                        warnedPendingFirstFrame = true;
                    }

                    continue;
                }
            }
            else
            {
                // We've received at least one frame, do ongoing checks for changes in connection health.
                lastFrameReceivedTimestamp.m_ticks = Interlocked.Read(ref m_lastFrameDeliveryTimestamp.m_ticks);
                bool receivedRecentFrame = lastFrameReceivedTimestamp.AgeSeconds < kRecentFrameThresholdSeconds;

                if (wasReceivingFrames == false && receivedRecentFrame == true)
                {
                    // Transition: Bad health -> good health.
                    wasReceivingFrames = true;
                    Debug.Log(GetType().FullName + ": Receiving streaming data from the server.", this);
                    continue;
                }
                else if (wasReceivingFrames == true && receivedRecentFrame == false)
                {
                    // Transition: Good health -> bad health.
                    wasReceivingFrames = false;
                    Debug.LogWarning(GetType().FullName + ": No streaming frames received from the server recently.", this);
                    continue;
                }
            }
        }
    }
 //Checks if OptiTrack data is stale
 public bool isSeen()
 {
     if (rbState == null || OptitrackHiResTimer.Now().SecondsSince(rbState.DeliveryTimestamp) > 0.1)
     {
         return(false);
     }
     else
     {
         return(true);
     }
 }
    // Handles the backtracking and saves the part of the throw that happened
    // before the detection occurred
    void HandleThrow()
    {
        if (useOptitrackTimestamp)
        {
            throwStartTime = OptitrackHiResTimer.Now().SecondsSince(customRB.zeroTime) - throwBacktrackingTime;
        }
        else
        {
            throwStartTime = Time.time - throwBacktrackingTime;
        }
        int firstThrowIndex = getBufferIndexFromTime(captureBuffer, throwStartTime);

        throwMode = 2;
        Debug.Log("Throw just began!");

        //get first FrisbeeLocation in throw
        FrisbeeLocation temp = captureBuffer.Get(firstThrowIndex);
        FrisbeeLocation prev = null;

        throwStartPos  = temp.pos;
        throwStartTime = temp.time;

        //reinitialize throwBuffer to empty list
        throwBuffer = new List <FrisbeeLocation>();

        //Transfer previous throwBacktrackingTime seconds of data to throwBuffer
        //to make sure that the beginning of the throw is not lost
        //TODO: maybe add a more sophisticated backtracking algorithm that is based on velocity
        for (int i = firstThrowIndex; i < captureBuffer.Count; i++)
        {
            temp = captureBuffer.Get(i);
            //manually initialize first timestamp to zero
            if (i == firstThrowIndex)
            {
                throwBuffer.Add(new FrisbeeLocation(temp.rot, temp.pos, 0F, 0F, 0F, temp.wasSeen));
            }
            else
            {
                if (i - firstThrowIndex >= 2)
                {
                    prev = throwBuffer[throwBuffer.Count - 2];
                }
                else
                {
                    prev = throwBuffer[throwBuffer.Count - 1];
                }
                float currentThrowTime = temp.time - throwStartTime; //rest of the timestamps will increment from zero
                //add backtracked points to throwBuffer
                throwBuffer.Add(new FrisbeeLocation(temp.rot, temp.pos, currentThrowTime, 0F, 0F, temp.wasSeen));
            }
            //Debug.Log("ThrowBuffer: " + throwBuffer[throwBuffer.Count - 1].rotSpeed.ToString());
        }
    }
Exemple #4
0
    // Use this for initialization
    void Start()
    {
        reference = OptitrackHiResTimer.Now();
        // Get the camera component
        Camera cam = GetComponentInChildren <Camera>();

        cam.nearClipPlane = 0.000001f;
        // set the OSC communication
        osc.SetAddressHandler("/Close", OnReceiveStop);
        writer = new StreamWriter(Paths.recording_path, true);
        mouse2 = GameObject.CreatePrimitive(PrimitiveType.Sphere);
        mouse2.SetActive(false);
    }
    // Use this for initialization
    void Start()
    {
        // get the reference timer
        reference = OptitrackHiResTimer.Now();
        // Set up the camera (so it doesn't clip objects too close to the mouse)
        Camera cam = GetComponentInChildren <Camera>();

        cam.nearClipPlane = 0.000001f;
        // set the OSC communication
        osc.SetAddressHandler("/Close", OnReceiveStop);
        // Set the writer
        writer = new StreamWriter(Paths.recording_path, true);
    }
Exemple #6
0
    // Use this for initialization
    void Start()
    {
        // Force full screen on projector
        Screen.fullScreen = true;

        // Get the reference timer
        reference = OptitrackHiResTimer.Now();

        // set the OSC communication
        osc.SetAddressHandler("/Close", OnReceiveStop);

        // Set up the camera (so it doesn't clip objects too close to the mouse)
        Camera cam = GetComponentInChildren <Camera>();

        cam.nearClipPlane = 0.000001f;

        // Set the writer
        writer = new StreamWriter(Paths.recording_path, true);

        // Get cricket object array sorted by name/number
        CricketObjs = FindObsWithTag("Cricket");
    }
    /// <summary>Get the most recently received state for the specified rigid body.</summary>
    /// <param name="rigidBodyId">Corresponds to the "User ID" field in Motive.</param>
    /// <returns>The most recent available state, or null if none available.</returns>
    public OptitrackRigidBodyState GetLatestRigidBodyState(Int32 rigidBodyId, bool networkCompensation = true)
    {
        OptitrackRigidBodyState rbState;

        if (!networkCompensation || m_client == null)
        {
            lock ( m_frameDataUpdateLock )
            {
                m_latestRigidBodyStates.TryGetValue(rigidBodyId, out rbState);
            }
        }
        else
        {
            sRigidBodyData rbData;
            m_client.GetPredictedRigidBodyPose(rigidBodyId, out rbData, 0.0);

            rbState = new OptitrackRigidBodyState();
            RigidBodyDataToState(rbData, OptitrackHiResTimer.Now(), rbState);
        }

        return(rbState);
    }
Exemple #8
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);
        }
    }