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());
}
}
// 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);
}
// 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);
}
/// <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);
}
}
